├── .gitignore ├── 3rdparty ├── DBoW2 │ ├── CMakeLists.txt │ ├── CMakeLists.txt~ │ ├── DBoW2 │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── FClass.h │ │ ├── FORB.cpp │ │ ├── FORB.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── LICENSE.txt │ └── README.txt └── line_descriptor │ ├── CMakeLists.txt │ ├── CMakeLists.txt~ │ ├── README.md │ ├── include │ ├── line_descriptor │ │ └── descriptor_custom.hpp │ └── line_descriptor_custom.hpp │ └── src │ ├── LSDDetector_custom.cpp │ ├── binary_descriptor_custom.cpp │ ├── binary_descriptor_matcher.cpp │ ├── bitarray_custom.hpp │ ├── bitops_custom.hpp │ ├── draw_custom.cpp │ ├── precomp_custom.hpp │ └── types_custom.hpp ├── CMakeLists.txt ├── LICENSE ├── README.md ├── app ├── plslam_dataset.cpp ├── plslam_mod.cpp └── plstvo_dataset.cpp ├── batch_script ├── Run_EuRoC.py ├── Run_Gazebo.py ├── Run_KITTI.py └── intuition.png ├── build.sh ├── build_dep.sh ├── clean.sh ├── cmake_modules ├── FindCholmod.cmake ├── FindEigen3.cmake └── FindG2O.cmake ├── config ├── asl │ ├── dataset_params.yaml │ └── gt-ass │ │ ├── mh_01 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── mh_02 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── mh_03 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── mh_04 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── mh_05 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── v1_01 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── v1_02 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── v1_03 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── v2_01 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ ├── v2_02 │ │ ├── associations.txt │ │ └── groundtruth.txt │ │ └── v2_03 │ │ ├── associations.txt │ │ └── groundtruth.txt ├── aux │ ├── help.png │ ├── img_aux.png │ ├── legend.png │ ├── legend_comp.png │ └── legend_full.png ├── dataset_params.yaml ├── euroc_params.yaml ├── gazebo_params.yaml ├── kitti │ ├── kitti00-02.yaml │ ├── kitti03.yaml │ └── kitti04-10.yaml ├── scene_config.ini └── scene_config_indoor.ini ├── include ├── ORBextractor.h ├── auxiliar.h ├── config.h ├── keyFrame.h ├── linespec.h ├── mapFeatures.h ├── mapHandler.h ├── pinholeStereoCamera.h ├── slamScene.h ├── stereoFeatures.h ├── stereoFrame.h ├── stereoFrameHandler.h └── voScene.h ├── src ├── ORBextractor.cc ├── auxiliar.cpp ├── config.cpp ├── keyFrame.cpp ├── linespec.cpp ├── mapFeatures.cpp ├── mapHandler.cpp ├── pinholeStereoCamera.cpp ├── slamScene.cpp ├── stereoFeatures.cpp ├── stereoFrame.cpp ├── stereoFrameHandler.cpp └── voScene.cpp ├── test ├── main.cpp └── testCut.cpp └── vocabulary └── voc.tar.gz /.gitignore: -------------------------------------------------------------------------------- 1 | vocabulary/*.yml 2 | lib/* 3 | build/* 4 | 3rdparty/DBoW2/build/* 5 | 3rdparty/line_descriptor/build/* 6 | 3rdparty/DBoW2/lib/* 7 | 3rdparty/line_descriptor/lib/* 8 | simu/line_cut/* 9 | config/aux/* 10 | CMakeLists.txt.user 11 | .vscode/ -------------------------------------------------------------------------------- /3rdparty/DBoW2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | set( OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev ) 28 | # set( OpenCV_DIR /opt/opencv3/share/OpenCV ) 29 | find_package(OpenCV 3 REQUIRED) 30 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 31 | 32 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 33 | 34 | include_directories(${OpenCV_INCLUDE_DIRS}) 35 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 36 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 37 | 38 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(DBoW2) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 6 | 7 | set(HDRS_DBOW2 8 | DBoW2/BowVector.h 9 | DBoW2/FORB.h 10 | DBoW2/FClass.h 11 | DBoW2/FeatureVector.h 12 | DBoW2/ScoringObject.h 13 | DBoW2/TemplatedVocabulary.h) 14 | set(SRCS_DBOW2 15 | DBoW2/BowVector.cpp 16 | DBoW2/FORB.cpp 17 | DBoW2/FeatureVector.cpp 18 | DBoW2/ScoringObject.cpp) 19 | 20 | set(HDRS_DUTILS 21 | DUtils/Random.h 22 | DUtils/Timestamp.h) 23 | set(SRCS_DUTILS 24 | DUtils/Random.cpp 25 | DUtils/Timestamp.cpp) 26 | 27 | # set( OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev ) 28 | set( OpenCV_DIR /opt/opencv3/share/OpenCV ) 29 | find_package(OpenCV 3 REQUIRED) 30 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 31 | 32 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 33 | 34 | include_directories(${OpenCV_INCLUDE_DIRS}) 35 | add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS}) 36 | target_link_libraries(DBoW2 ${OpenCV_LIBS}) 37 | 38 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/DBoW2/FORB.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.cpp 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | * Distance function has been modified 9 | * 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "FORB.h" 19 | 20 | using namespace std; 21 | 22 | namespace DBoW2 { 23 | 24 | // -------------------------------------------------------------------------- 25 | 26 | const int FORB::L=32; 27 | 28 | void FORB::meanValue(const std::vector &descriptors, 29 | FORB::TDescriptor &mean) 30 | { 31 | if(descriptors.empty()) 32 | { 33 | mean.release(); 34 | return; 35 | } 36 | else if(descriptors.size() == 1) 37 | { 38 | mean = descriptors[0]->clone(); 39 | } 40 | else 41 | { 42 | vector sum(FORB::L * 8, 0); 43 | 44 | for(size_t i = 0; i < descriptors.size(); ++i) 45 | { 46 | const cv::Mat &d = *descriptors[i]; 47 | const unsigned char *p = d.ptr(); 48 | 49 | for(int j = 0; j < d.cols; ++j, ++p) 50 | { 51 | if(*p & (1 << 7)) ++sum[ j*8 ]; 52 | if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; 53 | if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; 54 | if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; 55 | if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; 56 | if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; 57 | if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; 58 | if(*p & (1)) ++sum[ j*8 + 7 ]; 59 | } 60 | } 61 | 62 | mean = cv::Mat::zeros(1, FORB::L, CV_8U); 63 | unsigned char *p = mean.ptr(); 64 | 65 | const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; 66 | for(size_t i = 0; i < sum.size(); ++i) 67 | { 68 | if(sum[i] >= N2) 69 | { 70 | // set bit 71 | *p |= 1 << (7 - (i % 8)); 72 | } 73 | 74 | if(i % 8 == 7) ++p; 75 | } 76 | } 77 | } 78 | 79 | // -------------------------------------------------------------------------- 80 | 81 | int FORB::distance(const FORB::TDescriptor &a, 82 | const FORB::TDescriptor &b) 83 | { 84 | // Bit set count operation from 85 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 86 | 87 | const int *pa = a.ptr(); 88 | const int *pb = b.ptr(); 89 | 90 | int dist=0; 91 | 92 | for(int i=0; i<8; i++, pa++, pb++) 93 | { 94 | unsigned int v = *pa ^ *pb; 95 | v = v - ((v >> 1) & 0x55555555); 96 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 97 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 98 | } 99 | 100 | return dist; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | std::string FORB::toString(const FORB::TDescriptor &a) 106 | { 107 | stringstream ss; 108 | const unsigned char *p = a.ptr(); 109 | 110 | for(int i = 0; i < a.cols; ++i, ++p) 111 | { 112 | ss << (int)*p << " "; 113 | } 114 | 115 | return ss.str(); 116 | } 117 | 118 | // -------------------------------------------------------------------------- 119 | 120 | void FORB::fromString(FORB::TDescriptor &a, const std::string &s) 121 | { 122 | a.create(1, FORB::L, CV_8U); 123 | unsigned char *p = a.ptr(); 124 | 125 | stringstream ss(s); 126 | for(int i = 0; i < FORB::L; ++i, ++p) 127 | { 128 | int n; 129 | ss >> n; 130 | 131 | if(!ss.fail()) 132 | *p = (unsigned char)n; 133 | } 134 | 135 | } 136 | 137 | // -------------------------------------------------------------------------- 138 | 139 | void FORB::toMat32F(const std::vector &descriptors, 140 | cv::Mat &mat) 141 | { 142 | if(descriptors.empty()) 143 | { 144 | mat.release(); 145 | return; 146 | } 147 | 148 | const size_t N = descriptors.size(); 149 | 150 | mat.create(N, FORB::L*8, CV_32F); 151 | float *p = mat.ptr(); 152 | 153 | for(size_t i = 0; i < N; ++i) 154 | { 155 | const int C = descriptors[i].cols; 156 | const unsigned char *desc = descriptors[i].ptr(); 157 | 158 | for(int j = 0; j < C; ++j, p += 8) 159 | { 160 | p[0] = (desc[j] & (1 << 7) ? 1 : 0); 161 | p[1] = (desc[j] & (1 << 6) ? 1 : 0); 162 | p[2] = (desc[j] & (1 << 5) ? 1 : 0); 163 | p[3] = (desc[j] & (1 << 4) ? 1 : 0); 164 | p[4] = (desc[j] & (1 << 3) ? 1 : 0); 165 | p[5] = (desc[j] & (1 << 2) ? 1 : 0); 166 | p[6] = (desc[j] & (1 << 1) ? 1 : 0); 167 | p[7] = desc[j] & (1); 168 | } 169 | } 170 | } 171 | 172 | // -------------------------------------------------------------------------- 173 | 174 | void FORB::toMat8U(const std::vector &descriptors, 175 | cv::Mat &mat) 176 | { 177 | mat.create(descriptors.size(), 32, CV_8U); 178 | 179 | unsigned char *p = mat.ptr(); 180 | 181 | for(size_t i = 0; i < descriptors.size(); ++i, p += 32) 182 | { 183 | const unsigned char *d = descriptors[i].ptr(); 184 | std::copy(d, d+32, p); 185 | } 186 | 187 | } 188 | 189 | // -------------------------------------------------------------------------- 190 | 191 | } // namespace DBoW2 192 | 193 | 194 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/DBoW2/ScoringObject.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.cpp 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 | #include 11 | #include "TemplatedVocabulary.h" 12 | #include "BowVector.h" 13 | 14 | using namespace DBoW2; 15 | 16 | // If you change the type of WordValue, make sure you change also the 17 | // epsilon value (this is needed by the KL method) 18 | const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON 19 | 20 | // --------------------------------------------------------------------------- 21 | // --------------------------------------------------------------------------- 22 | 23 | double L1Scoring::score(const BowVector &v1, const BowVector &v2) const 24 | { 25 | BowVector::const_iterator v1_it, v2_it; 26 | const BowVector::const_iterator v1_end = v1.end(); 27 | const BowVector::const_iterator v2_end = v2.end(); 28 | 29 | v1_it = v1.begin(); 30 | v2_it = v2.begin(); 31 | 32 | double score = 0; 33 | 34 | while(v1_it != v1_end && v2_it != v2_end) 35 | { 36 | const WordValue& vi = v1_it->second; 37 | const WordValue& wi = v2_it->second; 38 | 39 | if(v1_it->first == v2_it->first) 40 | { 41 | score += fabs(vi - wi) - fabs(vi) - fabs(wi); 42 | 43 | // move v1 and v2 forward 44 | ++v1_it; 45 | ++v2_it; 46 | } 47 | else if(v1_it->first < v2_it->first) 48 | { 49 | // move v1 forward 50 | v1_it = v1.lower_bound(v2_it->first); 51 | // v1_it = (first element >= v2_it.id) 52 | } 53 | else 54 | { 55 | // move v2 forward 56 | v2_it = v2.lower_bound(v1_it->first); 57 | // v2_it = (first element >= v1_it.id) 58 | } 59 | } 60 | 61 | // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) 62 | // for all i | v_i != 0 and w_i != 0 63 | // (Nister, 2006) 64 | // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} 65 | score = -score/2.0; 66 | 67 | return score; // [0..1] 68 | } 69 | 70 | // --------------------------------------------------------------------------- 71 | // --------------------------------------------------------------------------- 72 | 73 | double L2Scoring::score(const BowVector &v1, const BowVector &v2) const 74 | { 75 | BowVector::const_iterator v1_it, v2_it; 76 | const BowVector::const_iterator v1_end = v1.end(); 77 | const BowVector::const_iterator v2_end = v2.end(); 78 | 79 | v1_it = v1.begin(); 80 | v2_it = v2.begin(); 81 | 82 | double score = 0; 83 | 84 | while(v1_it != v1_end && v2_it != v2_end) 85 | { 86 | const WordValue& vi = v1_it->second; 87 | const WordValue& wi = v2_it->second; 88 | 89 | if(v1_it->first == v2_it->first) 90 | { 91 | score += vi * wi; 92 | 93 | // move v1 and v2 forward 94 | ++v1_it; 95 | ++v2_it; 96 | } 97 | else if(v1_it->first < v2_it->first) 98 | { 99 | // move v1 forward 100 | v1_it = v1.lower_bound(v2_it->first); 101 | // v1_it = (first element >= v2_it.id) 102 | } 103 | else 104 | { 105 | // move v2 forward 106 | v2_it = v2.lower_bound(v1_it->first); 107 | // v2_it = (first element >= v1_it.id) 108 | } 109 | } 110 | 111 | // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) 112 | // for all i | v_i != 0 and w_i != 0 ) 113 | // (Nister, 2006) 114 | if(score >= 1) // rounding errors 115 | score = 1.0; 116 | else 117 | score = 1.0 - sqrt(1.0 - score); // [0..1] 118 | 119 | return score; 120 | } 121 | 122 | // --------------------------------------------------------------------------- 123 | // --------------------------------------------------------------------------- 124 | 125 | double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) 126 | const 127 | { 128 | BowVector::const_iterator v1_it, v2_it; 129 | const BowVector::const_iterator v1_end = v1.end(); 130 | const BowVector::const_iterator v2_end = v2.end(); 131 | 132 | v1_it = v1.begin(); 133 | v2_it = v2.begin(); 134 | 135 | double score = 0; 136 | 137 | // all the items are taken into account 138 | 139 | while(v1_it != v1_end && v2_it != v2_end) 140 | { 141 | const WordValue& vi = v1_it->second; 142 | const WordValue& wi = v2_it->second; 143 | 144 | if(v1_it->first == v2_it->first) 145 | { 146 | // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) 147 | // we move the -4 out 148 | if(vi + wi != 0.0) score += vi * wi / (vi + wi); 149 | 150 | // move v1 and v2 forward 151 | ++v1_it; 152 | ++v2_it; 153 | } 154 | else if(v1_it->first < v2_it->first) 155 | { 156 | // move v1 forward 157 | v1_it = v1.lower_bound(v2_it->first); 158 | } 159 | else 160 | { 161 | // move v2 forward 162 | v2_it = v2.lower_bound(v1_it->first); 163 | } 164 | } 165 | 166 | // this takes the -4 into account 167 | score = 2. * score; // [0..1] 168 | 169 | return score; 170 | } 171 | 172 | // --------------------------------------------------------------------------- 173 | // --------------------------------------------------------------------------- 174 | 175 | double KLScoring::score(const BowVector &v1, const BowVector &v2) const 176 | { 177 | BowVector::const_iterator v1_it, v2_it; 178 | const BowVector::const_iterator v1_end = v1.end(); 179 | const BowVector::const_iterator v2_end = v2.end(); 180 | 181 | v1_it = v1.begin(); 182 | v2_it = v2.begin(); 183 | 184 | double score = 0; 185 | 186 | // all the items or v are taken into account 187 | 188 | while(v1_it != v1_end && v2_it != v2_end) 189 | { 190 | const WordValue& vi = v1_it->second; 191 | const WordValue& wi = v2_it->second; 192 | 193 | if(v1_it->first == v2_it->first) 194 | { 195 | if(vi != 0 && wi != 0) score += vi * log(vi/wi); 196 | 197 | // move v1 and v2 forward 198 | ++v1_it; 199 | ++v2_it; 200 | } 201 | else if(v1_it->first < v2_it->first) 202 | { 203 | // move v1 forward 204 | score += vi * (log(vi) - LOG_EPS); 205 | ++v1_it; 206 | } 207 | else 208 | { 209 | // move v2_it forward, do not add any score 210 | v2_it = v2.lower_bound(v1_it->first); 211 | // v2_it = (first element >= v1_it.id) 212 | } 213 | } 214 | 215 | // sum rest of items of v 216 | for(; v1_it != v1_end; ++v1_it) 217 | if(v1_it->second != 0) 218 | score += v1_it->second * (log(v1_it->second) - LOG_EPS); 219 | 220 | return score; // cannot be scaled 221 | } 222 | 223 | // --------------------------------------------------------------------------- 224 | // --------------------------------------------------------------------------- 225 | 226 | double BhattacharyyaScoring::score(const BowVector &v1, 227 | const BowVector &v2) const 228 | { 229 | BowVector::const_iterator v1_it, v2_it; 230 | const BowVector::const_iterator v1_end = v1.end(); 231 | const BowVector::const_iterator v2_end = v2.end(); 232 | 233 | v1_it = v1.begin(); 234 | v2_it = v2.begin(); 235 | 236 | double score = 0; 237 | 238 | while(v1_it != v1_end && v2_it != v2_end) 239 | { 240 | const WordValue& vi = v1_it->second; 241 | const WordValue& wi = v2_it->second; 242 | 243 | if(v1_it->first == v2_it->first) 244 | { 245 | score += sqrt(vi * wi); 246 | 247 | // move v1 and v2 forward 248 | ++v1_it; 249 | ++v2_it; 250 | } 251 | else if(v1_it->first < v2_it->first) 252 | { 253 | // move v1 forward 254 | v1_it = v1.lower_bound(v2_it->first); 255 | // v1_it = (first element >= v2_it.id) 256 | } 257 | else 258 | { 259 | // move v2 forward 260 | v2_it = v2.lower_bound(v1_it->first); 261 | // v2_it = (first element >= v1_it.id) 262 | } 263 | } 264 | 265 | return score; // already scaled 266 | } 267 | 268 | // --------------------------------------------------------------------------- 269 | // --------------------------------------------------------------------------- 270 | 271 | double DotProductScoring::score(const BowVector &v1, 272 | const BowVector &v2) const 273 | { 274 | BowVector::const_iterator v1_it, v2_it; 275 | const BowVector::const_iterator v1_end = v1.end(); 276 | const BowVector::const_iterator v2_end = v2.end(); 277 | 278 | v1_it = v1.begin(); 279 | v2_it = v2.begin(); 280 | 281 | double score = 0; 282 | 283 | while(v1_it != v1_end && v2_it != v2_end) 284 | { 285 | const WordValue& vi = v1_it->second; 286 | const WordValue& wi = v2_it->second; 287 | 288 | if(v1_it->first == v2_it->first) 289 | { 290 | score += vi * wi; 291 | 292 | // move v1 and v2 forward 293 | ++v1_it; 294 | ++v2_it; 295 | } 296 | else if(v1_it->first < v2_it->first) 297 | { 298 | // move v1 forward 299 | v1_it = v1.lower_bound(v2_it->first); 300 | // v1_it = (first element >= v2_it.id) 301 | } 302 | else 303 | { 304 | // move v2 forward 305 | v2_it = v2.lower_bound(v1_it->first); 306 | // v2_it = (first element >= v1_it.id) 307 | } 308 | } 309 | 310 | return score; // cannot scale 311 | } 312 | 313 | // --------------------------------------------------------------------------- 314 | // --------------------------------------------------------------------------- 315 | 316 | -------------------------------------------------------------------------------- /3rdparty/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 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 44 | 45 | }; 46 | 47 | /** 48 | * Macro for defining Scoring classes 49 | * @param NAME name of class 50 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 51 | * @param NORM type of norm to use when MUSTNORMALIZE 52 | */ 53 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 54 | NAME: public GeneralScoring \ 55 | { public: \ 56 | /** \ 57 | * Computes score between two vectors \ 58 | * @param v \ 59 | * @param w \ 60 | * @return score between v and w \ 61 | */ \ 62 | virtual double score(const BowVector &v, const BowVector &w) const; \ 63 | \ 64 | /** \ 65 | * Says if a vector must be normalized according to the scoring function \ 66 | * @param norm (out) if true, norm to use 67 | * @return true iff vectors must be normalized \ 68 | */ \ 69 | virtual inline bool mustNormalize(LNorm &norm) const \ 70 | { norm = NORM; return MUSTNORMALIZE; } \ 71 | } 72 | 73 | /// L1 Scoring object 74 | class __SCORING_CLASS(L1Scoring, true, L1); 75 | 76 | /// L2 Scoring object 77 | class __SCORING_CLASS(L2Scoring, true, L2); 78 | 79 | /// Chi square Scoring object 80 | class __SCORING_CLASS(ChiSquareScoring, true, L1); 81 | 82 | /// KL divergence Scoring object 83 | class __SCORING_CLASS(KLScoring, true, L1); 84 | 85 | /// Bhattacharyya Scoring object 86 | class __SCORING_CLASS(BhattacharyyaScoring, true, L1); 87 | 88 | /// Dot product Scoring object 89 | class __SCORING_CLASS(DotProductScoring, false, L1); 90 | 91 | #undef __SCORING_CLASS 92 | 93 | } // namespace DBoW2 94 | 95 | #endif 96 | 97 | -------------------------------------------------------------------------------- /3rdparty/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 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/DUtils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | namespace DUtils { 19 | 20 | /// Functions to generate pseudo-random numbers 21 | class Random 22 | { 23 | public: 24 | class UnrepeatedRandomizer; 25 | 26 | public: 27 | /** 28 | * Sets the random number seed to the current time 29 | */ 30 | static void SeedRand(); 31 | 32 | /** 33 | * Sets the random number seed to the current time only the first 34 | * time this function is called 35 | */ 36 | static void SeedRandOnce(); 37 | 38 | /** 39 | * Sets the given random number seed 40 | * @param seed 41 | */ 42 | static void SeedRand(int seed); 43 | 44 | /** 45 | * Sets the given random number seed only the first time this function 46 | * is called 47 | * @param seed 48 | */ 49 | static void SeedRandOnce(int seed); 50 | 51 | /** 52 | * Returns a random number in the range [0..1] 53 | * @return random T number in [0..1] 54 | */ 55 | template 56 | static T RandomValue(){ 57 | return (T)rand()/(T)RAND_MAX; 58 | } 59 | 60 | /** 61 | * Returns a random number in the range [min..max] 62 | * @param min 63 | * @param max 64 | * @return random T number in [min..max] 65 | */ 66 | template 67 | static T RandomValue(T min, T max){ 68 | return Random::RandomValue() * (max - min) + min; 69 | } 70 | 71 | /** 72 | * Returns a random int in the range [min..max] 73 | * @param min 74 | * @param max 75 | * @return random int in [min..max] 76 | */ 77 | static int RandomInt(int min, int max); 78 | 79 | /** 80 | * Returns a random number from a gaussian distribution 81 | * @param mean 82 | * @param sigma standard deviation 83 | */ 84 | template 85 | static T RandomGaussianValue(T mean, T sigma) 86 | { 87 | // Box-Muller transformation 88 | T x1, x2, w, y1; 89 | 90 | do { 91 | x1 = (T)2. * RandomValue() - (T)1.; 92 | x2 = (T)2. * RandomValue() - (T)1.; 93 | w = x1 * x1 + x2 * x2; 94 | } while ( w >= (T)1. || w == (T)0. ); 95 | 96 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 97 | y1 = x1 * w; 98 | 99 | return( mean + y1 * sigma ); 100 | } 101 | 102 | private: 103 | 104 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 105 | static bool m_already_seeded; 106 | 107 | }; 108 | 109 | // --------------------------------------------------------------------------- 110 | 111 | /// Provides pseudo-random numbers with no repetitions 112 | class Random::UnrepeatedRandomizer 113 | { 114 | public: 115 | 116 | /** 117 | * Creates a randomizer that returns numbers in the range [min, max] 118 | * @param min 119 | * @param max 120 | */ 121 | UnrepeatedRandomizer(int min, int max); 122 | ~UnrepeatedRandomizer(){} 123 | 124 | /** 125 | * Copies a randomizer 126 | * @param rnd 127 | */ 128 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 129 | 130 | /** 131 | * Copies a randomizer 132 | * @param rnd 133 | */ 134 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 135 | 136 | /** 137 | * Returns a random number not given before. If all the possible values 138 | * were already given, the process starts again 139 | * @return unrepeated random number 140 | */ 141 | int get(); 142 | 143 | /** 144 | * Returns whether all the possible values between min and max were 145 | * already given. If get() is called when empty() is true, the behaviour 146 | * is the same than after creating the randomizer 147 | * @return true iff all the values were returned 148 | */ 149 | inline bool empty() const { return m_values.empty(); } 150 | 151 | /** 152 | * Returns the number of values still to be returned 153 | * @return amount of values to return 154 | */ 155 | inline unsigned int left() const { return m_values.size(); } 156 | 157 | /** 158 | * Resets the randomizer as it were just created 159 | */ 160 | void reset(); 161 | 162 | protected: 163 | 164 | /** 165 | * Creates the vector with available values 166 | */ 167 | void createValues(); 168 | 169 | protected: 170 | 171 | /// Min of range of values 172 | int m_min; 173 | /// Max of range of values 174 | int m_max; 175 | 176 | /// Available values 177 | std::vector m_values; 178 | 179 | }; 180 | 181 | } 182 | 183 | #endif 184 | 185 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/DUtils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/DUtils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | using namespace std; 15 | 16 | namespace DUtils { 17 | 18 | /// Timestamp 19 | class Timestamp 20 | { 21 | public: 22 | 23 | /// Options to initiate a timestamp 24 | enum tOptions 25 | { 26 | NONE = 0, 27 | CURRENT_TIME = 0x1, 28 | ZERO = 0x2 29 | }; 30 | 31 | public: 32 | 33 | /** 34 | * Creates a timestamp 35 | * @param option option to set the initial time stamp 36 | */ 37 | Timestamp(Timestamp::tOptions option = NONE); 38 | 39 | /** 40 | * Destructor 41 | */ 42 | virtual ~Timestamp(void); 43 | 44 | /** 45 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 46 | * when initiated with the ZERO flag 47 | * @return true iif secs == usecs == 0 48 | */ 49 | bool empty() const; 50 | 51 | /** 52 | * Sets this instance to the current time 53 | */ 54 | void setToCurrentTime(); 55 | 56 | /** 57 | * Sets the timestamp from seconds and microseconds 58 | * @param secs: seconds 59 | * @param usecs: microseconds 60 | */ 61 | inline void setTime(unsigned long secs, unsigned long usecs){ 62 | m_secs = secs; 63 | m_usecs = usecs; 64 | } 65 | 66 | /** 67 | * Returns the timestamp in seconds and microseconds 68 | * @param secs seconds 69 | * @param usecs microseconds 70 | */ 71 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 72 | { 73 | secs = m_secs; 74 | usecs = m_usecs; 75 | } 76 | 77 | /** 78 | * Sets the timestamp from a string with the time in seconds 79 | * @param stime: string such as "1235603336.036609" 80 | */ 81 | void setTime(const string &stime); 82 | 83 | /** 84 | * Sets the timestamp from a number of seconds from the epoch 85 | * @param s seconds from the epoch 86 | */ 87 | void setTime(double s); 88 | 89 | /** 90 | * Returns this timestamp as the number of seconds in (long) float format 91 | */ 92 | double getFloatTime() const; 93 | 94 | /** 95 | * Returns this timestamp as the number of seconds in fixed length string format 96 | */ 97 | string getStringTime() const; 98 | 99 | /** 100 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 101 | * If the order is swapped, a negative number is returned 102 | * @param t: timestamp to subtract from this timestamp 103 | * @return difference in seconds 104 | */ 105 | double operator- (const Timestamp &t) const; 106 | 107 | /** 108 | * Returns a copy of this timestamp + s seconds + us microseconds 109 | * @param s seconds 110 | * @param us microseconds 111 | */ 112 | Timestamp plus(unsigned long s, unsigned long us) const; 113 | 114 | /** 115 | * Returns a copy of this timestamp - s seconds - us microseconds 116 | * @param s seconds 117 | * @param us microseconds 118 | */ 119 | Timestamp minus(unsigned long s, unsigned long us) const; 120 | 121 | /** 122 | * Adds s seconds to this timestamp and returns a reference to itself 123 | * @param s seconds 124 | * @return reference to this timestamp 125 | */ 126 | Timestamp& operator+= (double s); 127 | 128 | /** 129 | * Substracts s seconds to this timestamp and returns a reference to itself 130 | * @param s seconds 131 | * @return reference to this timestamp 132 | */ 133 | Timestamp& operator-= (double s); 134 | 135 | /** 136 | * Returns a copy of this timestamp + s seconds 137 | * @param s: seconds 138 | */ 139 | Timestamp operator+ (double s) const; 140 | 141 | /** 142 | * Returns a copy of this timestamp - s seconds 143 | * @param s: seconds 144 | */ 145 | Timestamp operator- (double s) const; 146 | 147 | /** 148 | * Returns whether this timestamp is at the future of t 149 | * @param t 150 | */ 151 | bool operator> (const Timestamp &t) const; 152 | 153 | /** 154 | * Returns whether this timestamp is at the future of (or is the same as) t 155 | * @param t 156 | */ 157 | bool operator>= (const Timestamp &t) const; 158 | 159 | /** 160 | * Returns whether this timestamp and t represent the same instant 161 | * @param t 162 | */ 163 | bool operator== (const Timestamp &t) const; 164 | 165 | /** 166 | * Returns whether this timestamp is at the past of t 167 | * @param t 168 | */ 169 | bool operator< (const Timestamp &t) const; 170 | 171 | /** 172 | * Returns whether this timestamp is at the past of (or is the same as) t 173 | * @param t 174 | */ 175 | bool operator<= (const Timestamp &t) const; 176 | 177 | /** 178 | * Returns the timestamp in a human-readable string 179 | * @param machine_friendly if true, the returned string is formatted 180 | * to yyyymmdd_hhmmss, without weekday or spaces 181 | * @note This has not been tested under Windows 182 | * @note The timestamp is truncated to seconds 183 | */ 184 | string Format(bool machine_friendly = false) const; 185 | 186 | /** 187 | * Returns a string version of the elapsed time in seconds, with the format 188 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 189 | * @param s: elapsed seconds (given by getFloatTime) to format 190 | */ 191 | static string Format(double s); 192 | 193 | 194 | protected: 195 | /// Seconds 196 | unsigned long m_secs; // seconds 197 | /// Microseconds 198 | unsigned long m_usecs; // microseconds 199 | }; 200 | 201 | } 202 | 203 | #endif 204 | 205 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /3rdparty/DBoW2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( line-descriptor ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | 5 | set( OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev ) 6 | # set( OpenCV_DIR /opt/opencv3/share/OpenCV ) 7 | find_package( OpenCV 3 REQUIRED) 8 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 9 | 10 | if(COMMAND cmake_policy) 11 | cmake_policy(SET CMP0003 NEW) 12 | endif(COMMAND cmake_policy) 13 | link_directories(${OpenCV_LIBS_DIR}) 14 | include_directories(${OpenCV2_INCLUDE_DIRS}) 15 | 16 | SET(BUILD_SHARED_LIBS ON) 17 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 18 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -O3 -mtune=native -march=native") 20 | 21 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | include_directories( include ${OpenCV_INCLUDE_DIRS} ) 25 | list(APPEND LINK_LIBS ${OpenCV_LIBS} ) 26 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 27 | 28 | link_directories(${CMAKE_SOURCE_DIR}/src/) 29 | file(GLOB_RECURSE all_source_files RELATIVE "${CMAKE_SOURCE_DIR}src/" *.cpp ) 30 | 31 | add_custom_target( linedesc_includes DEPENDS ${all_include_files} SOURCES ${all_source_files} ) 32 | 33 | add_library( linedesc ${all_source_files} ) 34 | target_link_libraries( linedesc ${LINK_LIBS} ) 35 | 36 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | project( line-descriptor ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | 5 | # set( OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev ) 6 | set( OpenCV_DIR /opt/opencv3/share/OpenCV ) 7 | find_package( OpenCV 3 REQUIRED) 8 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 9 | 10 | if(COMMAND cmake_policy) 11 | cmake_policy(SET CMP0003 NEW) 12 | endif(COMMAND cmake_policy) 13 | link_directories(${OpenCV_LIBS_DIR}) 14 | include_directories(${OpenCV2_INCLUDE_DIRS}) 15 | 16 | SET(BUILD_SHARED_LIBS ON) 17 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 18 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 19 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -O3 -mtune=native -march=native") 20 | 21 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | include_directories( include ${OpenCV_INCLUDE_DIRS} ) 25 | list(APPEND LINK_LIBS ${OpenCV_LIBS} ) 26 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 27 | 28 | link_directories(${CMAKE_SOURCE_DIR}/src/) 29 | file(GLOB_RECURSE all_source_files RELATIVE "${CMAKE_SOURCE_DIR}src/" *.cpp ) 30 | 31 | add_custom_target( linedesc_includes DEPENDS ${all_include_files} SOURCES ${all_source_files} ) 32 | 33 | add_library( linedesc ${all_source_files} ) 34 | target_link_libraries( linedesc ${LINK_LIBS} ) 35 | 36 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/README.md: -------------------------------------------------------------------------------- 1 | Binary descriptors for lines extracted from an image 2 | ==================================================== -------------------------------------------------------------------------------- /3rdparty/line_descriptor/include/line_descriptor_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_LINE_DESCRIPTOR_HPP__ 43 | #define __OPENCV_LINE_DESCRIPTOR_HPP__ 44 | 45 | #include "line_descriptor/descriptor_custom.hpp" 46 | 47 | /** @defgroup line_descriptor Binary descriptors for lines extracted from an image 48 | 49 | Introduction 50 | ------------ 51 | 52 | One of the most challenging activities in computer vision is the extraction of useful information 53 | from a given image. Such information, usually comes in the form of points that preserve some kind of 54 | property (for instance, they are scale-invariant) and are actually representative of input image. 55 | 56 | The goal of this module is seeking a new kind of representative information inside an image and 57 | providing the functionalities for its extraction and representation. In particular, differently from 58 | previous methods for detection of relevant elements inside an image, lines are extracted in place of 59 | points; a new class is defined ad hoc to summarize a line's properties, for reuse and plotting 60 | purposes. 61 | 62 | Computation of binary descriptors 63 | --------------------------------- 64 | 65 | To obtatin a binary descriptor representing a certain line detected from a certain octave of an 66 | image, we first compute a non-binary descriptor as described in @cite LBD . Such algorithm works on 67 | lines extracted using EDLine detector, as explained in @cite EDL . Given a line, we consider a 68 | rectangular region centered at it and called *line support region (LSR)*. Such region is divided 69 | into a set of bands \f$\{B_1, B_2, ..., B_m\}\f$, whose length equals the one of line. 70 | 71 | If we indicate with \f$\bf{d}_L\f$ the direction of line, the orthogonal and clockwise direction to line 72 | \f$\bf{d}_{\perp}\f$ can be determined; these two directions, are used to construct a reference frame 73 | centered in the middle point of line. The gradients of pixels \f$\bf{g'}\f$ inside LSR can be projected 74 | to the newly determined frame, obtaining their local equivalent 75 | \f$\bf{g'} = (\bf{g}^T \cdot \bf{d}_{\perp}, \bf{g}^T \cdot \bf{d}_L)^T \triangleq (\bf{g'}_{d_{\perp}}, \bf{g'}_{d_L})^T\f$. 76 | 77 | Later on, a Gaussian function is applied to all LSR's pixels along \f$\bf{d}_\perp\f$ direction; first, 78 | we assign a global weighting coefficient \f$f_g(i) = (1/\sqrt{2\pi}\sigma_g)e^{-d^2_i/2\sigma^2_g}\f$ to 79 | *i*-th row in LSR, where \f$d_i\f$ is the distance of *i*-th row from the center row in LSR, 80 | \f$\sigma_g = 0.5(m \cdot w - 1)\f$ and \f$w\f$ is the width of bands (the same for every band). Secondly, 81 | considering a band \f$B_j\f$ and its neighbor bands \f$B_{j-1}, B_{j+1}\f$, we assign a local weighting 82 | \f$F_l(k) = (1/\sqrt{2\pi}\sigma_l)e^{-d'^2_k/2\sigma_l^2}\f$, where \f$d'_k\f$ is the distance of *k*-th 83 | row from the center row in \f$B_j\f$ and \f$\sigma_l = w\f$. Using the global and local weights, we obtain, 84 | at the same time, the reduction of role played by gradients far from line and of boundary effect, 85 | respectively. 86 | 87 | Each band \f$B_j\f$ in LSR has an associated *band descriptor(BD)* which is computed considering 88 | previous and next band (top and bottom bands are ignored when computing descriptor for first and 89 | last band). Once each band has been assignen its BD, the LBD descriptor of line is simply given by 90 | 91 | \f[LBD = (BD_1^T, BD_2^T, ... , BD^T_m)^T.\f] 92 | 93 | To compute a band descriptor \f$B_j\f$, each *k*-th row in it is considered and the gradients in such 94 | row are accumulated: 95 | 96 | \f[\begin{matrix} \bf{V1}^k_j = \lambda \sum\limits_{\bf{g}'_{d_\perp}>0}\bf{g}'_{d_\perp}, & \bf{V2}^k_j = \lambda \sum\limits_{\bf{g}'_{d_\perp}<0} -\bf{g}'_{d_\perp}, \\ \bf{V3}^k_j = \lambda \sum\limits_{\bf{g}'_{d_L}>0}\bf{g}'_{d_L}, & \bf{V4}^k_j = \lambda \sum\limits_{\bf{g}'_{d_L}<0} -\bf{g}'_{d_L}\end{matrix}.\f] 97 | 98 | with \f$\lambda = f_g(k)f_l(k)\f$. 99 | 100 | By stacking previous results, we obtain the *band description matrix (BDM)* 101 | 102 | \f[BDM_j = \left(\begin{matrix} \bf{V1}_j^1 & \bf{V1}_j^2 & \ldots & \bf{V1}_j^n \\ \bf{V2}_j^1 & \bf{V2}_j^2 & \ldots & \bf{V2}_j^n \\ \bf{V3}_j^1 & \bf{V3}_j^2 & \ldots & \bf{V3}_j^n \\ \bf{V4}_j^1 & \bf{V4}_j^2 & \ldots & \bf{V4}_j^n \end{matrix} \right) \in \mathbb{R}^{4\times n},\f] 103 | 104 | with \f$n\f$ the number of rows in band \f$B_j\f$: 105 | 106 | \f[n = \begin{cases} 2w, & j = 1||m; \\ 3w, & \mbox{else}. \end{cases}\f] 107 | 108 | Each \f$BD_j\f$ can be obtained using the standard deviation vector \f$S_j\f$ and mean vector \f$M_j\f$ of 109 | \f$BDM_J\f$. Thus, finally: 110 | 111 | \f[LBD = (M_1^T, S_1^T, M_2^T, S_2^T, \ldots, M_m^T, S_m^T)^T \in \mathbb{R}^{8m}\f] 112 | 113 | Once the LBD has been obtained, it must be converted into a binary form. For such purpose, we 114 | consider 32 possible pairs of BD inside it; each couple of BD is compared bit by bit and comparison 115 | generates an 8 bit string. Concatenating 32 comparison strings, we get the 256-bit final binary 116 | representation of a single LBD. 117 | */ 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/bitarray_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_BITARRAY_HPP 44 | #define __OPENCV_BITARRAY_HPP 45 | 46 | #ifdef _MSC_VER 47 | #pragma warning( disable : 4267 ) 48 | #endif 49 | 50 | #include "types_custom.hpp" 51 | #include 52 | #include 53 | #include 54 | 55 | /* class defining a sequence of bits */ 56 | class bitarray 57 | { 58 | 59 | public: 60 | /* pointer to bits sequence and sequence's length */ 61 | UINT32 *arr; 62 | UINT32 length; 63 | 64 | /* constructor setting default values */ 65 | bitarray() 66 | { 67 | arr = NULL; 68 | length = 0; 69 | } 70 | 71 | /* constructor setting sequence's length */ 72 | bitarray( UINT64 _bits ) 73 | { 74 | init( _bits ); 75 | } 76 | 77 | /* initializer of private fields */ 78 | void init( UINT64 _bits ) 79 | { 80 | length = (UINT32) ceil( _bits / 32.00 ); 81 | arr = new UINT32[length]; 82 | erase(); 83 | } 84 | 85 | /* destructor */ 86 | ~bitarray() 87 | { 88 | if( arr ) 89 | delete[] arr; 90 | } 91 | 92 | inline void flip( UINT64 index ) 93 | { 94 | arr[index >> 5] ^= ( (UINT32) 0x01 ) << ( index % 32 ); 95 | } 96 | 97 | inline void set( UINT64 index ) 98 | { 99 | arr[index >> 5] |= ( (UINT32) 0x01 ) << ( index % 32 ); 100 | } 101 | 102 | inline UINT8 get( UINT64 index ) 103 | { 104 | return ( arr[index >> 5] & ( ( (UINT32) 0x01 ) << ( index % 32 ) ) ) != 0; 105 | } 106 | 107 | /* reserve menory for an UINT32 */ 108 | inline void erase() 109 | { 110 | memset( arr, 0, sizeof(UINT32) * length ); 111 | } 112 | 113 | }; 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/bitops_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_BITOPTS_HPP 44 | #define __OPENCV_BITOPTS_HPP 45 | 46 | #include "precomp_custom.hpp" 47 | 48 | #ifdef _MSC_VER 49 | # include 50 | # define popcnt __popcnt 51 | # pragma warning( disable : 4267 ) 52 | #else 53 | # define popcnt __builtin_popcount 54 | 55 | #endif 56 | 57 | /* LUT */ 58 | const int lookup[] = 59 | { 60 | 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 61 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 62 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 63 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 64 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 65 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 66 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 67 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 68 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 69 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 70 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 71 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 72 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 73 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 74 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 75 | 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8 76 | }; 77 | 78 | namespace cv 79 | { 80 | namespace line_descriptor 81 | { 82 | /*matching function */ 83 | inline int match( UINT8*P, UINT8*Q, int codelb ) 84 | { 85 | int i, output = 0; 86 | for( i = 0; i <= codelb - 16; i += 16 ) 87 | { 88 | output += popcnt( *(UINT32*) (P+i) ^ *(UINT32*) (Q+i) ) + 89 | popcnt( *(UINT32*) (P+i+4) ^ *(UINT32*) (Q+i+4) ) + 90 | popcnt( *(UINT32*) (P+i+8) ^ *(UINT32*) (Q+i+8) ) + 91 | popcnt( *(UINT32*) (P+i+12) ^ *(UINT32*) (Q+i+12) ); 92 | } 93 | for( ; i < codelb; i++ ) 94 | output += lookup[P[i] ^ Q[i]]; 95 | return output; 96 | } 97 | 98 | /* splitting function (b <= 64) */ 99 | inline void split( UINT64 *chunks, UINT8 *code, int m, int mplus, int b ) 100 | { 101 | UINT64 temp = 0x0; 102 | int nbits = 0; 103 | int nbyte = 0; 104 | UINT64 mask = (b == 64) ? 0xFFFFFFFFFFFFFFFFull : ( ( UINT64_1 << b ) - UINT64_1 ); 105 | 106 | for ( int i = 0; i < m; i++ ) 107 | { 108 | while ( nbits < b ) 109 | { 110 | temp |= ( (UINT64) code[nbyte++] << nbits ); 111 | nbits += 8; 112 | } 113 | 114 | chunks[i] = temp & mask; 115 | temp = b == 64 ? 0x0 : temp >> b; 116 | nbits -= b; 117 | 118 | if( i == mplus - 1 ) 119 | { 120 | b--; /* b <= 63 */ 121 | mask = ( ( UINT64_1 << b ) - UINT64_1 ); 122 | } 123 | } 124 | } 125 | 126 | /* generates the next binary code (in alphabetical order) with the 127 | same number of ones as the input x. Taken from 128 | http://www.geeksforgeeks.org/archives/10375 */ 129 | inline UINT64 next_set_of_n_elements( UINT64 x ) 130 | { 131 | UINT64 smallest, ripple, new_smallest; 132 | 133 | smallest = x & -(signed) x; 134 | ripple = x + smallest; 135 | new_smallest = x ^ ripple; 136 | new_smallest = new_smallest / smallest; 137 | new_smallest >>= 2; 138 | return ripple | new_smallest; 139 | } 140 | 141 | /* print code */ 142 | inline void print_code( UINT64 tmp, int b ) 143 | { 144 | for ( long long int j = ( b - 1 ); j >= 0; j-- ) 145 | { 146 | printf( "%llu", (long long int) tmp / (UINT64) ( 1 << j ) ); 147 | tmp = tmp - ( tmp / (UINT64) ( 1 << j ) ) * (UINT64) ( 1 << j ); 148 | } 149 | 150 | printf( "\n" ); 151 | } 152 | 153 | inline UINT64 choose( int n, int r ) 154 | { 155 | UINT64 nchooser = 1; 156 | for ( int k = 0; k < r; k++ ) 157 | { 158 | nchooser *= n - k; 159 | nchooser /= k + 1; 160 | } 161 | 162 | return nchooser; 163 | } 164 | } 165 | } 166 | 167 | #endif 168 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/draw_custom.cpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #include "precomp_custom.hpp" 43 | 44 | namespace cv 45 | { 46 | namespace line_descriptor 47 | { 48 | /* draw matches between two images */ 49 | void drawLineMatches( const Mat& img1, const std::vector& keylines1, const Mat& img2, const std::vector& keylines2, 50 | const std::vector& matches1to2, Mat& outImg, const Scalar& matchColor, const Scalar& singleLineColor, 51 | const std::vector& matchesMask, int flags ) 52 | { 53 | 54 | if(img1.type() != img2.type()) 55 | { 56 | std::cout << "Input images have different types" << std::endl; 57 | CV_Assert(img1.type() == img2.type()); 58 | } 59 | 60 | /* initialize output matrix (if necessary) */ 61 | if( flags == DrawLinesMatchesFlags::DEFAULT ) 62 | { 63 | /* check how many rows are necessary for output matrix */ 64 | int totalRows = img1.rows >= img2.rows ? img1.rows : img2.rows; 65 | 66 | /* initialize output matrix */ 67 | outImg = Mat::zeros( totalRows, img1.cols + img2.cols, img1.type() ); 68 | 69 | } 70 | 71 | /* initialize random seed: */ 72 | srand( (unsigned int) time( NULL ) ); 73 | 74 | Scalar singleLineColorRGB; 75 | if( singleLineColor == Scalar::all( -1 ) ) 76 | { 77 | int R = ( rand() % (int) ( 255 + 1 ) ); 78 | int G = ( rand() % (int) ( 255 + 1 ) ); 79 | int B = ( rand() % (int) ( 255 + 1 ) ); 80 | 81 | singleLineColorRGB = Scalar( R, G, B ); 82 | } 83 | 84 | else 85 | singleLineColorRGB = singleLineColor; 86 | 87 | /* copy input images to output images */ 88 | Mat roi_left( outImg, Rect( 0, 0, img1.cols, img1.rows ) ); 89 | Mat roi_right( outImg, Rect( img1.cols, 0, img2.cols, img2.rows ) ); 90 | img1.copyTo( roi_left ); 91 | img2.copyTo( roi_right ); 92 | 93 | /* get columns offset */ 94 | int offset = img1.cols; 95 | 96 | /* if requested, draw lines from both images */ 97 | if( flags != DrawLinesMatchesFlags::NOT_DRAW_SINGLE_LINES ) 98 | { 99 | for ( size_t i = 0; i < keylines1.size(); i++ ) 100 | { 101 | KeyLine k1 = keylines1[i]; 102 | //line( outImg, Point2f( k1.startPointX, k1.startPointY ), Point2f( k1.endPointX, k1.endPointY ), singleLineColorRGB, 2 ); 103 | line( outImg, Point2f( k1.sPointInOctaveX, k1.sPointInOctaveY ), Point2f( k1.ePointInOctaveX, k1.ePointInOctaveY ), singleLineColorRGB, 2 ); 104 | 105 | } 106 | 107 | for ( size_t j = 0; j < keylines2.size(); j++ ) 108 | { 109 | KeyLine k2 = keylines2[j]; 110 | line( outImg, Point2f( k2.sPointInOctaveX + offset, k2.sPointInOctaveY ), Point2f( k2.ePointInOctaveX + offset, k2.ePointInOctaveY ), singleLineColorRGB, 2 ); 111 | } 112 | } 113 | 114 | /* draw matches */ 115 | for ( size_t counter = 0; counter < matches1to2.size(); counter++ ) 116 | { 117 | if( matchesMask[counter] != 0 ) 118 | { 119 | DMatch dm = matches1to2[counter]; 120 | KeyLine left = keylines1[dm.queryIdx]; 121 | KeyLine right = keylines2[dm.trainIdx]; 122 | 123 | Scalar matchColorRGB; 124 | if( matchColor == Scalar::all( -1 ) ) 125 | { 126 | int R = ( rand() % (int) ( 255 + 1 ) ); 127 | int G = ( rand() % (int) ( 255 + 1 ) ); 128 | int B = ( rand() % (int) ( 255 + 1 ) ); 129 | 130 | matchColorRGB = Scalar( R, G, B ); 131 | 132 | if( singleLineColor == Scalar::all( -1 ) ) 133 | singleLineColorRGB = matchColorRGB; 134 | } 135 | 136 | else 137 | matchColorRGB = matchColor; 138 | 139 | /* draw lines if necessary */ 140 | // line( outImg, Point2f( left.startPointX, left.startPointY ), Point2f( left.endPointX, left.endPointY ), singleLineColorRGB, 2 ); 141 | // 142 | // line( outImg, Point2f( right.startPointX + offset, right.startPointY ), Point2f( right.endPointX + offset, right.endPointY ), singleLineColorRGB, 143 | // 2 ); 144 | // 145 | // /* link correspondent lines */ 146 | // line( outImg, Point2f( left.startPointX, left.startPointY ), Point2f( right.startPointX + offset, right.startPointY ), matchColorRGB, 1 ); 147 | 148 | line( outImg, Point2f( left.sPointInOctaveX, left.sPointInOctaveY ), Point2f( left.ePointInOctaveX, left.ePointInOctaveY ), singleLineColorRGB, 2 ); 149 | 150 | line( outImg, Point2f( right.sPointInOctaveX + offset, right.sPointInOctaveY ), Point2f( right.ePointInOctaveX + offset, right.ePointInOctaveY ), singleLineColorRGB, 151 | 2 ); 152 | 153 | /* link correspondent lines */ 154 | line( outImg, Point2f( left.sPointInOctaveX, left.sPointInOctaveY ), Point2f( right.sPointInOctaveX + offset, right.sPointInOctaveY ), matchColorRGB, 1 ); 155 | } 156 | } 157 | } 158 | 159 | /* draw extracted lines on original image */ 160 | void drawKeylines( const Mat& image, const std::vector& keylines, Mat& outImage, const Scalar& color, int flags ) 161 | { 162 | if( flags == DrawLinesMatchesFlags::DEFAULT ) 163 | outImage = image.clone(); 164 | 165 | for ( size_t i = 0; i < keylines.size(); i++ ) 166 | { 167 | /* decide lines' color */ 168 | Scalar lineColor; 169 | if( color == Scalar::all( -1 ) ) 170 | { 171 | int R = ( rand() % (int) ( 255 + 1 ) ); 172 | int G = ( rand() % (int) ( 255 + 1 ) ); 173 | int B = ( rand() % (int) ( 255 + 1 ) ); 174 | 175 | lineColor = Scalar( R, G, B ); 176 | } 177 | 178 | else 179 | lineColor = color; 180 | 181 | /* get line */ 182 | KeyLine k = keylines[i]; 183 | 184 | /* draw line */ 185 | line( outImage, Point2f( k.startPointX, k.startPointY ), Point2f( k.endPointX, k.endPointY ), lineColor, 1 ); 186 | } 187 | } 188 | 189 | } 190 | } 191 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/precomp_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Biagio Montesano, all rights reserved. 14 | // Third party copyrights are property of their respective owners. 15 | // 16 | // Redistribution and use in source and binary forms, with or without modification, 17 | // are permitted provided that the following conditions are met: 18 | // 19 | // * Redistribution's of source code must retain the above copyright notice, 20 | // this list of conditions and the following disclaimer. 21 | // 22 | // * Redistribution's in binary form must reproduce the above copyright notice, 23 | // this list of conditions and the following disclaimer in the documentation 24 | // and/or other materials provided with the distribution. 25 | // 26 | // * The name of the copyright holders may not be used to endorse or promote products 27 | // derived from this software without specific prior written permission. 28 | // 29 | // This software is provided by the copyright holders and contributors "as is" and 30 | // any express or implied warranties, including, but not limited to, the implied 31 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 | // In no event shall the Intel Corporation or contributors be liable for any direct, 33 | // indirect, incidental, special, exemplary, or consequential damages 34 | // (including, but not limited to, procurement of substitute goods or services; 35 | // loss of use, data, or profits; or business interruption) however caused 36 | // and on any theory of liability, whether in contract, strict liability, 37 | // or tort (including negligence or otherwise) arising in any way out of 38 | // the use of this software, even if advised of the possibility of such damage. 39 | // 40 | //M*/ 41 | 42 | #ifndef __OPENCV_PRECOMP_H__ 43 | #define __OPENCV_PRECOMP_H__ 44 | 45 | #ifdef _MSC_VER 46 | #pragma warning( disable : 4267 ) 47 | #endif 48 | 49 | #define _USE_MATH_DEFINES 50 | 51 | #include 52 | #include "opencv2/core/utility.hpp" 53 | #include 54 | #include 55 | #include 56 | #include "opencv2/core.hpp" 57 | 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include "bitarray_custom.hpp" 71 | #include "bitops_custom.hpp" 72 | #include "types_custom.hpp" 73 | 74 | #include "line_descriptor_custom.hpp" 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /3rdparty/line_descriptor/src/types_custom.hpp: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2014, Mohammad Norouzi, Ali Punjani, David J. Fleet, 14 | // all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #if defined _MSC_VER && _MSC_VER <= 1700 44 | #include 45 | #else 46 | #include 47 | #endif 48 | 49 | #ifndef __OPENCV_TYPES_HPP 50 | #define __OPENCV_TYPES_HPP 51 | 52 | #ifdef _MSC_VER 53 | #pragma warning( disable : 4267 ) 54 | #endif 55 | 56 | /* define data types */ 57 | typedef uint64_t UINT64; 58 | typedef uint32_t UINT32; 59 | typedef uint16_t UINT16; 60 | typedef uint8_t UINT8; 61 | 62 | /* define constants */ 63 | #define UINT64_1 ((UINT64)0x01) 64 | #define UINT32_1 ((UINT32)0x01) 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project( GF_PL_SLAM ) 2 | 3 | cmake_minimum_required(VERSION 2.7) 4 | 5 | MESSAGE("CMake Build Type: " ${CMAKE_BUILD_TYPE}) 6 | 7 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 8 | 9 | set( OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev ) 10 | find_package(OpenCV 3 REQUIRED) 11 | # set( OpenCV_DIR /opt/opencv3/share/OpenCV ) 12 | # find_package(OpenCV 3.4.0 REQUIRED) 13 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 14 | 15 | find_package(Boost REQUIRED COMPONENTS thread system filesystem) 16 | 17 | # set(Eigen_DIR /usr/include/eigen3 ) 18 | # find_package(Eigen3 REQUIRED) 19 | SET( EIGEN3_INCLUDE_DIR /usr/include/eigen3 ) 20 | MESSAGE("EIGEN include dir: " ${EIGEN3_INCLUDE_DIR}) 21 | 22 | set( G2O_ROOT /opt/g2o ) 23 | find_package(G2O REQUIRED) 24 | #set( G2O_INCLUDE_DIR /opt/g2o/include/ ) 25 | #set( G2O_LIBRARIES g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse g2o_ext_csparse cholmod ) 26 | MESSAGE("G2O include dir: " ${G2O_INCLUDE_DIR}) 27 | MESSAGE("G2O libraries " ${G2O_LIBRARIES}) 28 | 29 | find_package(Cholmod REQUIRED) 30 | 31 | if(COMMAND cmake_policy) 32 | cmake_policy(SET CMP0003 NEW) 33 | endif(COMMAND cmake_policy) 34 | link_directories(${OpenCV_LIBS_DIR}) 35 | include_directories(${OpenCV2_INCLUDE_DIRS}) 36 | 37 | set(DEFAULT_HAS_MRPT ON) 38 | set(HAS_MRPT ${DEFAULT_HAS_MRPT} CACHE BOOL "Build the PointGrey Bumblebee2 SVO application that employs the MRPT library") 39 | #set(HAS_MRPT OFF) 40 | 41 | SET(BUILD_SHARED_LIBS ON) 42 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 43 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 44 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -O3 -mtune=native -march=native") 45 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c++11 -Wl,--no-as-needed -pthread -Wall -O3 -march=native ") 46 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wl,--no-as-needed -pthread -Wall -O3 -march=native") 47 | MESSAGE(">>>>> Compiler flags are: " ${CMAKE_CXX_FLAGS}) 48 | 49 | # MRPT library (optional, only with representation purposes) 50 | if(HAS_MRPT) 51 | set(MRPT_DIR /opt/mrpt/share/mrpt ) 52 | FIND_PACKAGE(MRPT REQUIRED base opengl gui hwdrivers) 53 | set(MRPT_DONT_USE_DBG_LIBS 1) #use release libraries for linking even if "Debug" CMake build 54 | add_definitions(-DHAS_MRPT) 55 | endif(HAS_MRPT) 56 | MESSAGE("MRPT libraries " ${MRPT_LIBS}) 57 | 58 | # YAML library 59 | set(YAML_CPP_INCLUDE_DIRS /usr/include/yaml-cpp/) 60 | set(YAML_CPP_LIBRARIES /usr/lib/x86_64-linux-gnu/libyaml-cpp.so) 61 | # find_library(YAML_CPP_LIBRARIES yaml-cpp) 62 | # if(NOT YAML_CPP_LIBRARIES) 63 | # # If yaml-cpp not found in the system, try finding it as a user CMake-generated project 64 | # find_package(yaml-cpp REQUIRED) 65 | # include_directories(${YAML_CPP_INCLUDE_DIRS}) 66 | # endif(NOT YAML_CPP_LIBRARIES) 67 | MESSAGE("YAML libraries " ${YAML_CPP_LIBRARIES}) 68 | 69 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 70 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 71 | 72 | # Include dirs 73 | include_directories( 74 | include 75 | ${YAML_CPP_INCLUDE_DIRS} 76 | ${EIGEN3_INCLUDE_DIR} 77 | ${OpenCV_INCLUDE_DIRS} 78 | ${Boost_INCLUDE_DIRS} 79 | ${PROJECT_SOURCE_DIR}/3rdparty/DBoW2/DBoW2/ 80 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/include/ 81 | ${G2O_INCLUDE_DIR} 82 | /usr/include/suitesparse # for cholmod 83 | ) 84 | 85 | # Set link libraries 86 | list(APPEND LINK_LIBS 87 | ${YAML_CPP_LIBRARIES} 88 | ${OpenCV_LIBS} 89 | ${Boost_LIBRARIES} 90 | cholmod 91 | ${G2O_CORE_LIBRARY} 92 | ${G2O_SOLVER_EIGEN} 93 | ${G2O_TYPES_DATA} 94 | ${G2O_TYPES_SLAM3D} 95 | ${PROJECT_SOURCE_DIR}/3rdparty/DBoW2/lib/libDBoW2.so 96 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/lib/liblinedesc.so 97 | ) 98 | 99 | # Set source files 100 | if(HAS_MRPT) 101 | list(APPEND SOURCEFILES 102 | src/linespec.cpp 103 | src/config.cpp 104 | src/auxiliar.cpp 105 | src/pinholeStereoCamera.cpp 106 | src/stereoFeatures.cpp 107 | src/stereoFrame.cpp 108 | src/stereoFrameHandler.cpp 109 | src/mapHandler.cpp 110 | src/mapFeatures.cpp 111 | src/keyFrame.cpp 112 | src/voScene.cpp 113 | src/slamScene.cpp 114 | src/ORBextractor.cc 115 | ) 116 | else() 117 | list(APPEND SOURCEFILES 118 | src/linespec.cpp 119 | src/config.cpp 120 | src/auxiliar.cpp 121 | src/pinholeStereoCamera.cpp 122 | src/stereoFeatures.cpp 123 | src/stereoFrame.cpp 124 | src/stereoFrameHandler.cpp 125 | src/mapHandler.cpp 126 | src/mapFeatures.cpp 127 | src/keyFrame.cpp 128 | src/ORBextractor.cc 129 | ) 130 | endif() 131 | 132 | # List all files (headers) contained by StVO-PL library 133 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 134 | 135 | # Visualize the files of this directory in IDE creating an custom empty target 136 | add_custom_target( plslam_includes DEPENDS ${all_include_files} SOURCES ${all_include_files} ) 137 | 138 | # Create StVO-PL library 139 | add_library(plslam SHARED ${SOURCEFILES}) 140 | 141 | if(HAS_MRPT) 142 | target_link_libraries(plslam ${LINK_LIBS} ${MRPT_LIBS} ) 143 | else() 144 | target_link_libraries(plslam ${LINK_LIBS}) 145 | endif() 146 | 147 | # Applications [TODO: ADD VO APPLICATIONS] 148 | if(HAS_MRPT) 149 | add_executable ( plslam_dataset app/plslam_dataset.cpp ) 150 | target_link_libraries( plslam_dataset plslam ) 151 | add_executable ( plstvo_dataset app/plstvo_dataset.cpp ) 152 | target_link_libraries( plstvo_dataset plslam ) 153 | # 154 | # add_executable ( plstvo_mod app/plstvo_mod.cpp ) 155 | # target_link_libraries( plstvo_mod plslam ) 156 | add_executable ( plslam_mod app/plslam_mod.cpp ) 157 | target_link_libraries( plslam_mod plslam ) 158 | endif(HAS_MRPT) 159 | 160 | # test cases 161 | #FIND_PACKAGE( Boost REQUIRED system thread ) 162 | #INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} ) 163 | #MESSAGE("Boost libs: " ${Boost_LIBRARIES}) 164 | 165 | #add_executable( testCut 166 | # ${SOURCEFILES} 167 | ## ./test/testCut_1.cpp 168 | # ./test/main.cpp 169 | # ) 170 | #target_link_libraries( testCut 171 | # ${LINK_LIBS} 172 | # ${GTest_LIBRARIES} 173 | #) 174 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Good Line Cutting Version of PL-SLAM 2 | 3 | Good line cutting is an enhancement module that is designed for line-assisted VSLAM, such as PL-SLAM. The primary objective of good line cutting is to pre-condition line-assisted pose optimization, therefore improving the robustness of optimization under 3D line triangulation error. 4 | 5 |
6 | Intuition 7 |
Intuition of good line cutting, in the presense of 3D triangulation error
8 |
9 | 10 | ## Build & Run 11 | 12 | This repo is an integration of good line cutting to stereo PL-SLAM. 13 | It also contains several bug fix (e.g. compacity with most recent g2o & Eigen) and improvements (e.g. better ORB extractor) compared with original PL-SLAM. 14 | When tested on multiple challenging scenarios that point feature may fail, GF-PL-SLAM has better performance than non-GF baseline and other state-of-the-art stereo SLAM systems. 15 | 16 | To build GF-PL-SLAM, first clone the repo to your ros workspace 17 | 18 | git clone https://github.com/ivalab/GF_PL_SLAM.git 19 | 20 | Build dependencies for GF-PL-SLAM with 21 | 22 | ./build_dep.sh 23 | 24 | Build the GF-PL-SLAM itself 25 | 26 | ./build.sh 27 | 28 | To run GF-PL-SLAM, please refer to some example batch evaluation scripts at folder 29 | 30 | batch_script 31 | 32 | ## Reference 33 | 34 | If you use GF-PL-SLAM in an academic work, please cite: 35 | 36 | @inproceedings{zhao2018good, 37 | title={Good Line Cutting: towards Accurate Pose Tracking of Line-assisted VO/VSLAM}, 38 | author={Zhao, Yipu and Vela, Patricio A}, 39 | booktitle={Proceedings of the European Conference on Computer Vision (ECCV)}, 40 | pages={516--531}, 41 | year={2018} 42 | } 43 | 44 | It's nice to include Ruben's original PL-SLAM implementation as well: 45 | 46 | @article{gomez2017pl, 47 | title = {{PL-SLAM: a Stereo SLAM System through the Combination of Points and Line Segments}}, 48 | author = {Gomez-Ojeda, Ruben and Zuñiga-Noël, David and Moreno, Francisco-Angel and Scaramuzza, Davide and Gonzalez-Jimenez, Javier}, 49 | journal = {arXiv preprint arXiv:1705.09479}, 50 | year = {2017} 51 | } 52 | 53 | --- 54 | 55 | 56 | Notice that this repository is only an open-source version of PL-SLAM released with the aim of being useful for the community, however, it is far from being optimized and we are not including some features of PL-SLAM. 57 | 58 | # PL-SLAM # 59 | 60 | This code contains an algorithm to compute stereo visual SLAM by using both point and line segment features. 61 | 62 | **Authors:** [Ruben Gomez-Ojeda](http://mapir.isa.uma.es/mapirwebsite/index.php/people/164-ruben-gomez), [David Zuñiga-Noël](http://mapir.isa.uma.es/mapirwebsite/index.php/people/270), [Francisco Angel Moreno](http://mapir.isa.uma.es/mapirwebsite/index.php/people/199-francisco-moreno-due%C3%B1as), [Davide Scaramuzza](http://rpg.ifi.uzh.ch/people_scaramuzza.html), and [Javier Gonzalez-Jimenez](http://mapir.isa.uma.es/mapirwebsite/index.php/people/95-javier-gonzalez-jimenez) 63 | 64 | **Related publication:** [*PL-SLAM: a Stereo SLAM System through the Combination of Points and Line Segments*](http://mapir.isa.uma.es/mapirwebsite/index.php/people/164-ruben-gomez) 65 | 66 | If you use PL-SLAM in your research work, please cite: 67 | 68 | @article{gomez2017pl, 69 | title = {{PL-SLAM: a Stereo SLAM System through the Combination of Points and Line Segments}}, 70 | author = {Gomez-Ojeda, Ruben and Zuñiga-Noël, David and Moreno, Francisco-Angel and Scaramuzza, Davide and Gonzalez-Jimenez, Javier}, 71 | journal = {arXiv preprint arXiv:1705.09479}, 72 | year = {2017} 73 | } 74 | 75 | The pdf file can be found at [https://arxiv.org/abs/1705.09479](https://arxiv.org/abs/1705.09479). 76 | 77 | [![PL-SLAM](https://img.youtube.com/vi/-lCTf_tAxhQ/0.jpg)](https://www.youtube.com/watch?v=-lCTf_tAxhQ) 78 | 79 | **Related publications:** 80 | 81 | [Gomez-Ojeda, R., Briales, J., & Gonzalez-Jimenez, J. (2016, October). PL-SVO: Semi-direct monocular visual odometry by combining points and line segments. In Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on (pp. 4211-4216). IEEE.](http://mapir.isa.uma.es/rgomez/publications/iros16plsvo.pdf) 82 | 83 | [Gomez-Ojeda, R., & Gonzalez-Jimenez, J. (2016, May). Robust stereo visual odometry through a probabilistic combination of points and line segments. In Robotics and Automation (ICRA), 2016 IEEE International Conference on (pp. 2521-2526). IEEE.](http://mapir.isa.uma.es/rgomez/publications/icra16plsvo.pdf). 84 | 85 | **License:** 86 | 87 | The provided code is published under the General Public License Version 3 (GPL v3). More information can be found in the "LICENSE" also included in the repository. 88 | 89 | Please do not hesitate to contact the authors if you have any further questions. 90 | 91 | 92 | ## 1. Prerequisites and dependencies 93 | 94 | ### OpenCV 3.x.x 95 | It can be easily found at http://opencv.org. 96 | 97 | ### Eigen3 (tested with 3.2.92) 98 | http://eigen.tuxfamily.org 99 | 100 | ### Boost 101 | Installation on Ubuntu: 102 | ``` 103 | sudo apt-get install libboost-dev 104 | ``` 105 | 106 | ### g2o - General Graph Optimization 107 | It can be found at: 108 | ``` 109 | https://github.com/RainerKuemmerle/g2o.git 110 | ``` 111 | 112 | ### YAML (tested with 0.5.2) 113 | Installation on Ubuntu: 114 | ``` 115 | sudo apt-get install libyaml-cpp-dev 116 | ``` 117 | 118 | ### stvo-pl 119 | It can be found at: 120 | ``` 121 | https://github.com/rubengooj/stvo-pl 122 | ``` 123 | 124 | ### MRPT 125 | In case of using the provided representation class. 126 | Download and install instructions can be found at: http://www.mrpt.org/ 127 | 128 | #### Known Issues: 129 | If working with the most recent versions of the MRPT library you might find some issues due to hard refactoring, for which we recommend to use this version instead (the last one we tested): 130 | ``` 131 | https://github.com/MRPT/mrpt/tree/0c3d605c3cbf5f2ffb8137089e43ebdae5a55de3 132 | ``` 133 | 134 | ### Line Descriptor 135 | We have modified the [*line_descriptor*](https://github.com/opencv/opencv_contrib/tree/master/modules/line_descriptor) module from the [OpenCV/contrib](https://github.com/opencv/opencv_contrib) library (both BSD) which is included in the *3rdparty* folder. 136 | 137 | 138 | ## 2. Configuration and generation 139 | 140 | Executing the file *build.sh* will configure and generate the *line_descriptor* and *DBoW2* modules, uncompress the vocabulary files, and then will configure and generate the *PL-SLAM* library for which we generate: **libplslam.so** in the lib folder, and the application **plslam_dataset** that works with our dataset format (explained in the next section). 141 | 142 | 143 | ## 3. Usage 144 | 145 | ### Datasets configuration 146 | We employ an environment variable, *${DATASETS_DIR}*, pointing the directory that contains our datasets. Each sequence from each dataset must contain in its root folder a file named *dataset_params.yaml*, that indicates at least the camera model and the subfolders with the left and right images. We provide dataset parameters files for several datasets and cameras with the format *xxxx_params.yaml*. 147 | 148 | ### Configuration files 149 | For running SLAM we can load the default parameters file or employ the *config_xxxx.yaml* files provided for every dataset. 150 | 151 | ### SLAM Application 152 | Usage: ./plslam_dataset [options] 153 | Options: 154 | -c Config file 155 | -o Offset (number of frames to skip in the dataset directory 156 | -n Number of frames to process the sequence 157 | -s Parameter to skip s-1 frames (default 1) 158 | 159 | A full command would be: 160 | 161 | ./plslam_dataset kitti/00 -c ../config/config_kitti.yaml -o 100 -s 2 -n 1000 162 | 163 | where we are processing the sequence 00 from the KITTI dataset (in our dataset folders) with the custom config file, with an offset *-c* allowing to skip the first 100 images, a parameter *-s* to consider only one every 2 images, and a parameter *-n* to only consider 1000 input pairs. 164 | 165 | -------------------------------------------------------------------------------- /batch_script/Run_EuRoC.py: -------------------------------------------------------------------------------- 1 | # This script is to run all the experiments in one program 2 | 3 | import os 4 | import subprocess 5 | import time 6 | import signal 7 | 8 | # SeqNameList = ['MH_01_easy', 'MH_02_easy', 'MH_03_medium', 'MH_04_difficult', 'MH_05_difficult']; 9 | # SeqNameList = ['V1_01_easy', 'V1_02_medium', 'V1_03_difficult', 'V2_01_easy', 'V2_02_medium', 'V2_03_difficult']; 10 | SeqNameList = ['V2_03_difficult']; 11 | Result_root = '/mnt/DATA/tmp/EuRoC/Cut_PointLine_1000_0.1/' 12 | # Result_root = '/mnt/DATA/tmp/LineCut/EuRoC_blur/' 13 | Num_Repeating = 1 # 10 # 20 # 5 # 14 | 15 | #---------------------------------------------------------------------------------------------------------------------- 16 | class bcolors: 17 | HEADER = '\033[95m' 18 | OKBLUE = '\033[94m' 19 | OKGREEN = '\033[92m' 20 | WARNING = '\033[93m' 21 | ALERT = '\033[91m' 22 | ENDC = '\033[0m' 23 | BOLD = '\033[1m' 24 | UNDERLINE = '\033[4m' 25 | 26 | for iteration in range(0, Num_Repeating): 27 | 28 | Experiment_dir = Result_root + 'Round' + str(iteration + 1) 29 | cmd_mkdir = 'mkdir -p ' + Experiment_dir 30 | subprocess.call(cmd_mkdir, shell=True) 31 | 32 | for sn, sname in enumerate(SeqNameList): 33 | 34 | print bcolors.ALERT + "====================================================================" + bcolors.ENDC 35 | 36 | SeqName = SeqNameList[sn] 37 | print bcolors.ALERT + "Round: " + str(iteration + 1) + "; Seq: " + SeqName 38 | 39 | File_Setting = '../config/euroc_params.yaml' 40 | File_stereo = '/mnt/DATA/Datasets/EuRoC_dataset/' + SeqName 41 | File_traj = Experiment_dir + '/' + SeqName # + '_blur_5' 42 | 43 | cmd_slam = str('../build/plslam_mod ' + 'euroc' + ' ' + File_Setting + ' ' + File_stereo + ' ' + File_traj) 44 | 45 | print bcolors.WARNING + "cmd_slam: \n" + cmd_slam + bcolors.ENDC 46 | 47 | print bcolors.OKGREEN + "Launching SLAM" + bcolors.ENDC 48 | # proc_slam = subprocess.Popen(cmd_slam, shell=True) 49 | subprocess.call(cmd_slam, shell=True) 50 | -------------------------------------------------------------------------------- /batch_script/Run_Gazebo.py: -------------------------------------------------------------------------------- 1 | # This script is to run all the experiments in one program 2 | 3 | import os 4 | import subprocess 5 | import time 6 | import signal 7 | 8 | SeqNameList = ['hard_wood_0.01']; 9 | # SeqNameList = ['brick_wall_0.01', 'hard_wood_0.01', 'wood_wall_0.01']; # ['hard_wood_0.01']; # 10 | # Result_root = '/mnt/DATA/tmp/Gazebo/PointLine_maxVol_0.30/' 11 | Result_root = '/mnt/DATA/tmp/LineCut/Gazebo/' 12 | Num_Repeating = 1 # 10 # 20 # 5 # 13 | 14 | #---------------------------------------------------------------------------------------------------------------------- 15 | class bcolors: 16 | HEADER = '\033[95m' 17 | OKBLUE = '\033[94m' 18 | OKGREEN = '\033[92m' 19 | WARNING = '\033[93m' 20 | ALERT = '\033[91m' 21 | ENDC = '\033[0m' 22 | BOLD = '\033[1m' 23 | UNDERLINE = '\033[4m' 24 | 25 | for iteration in range(0, Num_Repeating): 26 | 27 | Experiment_dir = Result_root + 'Round' + str(iteration + 1) 28 | cmd_mkdir = 'mkdir -p ' + Experiment_dir 29 | subprocess.call(cmd_mkdir, shell=True) 30 | 31 | for sn, sname in enumerate(SeqNameList): 32 | 33 | print bcolors.ALERT + "====================================================================" + bcolors.ENDC 34 | 35 | SeqName = SeqNameList[sn] 36 | print bcolors.ALERT + "Round: " + str(iteration + 1) + "; Seq: " + SeqName 37 | 38 | File_Setting = '../config/gazebo_params.yaml' 39 | File_stereo = '/mnt/DATA/Datasets/GazeboMaze/' + SeqName 40 | File_traj = Experiment_dir + '/' + SeqName 41 | 42 | cmd_slam = str('../build/plslam_mod ' + 'gazebo' + ' ' + File_Setting + ' ' + File_stereo + ' ' + File_traj) 43 | 44 | print bcolors.WARNING + "cmd_slam: \n" + cmd_slam + bcolors.ENDC 45 | 46 | print bcolors.OKGREEN + "Launching SLAM" + bcolors.ENDC 47 | # proc_slam = subprocess.Popen(cmd_slam, shell=True) 48 | subprocess.call(cmd_slam, shell=True) 49 | -------------------------------------------------------------------------------- /batch_script/Run_KITTI.py: -------------------------------------------------------------------------------- 1 | # This script is to run all the experiments in one program 2 | 3 | import os 4 | import subprocess 5 | import time 6 | import signal 7 | 8 | SeqIdxList = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 99]; 9 | Result_root = '/mnt/DATA/tmp/KITTI/PL_SLAM/Cut_PointLine_0.1/' 10 | Num_Repeating = 5 # 1 # 10 # 20 # 11 | 12 | #---------------------------------------------------------------------------------------------------------------------- 13 | class bcolors: 14 | HEADER = '\033[95m' 15 | OKBLUE = '\033[94m' 16 | OKGREEN = '\033[92m' 17 | WARNING = '\033[93m' 18 | ALERT = '\033[91m' 19 | ENDC = '\033[0m' 20 | BOLD = '\033[1m' 21 | UNDERLINE = '\033[4m' 22 | 23 | for iteration in range(0, Num_Repeating): 24 | 25 | Experiment_dir = Result_root + 'Round' + str(iteration + 1) 26 | cmd_mkdir = 'mkdir ' + Experiment_dir 27 | subprocess.call(cmd_mkdir, shell=True) 28 | 29 | for sn, SequenceIdx in enumerate(SeqIdxList): 30 | 31 | print bcolors.ALERT + "====================================================================" + bcolors.ENDC 32 | 33 | SeqIdx = str(SeqIdxList[sn]).zfill(2) 34 | print bcolors.ALERT + "Round: " + str(iteration + 1) + "; Seq: " + SeqIdx 35 | 36 | if sn < 3: 37 | File_Setting = '../config/kitti/kitti00-02.yaml' 38 | elif sn == 3: 39 | File_Setting = '../config/kitti/kitti03.yaml' 40 | else: 41 | File_Setting = '../config/kitti/kitti04-10.yaml' 42 | 43 | File_stereo = '/mnt/DATA/Datasets/Kitti_dataset/dataset/sequences/' + SeqIdx 44 | File_traj = Experiment_dir + '/' + SeqIdx 45 | 46 | cmd_slam = str('../build/plslam_mod ' + 'kitti' + ' ' + File_Setting + ' ' + File_stereo + ' ' + File_traj) 47 | 48 | print bcolors.WARNING + "cmd_slam: \n" + cmd_slam + bcolors.ENDC 49 | 50 | print bcolors.OKGREEN + "Launching SLAM" + bcolors.ENDC 51 | # proc_slam = subprocess.Popen(cmd_slam, shell=True) 52 | subprocess.call(cmd_slam, shell=True) 53 | -------------------------------------------------------------------------------- /batch_script/intuition.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/batch_script/intuition.png -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | echo "Building 3rdparty/line_descriptor ... " 2 | cd 3rdparty/line_descriptor 3 | mkdir build 4 | cd build 5 | cmake .. -DCMAKE_BUILD_TYPE=Release 6 | make -j 7 | cd ../../../ 8 | 9 | echo "Building 3rdparty/DBoW2 ... " 10 | cd 3rdparty/DBoW2 11 | mkdir build 12 | cd build 13 | cmake .. -DCMAKE_BUILD_TYPE=Release 14 | make -j 15 | cd ../../../ 16 | 17 | echo "Uncompressing vocabulary ..." 18 | cd vocabulary 19 | tar -xf voc.tar.gz 20 | cd .. 21 | 22 | echo "Building PL-SLAM ... " 23 | mkdir build 24 | cd build 25 | cmake .. -DCMAKE_BUILD_TYPE=Release 26 | make -j 27 | -------------------------------------------------------------------------------- /build_dep.sh: -------------------------------------------------------------------------------- 1 | # place all dependencies here 2 | export DEPENDENCIES_DIR=/mnt/DATA/SDK/ 3 | mkdir -p ${DEPENDENCIES_DIR} 4 | cd ${DEPENDENCIES_DIR} 5 | 6 | # Boost 7 | sudo apt-get install libboost-dev 8 | 9 | # YAML 10 | sudo apt-get install libyaml-cpp-dev 11 | 12 | # SuiteSparse 13 | sudo apt-get install libsuitesparse-dev 14 | 15 | # g2o 16 | cd ${DEPENDENCIES_DIR} 17 | git clone https://github.com/RainerKuemmerle/g2o.git 18 | cd g2o 19 | mkdir build 20 | cd build 21 | cmake .. -DCMAKE_BUILD_TYPE=Release .. -DCMAKE_INSTALL_PREFIX:PATH="/opt/g2o" 22 | make -j 23 | sudo make install 24 | 25 | # WxWidget 26 | sudo apt-get install wx3.0-headers libwxgtk3.0-dev 27 | 28 | # MRPT (viz only) 29 | # NOTE the old mrpt only works with opencv no later than 3.3.1, e.g. the opencv ships with ros kinetic 30 | # here I assume ros-kinectic already being installed; otherwise please build opencv-3.3.1 on your own 31 | cd ${DEPENDENCIES_DIR} 32 | git clone https://github.com/MRPT/mrpt.git 33 | cd mrpt 34 | git checkout 0c3d605c3cbf5f2ffb8137089e43ebdae5a55de3 35 | mkdir build 36 | cd build 37 | cmake .. -DCMAKE_BUILD_TYPE:STRING="Release" -DOpenCV_DIR:PATH="/opt/ros/kinetic/share/OpenCV-3.3.1-dev" -DCMAKE_INSTALL_PREFIX:PATH="/opt/mrpt" 38 | make -j 39 | sudo make install 40 | -------------------------------------------------------------------------------- /clean.sh: -------------------------------------------------------------------------------- 1 | rm -rf build 2 | rm -rf 3rdparty/line_descriptor/build 3 | rm -rf 3rdparty/DBoW2/build 4 | -------------------------------------------------------------------------------- /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/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | find_path(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | macro(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | find_library("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | find_library("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | find_library(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | find_library(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | if(NOT ${MYLIBRARY}_DEBUG) 70 | if(MYLIBRARY) 71 | set(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | endif(MYLIBRARY) 73 | endif( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | endmacro(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | set(G2O_SOLVERS_FOUND "NO") 105 | if(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | set(G2O_SOLVERS_FOUND "YES") 107 | endif(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | set(G2O_FOUND "NO") 111 | if(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | set(G2O_FOUND "YES") 113 | endif(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /config/asl/dataset_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | Kl: [458.654, 457.296, 367.215, 248.375] 3 | Kr: [457.587, 456.134, 379.999, 255.238] 4 | cam_bl: 0.110077842 5 | cam_height: 480 6 | cam_model: Pinhole 7 | cam_width: 752 8 | Rl: [0.9999663475300330, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 9 | Rr: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.9999451734846440] 10 | Dl: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 11 | Dr: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 12 | images_subfolder_l: cam0/data/ 13 | images_subfolder_r: cam1/data/ 14 | -------------------------------------------------------------------------------- /config/aux/help.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/config/aux/help.png -------------------------------------------------------------------------------- /config/aux/img_aux.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/config/aux/img_aux.png -------------------------------------------------------------------------------- /config/aux/legend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/config/aux/legend.png -------------------------------------------------------------------------------- /config/aux/legend_comp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/config/aux/legend_comp.png -------------------------------------------------------------------------------- /config/aux/legend_full.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/config/aux/legend_full.png -------------------------------------------------------------------------------- /config/dataset_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 601.8873000000 3 | cam_cy: 183.1104000000 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 707.0912 9 | cam_fy: 707.0912 10 | cam_bl: 0.54 11 | cam_height: 376 12 | cam_model: Pinhole 13 | cam_width: 1241 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: /mnt/DATA/Datasets/EuRoC_dataset/MH_01_easy/cam0/data/ 21 | images_subfolder_r: /mnt/DATA/Datasets/EuRoC_dataset/MH_01_easy/cam1/data/ 22 | -------------------------------------------------------------------------------- /config/euroc_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | Kl: [458.654, 457.296, 367.215, 248.375] 3 | Kr: [457.587, 456.134, 379.999, 255.238] 4 | Dl: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 5 | Dr: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] 6 | Rl: [0.9999663475300330, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 7 | Rr: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.9999451734846440] 8 | cam_bl: 0.110077842 9 | cam_height: 480 10 | cam_model: Pinhole 11 | cam_width: 752 12 | R: [ 9.99997256e-01, 2.31206719e-03, 3.76008102e-04, 13 | -2.31713572e-03, 9.99898049e-01, 1.40898358e-02, 14 | -3.43393121e-04, -1.40906685e-02, 9.99900663e-01 ] 15 | t: [ -0.11007381, 0.00039912, -0.0008537 ] 16 | images_subfolder_l: cam0/data/ 17 | images_subfolder_r: cam1/data/ -------------------------------------------------------------------------------- /config/gazebo_params.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | Kl: [554.25626, 554.25626, 320, 240] 3 | Kr: [554.25626, 554.25626, 320, 240] 4 | Dl: [0, 0, 0, 0] 5 | Dr: [0, 0, 0, 0] 6 | Rl: [1, 0, 0, 0, 1, 0, 0, 0, 1] 7 | Rr: [1, 0, 0, 0, 1, 0, 0, 0, 1] 8 | cam_bl: 0.1 9 | cam_height: 480 10 | cam_model: Pinhole 11 | cam_width: 640 12 | R: [ 1, 0, 0, 13 | 0, 1, 0, 14 | 0, 0, 1 ] 15 | t: [ -0.1, 0, 0 ] 16 | images_subfolder_l: cam0/data/ 17 | images_subfolder_r: cam1/data/ -------------------------------------------------------------------------------- /config/kitti/kitti00-02.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 607.1928 3 | cam_cy: 185.2157 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 718.856 9 | cam_fy: 718.856 10 | cam_bl: 0.537165719 11 | cam_height: 376 12 | cam_model: Pinhole 13 | cam_width: 1241 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/kitti/kitti03.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 609.5593 3 | cam_cy: 172.854 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 721.5377 9 | cam_fy: 721.5377 10 | cam_bl: 0.537150588 11 | cam_height: 375 12 | cam_model: Pinhole 13 | cam_width: 1242 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/kitti/kitti04-10.yaml: -------------------------------------------------------------------------------- 1 | cam0: 2 | cam_cx: 601.8873 3 | cam_cy: 183.1104 4 | cam_d0: 0.0 5 | cam_d1: 0.0 6 | cam_d2: 0.0 7 | cam_d3: 0.0 8 | cam_fx: 707.0912 9 | cam_fy: 707.0912 10 | cam_bl: 0.537150653 11 | cam_height: 370 12 | cam_model: Pinhole 13 | cam_width: 1226 14 | rx: 0.0 15 | ry: 0.0 16 | rz: 0.0 17 | tx: 0.0 18 | ty: 0.0 19 | tz: 0.0 20 | images_subfolder_l: image_2/ 21 | images_subfolder_r: image_3/ 22 | -------------------------------------------------------------------------------- /config/scene_config.ini: -------------------------------------------------------------------------------- 1 | [Scene] 2 | hasGT = true 3 | hasComparison = false 4 | hasText = true 5 | hasAxes = true 6 | hasLegend = false 7 | hasHelp = false 8 | hasImg = true 9 | hasLines = true 10 | hasPoints = true 11 | isKitti = true 12 | sazim = -90.0 13 | selev = 0.0 14 | szoom = 200.0 15 | sfrust = 0.5 16 | -------------------------------------------------------------------------------- /config/scene_config_indoor.ini: -------------------------------------------------------------------------------- 1 | [Scene] 2 | hasGT = true 3 | hasComparison = false 4 | hasText = true 5 | hasAxes = true 6 | hasLegend = false 7 | hasHelp = false 8 | hasImg = true 9 | hasLines = true 10 | hasPoints = true 11 | isKitti = false 12 | szoom = 10.0 13 | sfrust = 0.06 14 | sbb = 1.0 15 | -------------------------------------------------------------------------------- /include/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see . 19 | */ 20 | 21 | #ifndef ORBEXTRACTOR_H 22 | #define ORBEXTRACTOR_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | 29 | namespace ORB_SLAM2 30 | { 31 | 32 | class ExtractorNode 33 | { 34 | public: 35 | ExtractorNode():bNoMore(false){} 36 | 37 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 38 | 39 | std::vector vKeys; 40 | cv::Point2i UL, UR, BL, BR; 41 | std::list::iterator lit; 42 | bool bNoMore; 43 | }; 44 | 45 | class ORBextractor 46 | { 47 | public: 48 | 49 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 50 | 51 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 52 | int iniThFAST, int minThFAST); 53 | 54 | ~ORBextractor(){} 55 | 56 | // Compute the ORB features and descriptors on an image. 57 | // ORB are dispersed on the image using an octree. 58 | // Mask is ignored in the current implementation. 59 | void operator()( cv::InputArray image, cv::InputArray mask, 60 | std::vector& keypoints, 61 | cv::OutputArray descriptors); 62 | 63 | int inline GetLevels(){ 64 | return nlevels;} 65 | 66 | float inline GetScaleFactor(){ 67 | return scaleFactor;} 68 | 69 | std::vector inline GetScaleFactors(){ 70 | return mvScaleFactor; 71 | } 72 | 73 | std::vector inline GetInverseScaleFactors(){ 74 | return mvInvScaleFactor; 75 | } 76 | 77 | std::vector inline GetScaleSigmaSquares(){ 78 | return mvLevelSigma2; 79 | } 80 | 81 | std::vector inline GetInverseScaleSigmaSquares(){ 82 | return mvInvLevelSigma2; 83 | } 84 | 85 | std::vector mvImagePyramid; 86 | 87 | void inline SetFASTThres(int thFast_) { 88 | iniThFAST = thFast_; 89 | } 90 | 91 | protected: 92 | 93 | void ComputePyramid(cv::Mat image); 94 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 95 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 96 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 97 | 98 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 99 | std::vector pattern; 100 | 101 | int nfeatures; 102 | double scaleFactor; 103 | int nlevels; 104 | int iniThFAST; 105 | int minThFAST; 106 | 107 | std::vector mnFeaturesPerLevel; 108 | 109 | std::vector umax; 110 | 111 | std::vector mvScaleFactor; 112 | std::vector mvInvScaleFactor; 113 | std::vector mvLevelSigma2; 114 | std::vector mvInvLevelSigma2; 115 | }; 116 | 117 | } //namespace ORB_SLAM 118 | 119 | #endif 120 | 121 | -------------------------------------------------------------------------------- /include/auxiliar.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | using namespace std; 27 | 28 | #include 29 | #include 30 | #include 31 | using namespace Eigen; 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | using namespace cv; 40 | using namespace line_descriptor; 41 | 42 | 43 | typedef Matrix Vector6d; 44 | typedef Matrix Matrix6d; 45 | 46 | Eigen::Matrix toMatrix3d(const cv::Mat &cvMat3); 47 | std::vector toQuaternion(const Matrix3d &M); 48 | 49 | //void CopyCvMatToMatX(const cv::Mat &A, cv::Matx44f &B); 50 | //void CopyMatXToCvMat(const cv::Matx44f &A, cv::Mat &B); 51 | //void CopyMatXToEigen(const cv::Matx44f &A, Matrix4d &B); 52 | void CopyEigenToMatX(const Matrix3d &A, cv::Matx33d &B); 53 | void CopyEigenToAffine(const Matrix4d &A, cv::Affine3f &B); 54 | 55 | cv::RotatedRect getErrorEllipse(const double chisquare_val, 56 | const cv::Point2f mean, 57 | const cv::Mat covmat); 58 | 59 | 60 | //template 61 | //_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, 62 | // double epsilon = std::numeric_limits::epsilon()); 63 | 64 | // Kinematics functions 65 | Matrix3d skew(Vector3d v); 66 | Matrix3d fast_skewexp(Vector3d v); 67 | Vector3d skewcoords(Matrix3d M); 68 | Matrix3d skewlog(Matrix3d M); 69 | MatrixXd kroen_product(MatrixXd A, MatrixXd B); 70 | Matrix3d v_logmap(VectorXd x); 71 | MatrixXd diagonalMatrix(MatrixXd M, unsigned int N); 72 | 73 | 74 | Matrix4d inverse_se3(Matrix4d T); 75 | Matrix4d expmap_se3(Vector6d x); 76 | Vector6d logmap_se3(Matrix4d T); 77 | Matrix6d adjoint_se3(Matrix4d T); 78 | Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ); 79 | Matrix6d unccomp_se3(Matrix4d T1, Matrix6d covT1, Matrix6d covTinc ); 80 | Vector6d reverse_se3(Vector6d x); 81 | 82 | 83 | Vector3d logarithm_map_so3(Matrix3d R); 84 | MatrixXd der_logarithm_map(Matrix4d T); 85 | MatrixXd der_logarithm_map_appr(Matrix4d T, double delta); 86 | double diffManifoldError(Matrix4d T1, Matrix4d T2); 87 | bool is_finite(const MatrixXd x); 88 | bool is_nan(const MatrixXd x); 89 | double angDiff(double alpha, double beta); 90 | double angDiff_d(double alpha, double beta); 91 | 92 | // Auxiliar functions and structs for vectors 93 | double vector_stdv_mad( VectorXf residues); 94 | double vector_stdv_mad( vector residues); 95 | double vector_stdv_mad_nozero( vector residues); 96 | double vector_mean(vector v); 97 | double vector_stdv(vector v); 98 | double vector_stdv(vector v, double v_mean); 99 | 100 | struct compare_descriptor_by_NN_dist 101 | { 102 | inline bool operator()(const vector& a, const vector& b){ 103 | return ( a[0].distance < b[0].distance ); 104 | } 105 | }; 106 | 107 | struct compare_descriptor_by_NN12_dist 108 | { 109 | inline bool operator()(const vector& a, const vector& b){ 110 | return ( a[1].distance - a[0].distance > b[1].distance-b[0].distance ); 111 | } 112 | }; 113 | 114 | struct compare_descriptor_by_NN12_ratio 115 | { 116 | inline bool operator()(const vector& a, const vector& b){ 117 | return ( a[0].distance / a[1].distance > b[0].distance / b[1].distance ); 118 | } 119 | }; 120 | 121 | struct sort_descriptor_by_queryIdx 122 | { 123 | inline bool operator()(const vector& a, const vector& b){ 124 | return ( a[0].queryIdx < b[0].queryIdx ); 125 | } 126 | }; 127 | 128 | struct sort_descriptor_by_2nd_queryIdx 129 | { 130 | inline bool operator()(const vector& a, const vector& b){ 131 | return ( a[1].queryIdx < b[1].queryIdx ); 132 | } 133 | }; 134 | 135 | struct sort_descriptor_by_trainIdx 136 | { 137 | inline bool operator()(const vector& a, const vector& b){ 138 | return ( a[0].trainIdx < b[0].trainIdx ); 139 | } 140 | }; 141 | 142 | struct sort_confmat_by_score 143 | { 144 | inline bool operator()(const Vector2d& a, const Vector2d& b){ 145 | return ( a(1) > b(1) ); 146 | } 147 | }; 148 | 149 | struct sort_lines_by_response 150 | { 151 | inline bool operator()(const KeyLine& a, const KeyLine& b){ 152 | return ( a.response > b.response ); 153 | } 154 | }; 155 | 156 | struct sort_lines_by_length 157 | { 158 | inline bool operator()(const KeyLine& a, const KeyLine& b){ 159 | return ( a.lineLength > b.lineLength ); 160 | } 161 | }; 162 | 163 | -------------------------------------------------------------------------------- /include/keyFrame.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | #include 24 | #include 25 | #include 26 | 27 | #include "../3rdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 28 | #include "../3rdparty/DBoW2/DBoW2/FORB.h" 29 | #include "../3rdparty/DBoW2/DBoW2/BowVector.h" 30 | #include "../3rdparty/DBoW2/DBoW2/FClass.h" 31 | #include "../3rdparty/DBoW2/DBoW2/FeatureVector.h" 32 | #include "../3rdparty/DBoW2/DBoW2/ScoringObject.h" 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | using namespace cv; 39 | using namespace std; 40 | using namespace Eigen; 41 | using namespace StVO; 42 | 43 | typedef Matrix Vector6d; 44 | typedef Matrix Matrix6d; 45 | 46 | namespace PLSLAM{ 47 | 48 | class KeyFrame 49 | { 50 | 51 | public: 52 | 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 54 | 55 | KeyFrame(){}; 56 | KeyFrame( const StVO::StereoFrame* sf ); 57 | KeyFrame( const StVO::StereoFrame* sf, int kf_idx_ ); 58 | ~KeyFrame(){}; 59 | 60 | Mat plotKeyFrame(); 61 | 62 | bool local; 63 | 64 | int kf_idx; 65 | Matrix4d T_kf_w; 66 | Vector6d x_kf_w; 67 | Matrix6d xcov_kf_w; 68 | 69 | DBoW2::BowVector descDBoW_P, descDBoW_L; 70 | 71 | StVO::StereoFrame* stereo_frame; 72 | 73 | 74 | 75 | }; 76 | 77 | } 78 | -------------------------------------------------------------------------------- /include/linespec.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | using namespace std; 5 | 6 | #include 7 | #include 8 | using namespace Eigen; 9 | 10 | 11 | double get3DLineIntersection(const Vector3d line_1_sp, const Vector3d line_1_ep, 12 | const Vector3d line_2_sp, const Vector3d line_2_ep, 13 | Vector3d &ipt_1, Vector3d &ipt_2); 14 | 15 | void point2LineJacobian(const Matrix & rigframe_T_world, 16 | const Matrix & camera_T_rigframe, 17 | const Vector3d & lz, 18 | const Vector3d & pt3D, 19 | Matrix & jac_p2l); 20 | 21 | void pointPair2LineJacobian(const Matrix & rigframe_T_world, 22 | const Matrix & camera_T_rigframe, 23 | const Vector3d & lz, 24 | const Vector3d & spt3D, 25 | const Vector3d & ept3D, 26 | Matrix & jac_spt2l, 27 | Matrix & jac_ept2l); 28 | 29 | double getDiagonalProduct(const Matrix & mat66); 30 | 31 | double getDiagonalNorm(const Matrix & mat66); 32 | 33 | void getEigenValue(const Matrix & mat66, 34 | vector& ev); 35 | 36 | double getMinEigenValue(const Matrix & mat66); 37 | 38 | double getLogVolume(const Matrix & mat66); 39 | 40 | 41 | // set use_cholesky if M is symmetric - it's faster and more stable 42 | // for dep paring it won't be 43 | template 44 | inline typename MatrixType::Scalar logdet(const MatrixType& M) { 45 | using namespace Eigen; 46 | using std::log; 47 | typedef typename MatrixType::Scalar Scalar; 48 | Scalar ld = 0; 49 | LLT> chol(M); 50 | // auto& U = chol.matrixL(); 51 | // for (unsigned i = 0; i < M.rows(); ++i) 52 | // ld += log(U(i,i)); 53 | // ld *= 2; 54 | ld = 2 * chol.matrixL().toDenseMatrix().diagonal().array().log().sum(); 55 | return ld; 56 | } 57 | //template 58 | //inline typename MatrixType::Scalar logdet(const MatrixType& M, bool use_cholesky = false) { 59 | // using namespace Eigen; 60 | // using std::log; 61 | // typedef typename MatrixType::Scalar Scalar; 62 | // Scalar ld = 0; 63 | // if (use_cholesky) { 64 | // LLT> chol(M); 65 | // // auto& U = chol.matrixL(); 66 | // // for (unsigned i = 0; i < M.rows(); ++i) 67 | // // ld += log(U(i,i)); 68 | // // ld *= 2; 69 | // ld = 2 * chol.matrixL().toDenseMatrix().diagonal().array().log().sum(); 70 | // } else { 71 | // PartialPivLU> lu(M); 72 | // auto& LU = lu.matrixLU(); 73 | // Scalar c = lu.permutationP().determinant(); // -1 or 1 74 | // for (unsigned i = 0; i < LU.rows(); ++i) { 75 | // const auto& lii = LU(i,i); 76 | // if (lii < Scalar(0)) c *= -1; 77 | // ld += log(abs(lii)); 78 | // } 79 | // ld += log(c); 80 | // } 81 | // return ld; 82 | //} 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /include/mapFeatures.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | using namespace cv; 30 | using namespace std; 31 | using namespace Eigen; 32 | 33 | 34 | typedef Matrix Vector5i; 35 | typedef Matrix Vector6d; 36 | typedef Matrix Matrix6d; 37 | 38 | namespace PLSLAM{ 39 | 40 | class MapPoint 41 | { 42 | 43 | public: 44 | 45 | MapPoint(){}; 46 | MapPoint(int idx_, Vector3d point3D_, Mat desc_, int kf_obs_, Vector2d obs_, Vector3d dir_, double sigma2_ = 1.f); 47 | ~MapPoint(){}; 48 | 49 | void addMapPointObservation(Mat desc_, int kf_obs_, Vector2d obs_, Vector3d dir_, double sigma2_ = 1.f); 50 | void updateAverageDescDir(); 51 | 52 | int idx; 53 | 54 | bool inlier; 55 | bool local; 56 | Vector3d point3D; 57 | Vector3d med_obs_dir; 58 | Mat med_desc; 59 | 60 | vector desc_list; // list with the descriptor of each observation 61 | vector obs_list; // list with the coordinates of each observation 62 | vector dir_list; // list with the direction unit vector of each observation 63 | vector kf_obs_list; // list with KF index where the feature has been observed 64 | vector sigma_list; // list with the sigma scale of each observation 65 | 66 | }; 67 | 68 | class MapLine 69 | { 70 | 71 | public: 72 | 73 | MapLine(){}; 74 | MapLine(int idx_, Vector6d line3D_, Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_ = 1.f); 75 | ~MapLine(){}; 76 | 77 | void addMapLineObservation(Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_ = 1.f); 78 | void updateAverageDescDir(); 79 | 80 | int idx; 81 | 82 | bool inlier; 83 | bool local; 84 | Vector6d line3D; // 3D endpoints of the line segment 85 | Vector3d med_obs_dir; 86 | Mat med_desc; 87 | 88 | vector desc_list; // list with the descriptor of each observation 89 | vector obs_list; // list with the coordinates of each observation ( 2D line equation, normalized by sqrt(lx2+ly2) ) 90 | vector pts_list; // list with the coordinates of each endpoint (four coordinates) 91 | vector dir_list; // list with the direction unit vector of each observation (middle point) 92 | vector kf_obs_list; // list with KF index where the feature has been observed 93 | vector sigma_list; // list with the sigma scale of each observation 94 | 95 | }; 96 | 97 | } 98 | -------------------------------------------------------------------------------- /include/mapHandler.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include "../3rdparty/DBoW2/DBoW2/TemplatedVocabulary.h" 45 | #include "../3rdparty/DBoW2/DBoW2/FORB.h" 46 | #include "../3rdparty/DBoW2/DBoW2/BowVector.h" 47 | #include "../3rdparty/DBoW2/DBoW2/FClass.h" 48 | #include "../3rdparty/DBoW2/DBoW2/FeatureVector.h" 49 | #include "../3rdparty/DBoW2/DBoW2/ScoringObject.h" 50 | 51 | #ifdef HAS_MRPT 52 | #include 53 | #endif 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | 60 | using namespace std; 61 | using namespace Eigen; 62 | using namespace DBoW2; 63 | 64 | typedef Matrix Vector6i; 65 | typedef Matrix Vector6f; 66 | typedef Matrix Matrix6f; 67 | typedef Matrix Vector7f; 68 | typedef DBoW2::TemplatedVocabulary Vocabulary; 69 | 70 | namespace PLSLAM 71 | { 72 | 73 | class MapHandler 74 | { 75 | 76 | public: 77 | 78 | MapHandler(PinholeStereoCamera* cam_); 79 | ~MapHandler(){}; 80 | 81 | void initialize(KeyFrame* kf0); 82 | void finishSLAM(); 83 | void addKeyFrame(KeyFrame *curr_kf); 84 | 85 | void addKeyFrame_multiThread(KeyFrame *curr_kf); 86 | void loopClosureThread(); 87 | void localMappingThread(); 88 | 89 | void lookForCommonMatches(KeyFrame *kf0, KeyFrame *&kf1); 90 | void expandGraphs(); 91 | void formLocalMap(); 92 | void formLocalMap_old(); 93 | void fuseLandmarksFromLocalMap(); 94 | void removeBadMapLandmarks(); 95 | void removeRedundantKFs(); 96 | void loopClosure(); 97 | bool lookForLoopCandidates(int kf_idx_curr, int &kf_idx_prev); 98 | void insertKFBowVectorP(KeyFrame *&kf); 99 | void insertKFBowVectorL(KeyFrame *&kf); 100 | void insertKFBowVectorPL(KeyFrame *&kf); 101 | bool isLoopClosure(KeyFrame* kf0, KeyFrame* kf1, Vector6d &pose_inc, 102 | vector &lc_pt_idx, vector &lc_ls_idx, 103 | vector &lc_points, vector &lc_lines); 104 | bool computeRelativePoseGN( vector &lc_points, vector &lc_lines, 105 | vector &lc_pt_idx, vector &lc_ls_idx, 106 | Vector6d &pose_inc ); 107 | bool computeRelativePoseRobustGN( vector &lc_points, vector &lc_lines, 108 | vector &lc_pt_idx, vector &lc_ls_idx, 109 | Vector6d &pose_inc ); 110 | bool loopClosureOptimizationEssGraphG2O(); 111 | bool loopClosureOptimizationCovGraphG2O(); 112 | void loopClosureFuseLandmarks(); 113 | 114 | void localBundleAdjustment(); 115 | void levMarquardtOptimizationLBA( vector X_aux, vector kf_list, vector pt_list, vector ls_list, vector pt_obs_list, vector ls_obs_list ); 116 | 117 | void globalBundleAdjustment(); 118 | void levMarquardtOptimizationGBA( vector X_aux, vector kf_list, vector pt_list, vector ls_list, vector pt_obs_list, vector ls_obs_list ); 119 | 120 | 121 | 122 | PinholeStereoCamera* cam; 123 | 124 | vector map_keyframes; 125 | vector map_points; 126 | vector map_lines; 127 | 128 | map> map_points_kf_idx; // base KF list from which the LM is observed 129 | map> map_lines_kf_idx; 130 | 131 | vector< vector > full_graph; 132 | 133 | vector< vector > conf_matrix; 134 | Vocabulary dbow_voc_p, dbow_voc_l; 135 | 136 | unsigned int max_pt_idx, max_ls_idx, max_kf_idx ; 137 | 138 | // experiment variables 139 | Vector7f time; 140 | #ifdef HAS_MRPT 141 | mrpt::utils::CTicTac clock; 142 | #endif 143 | // lba variables 144 | vector lba_kfs; 145 | 146 | // lc variables 147 | enum LCStatus{ 148 | LC_IDLE, 149 | LC_ACTIVE, 150 | LC_READY 151 | }; 152 | LCStatus lc_status; 153 | vector< Vector3i > lc_idxs, lc_idx_list; 154 | vector< Vector6d > lc_poses, lc_pose_list; 155 | vector< vector > lc_pt_idxs; 156 | vector< vector > lc_ls_idxs; 157 | }; 158 | 159 | } 160 | -------------------------------------------------------------------------------- /include/pinholeStereoCamera.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | 24 | using namespace std; 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | using namespace cv; 31 | using namespace line_descriptor; 32 | 33 | #include 34 | using namespace Eigen; 35 | 36 | // Pinhole model for a Stereo Camera in an ideal configuration (horizontal) 37 | class PinholeStereoCamera 38 | { 39 | 40 | public: 41 | 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | 44 | PinholeStereoCamera( const int & width_, const int & height_, 45 | const double & fx_, const double & fy_, const double & cx_, const double & cy_, 46 | const double & b_, 47 | const double & d0 = 0.0, const double & d1 = 0.0, const double & d2 = 0.0, 48 | const double & d3 = 0.0, const double & d4 = 0.0); 49 | PinholeStereoCamera( const int & width_, const int & height_, 50 | const double & fx_, const double & fy_, const double & cx_, const double & cy_, 51 | const double & b_, 52 | const Mat & Rl, const Mat & Rr, 53 | const double & d0 = 0.0, const double & d1 = 0.0, const double & d2 = 0.0, 54 | const double & d3 = 0.0, const double & d4 = 0.0); 55 | //PinholeStereoCamera(int width_, int height_, double b_, Mat Kl_, Mat Kr_, Mat Rl_, Mat Rr_, Mat Dl_, Mat Dr_, bool equi ); 56 | PinholeStereoCamera( const int & width_, const int & height_, 57 | const double & b_, 58 | const Mat & Kl_, const Mat & Kr_, 59 | const Mat & R_, const Mat & t_, 60 | const Mat & Dl_, const Mat & Dr_, 61 | const bool & equi); 62 | 63 | ~PinholeStereoCamera(); 64 | 65 | // Image rectification 66 | void rectifyImage( const Mat& img_src, Mat& img_rec); 67 | void rectifyImagesLR( const Mat& img_src_l, Mat& img_rec_l, const Mat& img_src_r, Mat& img_rec_r ); 68 | 69 | // Proyection and Back-projection 70 | Vector3d backProjection_unit(const double &u, const double &v, const double &disp, double &depth); 71 | Vector3d backProjection(const double &u, const double &v, const double &disp); 72 | Vector2d projection(const Vector3d & P); 73 | Vector3d projectionNH(const Vector3d & P); 74 | Vector2d nonHomogeneous(const Vector3d & x); 75 | 76 | Vector2d normalizedCoordinates(const Vector2d & P ); 77 | double getDisparity( const double pZ ); 78 | Vector2d projectionNormalized(const Vector3d & P ); 79 | Vector2d projectionDistorted(const Vector3d & P, const double & K1, const double & K2, const double & K3 ); 80 | 81 | // Getters 82 | inline const int getWidth() const { return width; }; 83 | inline const int getHeight() const { return height; }; 84 | inline const Matrix3d& getK() const { return K; }; 85 | inline const double getB() const { return b; }; 86 | inline const Matrix getD() const { return d; }; 87 | inline const double getFx() const { return fx; }; 88 | inline const double getFy() const { return fy; }; 89 | inline const double getCx() const { return cx; }; 90 | inline const double getCy() const { return cy; }; 91 | // 92 | inline const cv::Mat getKl() const { return Kl; }; 93 | 94 | private: 95 | const int width, height; 96 | double fx, fy; 97 | double cx, cy; 98 | const double b; 99 | Matrix3d K; 100 | bool dist; 101 | Matrix d; 102 | Mat Kl, Kr, Dl, Dr, Rl, Rr, Pl, Pr, R, t, Q; 103 | Mat undistmap1l, undistmap2l, undistmap1r, undistmap2r; 104 | 105 | }; 106 | 107 | -------------------------------------------------------------------------------- /include/slamScene.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | using namespace std; 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | using namespace mrpt; 31 | using namespace mrpt::gui; 32 | using namespace mrpt::poses; 33 | using namespace mrpt::utils; 34 | using namespace mrpt::math; 35 | using namespace mrpt::opengl; 36 | 37 | #include 38 | using namespace cv; 39 | 40 | #include 41 | using namespace Eigen; 42 | 43 | #include 44 | 45 | namespace PLSLAM{ 46 | 47 | class slamScene{ 48 | 49 | public: 50 | 51 | slamScene(); 52 | slamScene(string configFile); 53 | ~slamScene(); 54 | 55 | void initializeScene(Matrix4d x_0); 56 | void initViewports(int W, int H); 57 | bool updateScene(); 58 | bool updateScene(const MapHandler* map); 59 | void updateSceneGraphs(const MapHandler* map); 60 | 61 | void setText(int frame_, float time_, int nPoints_, int nPointsH_, int nLines_, int nLinesH_); 62 | void setText(int frame_, float time_, int nPoints_, int nLines_ ); 63 | void setPose(Matrix4d x_); 64 | void setGT(Matrix4d xgt_); 65 | void setComparison(Matrix4d xcomp_); 66 | void setImage(Mat image_); 67 | void setImage(string image_); 68 | void setLegend(); 69 | void setHelp(); 70 | void setPoints(CMatrixFloat pData_); 71 | void setLines(CMatrixFloat lData_); 72 | void setStereoCalibration(Matrix3d K_, float b_); 73 | void setKF(Matrix4d Tfw); 74 | 75 | bool waitUntilClose(); 76 | bool isOpen(); 77 | bool getYPR(float &yaw, float &pitch, float &roll); 78 | bool getPose(Matrix4d &T); 79 | 80 | //private: 81 | 82 | CMatrixDouble getPoseFormat(Matrix4d T); 83 | CMatrixDouble33 getCovFormat(MatrixXd cov_); 84 | CPose3D getPoseXYZ(VectorXd x); 85 | 86 | CDisplayWindow3D* win; 87 | COpenGLScenePtr theScene; 88 | COpenGLViewportPtr image, legend, help; 89 | opengl::CSetOfObjectsPtr bbObj, bbObj1, srefObj, srefObj1, gtObj, srefObjGT, elliObjL, elliObjP; 90 | opengl::CEllipsoidPtr elliObj; 91 | opengl::CFrustumPtr frustObj, frustObj1; 92 | opengl::CAxisPtr axesObj; 93 | 94 | opengl::CSetOfLinesPtr lineObj, lineObj_local, kfsLinesObj; 95 | opengl::CPointCloudPtr pointObj, pointObj_local; 96 | opengl::CSetOfObjectsPtr kfsObj; 97 | 98 | 99 | float sbb, saxis, srad, sref, sline, sfreq, szoom, selli, selev, sazim, sfrust, slinef; 100 | CVectorDouble v_aux, v_aux_, v_aux1, v_aux1_, v_auxgt, gt_aux_, v_auxgt_; 101 | CPose3D pose, pose_0, pose_gt, pose_ini, ellPose, pose1, change, frustumL_, frustumR_; 102 | Matrix4d x_ini; 103 | mrptKeyModifier kmods; 104 | int key; 105 | CMatrixDouble33 cov3D; 106 | bool hasText, hasCov, hasGT, hasImg, hasLines, hasPoints, hasFrustum, hasComparison, hasLegend, hasHelp, hasAxes, hasTraj, isKitti; 107 | 108 | Matrix4d x, xgt, xcomp; 109 | MatrixXd cov, W; 110 | unsigned int frame, nPoints, nPointsH, nLines, nLinesH; 111 | float time; 112 | string img, img_legend, img_help; 113 | CMatrixFloat lData, pData; 114 | CImage img_mrpt_legend, img_mrpt_image, img_mrpt_help; 115 | 116 | float b, sigmaP, sigmaL, f, cx, cy, bsigmaL, bsigmaP; 117 | 118 | }; 119 | 120 | } 121 | -------------------------------------------------------------------------------- /include/stereoFeatures.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | using namespace std; 27 | 28 | #include 29 | #include 30 | using namespace Eigen; 31 | 32 | #include 33 | 34 | namespace StVO{ 35 | 36 | class PointFeature 37 | { 38 | 39 | public: 40 | 41 | PointFeature( const Vector3d & P_, const Vector2d & pl_obs_); 42 | PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_ ); 43 | PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_, 44 | const int & idx_ ); 45 | PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_, 46 | const int & idx_, const int & level_ ); 47 | PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_, 48 | const Vector2d & pl_obs_ ); 49 | ~PointFeature(){}; 50 | 51 | int idx; 52 | Vector2d pl, pl_obs; 53 | double disp; 54 | Vector3d P; 55 | bool inlier; 56 | 57 | int level; 58 | double sigma2 = 1.0; 59 | 60 | bool frame_matched = false; 61 | 62 | }; 63 | 64 | class LineFeature 65 | { 66 | 67 | public: 68 | 69 | LineFeature( const Vector3d & sP_, const Vector3d & eP_, const Vector3d & le_obs_); 70 | 71 | LineFeature( const Vector3d & sP_, const Vector3d & eP_, const Vector3d & le_obs_, 72 | const Vector2d & spl_obs_, const Vector2d & epl_obs_); 73 | 74 | LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 75 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 76 | const Vector3d & le_); 77 | 78 | LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 79 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 80 | const Vector3d & le_, const Vector3d & le_obs_); 81 | 82 | LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 83 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 84 | const Vector3d & le_, const int & idx_); 85 | 86 | LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 87 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 88 | const Vector3d & le_, const double & angle_, const int & idx_); 89 | 90 | LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 91 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 92 | const Vector3d & le_, const double & angle_, const int & idx_, const int & level_); 93 | 94 | ~LineFeature(){}; 95 | 96 | int idx; 97 | Vector2d spl, epl, spl_obs, epl_obs; 98 | double sdisp, edisp, angle, sdisp_obs, edisp_obs; 99 | Vector3d sP,eP; 100 | Vector3d le, le_obs; 101 | bool inlier; 102 | 103 | int level; 104 | double sigma2 = 1.0; 105 | 106 | cv::Scalar color; 107 | 108 | bool frame_matched = false; 109 | 110 | // NOTE 111 | // for uncertainty modeling only 112 | Matrix3d covSpt_proj2D, covEpt_proj2D; 113 | Matrix3d covSpt3D, covEpt3D; 114 | // 115 | double cutRatio[2]; 116 | vector volumeArr; 117 | // 118 | vector displPt2D; 119 | vector lPt2D; 120 | vector covlPt2D; 121 | vector varLoss2D; 122 | Matrix invCovPose; 123 | int fst_idx, lst_idx; 124 | 125 | // NOTE 126 | // for error assessment in synthetic data only 127 | Vector3d sPt_err, ePt_err; 128 | double sdisp_err, edisp_err; 129 | bool valid_eval; 130 | }; 131 | 132 | } 133 | -------------------------------------------------------------------------------- /include/stereoFrameHandler.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of GF-PL-SLAM. 3 | * 4 | * Copyright (C) 2019 Yipu Zhao 5 | * (Georgia Institute of Technology) 6 | * For more information see 7 | * 8 | * 9 | * GF-PL-SLAM is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * GF-PL-SLAM is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with GF-PL-SLAM. If not, see . 21 | */ 22 | 23 | /***************************************************************************** 24 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 25 | ****************************************************************************** 26 | ** ** 27 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 28 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 29 | ** ** 30 | ** This program is free software: you can redistribute it and/or modify ** 31 | ** it under the terms of the GNU General Public License (version 3) as ** 32 | ** published by the Free Software Foundation. ** 33 | ** ** 34 | ** This program is distributed in the hope that it will be useful, but ** 35 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 36 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 37 | ** GNU General Public License for more details. ** 38 | ** ** 39 | ** You should have received a copy of the GNU General Public License ** 40 | ** along with this program. If not, see . ** 41 | ** ** 42 | *****************************************************************************/ 43 | 44 | #pragma once 45 | #include 46 | #include 47 | 48 | typedef Matrix Matrix6d; 49 | typedef Matrix Vector6d; 50 | 51 | 52 | // enable visualization modules of PL-SLAM 53 | #define DO_VIZ 54 | 55 | // apply box filter to input image, to simulate motion blur 56 | // #define SIMU_MOTION_BLUR 57 | 58 | 59 | class StereoFrame; 60 | 61 | namespace StVO { 62 | 63 | class StereoFrameHandler 64 | { 65 | 66 | public: 67 | 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 69 | 70 | StereoFrameHandler( PinholeStereoCamera* cam_ ); 71 | ~StereoFrameHandler(); 72 | 73 | void initialize( const Mat & img_l_, const Mat & img_r_, 74 | const int idx_, const double time_stamp_); 75 | void updateFrame(); 76 | void updateFrame_ECCV18( const Matrix4d T_base ); 77 | void insertStereoPair(const Mat & img_l_, const Mat & img_r_, 78 | const int idx_, const double time_stamp_); 79 | // 80 | void crossFrameMatching_Brute(); 81 | void crossFrameMatching_Hybrid(); 82 | void crossFrameMatching_Proj(); 83 | // 84 | void optimizePose(); 85 | void optimizePose(Matrix4d DT_ini); 86 | double lineSegmentOverlap( Vector2d spl_obs, Vector2d epl_obs, 87 | Vector2d spl_proj, Vector2d epl_proj ); 88 | 89 | // 90 | /* parameter setup / update */ 91 | void setLeftCamExtrinCalib(const double cam_rx, 92 | const double cam_ry, 93 | const double cam_rz, 94 | const double cam_tx, 95 | const double cam_ty, 96 | const double cam_tz); 97 | void predictFramePose(); 98 | void setFramePose(const cv::Matx44f Tfw_curr); 99 | // 100 | /* line uncertainty modeling */ 101 | void estimateProjUncertainty_greedy(); 102 | void estimateProjUncertainty_descent(const double stepCutRatio, 103 | const double rngCutRatio[2]); 104 | void estimateProjUncertainty_submodular(const double stepCutRatio, 105 | const double rngCutRatio[2]); 106 | // 107 | void getPoseCovMatrixOnLine(const int fst_idx, 108 | const int lst_idx, 109 | const Matrix rigframe_T_world, 110 | const list::iterator it, 111 | Matrix & cov_pose_inv); 112 | 113 | void getPoseCovOnLine(const Matrix4d DT_inv, 114 | const Vector2d J_loss, 115 | const StVO::LineFeature * line, 116 | const double cutRatio[2], 117 | Matrix & cov_pose); 118 | 119 | void getPoseInfoOnLine(const Matrix4d DT_inv, 120 | const Vector2d J_loss, 121 | const StVO::LineFeature * line, 122 | const double cutRatio[2], 123 | Matrix & info_pose); 124 | 125 | void getPoseInfoPoint(const Matrix4d DT_inv, 126 | const StVO::PointFeature * point, 127 | Matrix & info_pose); 128 | 129 | void updateEndPointByRatio(StVO::LineFeature * line); 130 | 131 | void greedySolveLineCut_P3L(const Matrix4d DT_inv, 132 | const double stepCutRatio, 133 | const double rngCutRatio[2], 134 | vector line_P3L); 135 | 136 | // 137 | /* viz functions */ 138 | void plotLine3D( bool save_screen_shot ); 139 | void plotMatchedLines( bool vizUncertainty, bool save_screen_shot ); 140 | void plotMatchedPointLine(bool save_screen_shot); 141 | void appendPlotCutLine(bool save_screen_shot); 142 | void appendPlotOptLine(bool save_screen_shot); 143 | 144 | 145 | cv::viz::Viz3d * viz_3D_lines; 146 | cv::Mat canvas_match_frames; 147 | 148 | // adaptative fast 149 | int orb_fast_th; 150 | 151 | // slam-specific functions 152 | bool needNewKF(); 153 | void currFrameIsKF(); 154 | 155 | /* variables used in cross-frame line matching */ 156 | list matched_pt; 157 | list matched_ls; 158 | 159 | StereoFrame* prev_frame; 160 | StereoFrame* curr_frame; 161 | PinholeStereoCamera* cam; 162 | 163 | ORB_SLAM2::ORBextractor * mpORBextractor_l; 164 | ORB_SLAM2::ORBextractor * mpORBextractor_r; 165 | 166 | Matrix left_cam_T_rigframe; 167 | double NUM_PTS_SAMPLE = 50.0; // 20.0; 168 | 169 | int n_inliers, n_inliers_pt, n_inliers_ls; 170 | 171 | int numFrameLoss = 0; 172 | int numFrameSinceKeyframe = 0; 173 | 174 | // slam-specific variables 175 | bool prev_f_iskf; 176 | double entropy_first_prevKF; 177 | Matrix4d T_prevKF; 178 | Matrix6d cov_prevKF_currF; 179 | 180 | // 181 | /* variables used in debug and figure generation */ 182 | bool save_screen_shot = true; // false; // 183 | bool print_frame_info = false; // true; // 184 | 185 | vector< std::pair > T_allF; 186 | 187 | private: 188 | 189 | void removeOutliers( Matrix4d DT ); 190 | void gaussNewtonOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 191 | void optimizeFunctions(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e); 192 | 193 | Vector6d prior_inc; 194 | Matrix6d prior_cov; 195 | std::vector line_projection_end; 196 | std::vector line_projection_start; 197 | 198 | }; 199 | 200 | } 201 | -------------------------------------------------------------------------------- /include/voScene.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | using namespace std; 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | using namespace mrpt; 33 | using namespace mrpt::gui; 34 | using namespace mrpt::poses; 35 | using namespace mrpt::utils; 36 | using namespace mrpt::math; 37 | using namespace mrpt::opengl; 38 | using namespace mrpt::maps; 39 | 40 | #include 41 | using namespace cv; 42 | 43 | #include 44 | using namespace Eigen; 45 | 46 | #include 47 | using namespace StVO; 48 | 49 | class voScene{ 50 | 51 | public: 52 | 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 54 | 55 | voScene(); 56 | voScene(string configFile); 57 | ~voScene(); 58 | void initialize3DScene(Matrix4d x_0); 59 | void initialize3DSceneLines(Matrix4d x_0); 60 | void initialize3DSceneImg(Matrix4d x_0); 61 | void initialize3DSceneGT(Matrix4d x_0); 62 | 63 | void initializeScene(Matrix4d x_0, bool has_gt); 64 | void initializeScene(Matrix4d x_0, Matrix4d x_0gt); 65 | 66 | bool updateScene(); 67 | bool updateScene(list matched_pt); 68 | void plotPointsCovariances(); 69 | void plotLinesCovariances(); 70 | 71 | void setText(int frame_, float time_, int nPoints_, int nPointsH_, int nLines_, int nLinesH_); 72 | void setCov(MatrixXd cov_); 73 | void setPose(Matrix4d x_); 74 | void setGT(Matrix4d xgt_); 75 | void setComparison(Matrix4d xcomp_); 76 | void setImage(Mat image_); 77 | void setImage(string image_); 78 | void setLegend(); 79 | void setHelp(); 80 | void setPoints(CMatrixFloat pData_); 81 | void setLines(CMatrixFloat lData_); 82 | void setStereoCalibration(Matrix3d K_, float b_); 83 | void setKF(); 84 | void setKF(Matrix4d Tfw); 85 | 86 | bool waitUntilClose(); 87 | bool isOpen(); 88 | bool getYPR(float &yaw, float &pitch, float &roll); 89 | bool getPose(Matrix4d &T); 90 | 91 | private: 92 | 93 | CMatrixDouble getPoseFormat(Matrix4d T); 94 | CMatrixDouble33 getCovFormat(MatrixXd cov_); 95 | CPose3D getPoseXYZ(VectorXd x); 96 | 97 | CDisplayWindow3D* win; 98 | COpenGLScenePtr theScene; 99 | COpenGLViewportPtr image, legend, help; 100 | opengl::CSetOfObjectsPtr bbObj, bbObj1, srefObj, srefObj1, gtObj, srefObjGT, elliObjL, elliObjP; 101 | opengl::CEllipsoidPtr elliObj; 102 | opengl::CSetOfLinesPtr lineObj; 103 | opengl::CPointCloudPtr pointObj; 104 | 105 | //CPointsMapPtr pointsObj; 106 | 107 | opengl::CFrustumPtr frustObj, frustObj1; 108 | opengl::CAxisPtr axesObj; 109 | 110 | 111 | float sbb, saxis, srad, sref, sline, sfreq, szoom, selli, selev, sazim, sfrust, slinef; 112 | CVectorDouble v_aux, v_aux_, v_aux1, v_aux1_, v_auxgt, gt_aux_, v_auxgt_; 113 | CPose3D pose, pose_0, pose_gt, pose_ini, ellPose, pose1, change, frustumL_, frustumR_; 114 | Matrix4d x_ini; 115 | mrptKeyModifier kmods; 116 | int key; 117 | CMatrixDouble33 cov3D; 118 | bool hasCamFix, hasText, hasCov, hasGT, hasChange, hasImg, hasLines, hasPoints, hasFrustum, hasComparison, hasLegend, hasHelp, hasAxes, hasTraj, isKitti; 119 | 120 | Matrix4d x, xgt, xcomp; 121 | MatrixXd cov, W; 122 | unsigned int frame, nPoints, nPointsH, nLines, nLinesH; 123 | float time; 124 | string img, img_legend, img_help; 125 | CMatrixFloat lData, pData; 126 | CImage img_mrpt_legend, img_mrpt_image, img_mrpt_help; 127 | 128 | float b, sigmaP, sigmaL, f, cx, cy, bsigmaL, bsigmaP; 129 | 130 | }; 131 | 132 | -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | 24 | #define PI std::acos(-1.0) 25 | 26 | Config::Config() 27 | { 28 | 29 | // SLAM parameters 30 | // ----------------------------------------------------------------------------------------------------- 31 | // track loss definition 32 | max_num_frame_loss = 10; // 40; // 1000; 33 | // kf decision 34 | min_entropy_ratio = 0.90; 35 | max_kf_num_frames = 50; // 100; 36 | min_kf_n_feats = 30; 37 | max_kf_t_dist = 2.0; 38 | max_kf_r_dist = 5.0; 39 | // lm numbers and errors 40 | min_lm_obs = 2; 41 | max_common_fts_kf = 0.8; // 0.9; // tmp change by Yipu 42 | max_kf_epip_p = 1.0; 43 | max_kf_epip_l = 1.0; 44 | max_lm_3d_err = 1.0; 45 | max_lm_dir_err = 0.5; // cos(60deg) 46 | max_point_point_error = 0.1; 47 | max_point_line_error = 0.1; 48 | max_dir_line_error = 0.1; 49 | // graphs parameters 50 | min_lm_ess_graph = 100; 51 | min_lm_cov_graph = 30; 52 | // modified by Yipu 53 | min_kf_local_map = 3; // 6; // // try increasing the size of local map for better performance 54 | // LBA 55 | lambda_lba_lm = 0.001; // (if auto, this is the initial tau) 56 | lambda_lba_k = 10.0; 57 | max_iters_lba = 20; // 40; // tmp change by Yipu 58 | // Loop closure 59 | vocabulary_p = "../vocabulary/voc_all_datasets_orb.yml"; 60 | vocabulary_l = "../vocabulary/voc_all_datasets_bld.yml"; 61 | lc_mat = 0.50; 62 | lc_res = 1.5; 63 | lc_unc = 0.01; 64 | lc_inl = 0.3; 65 | lc_trs = 1.5; 66 | lc_rot = 35.0; 67 | max_iters_pgo = 100; 68 | lc_kf_dist = 100; 69 | lc_kf_max_dist = 20; 70 | lc_nkf_closest = 4; 71 | lc_inlier_ratio = 35.0; 72 | 73 | // StVO-PL options 74 | // ----------------------------------------------------------------------------------------------------- 75 | has_points = true; // false; // // true if using points 76 | has_lines = true; // false; // // true if using line segments 77 | lr_in_parallel = true; // false; // // true if detecting and matching features in parallel 78 | pl_in_parallel = true; // false; // // true if detecting points and line segments in parallel 79 | best_lr_matches = true; // true if double-checking the matches between the two images 80 | adaptative_fast = true; // true if using adaptative fast_threshold 81 | // use_line_weight = true; // false; // // true if weight line loss term according to the level of uncertainty 82 | // 83 | use_line_conf_cut = true; // false; // // true if cut line before feeding into optimization based on the level of uncertainty 84 | ratio_disp_std = 0.15; // 0.5; // 0.3; // 85 | ratio_disp_std_hor = 0.9; // 0.3; // 86 | max_vol_line_cut = true; // false; // // true if cut line with max-vol objective 87 | // // gazebo 88 | // max_line_match_num = 300; // 80; 89 | // max_point_match_num = 1000; // 150; 90 | // euroc 91 | // try setting a much tighter budget 92 | // max_line_match_num = 100; // 50; // 100; // 300; // 30; // 93 | // max_point_match_num = 50; // 100; // 500; // 30; // 150; // 50; // 94 | max_line_match_num = 300; // 80; 95 | max_point_match_num = 500; // 1000; // 150; 96 | 97 | 98 | // Tracking parameters 99 | // ----------------------------------------------------------------------------------------------------- 100 | // Point features 101 | max_dist_epip = 2.0; // max. epipolar distance in pixels 102 | min_disp = 1.0; // min. disparity (avoid points in the infinite) 103 | max_ratio_12_p = 0.9; // 0.1; // 0.5; // 0.75; // // min. ratio between the first and second best matches 104 | point_match_radius = 50.0; // 80.0; // 120.0; // 105 | // Line segment features 106 | stereo_overlap_th = 0.5; // 0.75; 107 | min_line_length = 0.025; // 0.04; // // min. line length (relative to img size) 108 | line_horiz_th = 0.1; // parameter to avoid horizontal lines 109 | desc_th_l = 0.1; // 0.9; // // parameter to avoid outliers in line matching 110 | line_cov_th = 10.0; // parameter to remove noisy line segments 111 | line_match_radius = 80.0; 112 | // Adaptative FAST parameters 113 | fast_min_th = 10; // min. value for FAST threshold 114 | fast_max_th = 50; // max. value for FAST threshold 115 | fast_inc_th = 5; // base increment for the FAST threshold 116 | fast_feat_th = 50; // base number of features to increase/decrease FAST threshold 117 | fast_err_th = 0.5; // threshold for the optimization error 118 | 119 | // Optimization parameters 120 | // ----------------------------------------------------------------------------------------------------- 121 | homog_th = 0.0000001; // avoid points in the infinite 122 | min_features = 10; // line only 20; // other 123 | // 124 | max_iters = 5; // 10; // tmp change by Yipu // max. number of iterations in the first stage of the optimization 125 | max_iters_ref = 10; // 20; // tmp change by Yipu // max. number of iterations in the refinement stage 126 | min_error = 0.0000001; // min. error to stop the optimization 127 | min_error_change = 0.0000001; // min. error change to stop the optimization 128 | inlier_k = 2.0; // factor to discard outliers before the refinement stage 129 | motion_step_th = 10; // 1.0; // 10 m/s as max. velocity 130 | 131 | // Feature detection parameters 132 | // ----------------------------------------------------------------------------------------------------- 133 | // ORB detector 134 | orb_nfeatures = 1000; // 1200; // 135 | orb_scale_factor = 1.2; 136 | orb_nlevels = 4; // 2; // 8; // 137 | orb_edge_th = 19; 138 | orb_wta_k = 2; 139 | orb_score = 1; // 0 - HARRIS | 1 - FAST 140 | orb_patch_size = 31; 141 | orb_fast_th = 20; // default FAST threshold 142 | // LSD parameters 143 | lsd_nfeatures = 300; // set to 0 if keeping all lines 144 | lsd_refine = 1; // 2; 145 | lsd_scale = 1; // 1.2; // 2; // 146 | lsd_octave_num = 1; // 2; // 147 | lsd_sigma_scale = 0.75; // 0.6; 148 | lsd_quant = 2.0; 149 | lsd_ang_th = 22.5; 150 | lsd_log_eps = 1.0; 151 | lsd_density_th = 0.6; 152 | lsd_n_bins = 1024; 153 | 154 | } 155 | 156 | Config::~Config(){} 157 | 158 | Config& Config::getInstance() 159 | { 160 | static Config instance; // Instantiated on first use and guaranteed to be destroyed 161 | return instance; 162 | } 163 | -------------------------------------------------------------------------------- /src/keyFrame.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | 24 | namespace PLSLAM 25 | { 26 | 27 | KeyFrame::KeyFrame(const StereoFrame *sf) 28 | { 29 | kf_idx = -1; 30 | T_kf_w = sf->Tfw; 31 | x_kf_w = logmap_se3(T_kf_w); 32 | xcov_kf_w = sf->Tfw_cov; 33 | 34 | stereo_frame = new StereoFrame(sf->rgbImg_l, sf->rgbImg_r, kf_idx, sf->cam, sf->time_stamp); 35 | stereo_frame->pdesc_l = sf->pdesc_l; 36 | stereo_frame->pdesc_r = sf->pdesc_r; 37 | stereo_frame->ldesc_l = sf->ldesc_l; 38 | stereo_frame->ldesc_r = sf->ldesc_r; 39 | stereo_frame->stereo_pt = sf->stereo_pt; 40 | stereo_frame->stereo_ls = sf->stereo_ls; 41 | } 42 | 43 | KeyFrame::KeyFrame(const StereoFrame *sf, int kf_idx_) 44 | { 45 | kf_idx = kf_idx_; 46 | T_kf_w = sf->Tfw; 47 | x_kf_w = logmap_se3(T_kf_w); 48 | xcov_kf_w = sf->Tfw_cov; 49 | 50 | stereo_frame = new StereoFrame(sf->rgbImg_l, sf->rgbImg_r, kf_idx, sf->cam, sf->time_stamp); 51 | stereo_frame->pdesc_l = sf->pdesc_l; 52 | stereo_frame->pdesc_r = sf->pdesc_r; 53 | stereo_frame->ldesc_l = sf->ldesc_l; 54 | stereo_frame->ldesc_r = sf->ldesc_r; 55 | stereo_frame->stereo_pt = sf->stereo_pt; 56 | stereo_frame->stereo_ls = sf->stereo_ls; 57 | } 58 | 59 | Mat KeyFrame::plotKeyFrame() 60 | { 61 | // create new image to modify it 62 | Mat img_l_aux; 63 | stereo_frame->rgbImg_l.copyTo(img_l_aux); 64 | // Variables 65 | unsigned int r = 0, g, b = 0; 66 | Point2f p, q; 67 | double thick = 1.5; 68 | int k = 0, radius = 3; 69 | // plot point features 70 | for (vector::iterator pt_it = stereo_frame->stereo_pt.begin(); pt_it != stereo_frame->stereo_pt.end(); pt_it++) 71 | { 72 | if ((*pt_it)->idx != -1) 73 | { 74 | g = 200; 75 | p = cv::Point(int((*pt_it)->pl(0)), int((*pt_it)->pl(1))); 76 | circle(img_l_aux, p, radius, Scalar(b, g, r), thick); 77 | } 78 | } 79 | // plot line segment features 80 | for (vector::iterator ls_it = stereo_frame->stereo_ls.begin(); ls_it != stereo_frame->stereo_ls.end(); ls_it++) 81 | { 82 | if ((*ls_it)->idx != -1) 83 | { 84 | g = 200; 85 | p = cv::Point(int((*ls_it)->spl(0)), int((*ls_it)->spl(1))); 86 | q = cv::Point(int((*ls_it)->epl(0)), int((*ls_it)->epl(1))); 87 | line(img_l_aux, p, q, Scalar(b, g, r), thick); 88 | } 89 | } 90 | return img_l_aux; 91 | } 92 | 93 | } // namespace PLSLAM 94 | -------------------------------------------------------------------------------- /src/mapFeatures.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | 24 | namespace PLSLAM 25 | { 26 | 27 | // Point features 28 | 29 | MapPoint::MapPoint(int idx_, Vector3d point3D_, Mat desc_, int kf_obs_, Vector2d obs_, Vector3d dir_, double sigma2_) : idx(idx_), point3D(point3D_), inlier(true) 30 | { 31 | desc_list.push_back(desc_); 32 | obs_list.push_back(obs_); 33 | kf_obs_list.push_back(kf_obs_); 34 | dir_list.push_back(dir_); 35 | sigma_list.push_back(sigma2_); 36 | med_obs_dir = dir_; 37 | med_desc = desc_; 38 | } 39 | 40 | void MapPoint::addMapPointObservation(Mat desc_, int kf_obs_, Vector2d obs_, Vector3d dir_, double sigma2_) 41 | { 42 | desc_list.push_back(desc_); 43 | obs_list.push_back(obs_); 44 | kf_obs_list.push_back(kf_obs_); 45 | dir_list.push_back(dir_); 46 | sigma_list.push_back(sigma2_); 47 | updateAverageDescDir(); 48 | } 49 | 50 | void MapPoint::updateAverageDescDir() 51 | { 52 | 53 | // descriptor 54 | // - check distances between all the observed descriptors 55 | int n = desc_list.size(); 56 | MatrixXf conf_desc(n, n); 57 | for (int i = 0; i < n; i++) 58 | { 59 | conf_desc(i, i) = 0; 60 | for (int j = i + 1; j < n; j++) 61 | { 62 | int d = norm(desc_list[i], desc_list[j], NORM_HAMMING); 63 | conf_desc(i, j) = d; 64 | conf_desc(j, i) = d; 65 | } 66 | } 67 | 68 | // - select the one with least mean distance to the rest 69 | int max_dist = 99999; 70 | int max_idx = 0; 71 | for (int i = 0; i < n; i++) 72 | { 73 | vector dist_idx; 74 | for (int j = 0; j < n; j++) 75 | dist_idx.push_back(conf_desc(i, j)); 76 | sort(dist_idx.begin(), dist_idx.end()); 77 | int idx_median = dist_idx[int(1 + 0.5 * (n - 1))]; 78 | if (idx_median < max_dist) 79 | { 80 | max_dist = idx_median; 81 | max_idx = i; 82 | } 83 | } 84 | med_desc = desc_list[max_idx]; 85 | 86 | // direction 87 | Vector3d med_dir; 88 | for (int i = 0; i < n; i++) 89 | med_dir += dir_list[i]; 90 | med_obs_dir = med_dir / n; 91 | } 92 | 93 | // Line segment features 94 | 95 | MapLine::MapLine(int idx_, Vector6d line3D_, Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_) : idx(idx_), line3D(line3D_), inlier(true) 96 | { 97 | desc_list.push_back(desc_); 98 | obs_list.push_back(obs_); 99 | kf_obs_list.push_back(kf_obs_); 100 | dir_list.push_back(dir_); 101 | pts_list.push_back(pts_); 102 | sigma_list.push_back(sigma2_); 103 | med_obs_dir = dir_; 104 | med_desc = desc_; 105 | } 106 | 107 | void MapLine::addMapLineObservation(Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_) 108 | { 109 | desc_list.push_back(desc_); 110 | obs_list.push_back(obs_); 111 | kf_obs_list.push_back(kf_obs_); 112 | dir_list.push_back(dir_); 113 | pts_list.push_back(pts_); 114 | sigma_list.push_back(sigma2_); 115 | updateAverageDescDir(); 116 | } 117 | 118 | void MapLine::updateAverageDescDir() 119 | { 120 | 121 | // descriptor 122 | // - check distances between all the observed descriptors 123 | int n = desc_list.size(); 124 | MatrixXf conf_desc(n, n); 125 | for (int i = 0; i < n; i++) 126 | { 127 | conf_desc(i, i) = 0; 128 | for (int j = i + 1; j < n; j++) 129 | { 130 | int d = norm(desc_list[i], desc_list[j], NORM_HAMMING); 131 | conf_desc(i, j) = d; 132 | conf_desc(j, i) = d; 133 | } 134 | } 135 | 136 | // - select the one with least mean distance to the rest 137 | int max_dist = 99999; 138 | int max_idx = 0; 139 | for (int i = 0; i < n; i++) 140 | { 141 | vector dist_idx; 142 | for (int j = 0; j < n; j++) 143 | dist_idx.push_back(conf_desc(i, j)); 144 | sort(dist_idx.begin(), dist_idx.end()); 145 | int idx_median = dist_idx[int(1 + 0.5 * (n - 1))]; 146 | if (idx_median < max_dist) 147 | { 148 | max_dist = idx_median; 149 | max_idx = i; 150 | } 151 | } 152 | med_desc = desc_list[max_idx]; 153 | 154 | // direction 155 | Vector3d med_dir; 156 | for (int i = 0; i < n; i++) 157 | med_dir += dir_list[i]; 158 | med_obs_dir = med_dir / n; 159 | } 160 | 161 | } // namespace PLSLAM 162 | -------------------------------------------------------------------------------- /src/pinholeStereoCamera.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include "pinholeStereoCamera.h" 23 | 24 | PinholeStereoCamera::PinholeStereoCamera(const int &width_, const int &height_, 25 | const double &fx_, const double &fy_, const double &cx_, const double &cy_, 26 | const double &b_, 27 | const double &d0, const double &d1, const double &d2, const double &d3, const double &d4) : width(width_), height(height_), fx(fx_), fy(fy_), cx(cx_), cy(cy_), b(b_), dist(d0 != 0.0) 28 | { 29 | d << d0, d1, d2, d3, d4; 30 | Kl = (Mat_(3, 3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0); 31 | Dl = (Mat_(1, 5) << d(0), d(1), d(2), d(3), d(4)); 32 | // Pl = ( Mat_(3,4) << fx, 0.0, cx, 0.0, 0.0, fx, cy, 0.0, 0.0, 0.0, 1.0, 0.0 ); 33 | // MODIFIED by Yipu; 34 | // seems like the 2nd row of Pl is wrong ??? 35 | // not matter for now since it's not being called 36 | Pl = (Mat_(3, 4) << fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0); 37 | 38 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 39 | // initialize undistort rectify map OpenCV 40 | initUndistortRectifyMap(Kl, Dl, cv::Mat_::eye(3, 3), Pl, cv::Size(width, height), CV_16SC2, undistmap1l, undistmap2l); 41 | undistmap1r = undistmap1l; 42 | undistmap2r = undistmap2l; 43 | } 44 | 45 | PinholeStereoCamera::PinholeStereoCamera(const int &width_, const int &height_, 46 | const double &fx_, const double &fy_, const double &cx_, const double &cy_, 47 | const double &b_, 48 | const cv::Mat &Rl_, const cv::Mat &Rr_, 49 | const double &d0, const double &d1, const double &d2, const double &d3, const double &d4) : width(width_), height(height_), fx(fx_), fy(fy_), cx(cx_), cy(cy_), b(b_), dist(d0 != 0.0), Rl(Rl_), Rr(Rr_) 50 | { 51 | d << d0, d1, d2, d3, d4; 52 | Kl = (Mat_(3, 3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0); 53 | Dl = (Mat_(1, 5) << d(0), d(1), d(2), d(3), d(4)); 54 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 55 | // initialize undistort rectify map OpenCV 56 | initUndistortRectifyMap(Kl, Dl, Rl, Pl, cv::Size(width, height), CV_16SC2, undistmap1l, undistmap2l); 57 | initUndistortRectifyMap(Kr, Dr, Rr, Pr, cv::Size(width, height), CV_16SC2, undistmap1r, undistmap2r); 58 | } 59 | 60 | PinholeStereoCamera::PinholeStereoCamera(const int &width_, const int &height_, 61 | const double &b_, 62 | const Mat &Kl_, const Mat &Kr_, 63 | const Mat &R_, const Mat &t_, 64 | const Mat &Dl_, const Mat &Dr_, 65 | const bool &equi) : width(width_), height(height_), Kl(Kl_), Kr(Kr_), b(b_), R(R_), t(t_), Dl(Dl_), Dr(Dr_) 66 | { 67 | 68 | // initialize undistort rectify map OpenCV 69 | if (equi) 70 | { 71 | stereoRectify(Kl, Dl, Kr, Dr, cv::Size(width, height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0); 72 | cv::fisheye::initUndistortRectifyMap(Kl, Dl, Rl, Pl, cv::Size(width, height), CV_16SC2, undistmap1l, undistmap2l); 73 | cv::fisheye::initUndistortRectifyMap(Kr, Dr, Rr, Pr, cv::Size(width, height), CV_16SC2, undistmap1r, undistmap2r); 74 | } 75 | else 76 | { 77 | stereoRectify(Kl, Dl, Kr, Dr, cv::Size(width, height), R, t, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY, 0); 78 | initUndistortRectifyMap(Kl, Dl, Rl, Pl, cv::Size(width, height), CV_16SC2, undistmap1l, undistmap2l); 79 | initUndistortRectifyMap(Kr, Dr, Rr, Pr, cv::Size(width, height), CV_16SC2, undistmap1r, undistmap2r); 80 | } 81 | 82 | dist = true; 83 | 84 | fx = Pl.at(0, 0); 85 | fy = Pl.at(1, 1); 86 | cx = Pl.at(0, 2); 87 | cy = Pl.at(1, 2); 88 | 89 | K << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; 90 | } 91 | 92 | PinholeStereoCamera::~PinholeStereoCamera(){}; 93 | 94 | void PinholeStereoCamera::rectifyImage(const Mat &img_src, Mat &img_rec) 95 | { 96 | if (dist) 97 | remap(img_src, img_rec, undistmap1l, undistmap2l, cv::INTER_LINEAR); 98 | else 99 | img_rec = img_src.clone(); 100 | } 101 | 102 | void PinholeStereoCamera::rectifyImagesLR(const Mat &img_src_l, Mat &img_rec_l, 103 | const Mat &img_src_r, Mat &img_rec_r) 104 | { 105 | if (dist) 106 | { 107 | remap(img_src_l, img_rec_l, undistmap1l, undistmap2l, cv::INTER_LINEAR); 108 | remap(img_src_r, img_rec_r, undistmap1r, undistmap2r, cv::INTER_LINEAR); 109 | } 110 | else 111 | { 112 | img_rec_l = img_src_l.clone(); 113 | img_rec_r = img_src_r.clone(); 114 | } 115 | } 116 | 117 | // Proyection and Back-projection (internally we are supposed to work with rectified images because of the line segments) 118 | Vector3d PinholeStereoCamera::backProjection_unit(const double &u, const double &v, const double &disp, 119 | double &depth) 120 | { 121 | Vector3d P_unit; 122 | P_unit(0) = (u - cx) / fx; 123 | P_unit(1) = (v - cy) / fy; 124 | P_unit(2) = 1.0; 125 | depth = b * fx / disp; 126 | return P_unit; 127 | } 128 | 129 | Vector3d PinholeStereoCamera::backProjection(const double &u, const double &v, const double &disp) 130 | { 131 | Vector3d P; 132 | double bd = b / disp; 133 | P(0) = bd * (u - cx); 134 | P(1) = bd * (v - cy); 135 | P(2) = bd * fx; 136 | return P; 137 | } 138 | 139 | Vector2d PinholeStereoCamera::normalizedCoordinates(const Vector2d &P) 140 | { 141 | Vector3d uv_unit; 142 | uv_unit(0) = (P(0) - cx) / fx; 143 | uv_unit(1) = (P(1) - cy) / fy; 144 | uv_unit(2) = 1.0; 145 | //double sum = uv_unit(0)*uv_unit(0) + uv_unit(1)*uv_unit(1)* + uv_unit(2)*uv_unit(2); 146 | double sum = uv_unit.norm(); 147 | uv_unit(0) = uv_unit(0) / sum; 148 | uv_unit(1) = uv_unit(1) / sum; 149 | uv_unit(2) = uv_unit(2) / sum; 150 | Vector2d uv_norm; 151 | uv_norm << uv_unit(0) / uv_unit(2), uv_unit(1) / uv_unit(2); 152 | return uv_norm; 153 | } 154 | 155 | double PinholeStereoCamera::getDisparity(const double pZ) 156 | { 157 | return fx * b / pZ; 158 | } 159 | 160 | Vector2d PinholeStereoCamera::projection(const Vector3d &P) 161 | { 162 | Vector2d uv_unit; 163 | uv_unit(0) = cx + fx * P(0) / P(2); 164 | uv_unit(1) = cy + fy * P(1) / P(2); 165 | return uv_unit; 166 | } 167 | 168 | Vector2d PinholeStereoCamera::projectionNormalized(const Vector3d &P) 169 | { 170 | Vector2d uv_unit; 171 | uv_unit(0) = P(0) / P(2); 172 | uv_unit(1) = P(1) / P(2); 173 | return uv_unit; 174 | } 175 | 176 | Vector3d PinholeStereoCamera::projectionNH(const Vector3d &P) 177 | { 178 | Vector3d uv_proj; 179 | uv_proj(0) = cx * P(2) + fx * P(0); 180 | uv_proj(1) = cy * P(2) + fy * P(1); 181 | uv_proj(2) = P(2); 182 | return uv_proj; 183 | } 184 | 185 | Vector2d PinholeStereoCamera::nonHomogeneous(const Vector3d &x) 186 | { 187 | Vector2d x_; 188 | x_ << x(0) / x(2), x(1) / x(2); 189 | return x_; 190 | } 191 | 192 | Vector2d PinholeStereoCamera::projectionDistorted(const Vector3d &P, 193 | const double &K1, const double &K2, const double &K3) 194 | { 195 | double x_norm = P(0) / P(2), y_norm = P(1) / P(2); 196 | double r_2 = x_norm * x_norm + y_norm * y_norm; 197 | // 198 | double dist_coeff = (1 + K1 * r_2 + K2 * pow(r_2, 2) + K3 * pow(r_2, 3)); 199 | double x_dist = x_norm * dist_coeff, 200 | y_dist = y_norm * dist_coeff; 201 | Vector2d uv_unit; 202 | uv_unit(0) = cx + fx * x_dist; 203 | uv_unit(1) = cy + fy * y_dist; 204 | return uv_unit; 205 | } 206 | -------------------------------------------------------------------------------- /src/stereoFeatures.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | ** PL-SLAM: stereo visual SLAM with points and line segment features ** 3 | ****************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2017, Ruben Gomez-Ojeda, University of Malaga ** 6 | ** Copyright(c) 2017, MAPIR group, University of Malaga ** 7 | ** ** 8 | ** This program is free software: you can redistribute it and/or modify ** 9 | ** it under the terms of the GNU General Public License (version 3) as ** 10 | ** published by the Free Software Foundation. ** 11 | ** ** 12 | ** This program is distributed in the hope that it will be useful, but ** 13 | ** 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 this program. If not, see . ** 19 | ** ** 20 | *****************************************************************************/ 21 | 22 | #include 23 | 24 | namespace StVO 25 | { 26 | 27 | // Point feature 28 | 29 | PointFeature::PointFeature(const Vector3d &P_, const Vector2d &pl_obs_) : P(P_), pl_obs(pl_obs_), level(0) 30 | { 31 | } 32 | 33 | PointFeature::PointFeature(const Vector2d &pl_, const double &disp_, const Vector3d &P_) : pl(pl_), disp(disp_), P(P_), inlier(true), level(0) 34 | { 35 | } 36 | 37 | PointFeature::PointFeature(const Vector2d &pl_, const double &disp_, const Vector3d &P_, 38 | const int &idx_) : pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(0) 39 | { 40 | } 41 | 42 | PointFeature::PointFeature(const Vector2d &pl_, const double &disp_, const Vector3d &P_, 43 | const int &idx_, const int &level_) : pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(level_) 44 | { 45 | for (int i = 0; i < level + 1; i++) 46 | sigma2 *= Config::orbScaleFactor(); 47 | sigma2 = 1.f / (sigma2 * sigma2); 48 | } 49 | 50 | PointFeature::PointFeature(const Vector2d &pl_, const double &disp_, const Vector3d &P_, 51 | const Vector2d &pl_obs_) : pl(pl_), disp(disp_), P(P_), pl_obs(pl_obs_), inlier(true), level(0) 52 | { 53 | } 54 | 55 | // Line segment feature 56 | 57 | LineFeature::LineFeature(const Vector3d &sP_, const Vector3d &eP_, const Vector3d &le_obs_) : sP(sP_), eP(eP_), le_obs(le_obs_), level(0) 58 | { 59 | } 60 | 61 | LineFeature::LineFeature(const Vector3d &sP_, const Vector3d &eP_, const Vector3d &le_obs_, 62 | const Vector2d &spl_obs_, const Vector2d &epl_obs_) : sP(sP_), eP(eP_), le_obs(le_obs_), spl_obs(spl_obs_), epl_obs(epl_obs_), level(0) 63 | { 64 | } 65 | 66 | LineFeature::LineFeature(const Vector2d &spl_, const double &sdisp_, const Vector3d &sP_, 67 | const Vector2d &epl_, const double &edisp_, const Vector3d &eP_, 68 | const Vector3d &le_) : spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), level(0) 69 | { 70 | } 71 | 72 | LineFeature::LineFeature(const Vector2d &spl_, const double &sdisp_, const Vector3d &sP_, 73 | const Vector2d &epl_, const double &edisp_, const Vector3d &eP_, 74 | const Vector3d &le_, const Vector3d &le_obs_) : spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), le_obs(le_obs_), inlier(true), level(0) 75 | { 76 | } 77 | 78 | LineFeature::LineFeature(const Vector2d &spl_, const double &sdisp_, const Vector3d &sP_, 79 | const Vector2d &epl_, const double &edisp_, const Vector3d &eP_, 80 | const Vector3d &le_, const int &idx_) : spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), level(0) 81 | { 82 | } 83 | 84 | LineFeature::LineFeature(const Vector2d &spl_, const double &sdisp_, const Vector3d &sP_, 85 | const Vector2d &epl_, const double &edisp_, const Vector3d &eP_, 86 | const Vector3d &le_, const double &angle_, const int &idx_) : spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(0) 87 | { 88 | } 89 | 90 | LineFeature::LineFeature(const Vector2d &spl_, const double &sdisp_, const Vector3d &sP_, 91 | const Vector2d &epl_, const double &edisp_, const Vector3d &eP_, 92 | const Vector3d &le_, const double &angle_, const int &idx_, const int &level_) : spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(level_) 93 | { 94 | for (int i = 0; i < level + 1; i++) 95 | sigma2 *= Config::lsdScale(); 96 | sigma2 = 1.f / (sigma2 * sigma2); 97 | } 98 | 99 | } // namespace StVO 100 | -------------------------------------------------------------------------------- /test/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of GF-PL-SLAM. 3 | * 4 | * Copyright (C) 2019 Yipu Zhao 5 | * (Georgia Institute of Technology) 6 | * For more information see 7 | * 8 | * 9 | * GF-PL-SLAM is free software: you can redistribute it and/or modify 10 | * it under the terms of the GNU General Public License as published by 11 | * the Free Software Foundation, either version 3 of the License, or 12 | * (at your option) any later version. 13 | * 14 | * GF-PL-SLAM is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | * GNU General Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License 20 | * along with GF-PL-SLAM. If not, see . 21 | */ 22 | 23 | #include "gtest/gtest.h" 24 | 25 | int main(int argc, char **argv) 26 | { 27 | ::testing::InitGoogleTest(&argc, argv); 28 | int ret = RUN_ALL_TESTS(); 29 | return ret; 30 | } 31 | -------------------------------------------------------------------------------- /test/testCut.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2005, Google Inc. 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are 6 | // met: 7 | // 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above 11 | // copyright notice, this list of conditions and the following disclaimer 12 | // in the documentation and/or other materials provided with the 13 | // distribution. 14 | // * Neither the name of Google Inc. nor the names of its 15 | // contributors may be used to endorse or promote products derived from 16 | // this software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | // A sample program demonstrating using Google C++ testing framework. 31 | // 32 | // Author: wan@google.com (Zhanyong Wan) 33 | 34 | 35 | // In this example, we use a more advanced feature of Google Test called 36 | // test fixture. 37 | // 38 | // A test fixture is a place to hold objects and functions shared by 39 | // all tests in a test case. Using a test fixture avoids duplicating 40 | // the test code necessary to initialize and cleanup those common 41 | // objects for each test. It is also useful for defining sub-routines 42 | // that your tests need to invoke a lot. 43 | // 44 | // 45 | // 46 | // The tests share the test fixture in the sense of code sharing, not 47 | // data sharing. Each test is given its own fresh copy of the 48 | // fixture. You cannot expect the data modified by one test to be 49 | // passed on to another test, which is a bad idea. 50 | // 51 | // The reason for this design is that tests should be independent and 52 | // repeatable. In particular, a test should not fail as the result of 53 | // another test's failure. If one test depends on info produced by 54 | // another test, then the two tests should really be one big test. 55 | // 56 | // The macros for indicating the success/failure of a test 57 | // (EXPECT_TRUE, FAIL, etc) need to know what the current test is 58 | // (when Google Test prints the test result, it tells you which test 59 | // each failure belongs to). Technically, these macros invoke a 60 | // member function of the Test class. Therefore, you cannot use them 61 | // in a global function. That's why you should put test sub-routines 62 | // in a test fixture. 63 | // 64 | // 65 | 66 | #include 67 | #include "stereoFrameHandler.h" 68 | #include "gtest/gtest.h" 69 | //#include "gtest/gmock.h" 70 | 71 | using namespace StVO; 72 | 73 | namespace { 74 | // To use a test fixture, derive a class from testing::Test. 75 | class TestLineCut : public testing::Test { 76 | protected: // You should make the members protected s.t. they can be 77 | // accessed from sub-classes. 78 | 79 | // virtual void SetUp() will be called before each test is run. You 80 | // should define it if you need to initialize the variables. 81 | // Otherwise, this can be skipped. 82 | virtual void SetUp() { 83 | 84 | // Load Settings and Check 85 | string strSettingsFile = "..//config//gazebo_params.yaml"; 86 | std::cout << "Load settings and check " << strSettingsFile << std::endl; 87 | // read content of the .yaml dataset configuration file 88 | YAML::Node dset_config = YAML::LoadFile( strSettingsFile.c_str() ); 89 | 90 | // setup camera 91 | YAML::Node cam_config = dset_config["cam0"]; 92 | string camera_model = cam_config["cam_model"].as(); 93 | PinholeStereoCamera* cam_pin; 94 | bool rectify = false; 95 | if( camera_model == "Pinhole" ) 96 | { 97 | // if EuRoC or Falcon yaml file 98 | if( cam_config["Kl"].IsDefined() ) 99 | { 100 | rectify = true; 101 | Mat Kl, Kr, Dl, Dr, R, t; 102 | vector Kl_ = cam_config["Kl"].as>(); 103 | vector Kr_ = cam_config["Kr"].as>(); 104 | vector Dl_ = cam_config["Dl"].as>(); 105 | vector Dr_ = cam_config["Dr"].as>(); 106 | Kl = ( Mat_(3,3) << Kl_[0], 0.0, Kl_[2], 0.0, Kl_[1], Kl_[3], 0.0, 0.0, 1.0 ); 107 | Kr = ( Mat_(3,3) << Kr_[0], 0.0, Kr_[2], 0.0, Kr_[1], Kr_[3], 0.0, 0.0, 1.0 ); 108 | // load rotation and translation 109 | vector R_ = cam_config["R"].as>(); 110 | vector t_ = cam_config["t"].as>(); 111 | R = Mat::eye(3,3,CV_64F); 112 | t = Mat::eye(3,1,CV_64F); 113 | int k = 0; 114 | for( int i = 0; i < 3; i++ ) 115 | { 116 | t.at(i,0) = t_[i]; 117 | for( int j = 0; j < 3; j++, k++ ) 118 | R.at(i,j) = R_[k]; 119 | } 120 | // load distortion parameters 121 | int Nd = Dl_.size(); 122 | Dl = Mat::eye(1,Nd,CV_64F); 123 | Dr = Mat::eye(1,Nd,CV_64F); 124 | for( int i = 0; i < Nd; i++ ) 125 | { 126 | Dl.at(0,i) = Dl_[i]; 127 | Dr.at(0,i) = Dr_[i]; 128 | } 129 | // if dtype is equidistant (now it is default) 130 | if( cam_config["dtype"].IsDefined() ) 131 | { 132 | cam_pin = new PinholeStereoCamera( 133 | cam_config["cam_width"].as(), 134 | cam_config["cam_height"].as(), 135 | cam_config["cam_bl"].as(), 136 | Kl, Kr, R, t, Dl, Dr, true); 137 | 138 | } 139 | else 140 | // create camera object for EuRoC 141 | cam_pin = new PinholeStereoCamera( 142 | cam_config["cam_width"].as(), 143 | cam_config["cam_height"].as(), 144 | cam_config["cam_bl"].as(), 145 | Kl, Kr, R, t, Dl, Dr,false); 146 | } 147 | } 148 | else { 149 | cam_pin = new PinholeStereoCamera( 150 | cam_config["cam_width"].as(), 151 | cam_config["cam_height"].as(), 152 | fabs(cam_config["cam_fx"].as()), 153 | fabs(cam_config["cam_fy"].as()), 154 | cam_config["cam_cx"].as(), 155 | cam_config["cam_cy"].as(), 156 | cam_config["cam_bl"].as(), 157 | cam_config["cam_d0"].as(), 158 | cam_config["cam_d1"].as(), 159 | cam_config["cam_d2"].as(), 160 | cam_config["cam_d3"].as() ); 161 | } 162 | 163 | 164 | StVO = new StereoFrameHandler(cam_pin); 165 | // StVO->initialize(img_l, img_r, 0, time_stamp); 166 | // StVO->insertStereoPair( img_l, img_r, frame_counter, time_stamp ); 167 | 168 | // set stereo feature at previous frame 169 | 170 | // set motion prediction and motion ground truth 171 | prev_frame->Tfw * prev_frame->DT; 172 | StVO->predictFramePose(); 173 | 174 | // set stereo feature at current frame 175 | 176 | 177 | // set pose 178 | 179 | 180 | } 181 | 182 | // virtual void TearDown() will be called after each test is run. 183 | // You should define it if there is cleanup work to do. Otherwise, 184 | // you don't have to provide it. 185 | // 186 | // virtual void TearDown() { 187 | // } 188 | 189 | // Declares the variables your tests want to use. 190 | StereoFrameHandler* StVO; 191 | }; 192 | 193 | // When you have a test fixture, you define a test using TEST_F 194 | // instead of TEST. 195 | // Tests the Kinematic/System Jacobian. 196 | TEST_F(TestLineCut, maxVolCut) { 197 | // 198 | double rngCutRatio[2] = {0, 1.0}; 199 | StVO->estimateProjUncertainty_submodular( 0.05, rngCutRatio ); 200 | 201 | StVO->optimizePose(StVO->prev_frame->DT); 202 | 203 | StVO->curr_frame->Tfw 204 | 205 | EXPECT_NEAR(arma::norm(obs_->kinematic[i].F_Q - F_Q, "inf"), 0, 0.002); 206 | 207 | } 208 | 209 | } // namespace 210 | -------------------------------------------------------------------------------- /vocabulary/voc.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ivalab/GF_PL_SLAM/26a218491b9718ffdf1a63b1eaeb8d8d253f1f37/vocabulary/voc.tar.gz --------------------------------------------------------------------------------