├── CMakeLists.txt ├── LICENSE.txt ├── License-gpl.txt ├── README.md ├── TODO.txt ├── Thirdparty ├── DBoW2 │ ├── CMakeLists.txt │ ├── DBoW2 │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── FClass.h │ │ ├── FORB.cpp │ │ ├── FORB.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── LICENSE.txt │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── LICENSE.txt │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── LICENSE.txt │ └── lib │ │ └── libDBoW2.so ├── g2o_EXTERNAL │ └── ceres │ │ ├── LICENSE │ │ ├── autodiff.h │ │ ├── autodiff.h~ │ │ ├── eigen.h │ │ ├── fixed_array.h │ │ ├── fpclassify.h │ │ ├── jet.h │ │ ├── macros.h │ │ ├── manual_constructor.h │ │ └── variadic_evaluate.h ├── libviso2 │ ├── CMakeLists.txt │ ├── include │ │ └── viso2 │ │ │ ├── MEstimator.h │ │ │ ├── TrackerData.h │ │ │ ├── filter.h │ │ │ ├── five-point.h │ │ │ ├── matcher.h │ │ │ ├── matrix.h │ │ │ ├── p_match.h │ │ │ ├── precomp.h │ │ │ ├── reconstruction.h │ │ │ ├── timer.h │ │ │ ├── triangle.h │ │ │ ├── viso.h │ │ │ ├── viso_mono.h │ │ │ └── viso_stereo.h │ ├── lib │ │ └── libviso2.so │ ├── readme.txt │ └── src │ │ ├── TrackerData.cpp │ │ ├── filter.cpp │ │ ├── five-point.cpp │ │ ├── matcher.cpp │ │ ├── matrix.cpp │ │ ├── ptsetreg.cpp │ │ ├── reconstruction.cpp │ │ ├── triangle.cpp │ │ ├── viso.cpp │ │ ├── viso_mono.cpp │ │ └── viso_stereo.cpp └── vikit_common │ ├── CMakeLists.txt │ ├── CMakeModules │ └── FindEigen.cmake │ ├── include │ └── vikit │ │ ├── abstract_camera.h │ │ ├── aligned_mem.h │ │ ├── math_utils.h │ │ ├── performance_monitor.h │ │ ├── pinhole_camera.h │ │ ├── timer.h │ │ └── vision.h │ ├── lib │ └── libvikit_common.so │ ├── src │ ├── math_utils.cpp │ ├── performance_monitor.cpp │ ├── pinhole_camera.cpp │ └── vision.cpp │ └── vikit_commonConfig.cmake.in ├── cmake_modules ├── FindCholmod.cmake ├── FindEigen3.cmake └── FindSuiteSparse.cmake ├── data ├── kittiseq00_imu_samplesnoisy.txt ├── rviz.rviz ├── rviz.vcg ├── settingfiles_stereo │ ├── kittiseq00.yaml │ └── tsukuba.yaml ├── settingfiles_stereo_imu │ ├── kittiseq00_imu.yaml │ └── tsukuba_imu.yaml ├── setttingfiles_mono │ ├── kittiseq00_mono.yaml │ └── tsukuba_mono.yaml └── tsukuba_imu_samplesnoisy.txt ├── g2o_types ├── GPSGrabber.cpp ├── GPSGrabber.h ├── IMUErrorModel.cpp ├── IMUErrorModel.h ├── IMU_constraint.cpp ├── IMU_constraint.h ├── PointStatistics.h ├── anchored_points.cpp ├── anchored_points.h ├── eigen_utils.cpp ├── eigen_utils.h ├── maths_utils.cpp ├── maths_utils.h ├── rand_sampler.cpp ├── rand_sampler.h ├── scale_solver.cpp ├── scale_solver.h ├── slidingwindow.cpp ├── slidingwindow.h ├── stopwatch.h ├── timegrabber.cpp └── timegrabber.h ├── include ├── Converter.h ├── FeatureGrid.h ├── Frame.h ├── FramePublisher.h ├── Initializer.h ├── KeyFrame.h ├── KeyFrameDatabase.h ├── LocalMapping.h ├── LoopClosing.h ├── Map.h ├── MapPoint.h ├── MapPublisher.h ├── MotionModel.hpp ├── ORBVocabulary.h ├── ORBextractor.h ├── ORBmatcher.h ├── Optimizer.h ├── PnPsolver.h ├── Sim3Solver.h ├── Tracking.h ├── config.h ├── global.h ├── qcv-feature.h ├── stereoSFM.h └── utils.h ├── package.xml └── src ├── Converter.cc ├── FeatureGrid.cpp ├── Frame.cc ├── FramePublisher.cc ├── Initializer.cc ├── KeyFrame.cc ├── KeyFrameDatabase.cc ├── LocalMapping.cc ├── LoopClosing.cc ├── Map.cc ├── MapPoint.cc ├── MapPublisher.cc ├── MotionModel.cpp ├── ORBextractor.cc ├── ORBmatcher.cc ├── Optimizer.cc ├── PnPsolver.cc ├── Sim3Solver.cc ├── Tracking.cc ├── config.cpp ├── main.cc ├── stereoSFM.cpp └── utils.cpp /LICENSE.txt: -------------------------------------------------------------------------------- 1 | ORB-SLAM is released under a GPLv3 license (see License-gpl.txt). 2 | Please note that we provide along ORB-SLAM a modified version of g2o and DBoW2 which are both BSD. 3 | 4 | For a closed-source version of ORB-SLAM for commercial purposes, please contact the authors. 5 | 6 | If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting: 7 | http://webdiis.unizar.es/~raulmur/orbslam/ 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /TODO.txt: -------------------------------------------------------------------------------- 1 | Issues and solutions 2 | 3 | 1. warning sparseoptimizer::optimize " 0 vertices to optimize, maybe forgot to call initializeOptimization()" occurs during localbundleadjustment or localoptimize. 4 | 5 | This warning is not caused by running two g2o optimizer at the same time, as I tested running hundreds of threads of g2o optimizers, no such warning occurs. 6 | 7 | 2. Add sigmaLevel0 for pixels at level 0 8 | 9 | 3. Add covariance estimates for optimized poses 10 | 11 | 4. Use anchored inverse depth parameterization for world points 12 | 13 | 5. Add monocular inertial SLAM 14 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo 5 | SET(CMAKE_VERBOSE_MAKEFILE OFF) 6 | SET(USE_SYSTEM_OPENCV true) # Set False if you want to build this package using independent OpenCV instead of libopencv-dev 7 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 9 | 10 | set(HDRS_DBOW2 11 | DBoW2/BowVector.h 12 | DBoW2/FORB.h 13 | DBoW2/FClass.h 14 | DBoW2/FeatureVector.h 15 | DBoW2/ScoringObject.h 16 | DBoW2/TemplatedVocabulary.h) 17 | set(SRCS_DBOW2 18 | DBoW2/BowVector.cpp 19 | DBoW2/FORB.cpp 20 | DBoW2/FeatureVector.cpp 21 | DBoW2/ScoringObject.cpp) 22 | 23 | set(HDRS_DUTILS 24 | DUtils/Random.h 25 | DUtils/Timestamp.h) 26 | set(SRCS_DUTILS 27 | DUtils/Random.cpp 28 | DUtils/Timestamp.cpp) 29 | IF(USE_SYSTEM_OPENCV) 30 | find_package(OpenCV REQUIRED) 31 | ELSE() 32 | SET(OPENCV_INCLUDE_DIR $ENV{HOME}/OpenCV/local_install/include) 33 | SET(OPENCV_LIBRARY_DIR $ENV{HOME}/OpenCV/local_install/lib) 34 | set(OpenCV_LIBS opencv_core opencv_features2d opencv_flann opencv_gpu 35 | opencv_nonfree opencv_highgui opencv_imgproc opencv_calib3d) 36 | link_directories(${OPENCV_LIBRARY_DIR}) 37 | ENDIF() 38 | 39 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 40 | 41 | include_directories(${OpenCV_INCLUDE_DIRS}) 42 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 43 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 44 | 45 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm 30 | { 31 | L1, 32 | L2 33 | }; 34 | 35 | /// Weighting type 36 | enum WeightingType 37 | { 38 | TF_IDF, 39 | TF, 40 | IDF, 41 | BINARY 42 | }; 43 | 44 | /// Scoring type 45 | enum ScoringType 46 | { 47 | L1_NORM, 48 | L2_NORM, 49 | CHI_SQUARE, 50 | KL, 51 | BHATTACHARYYA, 52 | DOT_PRODUCT, 53 | }; 54 | 55 | /// Vector of words to represent images 56 | class BowVector: 57 | public std::map 58 | { 59 | public: 60 | 61 | /** 62 | * Constructor 63 | */ 64 | BowVector(void); 65 | 66 | /** 67 | * Destructor 68 | */ 69 | ~BowVector(void); 70 | 71 | /** 72 | * Adds a value to a word value existing in the vector, or creates a new 73 | * word with the given value 74 | * @param id word id to look for 75 | * @param v value to create the word with, or to add to existing word 76 | */ 77 | void addWeight(WordId id, WordValue v); 78 | 79 | /** 80 | * Adds a word with a value to the vector only if this does not exist yet 81 | * @param id word id to look for 82 | * @param v value to give to the word if this does not exist 83 | */ 84 | void addIfNotExist(WordId id, WordValue v); 85 | 86 | /** 87 | * L1-Normalizes the values in the vector 88 | * @param norm_type norm used 89 | */ 90 | void normalize(LNorm norm_type); 91 | 92 | /** 93 | * Prints the content of the bow vector 94 | * @param out stream 95 | * @param v 96 | */ 97 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 98 | 99 | /** 100 | * Saves the bow vector as a vector in a matlab file 101 | * @param filename 102 | * @param W number of words in the vocabulary 103 | */ 104 | void saveM(const std::string &filename, size_t W) const; 105 | }; 106 | 107 | } // namespace DBoW2 108 | 109 | #endif 110 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass 26 | { 27 | class TDescriptor; 28 | typedef const TDescriptor *pDescriptor; 29 | 30 | /** 31 | * Calculates the mean value of a set of descriptors 32 | * @param descriptors 33 | * @param mean mean descriptor 34 | */ 35 | virtual void meanValue(const std::vector &descriptors, 36 | TDescriptor &mean) = 0; 37 | 38 | /** 39 | * Calculates the distance between two descriptors 40 | * @param a 41 | * @param b 42 | * @return distance 43 | */ 44 | static double distance(const TDescriptor &a, const TDescriptor &b); 45 | 46 | /** 47 | * Returns a string version of the descriptor 48 | * @param a descriptor 49 | * @return string version 50 | */ 51 | static std::string toString(const TDescriptor &a); 52 | 53 | /** 54 | * Returns a descriptor from a string 55 | * @param a descriptor 56 | * @param s string version 57 | */ 58 | static void fromString(TDescriptor &a, const std::string &s); 59 | 60 | /** 61 | * Returns a mat with the descriptors in float format 62 | * @param descriptors 63 | * @param mat (out) NxL 32F matrix 64 | */ 65 | static void toMat32F(const std::vector &descriptors, 66 | cv::Mat &mat); 67 | }; 68 | 69 | } // namespace DBoW2 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Functions to manipulate ORB descriptors 22 | class FORB: protected FClass 23 | { 24 | public: 25 | 26 | /// Descriptor type 27 | typedef cv::Mat TDescriptor; // CV_8U 28 | /// Pointer to a single descriptor 29 | typedef const TDescriptor *pDescriptor; 30 | /// Descriptor length (in bytes) 31 | static const int L; 32 | 33 | /** 34 | * Calculates the mean value of a set of descriptors 35 | * @param descriptors 36 | * @param mean mean descriptor 37 | */ 38 | static void meanValue(const std::vector &descriptors, 39 | TDescriptor &mean); 40 | 41 | /** 42 | * Calculates the distance between two descriptors 43 | * @param a 44 | * @param b 45 | * @return distance 46 | */ 47 | static int distance(const TDescriptor &a, const TDescriptor &b); 48 | 49 | /** 50 | * Returns a string version of the descriptor 51 | * @param a descriptor 52 | * @return string version 53 | */ 54 | static std::string toString(const TDescriptor &a); 55 | 56 | /** 57 | * Returns a descriptor from a string 58 | * @param a descriptor 59 | * @param s string version 60 | */ 61 | static void fromString(TDescriptor &a, const std::string &s); 62 | 63 | /** 64 | * Returns a mat with the descriptors in float format 65 | * @param descriptors 66 | * @param mat (out) NxL 32F matrix 67 | */ 68 | static void toMat32F(const std::vector &descriptors, 69 | cv::Mat &mat); 70 | 71 | static void toMat8U(const std::vector &descriptors, 72 | cv::Mat &mat); 73 | 74 | }; 75 | 76 | } // namespace DBoW2 77 | 78 | #endif 79 | 80 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) 20 | { 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | FeatureVector::~FeatureVector(void) 26 | { 27 | } 28 | 29 | // --------------------------------------------------------------------------- 30 | 31 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 32 | { 33 | FeatureVector::iterator vit = this->lower_bound(id); 34 | 35 | if(vit != this->end() && vit->first == id) 36 | { 37 | vit->second.push_back(i_feature); 38 | } 39 | else 40 | { 41 | vit = this->insert(vit, FeatureVector::value_type(id, 42 | std::vector() )); 43 | vit->second.push_back(i_feature); 44 | } 45 | } 46 | 47 | // --------------------------------------------------------------------------- 48 | 49 | std::ostream& operator<<(std::ostream &out, 50 | const FeatureVector &v) 51 | { 52 | if(!v.empty()) 53 | { 54 | FeatureVector::const_iterator vit = v.begin(); 55 | 56 | const std::vector* f = &vit->second; 57 | 58 | out << "<" << vit->first << ": ["; 59 | if(!f->empty()) out << (*f)[0]; 60 | for(unsigned int i = 1; i < f->size(); ++i) 61 | { 62 | out << ", " << (*f)[i]; 63 | } 64 | out << "]>"; 65 | 66 | for(++vit; vit != v.end(); ++vit) 67 | { 68 | f = &vit->second; 69 | 70 | out << ", <" << vit->first << ": ["; 71 | if(!f->empty()) out << (*f)[0]; 72 | for(unsigned int i = 1; i < f->size(); ++i) 73 | { 74 | out << ", " << (*f)[i]; 75 | } 76 | out << "]>"; 77 | } 78 | } 79 | 80 | return out; 81 | } 82 | 83 | // --------------------------------------------------------------------------- 84 | 85 | } // namespace DBoW2 86 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector: 22 | public std::map > 23 | { 24 | public: 25 | 26 | /** 27 | * Constructor 28 | */ 29 | FeatureVector(void); 30 | 31 | /** 32 | * Destructor 33 | */ 34 | ~FeatureVector(void); 35 | 36 | /** 37 | * Adds a feature to an existing node, or adds a new node with an initial 38 | * feature 39 | * @param id node id to add or to modify 40 | * @param i_feature index of feature to add to the given node 41 | */ 42 | void addFeature(NodeId id, unsigned int i_feature); 43 | 44 | /** 45 | * Sends a string versions of the feature vector through the stream 46 | * @param out stream 47 | * @param v feature vector 48 | */ 49 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 50 | 51 | }; 52 | 53 | } // namespace DBoW2 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of: 2 | 3 | DBoW2: bag-of-words library for C++ with generic descriptors 4 | 5 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 6 | All rights reserved. 7 | 8 | This version contains only a subset of the original components of the library 9 | and FORB.cpp has been modified (see the file for details). 10 | 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions 14 | are met: 15 | 1. Redistributions of source code must retain the above copyright 16 | notice, this list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 20 | 3. The original author of the work must be notified of any 21 | redistribution of source code or in binary form. 22 | 4. Neither the name of copyright holders nor the names of its 23 | contributors may be used to endorse or promote products derived 24 | from this software without specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 30 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 32 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 33 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 34 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 35 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | POSSIBILITY OF SUCH DAMAGE. 37 | 38 | If you use it in an academic work, please cite: 39 | 40 | @ARTICLE{GalvezTRO12, 41 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 42 | journal={IEEE Transactions on Robotics}, 43 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 44 | year={2012}, 45 | month={October}, 46 | volume={28}, 47 | number={5}, 48 | pages={1188--1197}, 49 | doi={10.1109/TRO.2012.2197158}, 50 | ISSN={1552-3098} 51 | } 52 | 53 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DBoW2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | namespace DBoW2 { 16 | 17 | /// Base class of scoring functions 18 | class GeneralScoring 19 | { 20 | public: 21 | /** 22 | * Computes the score between two vectors. Vectors must be sorted and 23 | * normalized if necessary 24 | * @param v (in/out) 25 | * @param w (in/out) 26 | * @return score 27 | */ 28 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 29 | 30 | /** 31 | * Returns whether a vector must be normalized before scoring according 32 | * to the scoring scheme 33 | * @param norm norm to use 34 | * @return true iff must normalize 35 | */ 36 | virtual bool mustNormalize(LNorm &norm) const = 0; 37 | 38 | /// Log of epsilon 39 | static const double LOG_EPS; 40 | // If you change the type of WordValue, make sure you change also the 41 | // epsilon value (this is needed by the KL method) 42 | 43 | }; 44 | 45 | /** 46 | * Macro for defining Scoring classes 47 | * @param NAME name of class 48 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 49 | * @param NORM type of norm to use when MUSTNORMALIZE 50 | */ 51 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 52 | NAME: public GeneralScoring \ 53 | { public: \ 54 | /** \ 55 | * Computes score between two vectors \ 56 | * @param v \ 57 | * @param w \ 58 | * @return score between v and w \ 59 | */ \ 60 | virtual double score(const BowVector &v, const BowVector &w) const; \ 61 | \ 62 | /** \ 63 | * Says if a vector must be normalized according to the scoring function \ 64 | * @param norm (out) if true, norm to use 65 | * @return true iff vectors must be normalized \ 66 | */ \ 67 | virtual inline bool mustNormalize(LNorm &norm) const \ 68 | { norm = NORM; return MUSTNORMALIZE; } \ 69 | } 70 | 71 | /// L1 Scoring object 72 | class __SCORING_CLASS(L1Scoring, true, L1); 73 | 74 | /// L2 Scoring object 75 | class __SCORING_CLASS(L2Scoring, true, L2); 76 | 77 | /// Chi square Scoring object 78 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 79 | 80 | /// KL divergence Scoring object 81 | class __SCORING_CLASS(KLScoring, true, L1); 82 | 83 | /// Bhattacharyya Scoring object 84 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 85 | 86 | /// Dot product Scoring object 87 | class __SCORING_CLASS(DotProductScoring, false, L1); 88 | 89 | #undef __SCORING_CLASS 90 | 91 | } // namespace DBoW2 92 | 93 | #endif 94 | 95 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of DLib: 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 4 | All rights reserved. 5 | 6 | This version only contains a subset of the original components of 7 | the library. 8 | 9 | Redistribution and use in source and binary forms, with or without 10 | modification, are permitted provided that the following conditions 11 | are met: 12 | 1. Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | 2. Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | 3. The original author of the work must be notified of any 18 | redistribution of source code or in binary form. 19 | 4. Neither the name of copyright holders nor the names of its 20 | contributors may be used to endorse or promote products derived 21 | from this software without specific prior written permission. 22 | 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 25 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 26 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 27 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 28 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 29 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 30 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 31 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 32 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | POSSIBILITY OF SUCH DAMAGE. 34 | 35 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include "Timestamp.h" 13 | #include 14 | using namespace std; 15 | 16 | bool DUtils::Random::m_already_seeded = false; 17 | 18 | void DUtils::Random::SeedRand(){ 19 | Timestamp time; 20 | time.setToCurrentTime(); 21 | srand((unsigned)time.getFloatTime()); 22 | } 23 | 24 | void DUtils::Random::SeedRandOnce() 25 | { 26 | if(!m_already_seeded) 27 | { 28 | DUtils::Random::SeedRand(); 29 | m_already_seeded = true; 30 | } 31 | } 32 | 33 | void DUtils::Random::SeedRand(int seed) 34 | { 35 | srand(seed); 36 | } 37 | 38 | void DUtils::Random::SeedRandOnce(int seed) 39 | { 40 | if(!m_already_seeded) 41 | { 42 | DUtils::Random::SeedRand(seed); 43 | m_already_seeded = true; 44 | } 45 | } 46 | 47 | int DUtils::Random::RandomInt(int min, int max){ 48 | int d = max - min + 1; 49 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | // --------------------------------------------------------------------------- 54 | 55 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 56 | { 57 | if(min <= max) 58 | { 59 | m_min = min; 60 | m_max = max; 61 | } 62 | else 63 | { 64 | m_min = max; 65 | m_max = min; 66 | } 67 | 68 | createValues(); 69 | } 70 | 71 | // --------------------------------------------------------------------------- 72 | 73 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 74 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 75 | { 76 | *this = rnd; 77 | } 78 | 79 | // --------------------------------------------------------------------------- 80 | 81 | int DUtils::Random::UnrepeatedRandomizer::get() 82 | { 83 | if(empty()) createValues(); 84 | 85 | DUtils::Random::SeedRandOnce(); 86 | 87 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 88 | int ret = m_values[k]; 89 | m_values[k] = m_values.back(); 90 | m_values.pop_back(); 91 | 92 | return ret; 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | void DUtils::Random::UnrepeatedRandomizer::createValues() 98 | { 99 | int n = m_max - m_min + 1; 100 | 101 | m_values.resize(n); 102 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 103 | } 104 | 105 | // --------------------------------------------------------------------------- 106 | 107 | void DUtils::Random::UnrepeatedRandomizer::reset() 108 | { 109 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 110 | } 111 | 112 | // --------------------------------------------------------------------------- 113 | 114 | DUtils::Random::UnrepeatedRandomizer& 115 | DUtils::Random::UnrepeatedRandomizer::operator= 116 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 117 | { 118 | if(this != &rnd) 119 | { 120 | this->m_min = rnd.m_min; 121 | this->m_max = rnd.m_max; 122 | this->m_values = rnd.m_values; 123 | } 124 | return *this; 125 | } 126 | 127 | // --------------------------------------------------------------------------- 128 | 129 | 130 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of: 2 | 3 | DBoW2: bag-of-words library for C++ with generic descriptors 4 | 5 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 6 | All rights reserved. 7 | 8 | This version contains only a subset of the original components of the library 9 | and DBoW2/FORB.cpp has been modified (see the file for details). 10 | 11 | 12 | Redistribution and use in source and binary forms, with or without 13 | modification, are permitted provided that the following conditions 14 | are met: 15 | 1. Redistributions of source code must retain the above copyright 16 | notice, this list of conditions and the following disclaimer. 17 | 2. Redistributions in binary form must reproduce the above copyright 18 | notice, this list of conditions and the following disclaimer in the 19 | documentation and/or other materials provided with the distribution. 20 | 3. The original author of the work must be notified of any 21 | redistribution of source code or in binary form. 22 | 4. Neither the name of copyright holders nor the names of its 23 | contributors may be used to endorse or promote products derived 24 | from this software without specific prior written permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 28 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 29 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 30 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 32 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 33 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 34 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 35 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | POSSIBILITY OF SUCH DAMAGE. 37 | 38 | If you use it in an academic work, please cite: 39 | 40 | @ARTICLE{GalvezTRO12, 41 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 42 | journal={IEEE Transactions on Robotics}, 43 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 44 | year={2012}, 45 | month={October}, 46 | volume={28}, 47 | number={5}, 48 | pages={1188--1197}, 49 | doi={10.1109/TRO.2012.2197158}, 50 | ISSN={1552-3098} 51 | } 52 | 53 | -------------------------------------------------------------------------------- /Thirdparty/DBoW2/lib/libDBoW2.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrywen2011/orb_slam_imu/79525ca372dd5aa1b5c48610779a5c234e7bc918/Thirdparty/DBoW2/lib/libDBoW2.so -------------------------------------------------------------------------------- /Thirdparty/g2o_EXTERNAL/ceres/LICENSE: -------------------------------------------------------------------------------- 1 | Ceres Solver - A fast non-linear least squares minimizer 2 | Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 | http://code.google.com/p/ceres-solver/ 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | * Neither the name of Google Inc. nor the names of its contributors may be 14 | used to endorse or promote products derived from this software without 15 | specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /Thirdparty/g2o_EXTERNAL/ceres/eigen.h: -------------------------------------------------------------------------------- 1 | // Ceres Solver - A fast non-linear least squares minimizer 2 | // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 | // http://code.google.com/p/ceres-solver/ 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // * Neither the name of Google Inc. nor the names of its contributors may be 14 | // used to endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // Author: sameeragarwal@google.com (Sameer Agarwal) 30 | 31 | #ifndef CERES_INTERNAL_EIGEN_H_ 32 | #define CERES_INTERNAL_EIGEN_H_ 33 | 34 | #include "Eigen/Core" 35 | 36 | namespace ceres { 37 | 38 | typedef Eigen::Matrix Vector; 39 | typedef Eigen::Matrix Matrix; 43 | typedef Eigen::Map VectorRef; 44 | typedef Eigen::Map MatrixRef; 45 | typedef Eigen::Map ConstVectorRef; 46 | typedef Eigen::Map ConstMatrixRef; 47 | 48 | // Column major matrices for DenseSparseMatrix/DenseQRSolver 49 | typedef Eigen::Matrix ColMajorMatrix; 53 | 54 | typedef Eigen::Map > ColMajorMatrixRef; 56 | 57 | typedef Eigen::Map > ConstColMajorMatrixRef; 60 | 61 | 62 | 63 | // C++ does not support templated typdefs, thus the need for this 64 | // struct so that we can support statically sized Matrix and Maps. 65 | template 66 | struct EigenTypes { 67 | typedef Eigen::Matrix 68 | Matrix; 69 | 70 | typedef Eigen::Map< 71 | Eigen::Matrix > 72 | MatrixRef; 73 | 74 | typedef Eigen::Matrix 75 | Vector; 76 | 77 | typedef Eigen::Map < 78 | Eigen::Matrix > 79 | VectorRef; 80 | 81 | 82 | typedef Eigen::Map< 83 | const Eigen::Matrix > 84 | ConstMatrixRef; 85 | 86 | typedef Eigen::Map < 87 | const Eigen::Matrix > 88 | ConstVectorRef; 89 | }; 90 | 91 | } // namespace ceres 92 | 93 | #endif // CERES_INTERNAL_EIGEN_H_ 94 | -------------------------------------------------------------------------------- /Thirdparty/g2o_EXTERNAL/ceres/fpclassify.h: -------------------------------------------------------------------------------- 1 | // Ceres Solver - A fast non-linear least squares minimizer 2 | // Copyright 2012 Google Inc. All rights reserved. 3 | // http://code.google.com/p/ceres-solver/ 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // * Neither the name of Google Inc. nor the names of its contributors may be 14 | // used to endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // Author: keir@google.com (Keir Mierle) 30 | // 31 | // Portable floating point classification. The names are picked such that they 32 | // do not collide with macros. For example, "isnan" in C99 is a macro and hence 33 | // does not respect namespaces. 34 | // 35 | // TODO(keir): Finish porting! 36 | 37 | #ifndef CERES_PUBLIC_FPCLASSIFY_H_ 38 | #define CERES_PUBLIC_FPCLASSIFY_H_ 39 | 40 | #if defined(_MSC_VER) 41 | #include 42 | #endif 43 | 44 | #include 45 | 46 | namespace ceres { 47 | 48 | #if defined(_MSC_VER) 49 | inline bool IsFinite (double x) { return _finite(x); } 50 | inline bool IsInfinite(double x) { return !_finite(x) && !_isnan(x); } 51 | inline bool IsNaN (double x) { return _isnan(x); } 52 | inline bool IsNormal (double x) { 53 | int classification = _fpclass(x); 54 | return classification == _FPCLASS_NN || 55 | classification == _FPCLASS_PN; 56 | } 57 | #elif defined(ANDROID) 58 | 59 | // On Android when using the GNU STL, the C++ fpclassify functions are not 60 | // available. Strictly speaking, the std functions are are not standard until 61 | // C++11. Instead use the C99 macros on Android. 62 | inline bool IsNaN (double x) { return isnan(x); } 63 | inline bool IsNormal (double x) { return isnormal(x); } 64 | 65 | // On Android NDK r6, when using STLPort, the isinf and isfinite functions are 66 | // not available, so reimplement them. 67 | # if defined(_STLPORT_VERSION) 68 | inline bool IsInfinite(double x) { 69 | return x == std::numeric_limits::infinity() || 70 | x == -std::numeric_limits::infinity(); 71 | } 72 | inline bool IsFinite(double x) { 73 | return !isnan(x) && !IsInfinite(x); 74 | } 75 | # else 76 | inline bool IsFinite (double x) { return isfinite(x); } 77 | inline bool IsInfinite(double x) { return isinf(x); } 78 | # endif // defined(_STLPORT_VERSION) 79 | #else 80 | // These definitions are for the normal Unix suspects. 81 | // TODO(keir): Test the "else" with more platforms. 82 | inline bool IsFinite (double x) { return std::isfinite(x); } 83 | inline bool IsInfinite(double x) { return std::isinf(x); } 84 | inline bool IsNaN (double x) { return std::isnan(x); } 85 | inline bool IsNormal (double x) { return std::isnormal(x); } 86 | #endif 87 | 88 | } // namespace ceres 89 | 90 | #endif // CERES_PUBLIC_FPCLASSIFY_H_ 91 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(HAVE_TOON FALSE) #Georg Klein's M-estimator for incremental motion 2 | SET(HAVE_OPENCV FALSE) #five-point algorithm to estimate incremental motion 3 | SET(USE_SYSTEM_OPENCV TRUE) # Set False if you want to build this package using independent OpenCV instead of libopencv-dev, will be ignored if HAVE_OPENCV is false 4 | SET(HAVE_FIVEPOINT_NGHIAHO FALSE) #use Nghia Ho's five point implementation 5 | # project 6 | SET(PROJECT_NAME viso2) 7 | project (${PROJECT_NAME}) 8 | 9 | cmake_minimum_required (VERSION 2.8.3) 10 | IF(NOT CMAKE_BUILD_TYPE) 11 | SET(CMAKE_BUILD_TYPE Release) 12 | ENDIF() 13 | IF(HAVE_TOON) 14 | add_definitions(-DUSE_TOON) 15 | ENDIF() 16 | IF(HAVE_OPENCV) 17 | add_definitions(-DUSE_OPENCV) 18 | ENDIF() 19 | IF(HAVE_OPENCV AND HAVE_FIVEPOINT_NGHIAHO) 20 | add_definitions(-DUSE_NGHIAHO) 21 | ENDIF() 22 | 23 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 24 | SET(CMAKE_VERBOSE_MAKEFILE OFF) 25 | 26 | # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid 27 | SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas") 28 | IF(DEFINED ENV{ARM_ARCHITECTURE}) 29 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a") 30 | ELSE() 31 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3") 32 | ENDIF() 33 | IF(CMAKE_COMPILER_IS_GNUCC) 34 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 35 | ELSE() 36 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 37 | ENDIF() 38 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 39 | MESSAGE("CMAKE_CXX_FLAGS:" ${CMAKE_CXX_FLAGS}) 40 | 41 | # use sse3 instruction set 42 | # SET(CMAKE_CXX_FLAGS "-msse3") 43 | 44 | include_directories(include/viso2) 45 | 46 | IF(HAVE_TOON) 47 | SET(TOON_INCLUDE_DIR $ENV{HOME}/ScaViSLAM/svslocal/include) 48 | include_directories(${TOON_INCLUDE_DIR}) 49 | ENDIF() 50 | 51 | 52 | # add opencv libraries for 5 point algorithm 53 | IF(HAVE_OPENCV) 54 | IF(USE_SYSTEM_OPENCV) 55 | FIND_PACKAGE(OpenCV REQUIRED) 56 | ELSE() 57 | SET(OPENCV_INCLUDE_DIR $ENV{HOME}/OpenCV/local_install/include) 58 | SET(OPENCV_LIBRARY_DIR $ENV{HOME}/OpenCV/local_install/lib) 59 | set(OpenCV_LIBS opencv_core opencv_features2d opencv_flann opencv_gpu 60 | opencv_nonfree opencv_highgui opencv_imgproc opencv_calib3d opencv_video) 61 | link_directories(${OPENCV_LIBRARY_DIR}) 62 | ENDIF() 63 | 64 | LIST(APPEND LINK_LIBS ${OpenCV_LIBS}) 65 | include_directories(${OpenCV_INCLUDE_DIRS}) 66 | ENDIF() 67 | 68 | IF(HAVE_OPENCV AND HAVE_FIVEPOINT_NGHIAHO) 69 | include_directories(5point_NghiaHo) 70 | ENDIF() 71 | 72 | # sources of library 73 | # FILE(GLOB LIBVISO2_SRC_FILES "viso2/*.cpp") 74 | LIST(APPEND SOURCEFILES 75 | src/filter.cpp 76 | src/matcher.cpp 77 | src/matrix.cpp 78 | include/viso2/p_match.h 79 | 80 | #include/viso2/timer.h 81 | #src/reconstruction.cpp 82 | 83 | src/triangle.cpp 84 | src/viso.cpp 85 | src/viso_mono.cpp 86 | src/viso_stereo.cpp 87 | ) 88 | 89 | IF(HAVE_TOON) 90 | LIST(APPEND SOURCEFILES 91 | include/viso2/MEstimator.h 92 | src/TrackerData.cpp) 93 | ENDIF() 94 | 95 | IF(HAVE_OPENCV) 96 | LIST(APPEND SOURCEFILES 97 | src/five-point.cpp 98 | src/ptsetreg.cpp) 99 | ENDIF() 100 | 101 | IF(HAVE_OPENCV AND HAVE_FIVEPOINT_NGHIAHO) 102 | LIST(APPEND SOURCEFILES 103 | 5point_NghiaHo/5point.cpp 104 | 5point_NghiaHo/Mblock.h 105 | 5point_NghiaHo/Polynomial.h 106 | 5point_NghiaHo/Rpoly.cpp) 107 | ENDIF() 108 | 109 | # Create library 110 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 111 | ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES}) 112 | TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS}) 113 | 114 | IF(HAVE_OPENCV) 115 | ADD_EXECUTABLE(demo test/demo.cpp) 116 | TARGET_LINK_LIBRARIES(demo ${PROJECT_NAME}) 117 | ENDIF() 118 | 119 | # install headers and .so lib 120 | INSTALL(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_PREFIX}/include/viso2 FILES_MATCHING PATTERN "*.h" ) 121 | INSTALL(TARGETS ${PROJECT_NAME} DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) 122 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/TrackerData.h: -------------------------------------------------------------------------------- 1 | #ifndef TRACKERDATA_H 2 | #define TRACKERDATA_H 3 | #include "TooN/TooN.h" 4 | #include "TooN/se3.h" 5 | #include 6 | namespace libviso2 { 7 | 8 | class TrackerData 9 | { 10 | public: 11 | TrackerData():isInlier(false), age(-1) 12 | {} 13 | TrackerData(const TrackerData& other): 14 | v3WorldPos(other.v3WorldPos),v2Image(other.v2Image), 15 | v2Found(other.v2Found), v2Error_CovScaled(other.v2Error_CovScaled), 16 | m26Jacobian(other.m26Jacobian),isInlier(other.isInlier), age(other.age) 17 | {} 18 | TrackerData & operator=(const TrackerData &other) 19 | { 20 | if(this==&other) 21 | return *this; 22 | v3WorldPos=other.v3WorldPos; 23 | v2Image=other.v2Image; 24 | v2Found=other.v2Found; 25 | v2Error_CovScaled=other.v2Error_CovScaled; 26 | m26Jacobian=other.m26Jacobian; 27 | isInlier=other.isInlier; age=other.age; 28 | return *this; 29 | } 30 | 31 | // Project point into image given certain pose and camera. 32 | // This can bail out at several stages if the point 33 | // will not be properly in the image. 34 | //Project 3d points onto right image of the stereo if baseline!=0 35 | void Project(const TooN::SE3<> &se3CFromW, const double calib_f, const double calib_cu,const double calib_cv, const double baseline=0); 36 | 37 | 38 | // Get the projection derivatives (depend only on the camera.) 39 | // This is called Unsafe because it depends on the camera caching 40 | // results from the previous projection: 41 | // Only do this right after the same point has been projected! 42 | static void GetDerivsUnsafe( double calib_f) 43 | { 44 | m2CamDerivs[0][0] = calib_f; 45 | m2CamDerivs[1][1] = 0; 46 | m2CamDerivs[0][1] = 0; 47 | m2CamDerivs[1][1] = calib_f; 48 | } 49 | 50 | void CalcJacobian(); 51 | 52 | // Sometimes in tracker instead of reprojecting, just update the error linearly! 53 | inline void LinearUpdate(const TooN::Vector<6> &v6) 54 | { 55 | v2Image += m26Jacobian * v6; 56 | } 57 | TooN::Vector<3> v3WorldPos; // Coords of the point in the world frame 58 | // Projection itermediates: 59 | TooN::Vector<3> v3Cam; // point position in current image frame 60 | TooN::Vector<2> v2Image; // predicted Pixel coords in LEVEL0 61 | static TooN::Matrix<2> m2CamDerivs; // Camera projection derivs d(u,v)/d(x/z, y/z) 62 | 63 | TooN::Vector<2> v2Found; // measured Pixel coords of found patch (L0) 64 | 65 | // Stuff for pose update: 66 | TooN::Vector<2> v2Error_CovScaled; 67 | TooN::Matrix<2,6> m26Jacobian; // Jacobian wrt camera position 68 | 69 | bool isInlier; 70 | int age; // used for integrated feature, age of feature in the last frame, age=0 in first frame 71 | static const double dSqrtInvNoise; // Only depends on search level.. 72 | }; 73 | TooN::Vector<6> CalcPoseUpdate(const std::vector & vTD, double dOverrideSigma, bool bMarkOutliers); 74 | } 75 | #endif // TRACKERDATA_H 76 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2012. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libelas. 7 | Authors: Julius Ziegler, Andreas Geiger 8 | 9 | libelas is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 3 of the License, or any later version. 12 | 13 | libelas is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __FILTER_H__ 23 | #define __FILTER_H__ 24 | 25 | #include 26 | #include 27 | 28 | // define fixed-width datatypes for Visual Studio projects 29 | #ifndef _MSC_VER 30 | #include 31 | #else 32 | typedef __int8 int8_t; 33 | typedef __int16 int16_t; 34 | typedef __int32 int32_t; 35 | typedef __int64 int64_t; 36 | typedef unsigned __int8 uint8_t; 37 | typedef unsigned __int16 uint16_t; 38 | typedef unsigned __int32 uint32_t; 39 | typedef unsigned __int64 uint64_t; 40 | #endif 41 | 42 | // fast filters: implements 3x3 and 5x5 sobel filters and 43 | // 5x5 blob and corner filters based on SSE2/3 instructions 44 | namespace filter { 45 | 46 | // private namespace, public user functions at the bottom of this file 47 | namespace detail { 48 | void integral_image( const uint8_t* in, int32_t* out, int w, int h ); 49 | void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 ); 50 | void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b ); 51 | 52 | // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output. 53 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 54 | void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 55 | 56 | // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output. 57 | // This one works on 16bit input and 8bit output. 58 | // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255]. 59 | void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h ); 60 | 61 | // convolve image with a (1,2,1) row vector. Result is accumulated into output. 62 | // This one works on 16bit input and 8bit output. 63 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 64 | void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 65 | 66 | // convolve image with a (1,0,-1) row vector. Result is accumulated into output. 67 | // This one works on 16bit input and 8bit output. 68 | // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255]. 69 | void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h ); 70 | 71 | void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 72 | 73 | void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h ); 74 | 75 | void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h ); 76 | 77 | void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h ); 78 | } 79 | 80 | void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 81 | 82 | void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h ); 83 | 84 | // -1 -1 0 1 1 85 | // -1 -1 0 1 1 86 | // 0 0 0 0 0 87 | // 1 1 0 -1 -1 88 | // 1 1 0 -1 -1 89 | void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h ); 90 | 91 | // -1 -1 -1 -1 -1 92 | // -1 1 1 1 -1 93 | // -1 1 8 1 -1 94 | // -1 1 1 1 -1 95 | // -1 -1 -1 -1 -1 96 | void blob5x5( const uint8_t* in, int16_t* out, int w, int h ); 97 | }; 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/p_match.h: -------------------------------------------------------------------------------- 1 | #ifndef P_MATCH_H 2 | #define P_MATCH_H 3 | // define fixed-width datatypes for Visual Studio projects 4 | #ifndef _MSC_VER 5 | #include 6 | #else 7 | typedef __int8 int8_t; 8 | typedef __int16 int16_t; 9 | typedef __int32 int32_t; 10 | typedef __int64 int64_t; 11 | typedef unsigned __int8 uint8_t; 12 | typedef unsigned __int16 uint16_t; 13 | typedef unsigned __int32 uint32_t; 14 | typedef unsigned __int64 uint64_t; 15 | #endif 16 | // structure for storing matches 17 | struct p_match { 18 | float u1p,v1p; // u,v-coordinates in previous left image 19 | int32_t i1p; // feature index (for tracking) 20 | float u2p,v2p; // u,v-coordinates in previous right image 21 | int32_t i2p; // feature index (for tracking) 22 | float u1c,v1c; // u,v-coordinates in current left image 23 | int32_t i1c; // feature index (for tracking) 24 | float u2c,v2c; // u,v-coordinates in current right image 25 | int32_t i2c; // feature index (for tracking) 26 | p_match(){} 27 | p_match(float u1p,float v1p,int32_t i1p,float u2p,float v2p,int32_t i2p, 28 | float u1c,float v1c,int32_t i1c,float u2c,float v2c,int32_t i2c): 29 | u1p(u1p),v1p(v1p),i1p(i1p),u2p(u2p),v2p(v2p),i2p(i2p), 30 | u1c(u1c),v1c(v1c),i1c(i1c),u2c(u2c),v2c(v2c),i2c(i2c) {} 31 | p_match& operator =(const p_match& other) 32 | { 33 | if(this==&other) 34 | return *this; 35 | u1p=other.u1p; 36 | v1p=other.v1p; 37 | i1p=other.i1p; 38 | u2p=other.u2p; 39 | v2p=other.v2p;i2p=other.i2p; 40 | u1c=other.u1c;v1c=other.v1c; 41 | i1c=other.i1c;u2c=other.u2c; 42 | v2c=other.v2c;i2c=other.i2c; 43 | return *this; 44 | } 45 | p_match(const p_match & other): 46 | u1p(other.u1p), 47 | v1p(other.v1p), 48 | i1p(other.i1p), 49 | u2p(other.u2p), 50 | v2p(other.v2p),i2p(other.i2p), 51 | u1c(other.u1c),v1c(other.v1c), 52 | i1c(other.i1c),u2c(other.u2c), 53 | v2c(other.v2c),i2c(other.i2c) 54 | { 55 | } 56 | }; 57 | 58 | #endif // P_MATCH_H 59 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/reconstruction.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef RECONSTRUCTION_H 23 | #define RECONSTRUCTION_H 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #define _USE_MATH_DEFINES 30 | #include 31 | 32 | #include "matcher.h" 33 | #include "matrix.h" 34 | namespace libviso2 { 35 | 36 | class Reconstruction { 37 | 38 | public: 39 | 40 | // constructor 41 | Reconstruction (); 42 | 43 | // deconstructor 44 | ~Reconstruction (); 45 | 46 | // a generic 3d point 47 | struct point3d { 48 | float x,y,z; 49 | point3d () {} 50 | point3d (float x,float y,float z) : x(x),y(y),z(z) {} 51 | }; 52 | 53 | // set calibration parameters (intrinsics), must be called at least once 54 | // input: f ....... focal length (assumes fu=fv) 55 | // cu,cv ... principal point 56 | void setCalibration (FLOAT f,FLOAT cu,FLOAT cv); 57 | 58 | // takes a set of monocular feature matches (flow method) and the egomotion 59 | // estimate between the 2 frames Tr, tries to associate the features with previous 60 | // frames (tracking) and computes 3d points once tracks gets lost. 61 | // point types: 0 ..... everything 62 | // 1 ..... road and above 63 | // 2 ..... only above road 64 | // min_track_length ... min number of frames a point needs to be tracked for reconstruction 65 | // max_dist ........... maximum point distance from camera 66 | // min_angle .......... avoid angles smaller than this for reconstruction (in degrees) 67 | void update (std::vector p_matched,Matrix Tr,int32_t point_type=1,int32_t min_track_length=2,double max_dist=30,double min_angle=2); 68 | 69 | // return currently computed 3d points (finished tracks) 70 | std::vector getPoints() { return points; } 71 | 72 | private: 73 | 74 | struct point2d { 75 | float u,v; 76 | point2d () {} 77 | point2d (float u,float v) : u(u),v(v) {} 78 | }; 79 | 80 | struct track { 81 | std::vector pixels; 82 | int32_t first_frame; 83 | int32_t last_frame; 84 | int32_t last_idx; 85 | }; 86 | 87 | enum result { UPDATED, FAILED, CONVERGED }; 88 | 89 | bool initPoint(const track &t,point3d &p); 90 | bool refinePoint(const track &t,point3d &p); 91 | double pointDistance(const track &t,point3d &p); 92 | double rayAngle(const track &t,point3d &p); 93 | int32_t pointType(const track &t,point3d &p); 94 | result updatePoint(const track &t,point3d &p,const FLOAT &step_size,const FLOAT &eps); 95 | void computeObservations(const std::vector &p); 96 | bool computePredictionsAndJacobian(const std::vector::iterator &P_begin,const std::vector::iterator &P_end,point3d &p); 97 | void testJacobian(); 98 | 99 | // calibration matrices 100 | Matrix K,Tr_cam_road; 101 | 102 | std::vector tracks; 103 | std::vector Tr_total; 104 | std::vector Tr_inv_total; 105 | std::vector P_total; 106 | std::vector points; 107 | 108 | FLOAT *J; // jacobian 109 | FLOAT *p_observe,*p_predict; // observed and predicted 2d points 110 | 111 | }; 112 | } 113 | #endif // RECONSTRUCTION_H 114 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef __TIMER_H__ 23 | #define __TIMER_H__ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | class Timer { 36 | 37 | public: 38 | 39 | Timer() {} 40 | 41 | ~Timer() {} 42 | 43 | void start (std::string title) { 44 | desc.push_back(title); 45 | push_back_time(); 46 | } 47 | 48 | void stop () { 49 | if (time.size()<=desc.size()) 50 | push_back_time(); 51 | } 52 | 53 | void plot () { 54 | stop(); 55 | float total_time = 0; 56 | for (int32_t i=0; i<(int32_t)desc.size(); i++) { 57 | float curr_time = getTimeDifferenceMilliseconds(time[i],time[i+1]); 58 | total_time += curr_time; 59 | std::cout.width(30); 60 | std::cout << desc[i] << " "; 61 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 62 | std::cout << curr_time; 63 | std::cout << " ms" << std::endl; 64 | } 65 | std::cout << "========================================" << std::endl; 66 | std::cout << " Total time "; 67 | std::cout << std::fixed << std::setprecision(1) << std::setw(6); 68 | std::cout << total_time; 69 | std::cout << " ms" << std::endl << std::endl; 70 | } 71 | 72 | void reset () { 73 | desc.clear(); 74 | time.clear(); 75 | } 76 | 77 | private: 78 | std::vector desc; 79 | std::vector time; 80 | 81 | void push_back_time () { 82 | timeval curr_time; 83 | gettimeofday(&curr_time,0); 84 | time.push_back(curr_time); 85 | } 86 | 87 | float getTimeDifferenceMilliseconds(timeval a,timeval b) { 88 | return ((float)(b.tv_sec -a.tv_sec ))*1e+3 + 89 | ((float)(b.tv_usec-a.tv_usec))*1e-3; 90 | } 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/include/viso2/viso_mono.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #ifndef VISO_MONO_H 23 | #define VISO_MONO_H 24 | 25 | #include "viso.h" 26 | namespace libviso2 { 27 | 28 | class VisualOdometryMono : public VisualOdometry { 29 | 30 | public: 31 | 32 | // monocular-specific parameters (mandatory: height,pitch) 33 | struct parameters : public VisualOdometry::parameters { 34 | double height; // camera height above ground (meters) 35 | double pitch; // camera pitch (rad, negative=pointing down) 36 | int32_t ransac_iters; // number of RANSAC iterations 37 | double inlier_threshold; // fundamental matrix inlier threshold 38 | double motion_threshold; // directly return false on small motions 39 | parameters () { 40 | height = 1.0; 41 | pitch = 0.0; 42 | ransac_iters = 2000; 43 | inlier_threshold = 0.00001; 44 | motion_threshold = 100.0; 45 | } 46 | }; 47 | 48 | // constructor, takes as inpute a parameter structure 49 | VisualOdometryMono (parameters param); 50 | 51 | // deconstructor 52 | ~VisualOdometryMono (); 53 | 54 | // process a new image, pushs the image back to an internal ring buffer. 55 | // valid motion estimates are available after calling process for two times. 56 | // inputs: I ......... pointer to rectified image (uint8, row-aligned) 57 | // dims[0] ... width of I 58 | // dims[1] ... height of I 59 | // dims[2] ... bytes per line (often equal to width) 60 | // replace ... replace current image with I, without copying last current 61 | // image to previous image internally. this option can be used 62 | // when small/no motions are observed to obtain Tr_delta wrt 63 | // an older coordinate system / time step than the previous one. 64 | // output: returns false if motion too small or an error occured 65 | bool process (uint8_t *I,int32_t* dims,bool replace=false); 66 | #ifdef USE_OPENCV 67 | //changed update motion method w.r.t process(), method 0 opencv 5 point algorithm, 1, viso2 monocular algorithm 68 | bool process2 (uint8_t *I,int32_t* dims,bool replace=false, bool bUseViso2=false); 69 | // use opencv five point algorithm to estimate monocular motion, method 0 opencv 5 point algorithm, 1, viso2 monocular algorithm 70 | std::vector estimateMotion2 (const std::vector &p_matched, 71 | bool bUseViso2= false); 72 | #endif 73 | private: 74 | 75 | template struct idx_cmp { 76 | idx_cmp(const T arr) : arr(arr) {} 77 | bool operator()(const size_t a, const size_t b) const { return arr[a] < arr[b]; } 78 | const T arr; 79 | }; 80 | public: 81 | std::vector estimateMotion (const std::vector &p_matched, 82 | const std::vector tr_delta= std::vector(6,0)); 83 | private: 84 | Matrix smallerThanMedian (Matrix &X,double &median); 85 | bool normalizeFeaturePoints (std::vector &p_matched,Matrix &Tp,Matrix &Tc); 86 | void fundamentalMatrix (const std::vector &p_matched,const std::vector &active,Matrix &F); 87 | void EtoRt(Matrix &E,Matrix &K,const std::vector &p_matched,Matrix &X,Matrix &R,Matrix &t); 88 | int32_t triangulateChieral (const std::vector &p_matched,Matrix &K,Matrix &R,Matrix &t,Matrix &X); 89 | std::vector getInlier (const std::vector &p_matched,Matrix &F); 90 | 91 | // parameters 92 | parameters param; 93 | }; 94 | } 95 | #endif // VISO_MONO_H 96 | 97 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/lib/libviso2.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrywen2011/orb_slam_imu/79525ca372dd5aa1b5c48610779a5c234e7bc918/Thirdparty/libviso2/lib/libviso2.so -------------------------------------------------------------------------------- /Thirdparty/libviso2/src/TrackerData.cpp: -------------------------------------------------------------------------------- 1 | #include "TrackerData.h" 2 | #include "TooN/helpers.h" 3 | #include "TooN/TooN.h" 4 | #include "TooN/wls.h" 5 | #include "MEstimator.h" 6 | namespace libviso2 { 7 | 8 | const double TrackerData::dSqrtInvNoise(1.0); 9 | TooN::Matrix<2> TrackerData::m2CamDerivs=Zeros; 10 | void TrackerData::Project(const SE3<> &se3CFromW, const double calib_f, const double calib_cu,const double calib_cv, const double baseline) 11 | { 12 | v3Cam = se3CFromW * v3WorldPos; 13 | v3Cam[0]-=baseline; 14 | // assert(v3Cam[2] >= 0.001); 15 | Vector<2> v2ImPlane = project(v3Cam); 16 | v2Image[0] = v2ImPlane[0]*calib_f+calib_cu; 17 | v2Image[1] = v2ImPlane[1]*calib_f+calib_cv; 18 | // assert(v2Image[0]>= 0 && v2Image[1]>= 0); 19 | } 20 | 21 | // Jacobian of projection W.R.T. the camera position 22 | // I.e. if p_cam = SE3Old * p_world, 23 | // SE3New = SE3Motion * SE3Old 24 | void TrackerData::CalcJacobian() 25 | { 26 | double dOneOverCameraZ = 1.0 / v3Cam[2]; 27 | for(int m=0; m<6; m++) 28 | { 29 | const Vector<4> v4Motion = SE3<>::generator_field(m, unproject(v3Cam)); 30 | Vector<2> v2CamFrameMotion; 31 | v2CamFrameMotion[0] = (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 32 | v2CamFrameMotion[1] = (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 33 | m26Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion; 34 | }; 35 | } 36 | 37 | TooN::Vector<6> CalcPoseUpdate(const std::vector &vTD, double dOverrideSigma, bool bMarkOutliers) 38 | { 39 | // Which M-estimator are we using? 0, "Tukey", 1, "Cauchy", 2, "Huber" 40 | int nEstimator = 0; 41 | 42 | // Find the covariance-scaled reprojection error for each measurement. 43 | // Also, store the square of these quantities for M-Estimator sigma squared estimation. 44 | std::vector vdErrorSquared; 45 | for(unsigned int f=0; f 0) 59 | dSigmaSquared = dOverrideSigma; // Bit of a waste having stored the vector of square errors in this case! 60 | else 61 | { 62 | if (nEstimator == 0) 63 | dSigmaSquared = Tukey::FindSigmaSquared(vdErrorSquared); 64 | else if(nEstimator == 1) 65 | dSigmaSquared = Cauchy::FindSigmaSquared(vdErrorSquared); 66 | else 67 | dSigmaSquared = Huber::FindSigmaSquared(vdErrorSquared); 68 | } 69 | 70 | // The TooN WLSCholesky class handles reweighted least squares. 71 | // It just needs errors and jacobians. 72 | WLS<6> wls; 73 | wls.add_prior(100.0); // Stabilising prior 74 | for(unsigned int f=0; f &v2 = TD.v2Error_CovScaled; 78 | double dErrorSq = v2 * v2; 79 | double dWeight; 80 | 81 | if(nEstimator == 0) 82 | dWeight= Tukey::Weight(dErrorSq, dSigmaSquared); 83 | else if(nEstimator == 1) 84 | dWeight= Cauchy::Weight(dErrorSq, dSigmaSquared); 85 | else 86 | dWeight= Huber::Weight(dErrorSq, dSigmaSquared); 87 | if(TD.age>0)// for integrated feature, always >0, otherwise, always==-1 88 | dWeight*=TD.age; 89 | // Inlier/outlier accounting, only really works for cut-off estimators such as Tukey. 90 | if(dWeight == 0.0) 91 | { 92 | if(bMarkOutliers) 93 | TD.isInlier=false; 94 | continue; 95 | } 96 | else{ 97 | if(bMarkOutliers) 98 | TD.isInlier=true; 99 | } 100 | Matrix<2,6> &m26Jac = TD.m26Jacobian; 101 | wls.add_mJ(v2[0], TrackerData::dSqrtInvNoise * m26Jac[0], dWeight); // These two lines are currently 102 | wls.add_mJ(v2[1], TrackerData::dSqrtInvNoise * m26Jac[1], dWeight); // the slowest bit of poseits 103 | } 104 | 105 | wls.compute(); 106 | //if (bMarkOutliers) //only true for last iteration, see code above... (iter==9) 107 | //mmCovariances=TooN::SVD<6>(wls.get_C_inv()).get_pinv(); 108 | 109 | return wls.get_mu(); 110 | } 111 | 112 | } 113 | -------------------------------------------------------------------------------- /Thirdparty/libviso2/src/viso.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | This file is part of libviso2. 7 | Authors: Andreas Geiger 8 | 9 | libviso2 is free software; you can redistribute it and/or modify it under the 10 | terms of the GNU General Public License as published by the Free Software 11 | Foundation; either version 2 of the License, or any later version. 12 | 13 | libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY 14 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 15 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License along with 18 | libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin 19 | Street, Fifth Floor, Boston, MA 02110-1301, USA 20 | */ 21 | 22 | #include "viso.h" 23 | 24 | #include 25 | 26 | using namespace std; 27 | namespace libviso2{ 28 | VisualOdometry::VisualOdometry (parameters param) : param(param) { 29 | J = 0; 30 | p_observe = 0; 31 | p_predict = 0; 32 | matcher = new Matcher(param.match); 33 | Tr_delta = Matrix::eye(4); 34 | Tr_valid = false; 35 | srand(0); 36 | } 37 | 38 | VisualOdometry::~VisualOdometry () { 39 | delete matcher; 40 | } 41 | 42 | bool VisualOdometry::updateMotion () { 43 | 44 | // estimate motion 45 | vector tr_delta = estimateMotion(p_matched); 46 | 47 | // on failure 48 | if (tr_delta.size()!=6) 49 | return false; 50 | 51 | // set transformation matrix (previous to current frame) 52 | Tr_delta = transformationVectorToMatrix(tr_delta); 53 | Tr_valid = true; 54 | 55 | // success 56 | return true; 57 | } 58 | 59 | Matrix VisualOdometry::transformationVectorToMatrix (vector tr) { 60 | 61 | // extract parameters 62 | double rx = tr[0]; 63 | double ry = tr[1]; 64 | double rz = tr[2]; 65 | double tx = tr[3]; 66 | double ty = tr[4]; 67 | double tz = tr[5]; 68 | 69 | // precompute sine/cosine 70 | double sx = sin(rx); 71 | double cx = cos(rx); 72 | double sy = sin(ry); 73 | double cy = cos(ry); 74 | double sz = sin(rz); 75 | double cz = cos(rz); 76 | 77 | // compute transformation 78 | Matrix Tr(4,4); 79 | Tr.val[0][0] = +cy*cz; Tr.val[0][1] = -cy*sz; Tr.val[0][2] = +sy; Tr.val[0][3] = tx; 80 | Tr.val[1][0] = +sx*sy*cz+cx*sz; Tr.val[1][1] = -sx*sy*sz+cx*cz; Tr.val[1][2] = -sx*cy; Tr.val[1][3] = ty; 81 | Tr.val[2][0] = -cx*sy*cz+sx*sz; Tr.val[2][1] = +cx*sy*sz+sx*cz; Tr.val[2][2] = +cx*cy; Tr.val[2][3] = tz; 82 | Tr.val[3][0] = 0; Tr.val[3][1] = 0; Tr.val[3][2] = 0; Tr.val[3][3] = 1; 83 | return Tr; 84 | } 85 | vector VisualOdometry::transformationMatrixToVector (Matrix Tr) { 86 | vector tr(6); 87 | // extract parameters 88 | tr[0]= atan2(-Tr.val[1][2],Tr.val[2][2]);//rx 89 | tr[1]=asin(Tr.val[0][2]);//ry 90 | tr[2]=atan2(-Tr.val[0][1], Tr.val[0][0]);//rz 91 | tr[3]=Tr.val[0][3];//tx 92 | tr[4]=Tr.val[1][3];//ty 93 | tr[5]=Tr.val[2][3];//tz 94 | return tr; 95 | } 96 | 97 | vector VisualOdometry::getRandomSample(int32_t N,int32_t num) { 98 | 99 | // init sample and totalset 100 | vector sample; 101 | vector totalset; 102 | 103 | // create vector containing all indices 104 | for (int32_t i=0; i 8 | # Copyright (c) 2008, 2009 Gael Guennebaud, 9 | # Copyright (c) 2009 Benoit Jacob 10 | # Redistribution and use is allowed according to the terms of the 2-clause BSD 11 | # license. 12 | # 13 | # 14 | # Input variables: 15 | # 16 | # - Eigen_ROOT_DIR (optional): When specified, header files and libraries 17 | # will be searched for in `${Eigen_ROOT_DIR}/include` and 18 | # `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order 19 | # will be ignored. When unspecified, the default CMake search order is used. 20 | # This variable can be specified either as a CMake or environment variable. 21 | # If both are set, preference is given to the CMake variable. 22 | # Use this variable for finding packages installed in a nonstandard location, 23 | # or for enforcing that one of multiple package installations is picked up. 24 | # 25 | # Cache variables (not intended to be used in CMakeLists.txt files) 26 | # 27 | # - Eigen_INCLUDE_DIR: Absolute path to package headers. 28 | # 29 | # 30 | # Output variables: 31 | # 32 | # - Eigen_FOUND: Boolean that indicates if the package was found 33 | # - Eigen_INCLUDE_DIRS: Paths to the necessary header files 34 | # - Eigen_VERSION: Version of Eigen library found 35 | # - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen 36 | # 37 | # 38 | # Example usage: 39 | # 40 | # # Passing the version means Eigen_FOUND will only be TRUE if a 41 | # # version >= the provided version is found. 42 | # find_package(Eigen 3.1.2) 43 | # if(NOT Eigen_FOUND) 44 | # # Error handling 45 | # endif() 46 | # ... 47 | # add_definitions(${Eigen_DEFINITIONS}) 48 | # ... 49 | # include_directories(${Eigen_INCLUDE_DIRS} ...) 50 | # 51 | ############################################################################### 52 | 53 | find_package(PkgConfig) 54 | pkg_check_modules(PC_EIGEN eigen3) 55 | set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) 56 | 57 | 58 | find_path(EIGEN_INCLUDE_DIR Eigen/Core 59 | HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} 60 | "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" 61 | "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility 62 | PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" 63 | "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" 64 | PATH_SUFFIXES eigen3 include/eigen3 include) 65 | 66 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) 70 | 71 | mark_as_advanced(EIGEN_INCLUDE_DIR) 72 | 73 | if(EIGEN_FOUND) 74 | message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") 75 | endif(EIGEN_FOUND) 76 | 77 | 78 | set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 79 | set(Eigen_FOUND ${EIGEN_FOUND}) 80 | set(Eigen_VERSION ${EIGEN_VERSION}) 81 | set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) 82 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/include/vikit/abstract_camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * abstract_camera.h 3 | * 4 | * Created on: Jul 23, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #ifndef ABSTRACT_CAMERA_H_ 9 | #define ABSTRACT_CAMERA_H_ 10 | 11 | #include 12 | 13 | namespace vk 14 | { 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | class AbstractCamera 20 | { 21 | protected: 22 | 23 | int width_; // TODO cannot be const because of omni-camera model 24 | int height_; 25 | 26 | public: 27 | 28 | AbstractCamera() {}; // need this constructor for omni camera 29 | AbstractCamera(int width, int height) : width_(width), height_(height) {}; 30 | 31 | virtual ~AbstractCamera() {}; 32 | 33 | /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. 34 | virtual Vector3d 35 | cam2world(const double& x, const double& y) const = 0; 36 | 37 | /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. 38 | virtual Vector3d 39 | cam2world(const Vector2d& px) const = 0; 40 | 41 | virtual Vector2d 42 | world2cam(const Vector3d& xyz_c) const = 0; 43 | 44 | /// projects unit plane coordinates to camera coordinates 45 | virtual Vector2d 46 | world2cam(const Vector2d& uv) const = 0; 47 | 48 | virtual double 49 | errorMultiplier2() const = 0; 50 | 51 | virtual double 52 | errorMultiplier() const = 0; 53 | 54 | inline int width() const { return width_; } 55 | 56 | inline int height() const { return height_; } 57 | 58 | inline bool isInFrame(const Vector2i & obs, int boundary=0) const 59 | { 60 | if(obs[0]>=boundary && obs[0]=boundary && obs[1]= boundary && obs[0] < width()/(1<= boundary && obs[1] // memset 16 | #include // memset 17 | #include 18 | #include 19 | 20 | namespace vk { 21 | namespace aligned_mem { 22 | 23 | /// Check if the pointer is aligned to the specified byte granularity 24 | inline bool 25 | is_aligned8(const void* ptr) 26 | { 27 | return ((reinterpret_cast(ptr)) & 0x7) == 0; 28 | } 29 | 30 | inline bool 31 | is_aligned16(const void* ptr) 32 | { 33 | return ((reinterpret_cast(ptr)) & 0xF) == 0; 34 | } 35 | 36 | template struct placement_delete 37 | { 38 | enum { Size = (1<= Size) { 52 | placement_delete::free(buf+Size,M-Size); 53 | placement_delete::destruct(buf); 54 | } else { 55 | placement_delete::free(buf, M); 56 | } 57 | } 58 | }; 59 | 60 | template struct placement_delete 61 | { 62 | static inline void free(T*, size_t ) {} 63 | }; 64 | 65 | inline void * aligned_alloc(size_t count, size_t alignment){ 66 | void * mem = NULL; 67 | assert(posix_memalign(&mem, alignment, count) == 0); 68 | return mem; 69 | } 70 | 71 | inline void aligned_free(void * memory) { 72 | free(memory); 73 | } 74 | 75 | template 76 | inline T * aligned_alloc(size_t count, size_t alignment){ 77 | void * data = aligned_alloc(sizeof(T)* count, alignment); 78 | return new (data) T[count]; 79 | } 80 | 81 | template 82 | inline void aligned_free(T * memory, size_t count){ 83 | placement_delete::free(memory, count); 84 | aligned_free(memory); 85 | } 86 | 87 | template inline void memfill(T* data, int n, const T val) 88 | { 89 | T* de = data + n; 90 | for(;data < de; data++) 91 | *data=val; 92 | } 93 | 94 | template<> inline void memfill(unsigned char* data, int n, const unsigned char val) 95 | { 96 | memset(data, val, n); 97 | } 98 | 99 | template<> inline void memfill(signed char* data, int n, const signed char val) 100 | { 101 | memset(data, val, n); 102 | } 103 | 104 | template<> inline void memfill(char* data, int n, const char val) 105 | { 106 | memset(data, val, n); 107 | } 108 | 109 | template 110 | struct AlignedMem { 111 | T* mem; 112 | size_t count; 113 | AlignedMem(size_t c) : count(c) { 114 | mem = aligned_alloc(count, N); 115 | } 116 | ~AlignedMem() { 117 | aligned_free(mem, count); 118 | } 119 | T* data() { return mem; } 120 | const T* data() const { return mem; } 121 | }; 122 | 123 | } // namespace aligned_mem 124 | } // namespace vikit 125 | 126 | 127 | #endif // VIKIT_ALIGNED_MEM_H_ 128 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/include/vikit/performance_monitor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * performance_monitor.h 3 | * 4 | * Created on: Aug 26, 2011 5 | * Author: Christian Forster 6 | */ 7 | 8 | #ifndef VIKIT_PERFORMANCE_MONITOR_H 9 | #define VIKIT_PERFORMANCE_MONITOR_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace vk 18 | { 19 | 20 | struct LogItem 21 | { 22 | double data; 23 | bool set; 24 | }; 25 | 26 | class PerformanceMonitor 27 | { 28 | public: 29 | PerformanceMonitor(); 30 | ~PerformanceMonitor(); 31 | void init(const std::string& trace_name, const std::string& trace_dir); 32 | void addTimer(const std::string& name); 33 | void addLog(const std::string& name); 34 | void writeToFile(); 35 | void startTimer(const std::string& name); 36 | void stopTimer(const std::string& name); 37 | double getTime(const std::string& name) const; 38 | void log(const std::string& name, double data); 39 | 40 | private: 41 | std::map timers_; 42 | std::map logs_; 43 | std::string trace_name_; // 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace vk { 18 | 19 | using namespace std; 20 | using namespace Eigen; 21 | 22 | class PinholeCamera : public AbstractCamera { 23 | 24 | private: 25 | const double fx_, fy_; 26 | const double cx_, cy_; 27 | bool distortion_; //!< is it pure pinhole model or has it radial distortion? 28 | double d_[5]; //!< distortion parameters, see http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html 29 | cv::Mat cvK_, cvD_; 30 | cv::Mat undist_map1_, undist_map2_; 31 | bool use_optimization_; 32 | Matrix3d K_; 33 | Matrix3d K_inv_; 34 | 35 | public: 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | 38 | PinholeCamera(double width, double height, 39 | double fx, double fy, double cx, double cy, 40 | double d0=0.0, double d1=0.0, double d2=0.0, double d3=0.0, double d4=0.0); 41 | 42 | ~PinholeCamera(); 43 | 44 | void 45 | initUnistortionMap(); 46 | 47 | virtual Vector3d 48 | cam2world(const double& x, const double& y) const; 49 | 50 | virtual Vector3d 51 | cam2world(const Vector2d& px) const; 52 | 53 | virtual Vector2d 54 | world2cam(const Vector3d& xyz_c) const; 55 | 56 | virtual Vector2d 57 | world2cam(const Vector2d& uv) const; 58 | 59 | const Vector2d focal_length() const 60 | { 61 | return Vector2d(fx_, fy_); 62 | } 63 | 64 | virtual double errorMultiplier2() const 65 | { 66 | return fabs(fx_); 67 | } 68 | 69 | virtual double errorMultiplier() const 70 | { 71 | return fabs(4.0*fx_*fy_); 72 | } 73 | 74 | inline const Matrix3d& K() const { return K_; }; 75 | inline const Matrix3d& K_inv() const { return K_inv_; }; 76 | inline double fx() const { return fx_; }; 77 | inline double fy() const { return fy_; }; 78 | inline double cx() const { return cx_; }; 79 | inline double cy() const { return cy_; }; 80 | inline double d0() const { return d_[0]; }; 81 | inline double d1() const { return d_[1]; }; 82 | inline double d2() const { return d_[2]; }; 83 | inline double d3() const { return d_[3]; }; 84 | inline double d4() const { return d_[4]; }; 85 | inline void setSize(double w, double h){width_= w; height_= h;} 86 | void undistortImage(const cv::Mat& raw, cv::Mat& rectified); 87 | 88 | }; 89 | 90 | } // end namespace vk 91 | 92 | 93 | #endif /* PINHOLE_CAMERA_H_ */ 94 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/include/vikit/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMER_H 2 | #define TIMER_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace vk 9 | { 10 | 11 | class Timer 12 | { 13 | private: 14 | timeval start_time_; 15 | double time_; 16 | double accumulated_; 17 | public: 18 | 19 | /// The constructor directly starts the timer. 20 | Timer() : 21 | time_(0.0), 22 | accumulated_(0.0) 23 | { 24 | start(); 25 | } 26 | 27 | ~Timer() 28 | {} 29 | 30 | inline void start() 31 | { 32 | accumulated_ = 0.0; 33 | gettimeofday(&start_time_, NULL); 34 | } 35 | 36 | inline void resume() 37 | { 38 | gettimeofday(&start_time_, NULL); 39 | } 40 | 41 | inline double stop() 42 | { 43 | timeval end_time; 44 | gettimeofday(&end_time, NULL); 45 | long seconds = end_time.tv_sec - start_time_.tv_sec; 46 | long useconds = end_time.tv_usec - start_time_.tv_usec; 47 | time_ = ((seconds) + useconds*0.000001) + accumulated_; 48 | accumulated_ = time_; 49 | return time_; 50 | } 51 | 52 | inline double getTime() const 53 | { 54 | return time_; 55 | } 56 | 57 | inline void reset() 58 | { 59 | time_ = 0.0; 60 | accumulated_ = 0.0; 61 | } 62 | 63 | static double getCurrentTime() 64 | { 65 | timeval time_now; 66 | gettimeofday(&time_now, NULL); 67 | return time_now.tv_sec + time_now.tv_usec*0.000001; 68 | } 69 | 70 | static double getCurrentSecond() 71 | { 72 | timeval time_now; 73 | gettimeofday(&time_now, NULL); 74 | return time_now.tv_sec; 75 | } 76 | 77 | }; 78 | 79 | } // end namespace vk 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/include/vikit/vision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * vision.h 3 | * 4 | * Created on: May 14, 2013 5 | * Author: cforster 6 | */ 7 | 8 | #ifndef VIKIT_VISION_H_ 9 | #define VIKIT_VISION_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace vk 16 | { 17 | 18 | //! Return value between 0 and 1 19 | //! WARNING This function does not check whether the x/y is within the border 20 | inline float 21 | interpolateMat_32f(const cv::Mat& mat, float u, float v) 22 | { 23 | assert(mat.type()==CV_32F); 24 | float x = floor(u); 25 | float y = floor(v); 26 | float subpix_x = u-x; 27 | float subpix_y = v-y; 28 | float wx0 = 1.0-subpix_x; 29 | float wx1 = subpix_x; 30 | float wy0 = 1.0-subpix_y; 31 | float wy1 = subpix_y; 32 | 33 | float val00 = mat.at(y,x); 34 | float val10 = mat.at(y,x+1); 35 | float val01 = mat.at(y+1,x); 36 | float val11 = mat.at(y+1,x+1); 37 | return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; 38 | } 39 | 40 | //! Return value between 0 and 255 41 | //! WARNING This function does not check whether the x/y is within the border 42 | inline float 43 | interpolateMat_8u(const cv::Mat& mat, float u, float v) 44 | { 45 | assert(mat.type()==CV_8U); 46 | int x = floor(u); 47 | int y = floor(v); 48 | float subpix_x = u-x; 49 | float subpix_y = v-y; 50 | 51 | float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); 52 | float w01 = (1.0f-subpix_x)*subpix_y; 53 | float w10 = subpix_x*(1.0f-subpix_y); 54 | float w11 = 1.0f - w00 - w01 - w10; 55 | 56 | const int stride = mat.step.p[0]; 57 | unsigned char* ptr = mat.data + y*stride + x; 58 | return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; 59 | } 60 | 61 | void halfSample(const cv::Mat& in, cv::Mat& out); 62 | 63 | float shiTomasiScore(const cv::Mat& img, int u, int v); 64 | 65 | void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); 66 | 67 | #ifdef __SSE2__ 68 | 69 | /// Used to convert a Kinect depthmap 70 | /// Code by Christian Kerl DVO, GPL Licence 71 | void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); 72 | 73 | #endif 74 | 75 | } // namespace vk 76 | 77 | #endif // VIKIT_VISION_H_ 78 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/lib/libvikit_common.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/henrywen2011/orb_slam_imu/79525ca372dd5aa1b5c48610779a5c234e7bc918/Thirdparty/vikit_common/lib/libvikit_common.so -------------------------------------------------------------------------------- /Thirdparty/vikit_common/src/performance_monitor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * performance_monitor.cpp 3 | * 4 | * Created on: Aug 26, 2011 5 | * Author: Christian Forster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace vk 13 | { 14 | using namespace std; 15 | 16 | PerformanceMonitor::PerformanceMonitor() 17 | {} 18 | 19 | PerformanceMonitor::~PerformanceMonitor() 20 | { 21 | ofs_.flush(); 22 | ofs_.close(); 23 | } 24 | 25 | void PerformanceMonitor::init( 26 | const string& trace_name, 27 | const string& trace_dir) 28 | { 29 | trace_name_ = trace_name; 30 | trace_dir_ = trace_dir; 31 | string filename(trace_dir + "/" + trace_name + ".csv"); 32 | ofs_.open(filename.c_str()); 33 | if(!ofs_.is_open()) 34 | { 35 | printf("Tracefile = %s\n", filename.c_str()); 36 | throw runtime_error("Could not open tracefile."); 37 | } 38 | traceHeader(); 39 | } 40 | 41 | void PerformanceMonitor::addTimer(const string& name) 42 | { 43 | timers_.insert(make_pair(name, Timer())); 44 | } 45 | 46 | void PerformanceMonitor::addLog(const string& name) 47 | { 48 | logs_.insert(make_pair(name, LogItem())); 49 | } 50 | 51 | void PerformanceMonitor::writeToFile() 52 | { 53 | trace(); 54 | 55 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 56 | it->second.reset(); 57 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 58 | { 59 | it->second.set = false; 60 | it->second.data = -1; 61 | } 62 | } 63 | 64 | void PerformanceMonitor::startTimer(const string& name) 65 | { 66 | auto t = timers_.find(name); 67 | if(t == timers_.end()) { 68 | printf("Timer = %s\n", name.c_str()); 69 | throw std::runtime_error("startTimer: Timer not registered"); 70 | } 71 | t->second.start(); 72 | } 73 | 74 | void PerformanceMonitor::stopTimer(const string& name) 75 | { 76 | auto t = timers_.find(name); 77 | if(t == timers_.end()) { 78 | printf("Timer = %s\n", name.c_str()); 79 | throw std::runtime_error("stopTimer: Timer not registered"); 80 | } 81 | t->second.stop(); 82 | } 83 | 84 | double PerformanceMonitor::getTime(const string& name) const 85 | { 86 | auto t = timers_.find(name); 87 | if(t == timers_.end()) { 88 | printf("Timer = %s\n", name.c_str()); 89 | throw std::runtime_error("Timer not registered"); 90 | } 91 | return t->second.getTime(); 92 | } 93 | 94 | void PerformanceMonitor::log(const string& name, double data) 95 | { 96 | auto l = logs_.find(name); 97 | if(l == logs_.end()) { 98 | printf("Logger = %s\n", name.c_str()); 99 | throw std::runtime_error("Logger not registered"); 100 | } 101 | l->second.data = data; 102 | l->second.set = true; 103 | } 104 | 105 | void PerformanceMonitor::trace() 106 | { 107 | char buffer[128]; 108 | bool first_value = true; 109 | if(!ofs_.is_open()) 110 | throw std::runtime_error("Performance monitor not correctly initialized"); 111 | ofs_.precision(15); 112 | ofs_.setf(std::ios::fixed, std::ios::floatfield ); 113 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 114 | { 115 | if(first_value) { 116 | ofs_ << it->second.getTime(); 117 | first_value = false; 118 | } 119 | else 120 | ofs_ << "," << it->second.getTime(); 121 | } 122 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 123 | { 124 | if(first_value) { 125 | ofs_ << it->second.data; 126 | first_value = false; 127 | } 128 | else 129 | ofs_ << "," << it->second.data; 130 | } 131 | ofs_ << "\n"; 132 | } 133 | 134 | void PerformanceMonitor::traceHeader() 135 | { 136 | if(!ofs_.is_open()) 137 | throw std::runtime_error("Performance monitor not correctly initialized"); 138 | bool first_value = true; 139 | for(auto it = timers_.begin(); it!=timers_.end(); ++it) 140 | { 141 | if(first_value) { 142 | ofs_ << it->first; 143 | first_value = false; 144 | } 145 | else 146 | ofs_ << "," << it->first; 147 | } 148 | for(auto it=logs_.begin(); it!=logs_.end(); ++it) 149 | { 150 | if(first_value) { 151 | ofs_ << it->first; 152 | first_value = false; 153 | } 154 | else 155 | ofs_ << "," << it->first; 156 | } 157 | ofs_ << "\n"; 158 | } 159 | 160 | } // namespace vk 161 | 162 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/src/pinhole_camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * pinhole_camera.cpp 3 | * 4 | * Created on: Jul 24, 2012 5 | * Author: cforster 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace vk { 18 | 19 | PinholeCamera:: 20 | PinholeCamera(double width, double height, 21 | double fx, double fy, 22 | double cx, double cy, 23 | double d0, double d1, double d2, double d3, double d4) : 24 | AbstractCamera(width, height), 25 | fx_(fx), fy_(fy), cx_(cx), cy_(cy), 26 | distortion_(fabs(d0) > 0.0000001), 27 | undist_map1_(height_, width_, CV_16SC2), 28 | undist_map2_(height_, width_, CV_16SC2), 29 | use_optimization_(false) 30 | { 31 | d_[0] = d0; d_[1] = d1; d_[2] = d2; d_[3] = d3; d_[4] = d4; 32 | cvK_ = (cv::Mat_(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0); 33 | cvD_ = (cv::Mat_(1, 5) << d_[0], d_[1], d_[2], d_[3], d_[4]); 34 | cv::initUndistortRectifyMap(cvK_, cvD_, cv::Mat_::eye(3,3), cvK_, 35 | cv::Size(width_, height_), CV_16SC2, undist_map1_, undist_map2_); 36 | K_ << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0; 37 | K_inv_ = K_.inverse(); 38 | } 39 | 40 | PinholeCamera:: 41 | ~PinholeCamera() 42 | {} 43 | 44 | Vector3d PinholeCamera:: 45 | cam2world(const double& u, const double& v) const 46 | { 47 | Vector3d xyz; 48 | if(!distortion_) 49 | { 50 | xyz[0] = (u - cx_)/fx_; 51 | xyz[1] = (v - cy_)/fy_; 52 | xyz[2] = 1.0; 53 | } 54 | else 55 | { 56 | cv::Point2f uv(u,v), px; 57 | const cv::Mat src_pt(1, 1, CV_32FC2, &uv.x); 58 | cv::Mat dst_pt(1, 1, CV_32FC2, &px.x); 59 | cv::undistortPoints(src_pt, dst_pt, cvK_, cvD_); 60 | xyz[0] = px.x; 61 | xyz[1] = px.y; 62 | xyz[2] = 1.0; 63 | } 64 | return xyz.normalized(); 65 | } 66 | 67 | Vector3d PinholeCamera:: 68 | cam2world (const Vector2d& uv) const 69 | { 70 | return cam2world(uv[0], uv[1]); 71 | } 72 | 73 | Vector2d PinholeCamera:: 74 | world2cam(const Vector3d& xyz) const 75 | { 76 | return world2cam(project2d(xyz)); 77 | } 78 | 79 | Vector2d PinholeCamera:: 80 | world2cam(const Vector2d& uv) const 81 | { 82 | Vector2d px; 83 | if(!distortion_) 84 | { 85 | px[0] = fx_*uv[0] + cx_; 86 | px[1] = fy_*uv[1] + cy_; 87 | } 88 | else 89 | { 90 | double x, y, r2, r4, r6, a1, a2, a3, cdist, xd, yd; 91 | x = uv[0]; 92 | y = uv[1]; 93 | r2 = x*x + y*y; 94 | r4 = r2*r2; 95 | r6 = r4*r2; 96 | a1 = 2*x*y; 97 | a2 = r2 + 2*x*x; 98 | a3 = r2 + 2*y*y; 99 | cdist = 1 + d_[0]*r2 + d_[1]*r4 + d_[4]*r6; 100 | xd = x*cdist + d_[2]*a1 + d_[3]*a2; 101 | yd = y*cdist + d_[2]*a3 + d_[3]*a1; 102 | px[0] = xd*fx_ + cx_; 103 | px[1] = yd*fy_ + cy_; 104 | } 105 | return px; 106 | } 107 | 108 | void PinholeCamera:: 109 | undistortImage(const cv::Mat& raw, cv::Mat& rectified) 110 | { 111 | if(distortion_) 112 | cv::remap(raw, rectified, undist_map1_, undist_map2_, CV_INTER_LINEAR); 113 | else 114 | rectified = raw.clone(); 115 | } 116 | 117 | } // end namespace vk 118 | -------------------------------------------------------------------------------- /Thirdparty/vikit_common/vikit_commonConfig.cmake.in: -------------------------------------------------------------------------------- 1 | ####################################################### 2 | # vikit_common source dir 3 | set( vikit_common_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") 4 | 5 | ####################################################### 6 | # vikit_common build dir 7 | set( vikit_common_DIR "@CMAKE_CURRENT_BINARY_DIR@") 8 | 9 | ####################################################### 10 | set( vikit_common_INCLUDE_DIR "@vikit_common_INCLUDE_DIR@" ) 11 | set( vikit_common_INCLUDE_DIRS "@vikit_common_INCLUDE_DIR@" ) 12 | 13 | set( vikit_common_LIBRARIES "@vikit_common_LIBRARIES@" ) 14 | set( vikit_common_LIBRARY "@vikit_common_LIBRARIES@" ) 15 | 16 | set( vikit_common_LIBRARY_DIR "@vikit_common_LIBRARY_DIR@" ) 17 | set( vikit_common_LIBRARY_DIRS "@vikit_common_LIBRARY_DIR@" ) 18 | -------------------------------------------------------------------------------- /cmake_modules/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /cmake_modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | # Different platforms seemingly require linking against different sets of libraries 39 | IF(CYGWIN) 40 | FIND_PACKAGE(PkgConfig) 41 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 42 | PATHS 43 | /usr/lib 44 | /usr/local/lib 45 | /opt/local/lib 46 | /sw/lib 47 | ) 48 | PKG_CHECK_MODULES(LAPACK lapack) 49 | 50 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 51 | 52 | # MacPorts build of the SparseSuite requires linking against extra libraries 53 | 54 | ELSEIF(APPLE) 55 | 56 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 57 | PATHS 58 | /usr/lib 59 | /usr/local/lib 60 | /opt/local/lib 61 | /sw/lib 62 | ) 63 | 64 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 65 | PATHS 66 | /usr/lib 67 | /usr/local/lib 68 | /opt/local/lib 69 | /sw/lib 70 | ) 71 | 72 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 73 | PATHS 74 | /usr/lib 75 | /usr/local/lib 76 | /opt/local/lib 77 | /sw/lib 78 | ) 79 | 80 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 81 | ELSE(APPLE) 82 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 83 | ENDIF(CYGWIN) 84 | 85 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 86 | SET(CHOLMOD_FOUND TRUE) 87 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 88 | SET(CHOLMOD_FOUND FALSE) 89 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 90 | 91 | # Look for csparse; note the difference in the directory specifications! 92 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 93 | PATHS 94 | /usr/include/suitesparse 95 | /usr/include 96 | /opt/local/include 97 | /usr/local/include 98 | /sw/include 99 | /usr/include/ufsparse 100 | /opt/local/include/ufsparse 101 | /usr/local/include/ufsparse 102 | /sw/include/ufsparse 103 | ) 104 | 105 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 106 | PATHS 107 | /usr/lib 108 | /usr/local/lib 109 | /opt/local/lib 110 | /sw/lib 111 | ) 112 | 113 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 114 | SET(CSPARSE_FOUND TRUE) 115 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 116 | SET(CSPARSE_FOUND FALSE) 117 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 118 | -------------------------------------------------------------------------------- /data/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Marker1 10 | - /Marker1/Namespaces1 11 | Splitter Ratio: 0.5 12 | Tree Height: 514 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz/Marker 36 | Enabled: true 37 | Marker Topic: /ORBSLAM_DWO/Map 38 | Name: Marker 39 | Namespaces: 40 | Camera: true 41 | Graph: true 42 | KeyFrames: true 43 | MapPoints: true 44 | Queue Size: 100 45 | Value: true 46 | Enabled: true 47 | Global Options: 48 | Background Color: 255; 255; 255 49 | Fixed Frame: /ORBSLAM_DWO/Camera 50 | Name: root 51 | Tools: 52 | - Class: rviz/Interact 53 | Hide Inactive Objects: true 54 | - Class: rviz/MoveCamera 55 | - Class: rviz/Select 56 | - Class: rviz/FocusCamera 57 | - Class: rviz/Measure 58 | - Class: rviz/SetInitialPose 59 | Topic: /initialpose 60 | - Class: rviz/SetGoal 61 | Topic: /move_base_simple/goal 62 | - Class: rviz/PublishPoint 63 | Single click: true 64 | Topic: /clicked_point 65 | Value: true 66 | Views: 67 | Current: 68 | Class: rviz/Orbit 69 | Distance: 3.996 70 | Focal Point: 71 | X: -0.111169 72 | Y: -0.187361 73 | Z: 0.713479 74 | Name: Current View 75 | Near Clip Distance: 0.01 76 | Pitch: -0.854796 77 | Target Frame: /ORBSLAM_DWO/Camera 78 | Value: Orbit (rviz) 79 | Yaw: 4.6604 80 | Saved: ~ 81 | Window Geometry: 82 | Displays: 83 | collapsed: false 84 | Height: 795 85 | Hide Left Dock: false 86 | Hide Right Dock: false 87 | QMainWindow State: 000000ff00000000fd00000004000000000000010f00000291fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000291000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000291fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000291000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a10000003efc0100000002fb0000000800540069006d00650100000000000003a1000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000028c0000029100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 88 | Selection: 89 | collapsed: false 90 | Time: 91 | collapsed: false 92 | Tool Properties: 93 | collapsed: false 94 | Views: 95 | collapsed: false 96 | Width: 929 97 | X: 590 98 | Y: 174 99 | -------------------------------------------------------------------------------- /data/rviz.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=1 2 | Background\ ColorG=1 3 | Background\ ColorR=1 4 | Camera\ Config=-1.3748 4.59369 4.31087 0.0334986 -0.381037 1.14662 5 | Camera\ Type=rviz::OrbitViewController 6 | Fixed\ Frame=/ORBSLAM_DWO/Camera 7 | Marker.Camera=1 8 | Marker.Enabled=1 9 | Marker.Graph=1 10 | Marker.KeyFrames=1 11 | Marker.MapPoints=1 12 | Marker.Marker\ Topic=/ORBSLAM_DWO/Map 13 | Marker.Queue\ Size=100 14 | Property\ Grid\ Splitter=502,78 15 | Property\ Grid\ State=expanded=.Global Options,Marker.Enabled;splitterratio=0.5 16 | QMainWindow=000000ff00000000fd00000003000000000000011d00000296fc0200000002fb000000100044006900730070006c006100790073010000001d00000296000000ee00fffffffb0000000a00560069006500770073000000011c000000f8000000bb00ffffff00000001000001310000026ffc0200000002fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000000a00000006700fffffffb0000001200530065006c0065006300740069006f006e00000001b1000000db0000006700ffffff00000003000003910000003efc0100000001fb0000000800540069006d0065010000000000000391000002bf00ffffff0000026e0000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 17 | Target\ Frame=/ORBSLAM_DWO/Camera 18 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 19 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 20 | [Display0] 21 | ClassName=rviz::MarkerDisplay 22 | Name=Marker 23 | [Window] 24 | Height=795 25 | Width=929 26 | X=734 27 | Y=-28 28 | -------------------------------------------------------------------------------- /data/settingfiles_stereo/kittiseq00.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | startIndex: 0 3 | finishIndex: 4540 4 | # choose one of KITTIOdoSeq, Tsukuba, MalagaUrbanExtract6 5 | dataset: "KITTIOdoSeq" 6 | use_imu_data:false 7 | 8 | input_path: /media/jhuai/Mag/kitti/dataset/sequences/00 9 | time_file: /media/jhuai/Mag/kitti/dataset/sequences/00/times.txt 10 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 11 | output_file: /media/jhuai/Mag/temp/kittiseq00.txt 12 | output_point_file: /media/jhuai/Mag/temp/kittiseq00_points.txt 13 | trace_dir: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/result" # where to put the slam profiled time 14 | 15 | sample_interval: 0.01 16 | na: !!opencv-matrix 17 | rows: 3 18 | cols: 1 19 | dt: d 20 | data: [80e-5, 80e-5, 80e-5 ] # 80 ug with 1 Hz 21 | # q_na=na^2 22 | nw: !!opencv-matrix 23 | rows: 3 24 | cols: 1 25 | dt: d 26 | data: [0.03, 0.03, 0.03] # 0.03 deg/sec/sqrt(Hz) 27 | #q_nw=(nw*pi/180)^2 28 | acc_bias_Tc: 1800 #sec 29 | acc_bias_var: !!opencv-matrix 30 | rows: 3 31 | cols: 1 32 | dt: d 33 | data: [.04e-2, .04e-2, .04e-2]#bias drift variability, 0.04 mg 34 | #q_n_ba=(.04e-2)^2*(2/acc_bias_Tc); 35 | gyro_bias_Tc: 1800 #sec 36 | gyro_bias_var: !!opencv-matrix 37 | rows: 3 38 | cols: 1 39 | dt: d 40 | data: [5e-3, 5e-3, 5e-3]#bias drift variability, 18 deg/hour 41 | #q_n_bw=(18*(pi/180.0)/3600)^2*(2/gyro_bias_Tc); # 18deg/hr 42 | 43 | Rs2c: !!opencv-matrix 44 | rows: 3 45 | cols: 3 46 | dt: d 47 | data: [ 0, 1, 0, 0, 0, 1, 1, 0, 0 ] 48 | tsinc: !!opencv-matrix 49 | rows: 3 50 | cols: 1 51 | dt: d 52 | data: [ 0.05, 0.1, -0.1]#the coordinate of sensor frame origin in left P0 camera frame 53 | gw: !!opencv-matrix # gravity in the local world frame 54 | rows: 3 55 | cols: 1 56 | dt: f 57 | data: [ -0.000391708956131998, 9.78093956865728, 6.81059618015123e-05] 58 | wiew: !!opencv-matrix # earth rotation in the local world frame 59 | rows: 3 60 | cols: 1 61 | dt: f 62 | data: [ -1.86714087981123e-05, -1.06396154889527e-05, 6.96826462834436e-05] 63 | # the world frame is the LEFT camera frame at startIndex 64 | vs0inw: !!opencv-matrix 65 | rows: 3 66 | cols: 1 67 | dt: d 68 | data: [ -0.441011, -0.268243, 8.28043 ] 69 | 70 | # Camera calibration parameters (OpenCV) 71 | Camera.width: 1241 72 | Camera.height: 376 73 | Camera.fx: 718.856 74 | Camera.fy: 718.856 75 | Camera.cx: 607.1928 76 | Camera.cy: 185.2157 77 | 78 | 79 | # Camera distortion paremeters (OpenCV) -- 80 | Camera.k1: 0 81 | Camera.k2: 0 82 | Camera.p1: 0.0 83 | Camera.p2: 0.0 84 | 85 | Stereo.se3Left2Right: !!opencv-matrix 86 | rows: 4 87 | cols: 4 88 | dt: f 89 | data: [ 1, 0, 0, -0.5372, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] 90 | 91 | Stereo.matRightK: !!opencv-matrix 92 | rows: 3 93 | cols: 3 94 | dt: f 95 | data: [ 718.856, 0, 607.1928, 0, 718.856, 185.2157, 0, 0, 1] 96 | # Stereo.matRightDistCoef: k1 k2 p1 p2 (Opencv) 97 | Stereo.matRightDistCoef: !!opencv-matrix 98 | rows: 4 99 | cols: 1 100 | dt: f 101 | data: [ 0, 0, 0, 0] 102 | 103 | # Camera frames per second 104 | Camera.fps: 10.0 105 | 106 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 107 | Camera.RGB: 1 108 | 109 | #-------------------------------------------------------------------------------------------- 110 | ### Changing the parameters below could seriously degradate the performance of the system 111 | 112 | # ORB Extractor: Number of features per image 113 | ORBextractor.nFeatures: 600 114 | 115 | # ORB Extractor: Scale factor between levels in the scale pyramid 116 | ORBextractor.scaleFactor: 1.2 117 | 118 | # ORB Extractor: Number of levels in the scale pyramid 119 | ORBextractor.nLevels: 1 120 | 121 | # ORB Extractor: Fast threshold (lower less restrictive) 122 | ORBextractor.fastTh: 20 123 | 124 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 125 | ORBextractor.nScoreType: 1 126 | 127 | # the following parameters determines necessary conditions to create a new keyframe 128 | Tracking.tracked_feature_ratio: 0.6 #if the current frame tracks less than this ratio of features in the reference keyframe 129 | Tracking.min_tracked_features: 120 #if the current frame tracks less than this number of features in the reference keyframe 130 | -------------------------------------------------------------------------------- /data/settingfiles_stereo/tsukuba.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # the world frame is the left camera frame (right, down, forward) at startIndex 3 | startIndex: 1 4 | finishIndex: 1800 5 | dataset: "Tsukuba" # choose one of KITTISeq00, Tsukuba, MalagaUrbanExtract6 6 | use_imu_data: false 7 | 8 | input_path: /media/jhuai/Mag/NewTsukubaStereoDataset/illumination/daylight 9 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 10 | output_file: /media/jhuai/Mag/temp/tsukuba.txt 11 | output_point_file: /media/jhuai/Mag/temp/tsukuba_points.txt 12 | trace_dir: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/result" # where to put the slam profiled time 13 | 14 | sample_interval: 0.01 15 | na: !!opencv-matrix 16 | rows: 3 17 | cols: 1 18 | dt: d 19 | data: [80e-5, 80e-5, 80e-5 ] # 80 ug with 1 Hz 20 | # q_na=na^2 21 | nw: !!opencv-matrix 22 | rows: 3 23 | cols: 1 24 | dt: d 25 | data: [0.03, 0.03, 0.03] # 0.03 deg/sec/sqrt(Hz) 26 | #q_nw=(nw*pi/180)^2 27 | acc_bias_Tc: 1800 #sec 28 | acc_bias_var: !!opencv-matrix 29 | rows: 3 30 | cols: 1 31 | dt: d 32 | data: [.04e-2, .04e-2, .04e-2]#bias drift variability, 0.04 mg 33 | #q_n_ba=(.04e-2)^2*(2/acc_bias_Tc); 34 | gyro_bias_Tc: 1800 #sec 35 | gyro_bias_var: !!opencv-matrix 36 | rows: 3 37 | cols: 1 38 | dt: d 39 | data: [5e-3, 5e-3, 5e-3]#bias drift variability, 18 deg/hour 40 | #q_n_bw=(18*(pi/180.0)/3600)^2*(2/gyro_bias_Tc); # 18deg/hr 41 | # rotation from IMU frame (forward, right, down) to camera frame(right, down, forward), 42 | Rs2c: !!opencv-matrix 43 | rows: 3 44 | cols: 3 45 | dt: d 46 | data: [ 0, 1, 0, 0, 0, 1, 1, 0, 0 ] 47 | tsinc: !!opencv-matrix 48 | rows: 3 49 | cols: 1 50 | dt: d 51 | data: [ 0.05, -0.06, 0.02 ] # N.B. camera_track.txt of Tsukuba used a central camera frame, in our program, we use left camera frame 52 | 53 | gw: !!opencv-matrix # gravity in the local world frame 54 | rows: 3 55 | cols: 1 56 | dt: f 57 | data: [ -0.000295986886093669, 9.80080743561639, -1.24212161392023e-05] 58 | wiew: !!opencv-matrix # earth rotation in the local world frame 59 | rows: 3 60 | cols: 1 61 | dt: d 62 | data: [ 0, -4.68759582209682e-05 , 5.58582013511762e-05] 63 | 64 | #vs0inw: velocity of IMU frame in world frame at startIndex 65 | vs0inw: !!opencv-matrix 66 | rows: 3 67 | cols: 1 68 | dt: d 69 | data: [ 5.66554e-05, -4.80246e-05, 0.000643521] 70 | 71 | 72 | # Camera calibration parameters (OpenCV) 73 | Camera.width: 640 74 | Camera.height: 480 75 | Camera.fx: 615 76 | Camera.fy: 615 77 | Camera.cx: 320 78 | Camera.cy: 240 79 | 80 | # Camera distortion paremeters (OpenCV) -- 81 | Camera.k1: 0 82 | Camera.k2: 0 83 | Camera.p1: 0.0 84 | Camera.p2: 0.0 85 | 86 | Stereo.se3Left2Right: !!opencv-matrix 87 | rows: 4 88 | cols: 4 89 | dt: f 90 | data: [ 1, 0, 0, -0.1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] 91 | 92 | Stereo.matRightK: !!opencv-matrix 93 | rows: 3 94 | cols: 3 95 | dt: f 96 | data: [ 615, 0, 320, 0, 615, 240, 0, 0, 1] 97 | # Stereo.matRightDistCoef: k1 k2 p1 p2 (Opencv) 98 | Stereo.matRightDistCoef: !!opencv-matrix 99 | rows: 4 100 | cols: 1 101 | dt: f 102 | data: [ 0, 0, 0, 0] 103 | 104 | # Camera frames per second 105 | Camera.fps: 30.0 106 | 107 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 108 | Camera.RGB: 1 109 | 110 | #-------------------------------------------------------------------------------------------- 111 | ### Changing the parameters below could seriously degradate the performance of the system 112 | 113 | # ORB Extractor: Number of features per image 114 | ORBextractor.nFeatures: 500 115 | 116 | # ORB Extractor: Scale factor between levels in the scale pyramid 117 | ORBextractor.scaleFactor: 1.2 118 | 119 | # ORB Extractor: Number of levels in the scale pyramid 120 | ORBextractor.nLevels: 1 121 | 122 | # ORB Extractor: Fast threshold (lower less restrictive) 123 | ORBextractor.fastTh: 20 124 | 125 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 126 | ORBextractor.nScoreType: 1 127 | 128 | -------------------------------------------------------------------------------- /data/settingfiles_stereo_imu/kittiseq00_imu.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | startIndex: 0 3 | finishIndex: 4540 4 | dataset: "KITTIOdoSeq" # choose one of KITTIOdoSeq, Tsukuba, MalagaUrbanExtract6 5 | use_imu_data:true 6 | 7 | input_path: /media/jhuai/Mag/kitti/dataset/sequences/00 8 | time_file: /media/jhuai/Mag/kitti/dataset/sequences/00/times.txt 9 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 10 | output_file: /media/jhuai/Mag/temp/kittiseq00_imu.txt 11 | output_point_file: /media/jhuai/Mag/temp/kittiseq00_imu_points.txt 12 | imu_file: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/kittiseq00_imu_samplesnoisy.txt" #samples added noise generated according to Microstrain 3DM-GX3-35 13 | trace_dir: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/result" # where to put the slam profiled time 14 | 15 | sample_interval: 0.01 16 | na: !!opencv-matrix 17 | rows: 3 18 | cols: 1 19 | dt: d 20 | data: [80e-5, 80e-5, 80e-5 ] # 80 ug with 1 Hz 21 | # q_na=na^2 22 | nw: !!opencv-matrix 23 | rows: 3 24 | cols: 1 25 | dt: d 26 | data: [0.03, 0.03, 0.03] # 0.03 deg/sec/sqrt(Hz) 27 | #q_nw=(nw*pi/180)^2 28 | acc_bias_Tc: 1800 #sec 29 | acc_bias_var: !!opencv-matrix 30 | rows: 3 31 | cols: 1 32 | dt: d 33 | data: [.04e-2, .04e-2, .04e-2]#bias drift variability, 0.04 mg 34 | #q_n_ba=(.04e-2)^2*(2/acc_bias_Tc); 35 | gyro_bias_Tc: 1800 #sec 36 | gyro_bias_var: !!opencv-matrix 37 | rows: 3 38 | cols: 1 39 | dt: d 40 | data: [5e-3, 5e-3, 5e-3]#bias drift variability, 18 deg/hour 41 | #q_n_bw=(18*(pi/180.0)/3600)^2*(2/gyro_bias_Tc); # 18deg/hr 42 | 43 | Rs2c: !!opencv-matrix 44 | rows: 3 45 | cols: 3 46 | dt: d 47 | data: [ 0, 1, 0, 0, 0, 1, 1, 0, 0 ] 48 | tsinc: !!opencv-matrix 49 | rows: 3 50 | cols: 1 51 | dt: d 52 | data: [ 0.05, 0.1, -0.1]#the coordinate of sensor frame origin in left P0 camera frame 53 | gw: !!opencv-matrix # gravity in the local world frame 54 | rows: 3 55 | cols: 1 56 | dt: f 57 | data: [ -0.000391708956131998, 9.78093956865728, 6.81059618015123e-05] 58 | wiew: !!opencv-matrix # earth rotation in the local world frame 59 | rows: 3 60 | cols: 1 61 | dt: f 62 | data: [ -1.86714087981123e-05, -1.06396154889527e-05, 6.96826462834436e-05] 63 | # the world frame is the LEFT camera frame at startIndex 64 | vs0inw: !!opencv-matrix 65 | rows: 3 66 | cols: 1 67 | dt: d 68 | data: [ -0.441011, -0.268243, 8.28043 ] 69 | 70 | # Camera calibration parameters (OpenCV) 71 | Camera.width: 1241 72 | Camera.height: 376 73 | Camera.fx: 718.856 74 | Camera.fy: 718.856 75 | Camera.cx: 607.1928 76 | Camera.cy: 185.2157 77 | 78 | 79 | # Camera distortion paremeters (OpenCV) -- 80 | Camera.k1: 0 81 | Camera.k2: 0 82 | Camera.p1: 0.0 83 | Camera.p2: 0.0 84 | 85 | Stereo.se3Left2Right: !!opencv-matrix 86 | rows: 4 87 | cols: 4 88 | dt: f 89 | data: [ 1, 0, 0, -0.5372, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] 90 | 91 | Stereo.matRightK: !!opencv-matrix 92 | rows: 3 93 | cols: 3 94 | dt: f 95 | data: [ 718.856, 0, 607.1928, 0, 718.856, 185.2157, 0, 0, 1] 96 | # Stereo.matRightDistCoef: k1 k2 p1 p2 (Opencv) 97 | Stereo.matRightDistCoef: !!opencv-matrix 98 | rows: 4 99 | cols: 1 100 | dt: f 101 | data: [ 0, 0, 0, 0] 102 | 103 | # Camera frames per second 104 | Camera.fps: 10.0 105 | 106 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 107 | Camera.RGB: 1 108 | 109 | #-------------------------------------------------------------------------------------------- 110 | ### Changing the parameters below could seriously degradate the performance of the system 111 | 112 | # ORB Extractor: Number of features per image 113 | ORBextractor.nFeatures: 600 114 | 115 | # ORB Extractor: Scale factor between levels in the scale pyramid 116 | ORBextractor.scaleFactor: 1.2 117 | 118 | # ORB Extractor: Number of levels in the scale pyramid 119 | ORBextractor.nLevels: 1 120 | 121 | # ORB Extractor: Fast threshold (lower less restrictive) 122 | ORBextractor.fastTh: 20 123 | 124 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 125 | ORBextractor.nScoreType: 1 126 | 127 | # the following parameters determines necessary conditions to create a new keyframe 128 | Tracking.tracked_feature_ratio: 0.6 #if the current frame tracks less than this ratio of features in the reference keyframe 129 | Tracking.min_tracked_features: 120 #if the current frame tracks less than this number of features in the reference keyframe 130 | -------------------------------------------------------------------------------- /data/settingfiles_stereo_imu/tsukuba_imu.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # the world frame is the left camera frame (right, down, forward) at startIndex 3 | 4 | startIndex: 1 5 | finishIndex: 1800 6 | dataset: "Tsukuba" # choose one of KITTISeq00, Tsukuba, MalagaUrbanExtract6 7 | use_imu_data: true 8 | 9 | input_path: /media/jhuai/Mag/NewTsukubaStereoDataset/illumination/daylight 10 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 11 | output_file: /media/jhuai/Mag/temp/tsukuba_imu.txt 12 | output_point_file: /media/jhuai/Mag/temp/tsukuba_imu_points.txt 13 | imu_file: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/tsukuba_imu_samplesnoisy.txt" #samples added noise generated according to Microstrain 3DM-GX3-35 14 | 15 | trace_dir: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/result" # where to put the slam profiled time 16 | 17 | sample_interval: 0.01 18 | na: !!opencv-matrix 19 | rows: 3 20 | cols: 1 21 | dt: d 22 | data: [80e-5, 80e-5, 80e-5 ] # 80 ug with 1 Hz 23 | # q_na=na^2 24 | nw: !!opencv-matrix 25 | rows: 3 26 | cols: 1 27 | dt: d 28 | data: [0.03, 0.03, 0.03] # 0.03 deg/sec/sqrt(Hz) 29 | #q_nw=(nw*pi/180)^2 30 | acc_bias_Tc: 1800 #sec 31 | acc_bias_var: !!opencv-matrix 32 | rows: 3 33 | cols: 1 34 | dt: d 35 | data: [.04e-2, .04e-2, .04e-2]#bias drift variability, 0.04 mg 36 | #q_n_ba=(.04e-2)^2*(2/acc_bias_Tc); 37 | gyro_bias_Tc: 1800 #sec 38 | gyro_bias_var: !!opencv-matrix 39 | rows: 3 40 | cols: 1 41 | dt: d 42 | data: [5e-3, 5e-3, 5e-3]#bias drift variability, 18 deg/hour 43 | #q_n_bw=(18*(pi/180.0)/3600)^2*(2/gyro_bias_Tc); # 18deg/hr 44 | # rotation from IMU frame (forward, right, down) to camera frame(right, down, forward), 45 | Rs2c: !!opencv-matrix 46 | rows: 3 47 | cols: 3 48 | dt: d 49 | data: [ 0, 1, 0, 0, 0, 1, 1, 0, 0 ] 50 | tsinc: !!opencv-matrix 51 | rows: 3 52 | cols: 1 53 | dt: d 54 | data: [ 0.05, -0.06, 0.02 ] # N.B. camera_track.txt of Tsukuba used a central camera frame, in our program, we use left camera frame 55 | 56 | gw: !!opencv-matrix # gravity in the local world frame 57 | rows: 3 58 | cols: 1 59 | dt: f 60 | data: [ -0.000295986886093669, 9.80080743561639, -1.24212161392023e-05] 61 | wiew: !!opencv-matrix # earth rotation in the local world frame 62 | rows: 3 63 | cols: 1 64 | dt: d 65 | data: [ 0, -4.68759582209682e-05 , 5.58582013511762e-05] 66 | 67 | #vs0inw: velocity of IMU frame in world frame at startIndex 68 | vs0inw: !!opencv-matrix 69 | rows: 3 70 | cols: 1 71 | dt: d 72 | data: [ 5.66554e-05, -4.80246e-05, 0.000643521] 73 | 74 | 75 | # Camera calibration parameters (OpenCV) 76 | Camera.width: 640 77 | Camera.height: 480 78 | Camera.fx: 615 79 | Camera.fy: 615 80 | Camera.cx: 320 81 | Camera.cy: 240 82 | 83 | # Camera distortion paremeters (OpenCV) -- 84 | Camera.k1: 0 85 | Camera.k2: 0 86 | Camera.p1: 0.0 87 | Camera.p2: 0.0 88 | 89 | Stereo.se3Left2Right: !!opencv-matrix 90 | rows: 4 91 | cols: 4 92 | dt: f 93 | data: [ 1, 0, 0, -0.1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] 94 | 95 | Stereo.matRightK: !!opencv-matrix 96 | rows: 3 97 | cols: 3 98 | dt: f 99 | data: [ 615, 0, 320, 0, 615, 240, 0, 0, 1] 100 | # Stereo.matRightDistCoef: k1 k2 p1 p2 (Opencv) 101 | Stereo.matRightDistCoef: !!opencv-matrix 102 | rows: 4 103 | cols: 1 104 | dt: f 105 | data: [ 0, 0, 0, 0] 106 | 107 | # Camera frames per second 108 | Camera.fps: 30.0 109 | 110 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 111 | Camera.RGB: 1 112 | 113 | #-------------------------------------------------------------------------------------------- 114 | ### Changing the parameters below could seriously degradate the performance of the system 115 | 116 | # ORB Extractor: Number of features per image 117 | ORBextractor.nFeatures: 500 118 | 119 | # ORB Extractor: Scale factor between levels in the scale pyramid 120 | ORBextractor.scaleFactor: 1.2 121 | 122 | # ORB Extractor: Number of levels in the scale pyramid 123 | ORBextractor.nLevels: 1 124 | 125 | # ORB Extractor: Fast threshold (lower less restrictive) 126 | ORBextractor.fastTh: 20 127 | 128 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 129 | ORBextractor.nScoreType: 1 130 | -------------------------------------------------------------------------------- /data/setttingfiles_mono/kittiseq00_mono.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | startIndex: 0 3 | finishIndex: 4540 4 | dataset: "KITTIOdoSeq" # choose one of KITTISeq00, Tsukuba, MalagaUrbanExtract6 5 | use_imu_data:false 6 | 7 | input_path: /media/jhuai/Mag/kitti/dataset/sequences/00 8 | time_file: /media/jhuai/Mag/kitti/dataset/sequences/00/times.txt 9 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 10 | output_file: /media/jhuai/Mag/temp/kittiseq00.txt 11 | output_point_file: /media/jhuai/Mag/temp/kittiseq00_points.txt 12 | 13 | trace_dir: "/home/jhuai/catkin_ws/src/orbslam_dwo/data/result" # where to put the slam profiled time 14 | 15 | # Camera calibration parameters (OpenCV) 16 | Camera.width: 1241 17 | Camera.height: 376 18 | Camera.fx: 718.856 19 | Camera.fy: 718.856 20 | Camera.cx: 607.1928 21 | Camera.cy: 185.2157 22 | 23 | # Camera distortion paremeters (OpenCV) -- 24 | Camera.k1: 0 25 | Camera.k2: 0 26 | Camera.p1: 0.0 27 | Camera.p2: 0.0 28 | 29 | # Camera frames per second 30 | Camera.fps: 10.0 31 | 32 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 33 | Camera.RGB: 1 34 | 35 | #-------------------------------------------------------------------------------------------- 36 | ### Changing the parameters below could seriously degradate the performance of the system 37 | 38 | # ORB Extractor: Number of features per image 39 | ORBextractor.nFeatures: 2000 40 | 41 | # ORB Extractor: Scale factor between levels in the scale pyramid 42 | ORBextractor.scaleFactor: 1.2 43 | 44 | # ORB Extractor: Number of levels in the scale pyramid 45 | ORBextractor.nLevels: 8 46 | 47 | # ORB Extractor: Fast threshold (lower less restrictive) 48 | ORBextractor.fastTh: 20 49 | 50 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 51 | ORBextractor.nScoreType: 1 52 | -------------------------------------------------------------------------------- /data/setttingfiles_mono/tsukuba_mono.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | startIndex: 1 3 | finishIndex: 1800 4 | dataset: "Tsukuba" # choose one of KITTISeq00, Tsukuba, MalagaUrbanExtract6 5 | use_imu_data:false 6 | 7 | input_path: /media/jhuai/Mag/NewTsukubaStereoDataset/illumination/daylight 8 | voc_file_path: /home/jhuai/catkin_ws/src/mono_csfm/ORB_SLAM2/Vocabulary/ORBvoc.txt 9 | output_file: /media/jhuai/Mag/temp/tsukuba_mono.txt 10 | output_point_file: /media/jhuai/Mag/temp/tsukuba_mono_points.txt 11 | trace_dir: /home/jhuai/catkin_ws/src/orbslam_dwo/data/result 12 | 13 | sample_interval: 0.01 14 | na: !!opencv-matrix 15 | rows: 3 16 | cols: 1 17 | dt: d 18 | data: [80e-5, 80e-5, 80e-5 ] # 80 ug with 1 Hz 19 | # q_na=na^2 20 | nw: !!opencv-matrix 21 | rows: 3 22 | cols: 1 23 | dt: d 24 | data: [0.03, 0.03, 0.03] # 0.03 deg/sec/sqrt(Hz) 25 | #q_nw=(nw*pi/180)^2 26 | acc_bias_Tc: 1800 #sec 27 | acc_bias_var: !!opencv-matrix 28 | rows: 3 29 | cols: 1 30 | dt: d 31 | data: [.04e-2, .04e-2, .04e-2]#bias drift variability, 0.04 mg 32 | #q_n_ba=(.04e-2)^2*(2/acc_bias_Tc); 33 | gyro_bias_Tc: 1800 #sec 34 | gyro_bias_var: !!opencv-matrix 35 | rows: 3 36 | cols: 1 37 | dt: d 38 | data: [5e-3, 5e-3, 5e-3]#bias drift variability, 18 deg/hour 39 | #q_n_bw=(18*(pi/180.0)/3600)^2*(2/gyro_bias_Tc); # 18deg/hr 40 | # rotation from IMU frame (forward, right, down) to camera frame(right, down, forward), 41 | Rs2c: !!opencv-matrix 42 | rows: 3 43 | cols: 3 44 | dt: d 45 | data: [ 0, 1, 0, 0, 0, 1, 1, 0, 0 ] 46 | tsinc: !!opencv-matrix 47 | rows: 3 48 | cols: 1 49 | dt: d 50 | data: [ 0.05, -0.06, 0.02 ] # N.B. camera_track.txt of Tsukuba used a central camera frame, in our program, we use left camera frame 51 | 52 | gw: !!opencv-matrix # gravity in the local world frame 53 | rows: 3 54 | cols: 1 55 | dt: f 56 | data: [ -0.000295986886093669, 9.80080743561639, -1.24212161392023e-05] 57 | wiew: !!opencv-matrix # earth rotation in the local world frame 58 | rows: 3 59 | cols: 1 60 | dt: d 61 | data: [ 0, -4.68759582209682e-05 , 5.58582013511762e-05] 62 | 63 | #vs0inw: velocity of IMU frame in world frame at startIndex 64 | vs0inw: !!opencv-matrix 65 | rows: 3 66 | cols: 1 67 | dt: d 68 | data: [ 5.66554e-05, -4.80246e-05, 0.000643521] 69 | 70 | 71 | # Camera calibration parameters (OpenCV) 72 | Camera.width: 640 73 | Camera.height: 480 74 | Camera.fx: 615 75 | Camera.fy: 615 76 | Camera.cx: 320 77 | Camera.cy: 240 78 | 79 | # Camera distortion paremeters (OpenCV) -- 80 | Camera.k1: 0 81 | Camera.k2: 0 82 | Camera.p1: 0.0 83 | Camera.p2: 0.0 84 | Stereo.baseline: 0.1 # baseline in meters 85 | 86 | # Camera frames per second 87 | Camera.fps: 30.0 88 | 89 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 90 | Camera.RGB: 1 91 | 92 | #-------------------------------------------------------------------------------------------- 93 | ### Changing the parameters below could seriously degradate the performance of the system 94 | 95 | # ORB Extractor: Number of features per image 96 | ORBextractor.nFeatures: 1000 97 | 98 | # ORB Extractor: Scale factor between levels in the scale pyramid 99 | ORBextractor.scaleFactor: 1.2 100 | 101 | # ORB Extractor: Number of levels in the scale pyramid 102 | ORBextractor.nLevels: 8 103 | 104 | # ORB Extractor: Fast threshold (lower less restrictive) 105 | ORBextractor.fastTh: 20 106 | 107 | # ORB Extractor: Score to sort features. 0 -> Harris Score, 1 -> FAST Score 108 | ORBextractor.nScoreType: 1 109 | -------------------------------------------------------------------------------- /g2o_types/GPSGrabber.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef GPSGRABBER_H 3 | #define GPSGRABBER_H 4 | 5 | #include "timegrabber.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | enum GPSSolutionType{TOW_ECEF=0, TOW_NED, UTC_ECEF, UTC_NED}; 13 | class GPSGrabber:DataGrabber{ 14 | public: 15 | // GPS TOW 16 | class RtklibPosPattern{ 17 | virtual std::ostream& print(std::ostream &) const; 18 | virtual std::istream& read(std::istream &); 19 | public: 20 | friend std::ostream& operator << (std::ostream &os, const RtklibPosPattern & rhs) 21 | { 22 | return rhs.print(os); 23 | } 24 | friend std::istream & operator>>(std::istream &is, RtklibPosPattern &rhs) 25 | { 26 | return rhs.read(is); 27 | } 28 | virtual ~RtklibPosPattern(){} 29 | public: 30 | int GPS_Week; 31 | double GPS_TOW; 32 | double xyz_ecef[3]; 33 | int Q; 34 | int ns; 35 | double sdxyz[3]; 36 | double sdxy_yz_zx[3];//square root of the absolute values of xy, yz, zx components of the covariance matrix, 37 | // their signs represents the sign of these components 38 | double age; 39 | double ratio; 40 | }; 41 | //civil time, UTC(GMT) 42 | class RtklibPosPatternUTCECEF: public RtklibPosPattern{ 43 | public: 44 | int ymd[3]; //year, month, date 45 | int hour; 46 | int minute; 47 | double second; 48 | 49 | std::ostream& print(std::ostream & os) const; 50 | std::istream& read(std::istream & is); 51 | }; 52 | //GTOW, lat, lon, h, sdn,e,u 53 | class RtklibPosPatternTOWNED: public RtklibPosPattern{ 54 | public: 55 | double llh_geo[3];//lat deg, lon deg, height 56 | double sdneu[3]; //meters 57 | double sdne_eu_un[3]; 58 | 59 | std::ostream& print(std::ostream & os) const; 60 | std::istream& read(std::istream & is); 61 | }; 62 | GPSGrabber(const std::string file, double interval=1e-5, GPSSolutionType patType= GPSSolutionType::TOW_ECEF); 63 | virtual ~GPSGrabber(); 64 | 65 | bool getObservation(double tk); 66 | bool getNextObservation(); 67 | Eigen::Matrix transMat; //temporary sensor reading which also holds data at t(p(k)-1); 68 | Eigen::Matrix measurement; //GPS TOW, XYZ ECEF, Q, number of satellites, std XYZ in ECEF 69 | RtklibPosPattern * pat_; 70 | }; 71 | void loadGPSData(std::string gps_file, std::vector > &gpsdata, 72 | double startGPSTime, double finishGPSTime); 73 | void testReadTOWECEF(); 74 | int gpstk_example1(); 75 | 76 | #endif // GPSGRABBER_H 77 | -------------------------------------------------------------------------------- /g2o_types/IMUErrorModel.cpp: -------------------------------------------------------------------------------- 1 | #include "IMUErrorModel.h" 2 | 3 | template 4 | IMUErrorModel::IMUErrorModel(const Eigen::Matrix b_ag): b_a(b_ag.template head<3>()), 5 | b_g(b_ag.template tail<3>()), 6 | S_a(Eigen::Matrix::Zero()),S_g(Eigen::Matrix::Zero()), T_s(Eigen::Matrix::Zero()), 7 | invT_a(Eigen::Matrix::Identity()),invT_g(Eigen::Matrix::Identity()) 8 | { 9 | } 10 | /*template 11 | IMUErrorModel::IMUErrorModel(const Eigen::Matrix bundle): b_a(bundle.block<3,1>(0,0)), b_g(bundle.block<3,1>(3, 0)) 12 | { 13 | for (int i=0; i<3; ++i){ 14 | for (int j=0; j<3; ++j){ 15 | S_a(i,j)=bundle(i*3+j+6); 16 | S_g(i,j)= bundle(i*3+j+15); 17 | T_s(i,j)=bundle(i*3+j+24); 18 | } 19 | } 20 | invT_a=(S_a+ Eigen::Matrix::Identity()).inverse(); 21 | invT_g=(S_g+ Eigen::Matrix::Identity()).inverse(); 22 | }*/ 23 | template 24 | IMUErrorModel::IMUErrorModel(const Eigen::Matrix b_ag, const Eigen::Matrix shapeMatrices): 25 | b_a(b_ag.template head<3>())/*block<3,1>(0,0))*/, b_g(b_ag.template tail<3>())//block<3,1>(3, 0)) 26 | { 27 | for (int i=0; i<3; ++i){ 28 | for (int j=0; j<3; ++j){ 29 | S_a(i,j)=shapeMatrices(i*3+j); 30 | S_g(i,j)= shapeMatrices(i*3+j+9); 31 | T_s(i,j)=shapeMatrices(i*3+j+18); 32 | } 33 | } 34 | invT_a=(S_a+ Eigen::Matrix::Identity()).inverse(); 35 | invT_g=(S_g+ Eigen::Matrix::Identity()).inverse(); 36 | } 37 | template 38 | void IMUErrorModel::estimate(const Eigen::Matrix a_m, const Eigen::Matrix w_m) 39 | { 40 | a_est= invT_a*(a_m- b_a); 41 | w_est= invT_g*(w_m- b_g - T_s* a_est); 42 | } 43 | template 44 | void IMUErrorModel::predict(const Eigen::Matrix a_s, const Eigen::Matrix w_s) 45 | { 46 | a_obs = S_a*a_s +a_s+ b_a; 47 | w_obs = S_g*w_s + w_s + T_s*a_s + b_g; 48 | } 49 | -------------------------------------------------------------------------------- /g2o_types/IMUErrorModel.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_ERROR_MODEL_H_ 2 | #define IMU_ERROR_MODEL_H_ 3 | #include 4 | //accelerometer and gyro error model by drawing inspirations from Mingyang Li 5 | // ICRA 2014, Titterton and Weston 12.5.2, and Tedaldi ICRA 2014 A robust and 6 | // easy to implement method. Here we follow exactly the model used in 7 | // Mingyang Li ICRA 2014 and Shelley 2014 master thesis 8 | template 9 | class IMUErrorModel 10 | { 11 | public: 12 | //a volatile class to estimate linear acceleration and angular rate given measurements, or predict measurements given estimated values 13 | Eigen::Matrix b_a; 14 | Eigen::Matrix b_g; 15 | Eigen::Matrix S_a; // S_a + I_3 = T_a 16 | Eigen::Matrix S_g; // S_g + I_3 = T_g 17 | Eigen::Matrix T_s; // T_s 18 | Eigen::Matrix invT_a; //inverse of T_a 19 | Eigen::Matrix invT_g; //inverse of T_g 20 | 21 | //intermediate variables 22 | Eigen::Matrix a_est; 23 | Eigen::Matrix w_est; 24 | Eigen::Matrix a_obs; 25 | Eigen::Matrix w_obs; 26 | 27 | //IMUErrorModel(): bInitialized(false){} 28 | IMUErrorModel(const Eigen::Matrix b_ag); 29 | //IMUErrorModel(const Eigen::Matrix bundle); 30 | IMUErrorModel(const Eigen::Matrix b_ag, const Eigen::Matrix shapeMatrices); 31 | void estimate(const Eigen::Matrix a_m, const Eigen::Matrix w_m); 32 | void predict(const Eigen::Matrix a_s, const Eigen::Matrix w_s); 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /g2o_types/PointStatistics.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef POINT_STATISTICS_H 3 | #define POINT_STATISTICS_H 4 | #include 5 | #include 6 | struct PointStatistics 7 | { 8 | PointStatistics(int USE_N_LEVELS_FOR_MATCHING=0) 9 | : num_matched_points(USE_N_LEVELS_FOR_MATCHING) 10 | { 11 | num_points_grid2x2.setZero(); 12 | num_points_grid3x3.setZero(); 13 | 14 | for (int l=0; l num_matched_points; //number of matched points at each pyramid level 64 | Eigen::Matrix2i num_points_grid2x2; 65 | Eigen::Matrix3i num_points_grid3x3;//number of points in each cell of the 3x3 grid at level 0 66 | }; 67 | #endif 68 | -------------------------------------------------------------------------------- /g2o_types/eigen_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "eigen_utils.h" 5 | #include "stopwatch.h" // only for tests 6 | #include "rand_sampler.h" // only for tests 7 | 8 | using namespace Eigen; 9 | using namespace std; 10 | Eigen::Vector3d rotro2eu(Eigen::Matrix3d R) 11 | { 12 | Eigen::Vector3d euler; 13 | euler[0] = atan2(R(2,1), R(2,2)); 14 | euler[1] = -(atan2(R(2,0), sqrt(1 - R(2,0) * R(2,0)))); 15 | euler[2] = atan2(R(1,0), R(0,0)); 16 | return euler; 17 | } 18 | 19 | //eul defined in "n": rotate "n" to obtain "b" 20 | //result: Cbn (from b to n) 21 | Eigen::Matrix3d roteu2ro(Eigen::Vector3d eul) 22 | { 23 | double cr = cos(eul[0]); double sr = sin(eul[0]); //roll 24 | double cp = cos(eul[1]); double sp = sin(eul[1]); //pitch 25 | double ch = cos(eul[2]); double sh = sin(eul[2]); //heading 26 | Eigen::Matrix3d dcm; 27 | dcm(0,0) = cp * ch; 28 | dcm(0,1) = (sp * sr * ch) - (cr * sh); 29 | dcm(0,2) = (cr * sp * ch) + (sh * sr); 30 | 31 | dcm(1,0) = cp * sh; 32 | dcm(1,1) = (sr * sp * sh) + (cr * ch); 33 | dcm(1,2) = (cr * sp * sh) - (sr * ch); 34 | 35 | dcm(2,0) = -sp; 36 | dcm(2,1) = sr * cp; 37 | dcm(2,2) = cr * cp; 38 | return dcm; 39 | } 40 | //input: lat, long in radians, height is immaterial 41 | //output: Ce2n 42 | Eigen::Matrix3d llh2dcm(const Eigen::Vector3d llh) 43 | { 44 | double sL = sin(llh[0]); 45 | double cL = cos(llh[0]); 46 | double sl = sin(llh[1]); 47 | double cl = cos(llh[1]); 48 | 49 | Eigen::Matrix3d Ce2n; 50 | Ce2n<< -sL * cl, -sL * sl, cL , -sl, cl, 0 , -cL * cl, -cL * sl, -sL; 51 | return Ce2n; 52 | } 53 | 54 | 55 | Eigen::Matrix nullspace(Eigen::Matrix A) 56 | { 57 | HouseholderQR qr(A); 58 | // ColPivHouseholderQR qr(A); //don't use column pivoting because in that case Q*R-A!=0 59 | // MatrixXd R = qr.matrixQR().triangularView(); 60 | MatrixXd nullQ = qr.householderQ(); 61 | // cout <<"Q*R-A"<cols); 64 | nullQ= nullQ.block(0,cols,rows,rows-cols).eval(); 65 | // std::cout << "The nullspace of A is:"<. 17 | 18 | #ifndef SCAVISLAM_MATHSUTILS_H 19 | #define SCAVISLAM_MATHSUTILS_H 20 | 21 | #define GL_GLEXT_PROTOTYPES 1 22 | 23 | #include 24 | #include 25 | 26 | namespace ScaViSLAM 27 | { 28 | 29 | Eigen::Vector2d 30 | project2d (const Eigen::Vector3d&); 31 | 32 | Eigen::Vector3d 33 | project3d (const Eigen::Vector4d&); 34 | 35 | Eigen::Vector3d 36 | unproject2d (const Eigen::Vector2d&); 37 | 38 | Eigen::Vector4d 39 | unproject3d (const Eigen::Vector3d&); 40 | 41 | double 42 | norm_max (const Eigen::VectorXd & v); 43 | 44 | inline Eigen::Vector3d invert_depth(const Eigen::Vector3d & x) 45 | { 46 | return unproject2d(x.head<2>())/x[2]; 47 | } 48 | 49 | template 50 | inline T Po2(const T& value) 51 | { 52 | return value * value; 53 | } 54 | 55 | template 56 | inline T Po3(const T& value) 57 | { 58 | return Po2(value) * value; 59 | } 60 | 61 | template 62 | T Po4(const T& value) 63 | { 64 | return Po2(Po2(value)); 65 | } 66 | 67 | template 68 | T Po5(const T& value) 69 | { 70 | return Po4(value) * value; 71 | } 72 | 73 | template 74 | T Po6(const T& value) 75 | { 76 | return Po4(value) * Po2(value); 77 | } 78 | 79 | template 80 | T Po7(const T& value) 81 | { 82 | return Po6(value) * value; 83 | } 84 | 85 | template 86 | T Po8(const T& value) 87 | { 88 | return Po2(Po4(value)); 89 | } 90 | 91 | template 92 | T median(const std::multiset & m_set) 93 | { 94 | int size = m_set.size(); 95 | assert(size>0); 96 | typename std::multiset::const_iterator it = m_set.begin(); 97 | if (m_set.size()%2==1) 98 | { 99 | for (int i=0; i 3 | #include 4 | 5 | namespace ScaViSLAM{ 6 | 7 | double uniform_rand(double lowerBndr, double upperBndr){ 8 | return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr); 9 | } 10 | 11 | double gauss_rand(double mean, double sigma){ 12 | double x, y, r2; 13 | do { 14 | x = -1.0 + 2.0 * uniform_rand(0.0, 1.0); 15 | y = -1.0 + 2.0 * uniform_rand(0.0, 1.0); 16 | r2 = x * x + y * y; 17 | } while (r2 > 1.0 || r2 == 0.0); 18 | return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2); 19 | } 20 | 21 | int Sample::uniform(int from, int to){ 22 | return static_cast(uniform_rand(from, to)); 23 | } 24 | 25 | double Sample::uniform(){ 26 | return uniform_rand(0., 1.); 27 | } 28 | 29 | double Sample::gaussian(double sigma){ 30 | return gauss_rand(0., sigma); 31 | } 32 | 33 | } 34 | -------------------------------------------------------------------------------- /g2o_types/rand_sampler.h: -------------------------------------------------------------------------------- 1 | #ifndef RAND_SAMPLER_H_ 2 | #define RAND_SAMPLER_H_ 3 | 4 | namespace ScaViSLAM{ 5 | // method to generate samples copied from g2o example ba_demo.cpp 6 | class Sample { 7 | public: 8 | static int uniform(int from, int to); 9 | static double uniform(); 10 | static double gaussian(double sigma); 11 | }; 12 | 13 | double uniform_rand(double lowerBndr, double upperBndr); 14 | 15 | double gauss_rand(double mean, double sigma); 16 | } 17 | #endif 18 | -------------------------------------------------------------------------------- /g2o_types/scale_solver.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALE_SOLVER_H_ 2 | #define SCALE_SOLVER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace ScaViSLAM{ 12 | 13 | class G2oVertexScale : public g2o::BaseVertex<1, double> 14 | { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | G2oVertexScale ():first_estimate(NULL){} 19 | ~G2oVertexScale (){ 20 | if(first_estimate) 21 | delete first_estimate; 22 | first_estimate=NULL; 23 | } 24 | virtual bool 25 | read (std::istream& is); 26 | 27 | virtual bool 28 | write (std::ostream& os) const; 29 | 30 | virtual void 31 | oplusImpl (const double * update); 32 | 33 | virtual void 34 | setToOriginImpl () {_estimate= 0.0;} 35 | void setFirstEstimate(const double& fe){ 36 | first_estimate=new double(fe); 37 | } 38 | //for first estimate Jacobians 39 | double * first_estimate; 40 | }; 41 | class G2oEdgeScale 42 | : public g2o::BaseBinaryEdge<1, double, G2oVertexScale, G2oVertexScale> 43 | { 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | 47 | G2oEdgeScale () {} 48 | 49 | virtual bool 50 | read (std::istream& is); 51 | virtual bool 52 | write (std::ostream& os) const; 53 | void 54 | computeError (); 55 | virtual void 56 | linearizeOplus (); 57 | }; 58 | 59 | //scale + translation 60 | class G2oVertexScaleTrans : public g2o::BaseVertex<4, Eigen::Matrix > 61 | { 62 | public: 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | 65 | G2oVertexScaleTrans (Sophus::SO3d _Rw2i= Sophus::SO3d()):first_estimate(NULL), Rw2i(_Rw2i){} 66 | ~G2oVertexScaleTrans (){ 67 | if(first_estimate) 68 | delete first_estimate; 69 | first_estimate=NULL; 70 | } 71 | virtual bool 72 | read (std::istream& is); 73 | 74 | virtual bool 75 | write (std::ostream& os) const; 76 | 77 | virtual void 78 | oplusImpl (const double * update); 79 | 80 | virtual void 81 | setToOriginImpl () {_estimate.setZero();} 82 | void setFirstEstimate(const Eigen::Matrix& fe){ 83 | first_estimate=new Eigen::Matrix(fe); 84 | } 85 | 86 | //for first estimate Jacobians 87 | Eigen::Matrix * first_estimate; 88 | Sophus::SO3d Rw2i; 89 | }; 90 | class G2oEdgeScaleTrans 91 | : public g2o::BaseBinaryEdge<4, Eigen::Matrix, G2oVertexScaleTrans, G2oVertexScaleTrans> 92 | { 93 | public: 94 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 95 | 96 | G2oEdgeScaleTrans () {} 97 | 98 | virtual bool 99 | read (std::istream& is); 100 | virtual bool 101 | write (std::ostream& os) const; 102 | void 103 | computeError (); 104 | virtual void 105 | linearizeOplus (); 106 | }; 107 | class TestScaleTransOptimizer{ 108 | public: 109 | const int num_poses; 110 | static const double scale; 111 | std::vector sw2i; 112 | std::vector sw2i_est; 113 | 114 | std::vector twini; 115 | std::vector twini_est; 116 | 117 | TestScaleTransOptimizer(const int _num_poses=50): num_poses(_num_poses), 118 | sw2i(num_poses), sw2i_est(num_poses), 119 | twini(num_poses),twini_est(num_poses) 120 | {} 121 | void optimizeScale(); 122 | void optimizeScaleTrans(); 123 | }; 124 | } 125 | #endif 126 | -------------------------------------------------------------------------------- /g2o_types/stopwatch.h: -------------------------------------------------------------------------------- 1 | // This file is part of VisionTools. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to 7 | // deal in the Software without restriction, including without limitation the 8 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | // sell copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in 13 | // all copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | // IN THE SOFTWARE. 22 | 23 | #ifndef VISIONTOOLS_STOP_WATCH_H 24 | #define VISIONTOOLS_STOP_WATCH_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace VisionTools 35 | { 36 | 37 | class StopWatch 38 | { 39 | public: 40 | StopWatch() 41 | { 42 | running_ = false; 43 | time_ = 0; 44 | } 45 | 46 | void start() 47 | { 48 | assert(running_==false); 49 | gettimeofday(&start_time_, NULL); 50 | running_ = true; 51 | } 52 | 53 | void stop() 54 | { 55 | assert(running_); 56 | gettimeofday(&end_time_, NULL); 57 | long seconds = end_time_.tv_sec - start_time_.tv_sec; 58 | long useconds = end_time_.tv_usec - start_time_.tv_usec; 59 | time_ = ((seconds) + useconds*0.000001); 60 | running_ = false; 61 | } 62 | 63 | double read_current_time() 64 | { 65 | assert(running_); 66 | timeval cur_time; 67 | gettimeofday(&cur_time, NULL); 68 | long seconds = cur_time.tv_sec - start_time_.tv_sec; 69 | long useconds = cur_time.tv_usec - start_time_.tv_usec; 70 | return ((seconds) + useconds*0.000001); 71 | } 72 | 73 | double get_stopped_time() 74 | { 75 | assert(running_==false); 76 | return time_; 77 | } 78 | 79 | inline void reset() 80 | { 81 | time_ = 0; 82 | } 83 | 84 | private: 85 | timeval start_time_; 86 | timeval end_time_; 87 | double time_; 88 | bool running_; 89 | }; 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /include/Converter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef CONVERTER_H 22 | #define CONVERTER_H 23 | 24 | #include 25 | 26 | #include 27 | //#include"Thirdparty/g2o/g2o/types/sba/types_six_dof_expmap.h" 28 | //#include"Thirdparty/g2o/g2o/types/sim3/types_seven_dof_expmap.h" 29 | #include 30 | #include 31 | #include "sophus/se3.hpp" 32 | #include "sophus/sim3.hpp" 33 | #include "viso2/matrix.h" 34 | 35 | namespace ORB_SLAM 36 | { 37 | 38 | class Converter 39 | { 40 | public: 41 | static std::vector toDescriptorVector(const cv::Mat &Descriptors); 42 | 43 | 44 | static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT); 45 | static Sophus::Sim3d toSim3d(const Sophus::SE3d &se3); 46 | static Sophus::Sim3d toSim3d(const g2o::Sim3 &se3q); 47 | static Eigen::Vector4d toScaleTrans(const g2o::Sim3 & se3q); 48 | 49 | static Sophus::SE3d toSE3d(const Sophus::Sim3d &sim3); 50 | static Sophus::SE3d toSE3d(const cv::Mat &cvT); 51 | static g2o::SE3Quat toSE3Quat(const Sophus::SE3d &se3d); 52 | static Sophus::SE3d toSE3d(const g2o::SE3Quat &se3q); 53 | 54 | static cv::Mat toCvMat(const g2o::SE3Quat &SE3); 55 | static cv::Mat toCvMat(const g2o::Sim3 &Sim3); 56 | static cv::Mat toCvMat(const Eigen::Matrix &m); 57 | 58 | static Sophus::SE3d toSE3d(const libviso2::Matrix& Tr); 59 | static libviso2::Matrix toViso2Matrix(const Sophus::SE3d & Tr); 60 | 61 | static cv::Mat toCvMat(const Eigen::Matrix3d &m); 62 | static cv::Mat toCvMat(const Eigen::Matrix &m); 63 | static cv::Mat toCvSE3(const Eigen::Matrix &R, const Eigen::Matrix &t); 64 | static cv::Mat toCvSE3(const Sophus::SE3d &T ); 65 | static Eigen::Matrix toVector3d(const cv::Mat &cvVector); 66 | static Eigen::Matrix toVector3d(const cv::Point3f &cvPoint); 67 | static Eigen::Matrix toVector2d(const cv::Mat &cvVector); 68 | static Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 69 | 70 | static std::vector toQuaternion(const cv::Mat &M); 71 | 72 | static cv::Mat SkewSymmetricMatrix(const cv::Mat &v); 73 | }; 74 | 75 | }// namespace ORB_SLAM 76 | 77 | #endif // CONVERTER_H 78 | -------------------------------------------------------------------------------- /include/FeatureGrid.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef FEATUREGRID_H 3 | #define FEATUREGRID_H 4 | 5 | #include 6 | #include 7 | #include 8 | namespace ORB_SLAM 9 | { 10 | 11 | class FeatureGrid 12 | { 13 | public: 14 | FeatureGrid(const int cell_size, const int nFeaturesInImage, const int nMaxX, 15 | const int nMinX, const int nMaxY, const int nMinY); 16 | ~FeatureGrid(); 17 | void resetGrid(); 18 | 19 | inline bool IsPointEligible(const cv::KeyPoint& kpUn, int & posX, int & posY) 20 | { 21 | posX = round((kpUn.pt.x- mnMinX)*mfGridElementWidthInv); 22 | posY = round((kpUn.pt.y- mnMinY)*mfGridElementHeightInv); 23 | if(posX<0 || posX>=mnGridCols || posY<0 || posY>=mnGridRows || mvMPGrid[posX][posY].size()>= mnPointPerCell) 24 | return false; 25 | return true; 26 | } 27 | //check posX and posY are valid before calling this function 28 | inline void AddMapPoint(const int posX, const int posY, const int nIdInFrame) 29 | { 30 | mvMPGrid[posX][posY].push_back(nIdInFrame); 31 | } 32 | const int mnCellSize; 33 | const int mnMinX; 34 | const int mnMinY; 35 | const int mnGridCols; 36 | const int mnGridRows; 37 | const float mfGridElementWidthInv; 38 | const float mfGridElementHeightInv; 39 | const float mnPointPerCell; //max number of map points per cell 40 | std::vector< std::vector< std::vector > > mvMPGrid;// record observations of mappoints in the new keyframe 41 | }; 42 | 43 | } //namespace ORB_SLAM 44 | 45 | #endif 46 | 47 | -------------------------------------------------------------------------------- /include/FramePublisher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef FRAMEPUBLISHER_H 22 | #define FRAMEPUBLISHER_H 23 | 24 | #include "Tracking.h" 25 | #include "MapPoint.h" 26 | #include "Map.h" 27 | 28 | #ifdef SLAM_USE_ROS 29 | #include "ros/ros.h" 30 | #endif 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | 38 | namespace ORB_SLAM 39 | { 40 | 41 | class Tracking; 42 | 43 | class FramePublisher 44 | { 45 | public: 46 | FramePublisher(); 47 | 48 | void Update(Tracking *pTracker, const cv::Mat & im); 49 | 50 | void Refresh(); 51 | 52 | void SetMap(Map* pMap); 53 | 54 | protected: 55 | 56 | cv::Mat DrawFrame(); 57 | 58 | void PublishFrame(); 59 | 60 | void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText); 61 | 62 | cv::Mat mIm; 63 | vector mvCurrentKeys; 64 | 65 | vector mvbOutliers; 66 | 67 | vector mvpMatchedMapPoints; 68 | int mnTracked; 69 | vector mvIniKeys; 70 | vector mvIniMatches; 71 | #ifdef SLAM_USE_ROS 72 | ros::NodeHandle mNH; 73 | ros::Publisher mImagePub; 74 | #endif 75 | int mState; 76 | 77 | bool mbUpdated; 78 | 79 | Map* mpMap; 80 | 81 | boost::mutex mMutex; 82 | }; 83 | 84 | } //namespace ORB_SLAM 85 | 86 | #endif // FRAMEPUBLISHER_H 87 | -------------------------------------------------------------------------------- /include/Initializer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef INITIALIZER_H 22 | #define INITIALIZER_H 23 | 24 | #include 25 | #include "Frame.h" 26 | 27 | 28 | namespace ORB_SLAM 29 | { 30 | 31 | class Initializer 32 | { 33 | typedef pair Match; 34 | 35 | public: 36 | 37 | // Fix the reference frame 38 | Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200); 39 | 40 | // Computes in parallel a fundamental matrix and a homography 41 | // Selects a model and tries to recover the motion and the structure from motion 42 | bool Initialize(const Frame &CurrentFrame, const vector &vMatches12, 43 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated); 44 | 45 | 46 | private: 47 | 48 | void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21); 49 | void FindFundamental(vector &vbInliers, float &score, cv::Mat &F21); 50 | 51 | cv::Mat ComputeH21(const vector &vP1, const vector &vP2); 52 | cv::Mat ComputeF21(const vector &vP1, const vector &vP2); 53 | 54 | float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector &vbMatchesInliers, float sigma); 55 | 56 | float CheckFundamental(const cv::Mat &F21, vector &vbMatchesInliers, float sigma); 57 | 58 | bool ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, 59 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 60 | 61 | bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, 62 | cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); 63 | 64 | void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); 65 | 66 | void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); 67 | 68 | int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector &vKeys1, const vector &vKeys2, 69 | const vector &vMatches12, vector &vbInliers, 70 | const cv::Mat &K, vector &vP3D, float th2, vector &vbGood, float ¶llax); 71 | 72 | void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t); 73 | 74 | 75 | // Keypoints from Reference Frame (Frame 1) 76 | vector mvKeys1; 77 | 78 | // Keypoints from Current Frame (Frame 2) 79 | vector mvKeys2; 80 | 81 | // Current Matches from Reference to Current 82 | vector mvMatches12; 83 | vector mvbMatched1; 84 | 85 | // Calibration 86 | cv::Mat mK; 87 | 88 | // Standard Deviation and Variance 89 | float mSigma, mSigma2; 90 | 91 | // Ransac max iterations 92 | int mMaxIterations; 93 | 94 | // Ransac sets 95 | vector > mvSets; 96 | 97 | }; 98 | 99 | } //namespace ORB_SLAM 100 | 101 | #endif // INITIALIZER_H 102 | -------------------------------------------------------------------------------- /include/KeyFrameDatabase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef KEYFRAMEDATABASE_H 22 | #define KEYFRAMEDATABASE_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "KeyFrame.h" 29 | #include "Frame.h" 30 | #include "ORBVocabulary.h" 31 | 32 | #include 33 | 34 | 35 | namespace ORB_SLAM 36 | { 37 | 38 | class Frame; 39 | class KeyFrame; 40 | 41 | class KeyFrameDatabase 42 | { 43 | public: 44 | 45 | KeyFrameDatabase(const ORBVocabulary &voc); 46 | 47 | void add(KeyFrame* pKF); 48 | 49 | void erase(KeyFrame* pKF); 50 | 51 | void clear(); 52 | 53 | // Loop Detection 54 | std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); 55 | 56 | // Relocalisation 57 | std::vector DetectRelocalisationCandidates(Frame* F); 58 | 59 | protected: 60 | 61 | // Associated vocabulary 62 | const ORBVocabulary* mpVoc; 63 | 64 | // Inverted file 65 | std::vector > mvInvertedFile; 66 | 67 | // Mutex 68 | boost::mutex mMutex; 69 | }; 70 | 71 | } //namespace ORB_SLAM 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /include/LocalMapping.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef LOCALMAPPING_H 22 | #define LOCALMAPPING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "Map.h" 26 | #include "LoopClosing.h" 27 | #include "Tracking.h" 28 | #include 29 | #include "KeyFrameDatabase.h" 30 | 31 | 32 | namespace ORB_SLAM 33 | { 34 | 35 | class Tracking; 36 | class LoopClosing; 37 | class Map; 38 | 39 | class LocalMapping 40 | { 41 | public: 42 | LocalMapping(Map* pMap); 43 | 44 | void SetLoopCloser(LoopClosing* pLoopCloser); 45 | 46 | void SetTracker(Tracking* pTracker); 47 | 48 | void Run(); 49 | 50 | void InsertKeyFrame(KeyFrame* pKF); 51 | 52 | // Thread Synch 53 | void RequestStop(); 54 | void RequestReset(); 55 | 56 | void Stop(); 57 | 58 | void Release(); 59 | 60 | bool isStopped(); 61 | 62 | bool stopRequested(); 63 | 64 | bool AcceptKeyFrames(); 65 | void SetAcceptKeyFrames(bool flag); 66 | 67 | void InterruptBA(); 68 | void ProcessNewKeyFrame(); 69 | protected: 70 | 71 | bool CheckNewKeyFrames(); 72 | 73 | void CreateNewMapPoints(); 74 | void CreateNewMapPointsStereo(); 75 | 76 | void MapPointCulling(); 77 | void SearchInNeighbors(); 78 | 79 | void KeyFrameCulling(); 80 | 81 | void ResetIfRequested(); 82 | bool mbResetRequested; 83 | boost::mutex mMutexReset; 84 | 85 | Map* mpMap; 86 | 87 | LoopClosing* mpLoopCloser; 88 | Tracking* mpTracker; 89 | 90 | std::list mlNewKeyFrames; 91 | 92 | KeyFrame* mpCurrentKeyFrame; 93 | 94 | std::list mlpRecentAddedMapPoints; 95 | 96 | boost::mutex mMutexNewKFs; 97 | 98 | bool mbAbortBA; 99 | 100 | bool mbStopped; 101 | bool mbStopRequested; 102 | boost::mutex mMutexStop; 103 | 104 | bool mbAcceptKeyFrames; 105 | boost::mutex mMutexAccept; 106 | }; 107 | 108 | } //namespace ORB_SLAM 109 | 110 | #endif // LOCALMAPPING_H 111 | -------------------------------------------------------------------------------- /include/LoopClosing.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef LOOPCLOSING_H 22 | #define LOOPCLOSING_H 23 | 24 | #include "KeyFrame.h" 25 | #include "LocalMapping.h" 26 | #include "Map.h" 27 | #include "ORBVocabulary.h" 28 | #include "Tracking.h" 29 | #include "sophus/sim3.hpp" 30 | #include 31 | 32 | #include "KeyFrameDatabase.h" 33 | 34 | //#include "Thirdparty/g2o/g2o/types/sim3/types_seven_dof_expmap.h" 35 | #include 36 | 37 | namespace ORB_SLAM 38 | { 39 | 40 | class Tracking; 41 | class LocalMapping; 42 | class KeyFrameDatabase; 43 | 44 | 45 | class LoopClosing 46 | { 47 | public: 48 | 49 | typedef pair,int> ConsistentGroup; 50 | typedef map, 51 | Eigen::aligned_allocator > > KeyFrameAndPose; 52 | typedef map, 53 | Eigen::aligned_allocator > > KeyFrameAndSE3Pose; 54 | public: 55 | 56 | LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc); 57 | 58 | void SetTracker(Tracking* pTracker); 59 | 60 | void SetLocalMapper(LocalMapping* pLocalMapper); 61 | 62 | void Run(); 63 | 64 | void InsertKeyFrame(KeyFrame *pKF); 65 | 66 | void RequestReset(); 67 | 68 | Sophus::Sim3d GetSnew2old(); 69 | Sophus::SE3d GetTnew2old(); 70 | 71 | 72 | public: 73 | 74 | bool CheckNewKeyFrames(); 75 | 76 | bool DetectLoop(); 77 | 78 | bool ComputeSim3(); 79 | 80 | void SearchAndFuse(KeyFrameAndPose &CorrectedPosesMap); 81 | void SearchAndFuse(KeyFrameAndSE3Pose &CorrectedPosesMap); 82 | void CorrectLoop(); 83 | void CorrectLoopSE3(); 84 | void ResetIfRequested(); 85 | bool mbResetRequested; 86 | boost::mutex mMutexReset; 87 | 88 | Map* mpMap; 89 | Tracking* mpTracker; 90 | 91 | KeyFrameDatabase* mpKeyFrameDB; 92 | ORBVocabulary* mpORBVocabulary; 93 | 94 | LocalMapping *mpLocalMapper; 95 | 96 | std::list mlpLoopKeyFrameQueue; 97 | 98 | boost::mutex mMutexLoopQueue; 99 | 100 | std::vector mvfLevelSigmaSquare; 101 | 102 | // Loop detector parameters 103 | float mnCovisibilityConsistencyTh; 104 | 105 | // Loop detector variables 106 | KeyFrame* mpCurrentKF; 107 | KeyFrame* mpMatchedKF; 108 | std::vector mvConsistentGroups; 109 | std::vector mvpEnoughConsistentCandidates; 110 | std::vector mvpCurrentConnectedKFs; 111 | std::vector mvpCurrentMatchedPoints; 112 | std::vector mvpLoopMapPoints; 113 | g2o::Sim3 mg2oScw; //transform from new world frame to current keyframe 114 | double mScale_cw; 115 | 116 | long unsigned int mLastLoopKFid; 117 | 118 | }; 119 | 120 | } //namespace ORB_SLAM 121 | 122 | #endif // LOOPCLOSING_H 123 | -------------------------------------------------------------------------------- /include/Map.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef MAP_H 22 | #define MAP_H 23 | 24 | #include "MapPoint.h" 25 | #include "KeyFrame.h" 26 | #include 27 | 28 | #include 29 | 30 | 31 | 32 | namespace ORB_SLAM 33 | { 34 | 35 | class MapPoint; 36 | class KeyFrame; 37 | 38 | class Map 39 | { 40 | public: 41 | Map(); 42 | ~Map(); 43 | void AddKeyFrame(KeyFrame* pKF); 44 | void AddMapPoint(MapPoint* pMP); 45 | void EraseMapPoint(MapPoint* pMP); 46 | void EraseKeyFrame(KeyFrame* pKF); 47 | void SetCurrentCameraPose(cv::Mat Tcw); 48 | void SetReferenceKeyFrames(const std::vector &vpKFs); 49 | void SetReferenceMapPoints(const std::vector &vpMPs); 50 | 51 | std::vector GetAllKeyFrames(); 52 | std::vector GetAllMapPoints(); 53 | cv::Mat GetCameraPose(); 54 | std::vector GetReferenceKeyFrames(); 55 | std::vector GetReferenceMapPoints(); 56 | 57 | int MapPointsInMap(); 58 | int KeyFramesInMap(); 59 | 60 | void SetFlagAfterBA(); 61 | bool isMapUpdated(); 62 | void ResetUpdated(); 63 | 64 | unsigned int GetMaxKFid(); 65 | 66 | void clear(); 67 | 68 | protected: 69 | std::set mspMapPoints; 70 | std::set mspKeyFrames; 71 | 72 | std::vector mvpReferenceMapPoints; 73 | 74 | unsigned int mnMaxKFid; 75 | 76 | boost::mutex mMutexMap; 77 | bool mbMapUpdated; 78 | public: 79 | // external lock, make sure that the points' positions and keyframe poses are consistent 80 | // during this lock period, used in reading and writing data between the map and an optimizer of the map 81 | // TODO: Does map point observations needs to be protected? For now, I assume no. 82 | boost::mutex mPointPoseConsistencyMutex; 83 | // does loop closing optimization just finished? This boolean prevent other optimization functions 84 | // from restoring their results if loop closing is just finished. It is also pretected by mPointPoseConsistencyMutex 85 | bool mbFinishedLoopClosing; 86 | }; 87 | 88 | } //namespace ORB_SLAM 89 | 90 | #endif // MAP_H 91 | -------------------------------------------------------------------------------- /include/MapPublisher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef MAPPUBLISHER_H 22 | #define MAPPUBLISHER_H 23 | 24 | #ifdef SLAM_USE_ROS 25 | #include 26 | #include 27 | #endif 28 | 29 | #include"Map.h" 30 | #include"MapPoint.h" 31 | #include"KeyFrame.h" 32 | 33 | namespace ORB_SLAM 34 | { 35 | 36 | class MapPublisher 37 | { 38 | public: 39 | MapPublisher(Map* pMap); 40 | 41 | Map* mpMap; 42 | 43 | void Refresh(); 44 | void PublishMapPoints(const std::vector &vpMPs, const std::vector &vpRefMPs); 45 | void PublishKeyFrames(const std::vector &vpKFs); 46 | void PublishCurrentCamera(const Sophus::SE3d &Tcw); 47 | void SetCurrentCameraPose(const Sophus::SE3d &Tcw); 48 | 49 | private: 50 | 51 | Sophus::SE3d GetCurrentCameraPose(); 52 | bool isCamUpdated(); 53 | void ResetCamFlag(); 54 | #ifdef SLAM_USE_ROS 55 | ros::NodeHandle nh; 56 | ros::Publisher publisher; 57 | 58 | visualization_msgs::Marker mPoints; 59 | visualization_msgs::Marker mReferencePoints; 60 | visualization_msgs::Marker mKeyFrames; 61 | visualization_msgs::Marker mReferenceKeyFrames; 62 | visualization_msgs::Marker mCovisibilityGraph; 63 | visualization_msgs::Marker mMST; 64 | visualization_msgs::Marker mCurrentCamera; 65 | #endif 66 | float fCameraSize; 67 | float fPointSize; 68 | 69 | Sophus::SE3d mCameraPose; 70 | bool mbCameraUpdated; 71 | 72 | boost::mutex mMutexCamera; 73 | }; 74 | 75 | } //namespace ORB_SLAM 76 | 77 | #endif // MAPPUBLISHER_H 78 | -------------------------------------------------------------------------------- /include/MotionModel.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2015 Taihú Pire and Thomas Fischer 5 | * For more information see 6 | * 7 | * S-PTAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * S-PTAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with S-PTAM. If not, see . 19 | * 20 | * Authors: Taihú Pire 21 | * Thomas Fischer 22 | * 23 | * Laboratory of Robotics and Embedded Systems 24 | * Department of Computer Science 25 | * Faculty of Exact and Natural Sciences 26 | * University of Buenos Aires 27 | */ 28 | #pragma once 29 | 30 | #include 31 | 32 | /** 33 | * This motion model follows the one proposed in the master thesis of 34 | * Christof Hoppe: Large-Scale Robotic SLAM through Visual Mapping p.43 35 | * TODO (Maybe not anymore) 36 | */ 37 | class MotionModel 38 | { 39 | public: 40 | 41 | MotionModel(const Eigen::Vector3d initialPosition, const Eigen::Quaterniond initialOrientation); 42 | 43 | // Get the current camera pose. 44 | void CurrentCameraPose(Eigen::Vector3d& currentPosition, Eigen::Quaterniond& currentOrientation) const; 45 | 46 | // Predict the next camera pose. 47 | void PredictNextCameraPose(Eigen::Vector3d& predictedPosition, Eigen::Quaterniond& predictedOrientation) const; 48 | // predict the incremental motion (tcp, rcp) of transform from previous to current frame, based on decay velocity model 49 | void PredictNextCameraMotion(Eigen::Vector3d& tcp, Eigen::Quaterniond& rcp) const; 50 | // Update the motion model given a new camera pose. 51 | void UpdateCameraPose(const Eigen::Vector3d& newPosition, const Eigen::Quaterniond& newOrientation); 52 | 53 | private: 54 | 55 | Eigen::Vector3d position_;// camera position in a world frame 56 | 57 | Eigen::Quaterniond orientation_;//rotation from camera frame to a world frame 58 | 59 | Eigen::Vector3d linearVelocity_; 60 | 61 | Eigen::Quaterniond angularVelocity_; 62 | }; 63 | -------------------------------------------------------------------------------- /include/ORBVocabulary.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | 22 | #ifndef ORBVOCABULARY_H 23 | #define ORBVOCABULARY_H 24 | 25 | #include"Thirdparty/DBoW2/DBoW2/FORB.h" 26 | #include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 27 | 28 | namespace ORB_SLAM 29 | { 30 | 31 | typedef DBoW2::TemplatedVocabulary 32 | ORBVocabulary; 33 | 34 | } //namespace ORB_SLAM 35 | 36 | #endif // ORBVOCABULARY_H 37 | -------------------------------------------------------------------------------- /include/Sim3Solver.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #ifndef SIM3SOLVER_H 22 | #define SIM3SOLVER_H 23 | 24 | #include 25 | #include 26 | 27 | #include "KeyFrame.h" 28 | 29 | 30 | 31 | namespace ORB_SLAM 32 | { 33 | 34 | class Sim3Solver 35 | { 36 | public: 37 | 38 | Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector &vpMatched12); 39 | 40 | void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300); 41 | 42 | cv::Mat find(std::vector &vbInliers12, int &nInliers); 43 | 44 | cv::Mat iterate(int nIterations, bool &bNoMore, std::vector &vbInliers, int &nInliers); 45 | 46 | cv::Mat GetEstimatedRotation(); 47 | cv::Mat GetEstimatedTranslation(); 48 | float GetEstimatedScale(); 49 | 50 | 51 | protected: 52 | 53 | bool Refine(); 54 | 55 | void centroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C); 56 | 57 | void computeT(cv::Mat &P1, cv::Mat &P2); 58 | 59 | void CheckInliers(); 60 | 61 | void Project(const std::vector &vP3Dw, std::vector &vP2D, cv::Mat Tcw, Eigen::Matrix3d K); 62 | void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, Eigen::Matrix3d K); 63 | 64 | 65 | protected: 66 | 67 | // KeyFrames and matches 68 | KeyFrame* mpKF1; 69 | KeyFrame* mpKF2; 70 | 71 | std::vector mvX3Dc1; 72 | std::vector mvX3Dc2; 73 | std::vector mvpMapPoints1; 74 | std::vector mvpMapPoints2; 75 | std::vector mvpMatches12; 76 | std::vector mvnIndices1; 77 | std::vector mvSigmaSquare1; 78 | std::vector mvSigmaSquare2; 79 | std::vector mvnMaxError1; 80 | std::vector mvnMaxError2; 81 | 82 | int N; 83 | int mN1; 84 | 85 | // Current Estimation 86 | cv::Mat mR12i; 87 | cv::Mat mt12i; 88 | float ms12i; 89 | cv::Mat mT12i; 90 | cv::Mat mT21i; 91 | std::vector mvbInliersi; 92 | int mnInliersi; 93 | 94 | // Current Ransac State 95 | int mnIterations; 96 | std::vector mvbBestInliers; 97 | int mnBestInliers; 98 | cv::Mat mBestT12; 99 | cv::Mat mBestRotation; 100 | cv::Mat mBestTranslation; 101 | float mBestScale; 102 | 103 | // Refined 104 | cv::Mat mRefinedT12; 105 | std::vector mvbRefinedInliers; 106 | int mnRefinedInliers; 107 | 108 | // Indices for random selection 109 | std::vector mvAllIndices; 110 | 111 | // Projections 112 | std::vector mvP1im1; 113 | std::vector mvP2im2; 114 | 115 | // RANSAC probability 116 | double mRansacProb; 117 | 118 | // RANSAC min inliers 119 | int mRansacMinInliers; 120 | 121 | // RANSAC max iterations 122 | int mRansacMaxIts; 123 | 124 | // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2 125 | float mTh; 126 | float mSigma2; 127 | 128 | // Calibration 129 | Eigen::Matrix3d mK1; 130 | Eigen::Matrix3d mK2; 131 | 132 | }; 133 | 134 | } //namespace ORB_SLAM 135 | 136 | #endif // SIM3SOLVER_H 137 | -------------------------------------------------------------------------------- /include/config.h: -------------------------------------------------------------------------------- 1 | #ifndef ORBSLAM_CONFIG_H_ 2 | #define ORBSLAM_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ORB_SLAM { 9 | 10 | using std::string; 11 | 12 | /// Global configuration file of ORB_SLAM contains these parameters remain relatively stable over tests 13 | /// Implements the Singleton design pattern to allow global access and to ensure 14 | /// that only one instance exists. 15 | class Config 16 | { 17 | public: 18 | static Config& getInstance(); 19 | 20 | /// Initialization: Minimum number of tracked features. 21 | static int& initMinTracked() { return getInstance().init_min_tracked; } 22 | 23 | /// Reprojection threshold [px]. 24 | static double& reprojThresh2() { return getInstance().reproj_thresh2; } 25 | 26 | /// the minimum disparity (in pixels) between two observations of stereo frames to triangulate a point 27 | /// set triang_min_disp as $f sqrt(2-2c)$ where c is triang_max_cos_rays, so that the two are roughly equivalent 28 | static float & triangMinDisp() { return getInstance().triang_min_disp;} 29 | 30 | /// the maximum cos of rays to triangulate a point 31 | static float & triangMaxCosRays() { return getInstance().triang_max_cos_rays;} 32 | 33 | /// features outside crop ROI is not considered for motion estimation, 34 | /// xl is the minimum x value of a feature, xr is the maximum, if do not want crop effect, then set 35 | /// crop_roi_xl 0, crop_roi_xr column number of image 36 | static float & cropROIXL(){ return getInstance().crop_roi_xl;} 37 | 38 | static float & cropROIXR(){ return getInstance().crop_roi_xr;} 39 | 40 | /// do we use a decay motion model 41 | static bool &useDecayVelocityModel(){return getInstance().use_decay_velocity_model;} 42 | 43 | /// temporal window size not counting the previous and current frame 44 | static size_t &temporalWindowSize(){return getInstance().temporal_window_size;} 45 | 46 | /// spatial window size of keyframes 47 | static int &spatialWindowSize(){return getInstance().spatial_window_size;} 48 | private: 49 | Config(); 50 | Config(Config const&); 51 | void operator=(Config const&); 52 | 53 | int init_min_tracked; 54 | double reproj_thresh2; 55 | float triang_min_disp; 56 | float triang_max_cos_rays; 57 | float crop_roi_xl; 58 | float crop_roi_xr; 59 | bool use_decay_velocity_model; 60 | size_t temporal_window_size; 61 | int spatial_window_size; 62 | }; 63 | 64 | } // namespace 65 | 66 | #endif // ORBSLAM_CONFIG_H_ 67 | -------------------------------------------------------------------------------- /include/global.h: -------------------------------------------------------------------------------- 1 | // This file is part of ScaViSLAM. 2 | // 3 | // Copyright 2011 Hauke Strasdat (Imperial College London) 4 | // 5 | // ScaViSLAM is free software: you can redistribute it and/or modify 6 | // it under the terms of the GNU Lesser General Public License as published 7 | // by the Free Software Foundation, either version 3 of the License, or 8 | // any later version. 9 | // 10 | // ScaViSLAM is distributed in the hope that it will be useful, 11 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | // GNU Lesser General Public License for more details. 14 | // 15 | // You should have received a copy of the GNU Lesser General Public License 16 | // along with ScaViSLAM. If not, see . 17 | 18 | #ifndef GLOBAL_H 19 | #define GLOBAL_H 20 | 21 | #include 22 | #ifdef SLAM_TRACE 23 | #include 24 | #endif 25 | 26 | #ifdef SLAM_USE_ROS 27 | #include 28 | #define SLAM_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x) 29 | #define SLAM_INFO_STREAM(x) ROS_INFO_STREAM(x) 30 | #define SLAM_WARN_STREAM(x) ROS_WARN_STREAM(x) 31 | #define SLAM_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x) 32 | #define SLAM_ERROR_STREAM(x) ROS_ERROR_STREAM(x) 33 | #else 34 | #define SLAM_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "< // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc. 43 | #define SLAM_WARN_STREAM_THROTTLE(rate, x) \ 44 | do { \ 45 | static double __log_stream_throttle__last_hit__ = 0.0; \ 46 | std::chrono::time_point __log_stream_throttle__now__ = \ 47 | std::chrono::system_clock::now(); \ 48 | if (__log_stream_throttle__last_hit__ + rate <= \ 49 | std::chrono::duration_cast( \ 50 | __log_stream_throttle__now__.time_since_epoch()).count()) { \ 51 | __log_stream_throttle__last_hit__ = \ 52 | std::chrono::duration_cast( \ 53 | __log_stream_throttle__now__.time_since_epoch()).count(); \ 54 | SLAM_WARN_STREAM(x); \ 55 | } \ 56 | } while(0) 57 | #endif 58 | 59 | namespace ORB_SLAM 60 | { 61 | #ifdef SLAM_TRACE 62 | extern vk::PerformanceMonitor* g_permon; 63 | #define SLAM_LOG(value) g_permon->log(std::string((#value)),(value)) 64 | #define SLAM_LOG2(value1, value2) SLAM_LOG(value1); SLAM_LOG(value2) 65 | #define SLAM_LOG3(value1, value2, value3) SLAM_LOG2(value1, value2); SLAM_LOG(value3) 66 | #define SLAM_LOG4(value1, value2, value3, value4) SLAM_LOG2(value1, value2); SLAM_LOG2(value3, value4) 67 | #define SLAM_START_TIMER(name) g_permon->startTimer((name)) 68 | #define SLAM_STOP_TIMER(name) g_permon->stopTimer((name)) 69 | #else 70 | #define SLAM_LOG(v) 71 | #define SLAM_LOG2(v1, v2) 72 | #define SLAM_LOG3(v1, v2, v3) 73 | #define SLAM_LOG4(v1, v2, v3, v4) 74 | #define SLAM_START_TIMER(name) 75 | #define SLAM_STOP_TIMER(name) 76 | #endif 77 | const double EPS = 0.0000000001; 78 | const double PI = 3.14159265; 79 | } 80 | #endif 81 | -------------------------------------------------------------------------------- /include/qcv-feature.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Hernan Badino 3 | * 4 | * This file is part of QCV 5 | * 6 | * QCV is under the terms of the GNU Lesser General Public License 7 | * version 2.1. See the GNU LGPL version 2.1 for details. 8 | * QCV is distributed "AS IS" without ANY WARRANTY, without even the 9 | * implied warranty of merchantability or fitness for a particular 10 | * purpose. 11 | * 12 | * In no event shall the authors or contributors be liable 13 | * for any direct, indirect, incidental, special, exemplary, or 14 | * consequential damages arising in any way out of the use of this 15 | * software. 16 | * 17 | * By downloading, copying, installing or using the software you agree 18 | * to this license. Do not download, install, copy or use the 19 | * software, if you do not agree to this license. 20 | */ 21 | 22 | #ifndef __FEATURE_H 23 | #define __FEATURE_H 24 | 25 | /** 26 | ******************************************************************************* 27 | * 28 | * @file Feature.h 29 | * 30 | * \class SFeature 31 | * \author Hernan Badino (hernan.badino@gmail.com) 32 | * 33 | * \brief Implements a data struct for modeling features. 34 | * 35 | * Implements a data struct for modeling features. 36 | * 37 | *******************************************************************************/ 38 | 39 | /* INCLUDES */ 40 | #include 41 | #include 42 | 43 | /* CONSTANTS */ 44 | 45 | namespace QCV 46 | { 47 | class SFeature 48 | { 49 | 50 | /// Public data types 51 | public: 52 | typedef enum 53 | { 54 | FS_UNINITIALIZED, 55 | FS_NEW, 56 | FS_TRACKED, 57 | FS_LOST 58 | } EFeatureState; 59 | 60 | static const char * FeatureStateName_v[]; 61 | 62 | 63 | public: 64 | SFeature() 65 | { 66 | clear(); 67 | } 68 | 69 | 70 | void print() const 71 | { 72 | printf("u: %f v: %f d: %f age: %i e: %f state: %s\n", 73 | u,v,d,t,e, 74 | state==FS_UNINITIALIZED?"Uninitialized": 75 | state==FS_NEW?"New": 76 | state==FS_TRACKED?"Tracked": 77 | "Lost" ); 78 | } 79 | 80 | void clear() 81 | { 82 | state = FS_UNINITIALIZED; 83 | idx = -1; 84 | u = v = d = -1; 85 | t = 0; 86 | e = 0.; 87 | } 88 | 89 | public: 90 | /// u image position. 91 | double u; 92 | 93 | /// v image position. 94 | double v; 95 | 96 | /// d image position. 97 | double d; 98 | 99 | /// number of times tracked. 100 | int t; 101 | 102 | /// store user defined information (i.e. confidence). 103 | double e; 104 | 105 | /// Frame up to which feature has been updated. 106 | size_t f; 107 | 108 | /// General flag used for user's purposes. 109 | int idx; 110 | 111 | /// Feature state. 112 | EFeatureState state; 113 | }; 114 | 115 | typedef std::vector CFeatureVector; 116 | } 117 | 118 | 119 | #endif // __FEATURE_H 120 | -------------------------------------------------------------------------------- /include/stereoSFM.h: -------------------------------------------------------------------------------- 1 | #ifndef STEREO_SFM_H 2 | #define STEREO_SFM_H 3 | #include "viso2/p_match.h" 4 | #include "qcv-feature.h" 5 | #include "sophus/se3.hpp" 6 | #include 7 | #include 8 | #include 9 | #include 10 | namespace ORB_SLAM{ 11 | class StereoSFM{ 12 | public: 13 | StereoSFM(): mnId(-2), m_pIdx_i (0), m_cIdx_i (1), m_trackHistoryACTUAL_v (2), mb_Tr_valid(false) 14 | { 15 | } 16 | void init(std::string feature_tracks= "", std::string delta_motion="") 17 | { 18 | mTracks.open(feature_tracks, std::ifstream::in); 19 | mDelta.open(delta_motion, std::ifstream::in); 20 | if (!mTracks.is_open()) { 21 | std::cerr<< "Error in opening feature tracks file"< &p_matched); 36 | ///get matches between two stereo pairs from the previous and currrent feature vector 37 | void getQuadMatches(std::vector &p_matched); 38 | 39 | Sophus::SE3d getDeltaMotion(){ return mTcp;} 40 | 41 | size_t getNumDenseFeatures(){return m_trackHistoryACTUAL_v[m_cIdx_i].size();} 42 | public: 43 | std::ifstream mTracks; 44 | std::ifstream mDelta; 45 | /// absolute current frame id as from the feature track file, often 0 based 46 | int mnId; 47 | /// delta motion, SE3 transform from previous to current frame, camera frame is right, down, forward 48 | Sophus::SE3d mTcp; 49 | /// binarized previous frame index, only taking values of 0 or 1. 50 | int m_pIdx_i; 51 | /// binarized current frame index. 52 | int m_cIdx_i; 53 | /// feature vector, only stores previous and current feature vectors which are used to extract quad matches 54 | std::vector m_trackHistoryACTUAL_v; 55 | ///is the incremental motion valid 56 | bool mb_Tr_valid; 57 | }; 58 | std::ostream& operator << (std::ostream &os, const QCV::SFeature & rhs); 59 | std::istream & operator>>(std::istream &is, QCV::SFeature &rhs); 60 | 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /include/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H 2 | #define UTILS_H 3 | #include "viso2/p_match.h" 4 | #include 5 | std::vector cropMatches(const std::vector &pMatches, float xl, float xr); 6 | /// Round up to next higher power of 2 (return x if it's already a power 7 | /// of 2). 8 | int pow2roundup (int x); 9 | int GetDownScale(int w, int h, int maxEdge= 2000); 10 | #endif 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orbslam_dwo 4 | 0.0.0 5 | The orbslam_dwo package 6 | 7 | jianzhu 8 | 9 | GPLv3 10 | 11 | https://github.com/JzHuai0108/ORB_SLAM 12 | 13 | Jianzhu Huai 14 | 15 | 16 | catkin 17 | 18 | 19 | cmake_modules 20 | roscpp 21 | roslib 22 | tf 23 | sensor_msgs 24 | image_transport 25 | cv_bridge 26 | 27 | 28 | roscpp 29 | roslib 30 | tf 31 | sensor_msgs 32 | image_transport 33 | cv_bridge 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /src/FeatureGrid.cpp: -------------------------------------------------------------------------------- 1 | #include "FeatureGrid.h" 2 | namespace ORB_SLAM 3 | { 4 | FeatureGrid::FeatureGrid(const int cell_size, const int nFeaturesInImage, const int nMaxX, 5 | const int nMinX, const int nMaxY, const int nMinY): 6 | mnCellSize(cell_size), mnMinX(nMinX), mnMinY(nMinY), 7 | mnGridCols( (nMaxX - nMinX)/ cell_size +2), 8 | mnGridRows( (nMaxY - nMinY)/ cell_size +2), 9 | mfGridElementWidthInv(1.f/cell_size), 10 | mfGridElementHeightInv(1.f/cell_size), 11 | mnPointPerCell(nFeaturesInImage/(mnGridCols*mnGridRows) +1) 12 | { 13 | mvMPGrid.resize(mnGridCols); 14 | for(auto it= mvMPGrid.begin(), ite= mvMPGrid.end(); it!=ite;++it) 15 | { 16 | it->resize(mnGridRows); 17 | for(auto it2= it->begin(), it2e= it->end(); it2!=it2e; ++it2) 18 | it2->reserve(mnPointPerCell); 19 | } 20 | } 21 | 22 | FeatureGrid::~FeatureGrid(){ 23 | mvMPGrid.clear(); 24 | } 25 | void FeatureGrid::resetGrid() 26 | { 27 | for(auto it= mvMPGrid.begin(), ite= mvMPGrid.end(); it!=ite;++it) 28 | { 29 | for(auto it2= it->begin(), it2e= it->end(); it2!=it2e; ++it2) 30 | it2->clear(); 31 | } 32 | } 33 | 34 | } //namespace ORB_SLAM 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/Map.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM. 3 | * 4 | * Copyright (C) 2014 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM. If not, see . 19 | */ 20 | 21 | #include "Map.h" 22 | 23 | namespace ORB_SLAM 24 | { 25 | 26 | Map::Map():mbFinishedLoopClosing(false) 27 | { 28 | mbMapUpdated= false; 29 | mnMaxKFid = 0; 30 | } 31 | Map::~Map() 32 | { 33 | clear(); 34 | } 35 | 36 | void Map::AddKeyFrame(KeyFrame *pKF) 37 | { 38 | boost::mutex::scoped_lock lock(mMutexMap); 39 | mspKeyFrames.insert(pKF); 40 | if(pKF->mnFrameId>mnMaxKFid) 41 | mnMaxKFid=pKF->mnFrameId; 42 | mbMapUpdated=true; 43 | } 44 | 45 | void Map::AddMapPoint(MapPoint *pMP) 46 | { 47 | boost::mutex::scoped_lock lock(mMutexMap); 48 | mspMapPoints.insert(pMP); 49 | mbMapUpdated=true; 50 | } 51 | 52 | void Map::EraseMapPoint(MapPoint *pMP) 53 | { 54 | boost::mutex::scoped_lock lock(mMutexMap); 55 | mspMapPoints.erase(pMP); 56 | mbMapUpdated=true; 57 | } 58 | 59 | void Map::EraseKeyFrame(KeyFrame *pKF) 60 | { 61 | boost::mutex::scoped_lock lock(mMutexMap); 62 | mspKeyFrames.erase(pKF); 63 | mbMapUpdated=true; 64 | } 65 | 66 | void Map::SetReferenceMapPoints(const vector &vpMPs) 67 | { 68 | boost::mutex::scoped_lock lock(mMutexMap); 69 | mvpReferenceMapPoints = vpMPs; 70 | mbMapUpdated=true; 71 | } 72 | 73 | vector Map::GetAllKeyFrames() 74 | { 75 | boost::mutex::scoped_lock lock(mMutexMap); 76 | return vector(mspKeyFrames.begin(),mspKeyFrames.end()); 77 | } 78 | 79 | vector Map::GetAllMapPoints() 80 | { 81 | boost::mutex::scoped_lock lock(mMutexMap); 82 | return vector(mspMapPoints.begin(),mspMapPoints.end()); 83 | } 84 | 85 | int Map::MapPointsInMap() 86 | { 87 | boost::mutex::scoped_lock lock(mMutexMap); 88 | return mspMapPoints.size(); 89 | } 90 | 91 | int Map::KeyFramesInMap() 92 | { 93 | boost::mutex::scoped_lock lock(mMutexMap); 94 | return mspKeyFrames.size(); 95 | } 96 | 97 | vector Map::GetReferenceMapPoints() 98 | { 99 | boost::mutex::scoped_lock lock(mMutexMap); 100 | return mvpReferenceMapPoints; 101 | } 102 | 103 | bool Map::isMapUpdated() 104 | { 105 | boost::mutex::scoped_lock lock(mMutexMap); 106 | return mbMapUpdated; 107 | } 108 | 109 | void Map::SetFlagAfterBA() 110 | { 111 | boost::mutex::scoped_lock lock(mMutexMap); 112 | mbMapUpdated=true; 113 | 114 | } 115 | 116 | void Map::ResetUpdated() 117 | { 118 | boost::mutex::scoped_lock lock(mMutexMap); 119 | mbMapUpdated=false; 120 | } 121 | 122 | unsigned int Map::GetMaxKFid() 123 | { 124 | boost::mutex::scoped_lock lock(mMutexMap); 125 | return mnMaxKFid; 126 | } 127 | 128 | void Map::clear() 129 | { 130 | for(set::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++) 131 | delete *sit; 132 | 133 | for(set::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++) 134 | delete *sit; 135 | 136 | mspMapPoints.clear(); 137 | mspKeyFrames.clear(); 138 | mnMaxKFid = 0; 139 | mvpReferenceMapPoints.clear(); 140 | } 141 | 142 | } //namespace ORB_SLAM 143 | -------------------------------------------------------------------------------- /src/MotionModel.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2015 Taihú Pire and Thomas Fischer 5 | * For more information see 6 | * 7 | * S-PTAM is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * S-PTAM is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with S-PTAM. If not, see . 19 | * 20 | * Authors: Taihú Pire 21 | * Thomas Fischer 22 | * 23 | * Laboratory of Robotics and Embedded Systems 24 | * Department of Computer Science 25 | * Faculty of Exact and Natural Sciences 26 | * University of Buenos Aires 27 | */ 28 | 29 | #include "MotionModel.hpp" 30 | 31 | MotionModel::MotionModel(const Eigen::Vector3d initialPosition, const Eigen::Quaterniond initialOrientation) 32 | : position_( initialPosition ), orientation_(initialOrientation), linearVelocity_(0, 0, 0), 33 | angularVelocity_( 1, 0, 0, 0) 34 | { 35 | } 36 | 37 | void MotionModel::CurrentCameraPose(Eigen::Vector3d& currentPosition, Eigen::Quaterniond& currentOrientation) const 38 | { 39 | currentPosition = position_; 40 | currentOrientation = orientation_; 41 | } 42 | 43 | void MotionModel::PredictNextCameraPose(Eigen::Vector3d& predictedPosition, Eigen::Quaterniond& predictedOrientation) const 44 | { 45 | // Compute predicted position by integrating linear velocity 46 | 47 | predictedPosition = position_ + linearVelocity_; 48 | 49 | // Compute predicted orientation by integrating angular velocity 50 | 51 | Eigen::Quaterniond predictedOrientation_e = orientation_ * angularVelocity_; 52 | predictedOrientation_e.normalize(); 53 | predictedOrientation = predictedOrientation_e; 54 | } 55 | // subscript c means current, p means previous 56 | void MotionModel::PredictNextCameraMotion(Eigen::Vector3d& tcp, Eigen::Quaterniond& rcp) const 57 | { 58 | // Compute predicted orientation by integrating angular velocity 59 | Eigen::Quaterniond predictedOrientation_e = orientation_ * angularVelocity_; 60 | predictedOrientation_e.normalize(); 61 | tcp = predictedOrientation_e.inverse()*(-linearVelocity_); 62 | rcp = angularVelocity_.inverse(); 63 | } 64 | // update the camera state given a new camera pose 65 | void MotionModel::UpdateCameraPose(const Eigen::Vector3d& newPosition, const Eigen::Quaterniond& newOrientation) 66 | { 67 | // Compute linear velocity 68 | Eigen::Vector3d newLinearVelocity( newPosition - position_ ); 69 | // In order to be robust against fast camera movements linear velocity is smoothed over time 70 | newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5; 71 | 72 | // compute rotation between q1 and q2: q2 * qInverse(q1); 73 | Eigen::Quaterniond newAngularVelocity = orientation_.inverse()*newOrientation; 74 | 75 | // In order to be robust against fast camera movements angular velocity is smoothed over time 76 | newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_); 77 | 78 | newAngularVelocity.normalize(); 79 | 80 | // Update the current state variables 81 | 82 | position_ = newPosition; 83 | orientation_ = newOrientation; 84 | linearVelocity_ = newLinearVelocity; 85 | angularVelocity_ = newAngularVelocity; 86 | } 87 | -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | #include "config.h" 2 | #ifdef SLAM_USE_ROS 3 | #include 4 | #endif 5 | 6 | namespace ORB_SLAM { 7 | 8 | Config::Config(): 9 | init_min_tracked(16), 10 | reproj_thresh2(5.991),//experiment show that 5.991 is an optimal setting 11 | triang_min_disp(3.5f), 12 | triang_max_cos_rays(0.99995f), 13 | crop_roi_xl(0), 14 | crop_roi_xr(1241), //for KITTI set to 292 and 949 15 | use_decay_velocity_model(true), 16 | temporal_window_size(3), 17 | spatial_window_size(7) 18 | { 19 | #if defined(SLAM_USE_ROS) && defined(SLAM_DEBUG_OUTPUT) 20 | if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)){ 21 | ros::console::notifyLoggerLevelsChanged(); 22 | } 23 | #endif 24 | } 25 | 26 | Config& Config::getInstance() 27 | { 28 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 29 | return instance; 30 | } 31 | 32 | } // namespace ORB_SLAM 33 | 34 | -------------------------------------------------------------------------------- /src/stereoSFM.cpp: -------------------------------------------------------------------------------- 1 | #include "stereoSFM.h" 2 | #include 3 | using namespace std; 4 | using namespace QCV; 5 | namespace ORB_SLAM{ 6 | const double minDisparity_d (0.01); 7 | const int cropOffset = 293; //for KITTI dataset, stereo SFM used a FOV scale factor 0.6 8 | //in kitti/params_stereoSFM.xml. This results in better location accuracy and an offset in cx of principal point 9 | // as a result, u = u' + [(w-w')/2.f], where w is original image width, w' is cropped image width, 10 | // u is original feature x axis coordinate, u' is stored feature x coordinate 11 | std::ostream& operator << (std::ostream &os, const QCV::SFeature & rhs) 12 | { 13 | os<< rhs.u<<' '<>(std::istream &is, QCV::SFeature &rhs) 20 | { 21 | int stateNum(-1); 22 | is>>rhs.u>>rhs.v>>rhs.d>> rhs.t>>rhs.e>>rhs.f>> rhs.idx>>stateNum; 23 | rhs.state= (QCV::SFeature::EFeatureState)stateNum; 24 | return is; 25 | } 26 | void StereoSFM::nextFrame() 27 | { 28 | //featrue tracks 29 | int nFrameId(-1),nFrameId2(-1), nFeatures(-1), lineNum(0); 30 | QCV::CFeatureVector featureVector; 31 | featureVector.reserve(3200); 32 | string tempStr; 33 | QCV::SFeature pat; 34 | 35 | mTracks>> nFrameId >> nFeatures; 36 | getline(mTracks, tempStr); //read out the enter character at the line end 37 | while(lineNum< nFeatures){ 38 | getline(mTracks, tempStr); 39 | if(mTracks.fail()) 40 | break; 41 | stringstream line_str(tempStr); 42 | line_str>>pat; 43 | pat.u += cropOffset; 44 | featureVector.push_back(pat); 45 | ++lineNum; 46 | } 47 | assert(nFeatures==3072); 48 | // delta motion frame id + 3x4 row major matrix 49 | Eigen::Matrix3d rot; 50 | Eigen::Vector3d trans; 51 | mDelta>> nFrameId2>> rot(0,0) >> rot(0,1) >> rot(0,2)>> trans(0) >> 52 | rot(1,0) >> rot(1,1) >> rot(1,2)>> trans(1) >> 53 | rot(2,0) >> rot(2,1) >> rot(2,2)>> trans(2); 54 | assert(nFrameId2 == nFrameId); 55 | mTcp=Sophus::SE3d(rot, trans); 56 | mb_Tr_valid= nFrameId == mnId+1; 57 | mnId= nFrameId; 58 | m_pIdx_i = m_cIdx_i; 59 | m_cIdx_i++; m_cIdx_i %= 2; 60 | 61 | m_trackHistoryACTUAL_v[m_cIdx_i] = featureVector; 62 | } 63 | void StereoSFM::getStereoMatches(std::vector &p_matched) 64 | { 65 | p_matched.clear(); 66 | 67 | for (int i = 0; i < (signed)m_trackHistoryACTUAL_v[m_cIdx_i].size(); ++i) 68 | { 69 | const SFeature &curr = m_trackHistoryACTUAL_v[m_cIdx_i][i]; 70 | if ( ( curr.state == SFeature::FS_TRACKED || 71 | curr.state == SFeature::FS_NEW ) && 72 | curr.d > minDisparity_d ) 73 | { 74 | p_matched.push_back(p_match(-1,-1,-1,-1,-1,-1, curr.u,curr.v,i,curr.u -curr.d,curr.v,i)); 75 | } 76 | } 77 | } 78 | void StereoSFM::getQuadMatches(std::vector &p_matched) 79 | { 80 | p_matched.clear(); 81 | if ( m_trackHistoryACTUAL_v[m_cIdx_i].size() == m_trackHistoryACTUAL_v[m_pIdx_i].size() ) 82 | { 83 | for (int i = 0; i < (signed)m_trackHistoryACTUAL_v[m_cIdx_i].size(); ++i) 84 | { 85 | if ( m_trackHistoryACTUAL_v[m_cIdx_i][i].state == SFeature::FS_TRACKED && 86 | ( m_trackHistoryACTUAL_v[m_pIdx_i][i].state == SFeature::FS_TRACKED || 87 | m_trackHistoryACTUAL_v[m_pIdx_i][i].state == SFeature::FS_NEW ) && 88 | m_trackHistoryACTUAL_v[m_pIdx_i][i].d > minDisparity_d ) 89 | { 90 | 91 | const SFeature &curr = m_trackHistoryACTUAL_v[m_cIdx_i][i]; 92 | const SFeature &prev = m_trackHistoryACTUAL_v[m_pIdx_i][i]; 93 | if (curr.t >= 1 && curr.d > minDisparity_d) 94 | { 95 | p_matched.push_back(p_match(prev.u,prev.v,i,prev.u - prev.d,prev.v,i, 96 | curr.u,curr.v,i,curr.u - curr.d,curr.v,i)); 97 | } 98 | } 99 | } 100 | } 101 | else{ 102 | printf("m_trackHistoryACTUAL_v[m_cIdx_i] and [m_pIdx_i] not equal size %d, %d\n", 103 | (int) m_trackHistoryACTUAL_v[m_cIdx_i].size(), (int)m_trackHistoryACTUAL_v[m_pIdx_i].size()); 104 | assert(mnId==0); 105 | } 106 | } 107 | 108 | } 109 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | #include 3 | //remove matches that have u1c falls outside of [xl, xr) 4 | std::vector cropMatches(const std::vector &pMatches, float xl, float xr) 5 | { 6 | std::vector resMatches; 7 | resMatches.reserve(pMatches.size()); 8 | for(auto it= pMatches.begin(), ite= pMatches.end(); it!=ite; ++it) 9 | { 10 | if(it->u1cu1c>=xr) 11 | continue; 12 | else 13 | resMatches.push_back(*it); 14 | } 15 | return resMatches; 16 | } 17 | /// Round up to next higher power of 2 (return x if it's already a power 18 | /// of 2). 19 | int pow2roundup (int x) 20 | { 21 | if (x < 0) 22 | return 0; 23 | --x; 24 | x |= x >> 1; 25 | x |= x >> 2; 26 | x |= x >> 4; 27 | x |= x >> 8; 28 | x |= x >> 16; 29 | return x+1; 30 | } 31 | // maxEdge maximum edge length in pixel units 32 | int GetDownScale(int w, int h, int maxEdge) 33 | { 34 | float ds= float(std::max(w,h))/ maxEdge; 35 | return pow2roundup((int)std::ceil(ds)); 36 | } 37 | --------------------------------------------------------------------------------