├── .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 └── plstvo_mod.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 ├── 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 ├── simu ├── line_debug.py ├── line_detect_assessment.py ├── line_match_assessment.py ├── line_merge_assessment.py ├── line_stereo_assessment.py ├── project_error_assessment.py ├── simu_line_vol_assessment.py ├── simu_point_line_loss.py ├── simulate_line_vol.log └── stereo_error_assessment.py ├── src ├── ORBextractor.cc ├── auxiliar.cpp ├── config.cpp ├── keyFrame.cpp ├── linespec.cpp ├── mapFeatures.cpp ├── mapHandler.cpp ├── pinholeStereoCamera.cpp ├── simulate_line_cut.cpp ├── simulate_pl_loss.cpp ├── slamScene.cpp ├── stereoFeatures.cpp ├── stereoFrame.cpp ├── stereoFrameHandler.cpp ├── test_line_matching.cpp └── voScene.cpp └── test ├── main.cpp └── testCut_1.cpp /.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 | .idea 12 | cmake-build-debug/ 13 | vocabulary/voc_all_datasets_orb.yml 14 | vocabulary/voc_all_datasets_bld.yml 15 | vocabulary/voc.tar.gz 16 | vocabulary/voc/* 17 | -------------------------------------------------------------------------------- /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.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 | MESSAGE("CMake Build Type: " ${CMAKE_BUILD_TYPE}) 5 | 6 | LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) 7 | 8 | find_package(OpenCV 3 REQUIRED) 9 | MESSAGE("OpenCV include dir: " ${OpenCV_INCLUDE_DIRS}) 10 | 11 | find_package(Boost REQUIRED COMPONENTS thread system filesystem) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | MESSAGE("EIGEN include dir: " ${EIGEN3_INCLUDE_DIR}) 15 | 16 | # set( G2O_DIR /opt/g2o/lib/g2o/ ) 17 | # find_package(G2O REQUIRED) 18 | # MESSAGE("G2O include dir: " ${G2O_INCLUDE_DIR}) 19 | # MESSAGE("G2O libraries " ${G2O_LIBRARIES}) 20 | 21 | set (G2O_INCLUDE_DIR /home/pang/software/g2o_install/include) 22 | set (G2O_LIBRARIES 23 | /home/pang/software/g2o_install/lib/libg2o_cli.so 24 | /home/pang/software/g2o_install/lib/libg2o_core.so 25 | /home/pang/software/g2o_install/lib/libg2o_csparse_extension.so 26 | /home/pang/software/g2o_install/lib/libg2o_ext_freeglut_minimal.so 27 | /home/pang/software/g2o_install/lib/libg2o_hierarchical.so 28 | /home/pang/software/g2o_install/lib/libg2o_incremental.so 29 | /home/pang/software/g2o_install/lib/libg2o_interactive.so 30 | /home/pang/software/g2o_install/lib/libg2o_interface.so 31 | /home/pang/software/g2o_install/lib/libg2o_opengl_helper.so 32 | /home/pang/software/g2o_install/lib/libg2o_parser.so 33 | /home/pang/software/g2o_install/lib/libg2o_simulator.so 34 | /home/pang/software/g2o_install/lib/libg2o_solver_cholmod.so 35 | /home/pang/software/g2o_install/lib/libg2o_solver_csparse.so 36 | /home/pang/software/g2o_install/lib/libg2o_solver_dense.so 37 | /home/pang/software/g2o_install/lib/libg2o_solver_pcg.so 38 | /home/pang/software/g2o_install/lib/libg2o_solver_slam2d_linear.so 39 | /home/pang/software/g2o_install/lib/libg2o_solver_structure_only.so 40 | /home/pang/software/g2o_install/lib/libg2o_stuff.so 41 | /home/pang/software/g2o_install/lib/libg2o_types_data.so 42 | /home/pang/software/g2o_install/lib/libg2o_types_icp.so 43 | /home/pang/software/g2o_install/lib/libg2o_types_sba.so 44 | /home/pang/software/g2o_install/lib/libg2o_types_sclam2d.so 45 | /home/pang/software/g2o_install/lib/libg2o_types_sim3.so 46 | /home/pang/software/g2o_install/lib/libg2o_types_slam2d_addons.so 47 | /home/pang/software/g2o_install/lib/libg2o_types_slam2d.so 48 | /home/pang/software/g2o_install/lib/libg2o_types_slam3d_addons.so 49 | /home/pang/software/g2o_install/lib/libg2o_types_slam3d.so) 50 | 51 | 52 | find_package(Cholmod REQUIRED) 53 | 54 | if(COMMAND cmake_policy) 55 | cmake_policy(SET CMP0003 NEW) 56 | endif(COMMAND cmake_policy) 57 | link_directories(${OpenCV_LIBS_DIR}) 58 | include_directories(${OpenCV2_INCLUDE_DIRS}) 59 | 60 | set(DEFAULT_HAS_MRPT OFF) 61 | set(HAS_MRPT ${DEFAULT_HAS_MRPT} CACHE BOOL "Build the PointGrey Bumblebee2 SVO application that employs the MRPT library") 62 | 63 | SET(BUILD_SHARED_LIBS ON) 64 | SET(CMAKE_MODULE_PATH $ENV{CMAKE_MODULE_PATH}) 65 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -O3 -mtune=native -march=native") 66 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread -O3 -mtune=native -march=native") 67 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c++11 -Wl,--no-as-needed -pthread -Wall -O3 -march=native ") 68 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wl,--no-as-needed -pthread -Wall -O3 -march=native") 69 | MESSAGE(">>>>> Compiler flags are: " ${CMAKE_CXX_FLAGS}) 70 | 71 | # MRPT library (optional, only with representation purposes) 72 | if(HAS_MRPT) 73 | set(MRPT_DIR /home/pang/software/mrpt_install/share/mrpt ) 74 | FIND_PACKAGE(MRPT REQUIRED base opengl gui hwdrivers) 75 | set(MRPT_DONT_USE_DBG_LIBS 1) #use release libraries for linking even if "Debug" CMake build 76 | add_definitions(-DHAS_MRPT) 77 | endif(HAS_MRPT) 78 | MESSAGE("MRPT libraries " ${MRPT_LIBS}) 79 | 80 | # YAML library 81 | # set(YAML_CPP_INCLUDE_DIRS /usr/include/yaml-cpp/) 82 | # set(YAML_CPP_LIBRARIES /usr/lib/x86_64-linux-gnu/libyaml-cpp.so) 83 | find_library(YAML_CPP_LIBRARIES yaml-cpp) 84 | if(NOT YAML_CPP_LIBRARIES) 85 | # If yaml-cpp not found in the system, try finding it as a user CMake-generated project 86 | find_package(yaml-cpp REQUIRED) 87 | include_directories(${YAML_CPP_INCLUDE_DIRS}) 88 | endif(NOT YAML_CPP_LIBRARIES) 89 | MESSAGE("YAML libraries " ${YAML_CPP_LIBRARIES}) 90 | 91 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build) 92 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 93 | 94 | # Include dirs 95 | include_directories( 96 | include 97 | ${YAML_CPP_INCLUDE_DIRS} 98 | ${OpenCV_INCLUDE_DIRS} 99 | ${Boost_INCLUDE_DIRS} 100 | ${PROJECT_SOURCE_DIR}/3rdparty/DBoW2/DBoW2/ 101 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/include/ 102 | ${G2O_INCLUDE_DIR} 103 | ${EIGEN3_INCLUDE_DIR} 104 | 105 | /usr/include/suitesparse # for cholmod 106 | ) 107 | 108 | # Set link libraries 109 | list(APPEND LINK_LIBS 110 | ${YAML_CPP_LIBRARIES} 111 | ${OpenCV_LIBS} 112 | ${Boost_LIBRARIES} 113 | cholmod 114 | ${G2O_LIBRARIES} 115 | ${PROJECT_SOURCE_DIR}/3rdparty/DBoW2/lib/libDBoW2.so 116 | ${PROJECT_SOURCE_DIR}/3rdparty/line_descriptor/lib/liblinedesc.so 117 | ) 118 | 119 | # Set source files 120 | if(HAS_MRPT) 121 | list(APPEND SOURCEFILES 122 | src/linespec.cpp 123 | src/config.cpp 124 | src/auxiliar.cpp 125 | src/pinholeStereoCamera.cpp 126 | src/stereoFeatures.cpp 127 | src/stereoFrame.cpp 128 | src/stereoFrameHandler.cpp 129 | src/mapHandler.cpp 130 | src/mapFeatures.cpp 131 | src/keyFrame.cpp 132 | src/voScene.cpp 133 | src/slamScene.cpp 134 | src/ORBextractor.cc 135 | ) 136 | else() 137 | list(APPEND SOURCEFILES 138 | src/linespec.cpp 139 | src/config.cpp 140 | src/auxiliar.cpp 141 | src/pinholeStereoCamera.cpp 142 | src/stereoFeatures.cpp 143 | src/stereoFrame.cpp 144 | src/stereoFrameHandler.cpp 145 | src/mapHandler.cpp 146 | src/mapFeatures.cpp 147 | src/keyFrame.cpp 148 | src/ORBextractor.cc 149 | ) 150 | endif() 151 | 152 | # List all files (headers) contained by StVO-PL library 153 | file(GLOB_RECURSE all_include_files RELATIVE "${CMAKE_SOURCE_DIR}" *.h *.hpp) 154 | 155 | # Visualize the files of this directory in IDE creating an custom empty target 156 | add_custom_target( plslam_includes DEPENDS ${all_include_files} SOURCES ${all_include_files} ) 157 | 158 | # Create StVO-PL library 159 | add_library(plslam SHARED ${SOURCEFILES}) 160 | 161 | if(HAS_MRPT) 162 | target_link_libraries(plslam ${LINK_LIBS} ${MRPT_LIBS} ) 163 | else() 164 | target_link_libraries(plslam ${LINK_LIBS}) 165 | endif() 166 | 167 | # Applications [TODO: ADD VO APPLICATIONS] 168 | # if(HAS_MRPT) 169 | # add_executable ( plslam_dataset app/plslam_dataset.cpp ) 170 | # target_link_libraries( plslam_dataset plslam ) 171 | # add_executable ( plstvo_dataset app/plstvo_dataset.cpp ) 172 | # target_link_libraries( plstvo_dataset plslam ) 173 | # # 174 | # add_executable ( plstvo_mod app/plstvo_mod.cpp ) 175 | # target_link_libraries( plstvo_mod plslam ) 176 | add_executable ( plslam_mod app/plslam_mod.cpp ) 177 | target_link_libraries( plslam_mod plslam ) 178 | # endif(HAS_MRPT) 179 | 180 | # test cases 181 | #FIND_PACKAGE( Boost REQUIRED system thread ) 182 | #INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} ) 183 | #MESSAGE("Boost libs: " ${Boost_LIBRARIES}) 184 | 185 | #add_executable( testCut 186 | # ${SOURCEFILES} 187 | ## ./test/testCut_1.cpp 188 | # ./test/main.cpp 189 | # ) 190 | #target_link_libraries( testCut 191 | # ${LINK_LIBS} 192 | # ${GTest_LIBRARIES} 193 | #) 194 | 195 | 196 | # add_executable( test_line_matching src/test_line_matching.cpp) 197 | # target_link_libraries(test_line_matching plslam) 198 | -------------------------------------------------------------------------------- /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 | [![Example Run of GF-PL-SLAM](https://img.youtube.com/vi/YAq5uzDtSXE/0.jpg)](https://youtu.be/YAq5uzDtSXE) 6 | 7 | ## Build & Run 8 | 9 | This repo is an integration of good line cutting to stereo PL-SLAM. When tested on multiple challenging scenarios that point feature may fail, GF-PL-SLAM has better performance than PL-SLAM baseline and other state-of-the-art stereo SLAM systems. 10 | 11 | To build GF-PL-SLAM, first clone the repo to your ros workspace 12 | 13 | git clone git@github.com:YipuZhao/GF_PL_SLAM.git 14 | 15 | Build dependencies for GF-PL-SLAM with 16 | 17 | ./build_dep.sh 18 | 19 | Build the GF-PL-SLAM itself 20 | 21 | ./build.sh 22 | 23 | With the head version of g2o, it's possible to prompt error when building GF-PL-SLAM. When getting error like "no matching function for BlockSolver XXX", refer to the fix at 24 | 25 | https://github.com/rubengooj/pl-slam/issues/29#issuecomment-437773067 26 | 27 | To run GF-PL-SLAM, please refer to some example batch evaluation scripts at folder 28 | 29 | batch_script 30 | 31 | ## Reference 32 | 33 | If you use GF-PL-SLAM in an academic work, please cite: 34 | 35 | @inproceedings{zhao2018good, 36 | title={Good Line Cutting: towards Accurate Pose Tracking of Line-assisted VO/VSLAM}, 37 | author={Zhao, Yipu and Vela, Patricio A}, 38 | booktitle={Proceedings of the European Conference on Computer Vision (ECCV)}, 39 | pages={516--531}, 40 | year={2018} 41 | } 42 | 43 | --- 44 | 45 | 46 | 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. 47 | 48 | # PL-SLAM # 49 | 50 | This code contains an algorithm to compute stereo visual SLAM by using both point and line segment features. 51 | 52 | **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) 53 | 54 | **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) 55 | 56 | If you use PL-SLAM in your research work, please cite: 57 | 58 | @article{gomez2017pl, 59 | title = {{PL-SLAM: a Stereo SLAM System through the Combination of Points and Line Segments}}, 60 | author = {Gomez-Ojeda, Ruben and Zuñiga-Noël, David and Moreno, Francisco-Angel and Scaramuzza, Davide and Gonzalez-Jimenez, Javier}, 61 | journal = {arXiv preprint arXiv:1705.09479}, 62 | year = {2017} 63 | } 64 | 65 | The pdf file can be found at [https://arxiv.org/abs/1705.09479](https://arxiv.org/abs/1705.09479). 66 | 67 | [![PL-SLAM](https://img.youtube.com/vi/-lCTf_tAxhQ/0.jpg)](https://www.youtube.com/watch?v=-lCTf_tAxhQ) 68 | 69 | **Related publications:** 70 | 71 | [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) 72 | 73 | [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). 74 | 75 | **License:** 76 | 77 | 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. 78 | 79 | Please do not hesitate to contact the authors if you have any further questions. 80 | 81 | 82 | ## 1. Prerequisites and dependencies 83 | 84 | ### OpenCV 3.x.x 85 | It can be easily found at http://opencv.org. 86 | 87 | ### Eigen3 (tested with 3.2.92) 88 | http://eigen.tuxfamily.org 89 | 90 | ### Boost 91 | Installation on Ubuntu: 92 | ``` 93 | sudo apt-get install libboost-dev 94 | ``` 95 | 96 | ### g2o - General Graph Optimization 97 | It can be found at: 98 | ``` 99 | https://github.com/RainerKuemmerle/g2o.git 100 | ``` 101 | 102 | ### YAML (tested with 0.5.2) 103 | Installation on Ubuntu: 104 | ``` 105 | sudo apt-get install libyaml-cpp-dev 106 | ``` 107 | 108 | ### stvo-pl 109 | It can be found at: 110 | ``` 111 | https://github.com/rubengooj/stvo-pl 112 | ``` 113 | 114 | ### MRPT 115 | In case of using the provided representation class. 116 | Download and install instructions can be found at: http://www.mrpt.org/ 117 | 118 | #### Known Issues: 119 | 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): 120 | ``` 121 | https://github.com/MRPT/mrpt/tree/0c3d605c3cbf5f2ffb8137089e43ebdae5a55de3 122 | ``` 123 | 124 | ### Line Descriptor 125 | 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. 126 | 127 | 128 | ## 2. Configuration and generation 129 | 130 | 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). 131 | 132 | 133 | ## 3. Usage 134 | 135 | ### Datasets configuration 136 | 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*. 137 | 138 | ### Configuration files 139 | For running SLAM we can load the default parameters file or employ the *config_xxxx.yaml* files provided for every dataset. 140 | 141 | ### SLAM Application 142 | Usage: ./plslam_dataset [options] 143 | Options: 144 | -c Config file 145 | -o Offset (number of frames to skip in the dataset directory 146 | -n Number of frames to process the sequence 147 | -s Parameter to skip s-1 frames (default 1) 148 | 149 | A full command would be: 150 | 151 | ./plslam_dataset kitti/00 -c ../config/config_kitti.yaml -o 100 -s 2 -n 1000 152 | 153 | 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. 154 | 155 | -------------------------------------------------------------------------------- /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 = ['MH_01_easy']; 11 | # Result_root = '/mnt/DATA/tmp/EuRoC/Cut_PointLine_1000_0.1/' 12 | Result_root = '/home/pang/data/dataset/euroc/output/' 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 = '/home/pang/data/dataset/euroc/' + SeqName + '/mav0' 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/SimonsRoad/gf-pl-slam/dcd9208c868164774e9980fc72fbf96fc6fd14d4/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 | # g2o 13 | cd ${DEPENDENCIES_DIR} 14 | git clone https://github.com/RainerKuemmerle/g2o.git 15 | cd g2o 16 | mkdir build 17 | cd build 18 | cmake .. -DCMAKE_BUILD_TYPE=Release .. -DCMAKE_INSTALL_PREFIX:PATH="/opt/g2o" 19 | make -j 20 | sudo make install 21 | 22 | # build opencv 3.4 23 | cd ${DEPENDENCIES_DIR} 24 | wget https://github.com/opencv/opencv/archive/3.4.1.tar.gz 25 | tar xf 3.4.1.tar.gz 26 | wget https://github.com/opencv/opencv_contrib/archive/3.4.1.tar.gz 27 | tar xf 3.4.1.tar.gz.1 28 | cd ${DEPENDENCIES_DIR}/opencv-3.4.1 29 | mkdir build 30 | cd build 31 | # build opencv without cuda support 32 | # cmake .. -DCMAKE_INSTALL_PREFIX:PATH="/opt/opencv3" -DBUILD_TBB:BOOL="1" -DWITH_TBB:BOOL="1" -DCMAKE_BUILD_TYPE:STRING="Release" -DWITH_OPENMP:BOOL="1" -DBUILD_opencv_gpu:BOOL="0" -DOPENCV_EXTRA_MODULES_PATH:PATH="/mnt/DATA/SDK/opencv_contrib-3.4.1/modules" -DBUILD_opencv_cudaobjdetect:BOOL="0" -DWITH_CUFFT:BOOL="0" -DBUILD_opencv_cudaimgproc:BOOL="0" -DBUILD_opencv_cudastereo:BOOL="0" -DBUILD_opencv_cudaoptflow:BOOL="0" -DBUILD_opencv_cudabgsegm:BOOL="0" -DBUILD_opencv_cudaarithm:BOOL="0" -DWITH_CUDA:BOOL="0" -DOPENCV_ENABLE_NONFREE:BOOL="1" -DBUILD_opencv_cudacodec:BOOL="0" -DWITH_CUBLAS:BOOL="0" -DBUILD_opencv_cudawarping:BOOL="0" -DBUILD_opencv_cudafilters:BOOL="0" -DCUDA_64_BIT_DEVICE_CODE:BOOL="0" -DBUILD_opencv_cudafeatures2d:BOOL="0" -DBUILD_opencv_cudalegacy:BOOL="0" 33 | # build opencv with cuda support 34 | cmake .. -DCMAKE_INSTALL_PREFIX:PATH="/opt/opencv3" -DBUILD_TBB:BOOL="1" -DWITH_TBB:BOOL="1" -DCMAKE_BUILD_TYPE:STRING="Release" -DWITH_OPENMP:BOOL="1" -DBUILD_opencv_gpu:BOOL="1" -DOPENCV_EXTRA_MODULES_PATH:PATH=${DEPENDENCIES_DIR}/opencv_contrib-3.4.1/modules -DBUILD_opencv_cudaobjdetect:BOOL="1" -DWITH_CUFFT:BOOL="1" -DBUILD_opencv_cudaimgproc:BOOL="1" -DBUILD_opencv_cudastereo:BOOL="1" -DBUILD_opencv_cudaoptflow:BOOL="1" -DBUILD_opencv_cudabgsegm:BOOL="1" -DBUILD_opencv_cudaarithm:BOOL="1" -DWITH_CUDA:BOOL="1" -DOPENCV_ENABLE_NONFREE:BOOL="1" -DBUILD_opencv_cudacodec:BOOL="1" -DWITH_CUBLAS:BOOL="1" -DBUILD_opencv_cudawarping:BOOL="1" -DBUILD_opencv_cudafilters:BOOL="1" -DCUDA_64_BIT_DEVICE_CODE:BOOL="1" -DBUILD_opencv_cudafeatures2d:BOOL="1" -DBUILD_opencv_cudalegacy:BOOL="1" 35 | make -j 36 | sudo make install 37 | 38 | # MRPT (viz only) 39 | cd ${DEPENDENCIES_DIR} 40 | git clone https://github.com/MRPT/mrpt.git 41 | cd mrpt 42 | git checkout 0c3d605c3cbf5f2ffb8137089e43ebdae5a55de3 43 | mkdir build 44 | cd build 45 | cmake .. -DCMAKE_BUILD_TYPE=Release .. -DCMAKE_INSTALL_PREFIX:PATH="/opt/mrpt" 46 | make -j 47 | sudo make install -------------------------------------------------------------------------------- /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 | # Locate the g2o libraries 2 | # A general framework for graph optimization. 3 | # 4 | # This module defines 5 | # G2O_FOUND, if false, do not try to link against g2o 6 | # G2O_LIBRARIES, path to the libg2o 7 | # G2O_INCLUDE_DIR, where to find the g2o header files 8 | # 9 | # Niko Suenderhauf 10 | # Adapted by Felix Endres 11 | 12 | IF(UNIX) 13 | 14 | IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 15 | # in cache already 16 | SET(G2O_FIND_QUIETLY TRUE) 17 | ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) 18 | 19 | MESSAGE(STATUS "Searching for g2o ...") 20 | # FIND_PATH(G2O_INCLUDE_DIR 21 | # NAMES core math_groups types 22 | # PATHS /usr/local /usr /opt/g2o 23 | # PATH_SUFFIXES include/g2o include) 24 | set(G2O_INCLUDE_DIR /opt/g2o/include) 25 | 26 | IF (G2O_INCLUDE_DIR) 27 | MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") 28 | ENDIF (G2O_INCLUDE_DIR) 29 | 30 | FIND_LIBRARY(G2O_CORE_LIB 31 | NAMES g2o_core 32 | PATHS /usr/local /usr /opt/g2o 33 | PATH_SUFFIXES lib) 34 | FIND_LIBRARY(G2O_STUFF_LIB 35 | NAMES g2o_stuff 36 | PATHS /usr/local /usr /opt/g2o 37 | PATH_SUFFIXES lib) 38 | FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB 39 | NAMES g2o_types_slam3d 40 | PATHS /usr/local /usr /opt/g2o 41 | PATH_SUFFIXES lib) 42 | FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB 43 | NAMES g2o_solver_cholmod 44 | PATHS /usr/local /usr /opt/g2o 45 | PATH_SUFFIXES lib) 46 | FIND_LIBRARY(G2O_SOLVER_PCG_LIB 47 | NAMES g2o_solver_pcg 48 | PATHS /usr/local /usr /opt/g2o 49 | PATH_SUFFIXES lib) 50 | FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB 51 | NAMES g2o_solver_csparse 52 | PATHS /usr/local /usr /opt/g2o 53 | PATH_SUFFIXES lib) 54 | FIND_LIBRARY(G2O_INCREMENTAL_LIB 55 | NAMES g2o_incremental 56 | PATHS /usr/local /usr /opt/g2o 57 | PATH_SUFFIXES lib) 58 | FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB 59 | NAMES g2o_csparse_extension 60 | PATHS /usr/local /usr /opt/g2o 61 | PATH_SUFFIXES lib) 62 | 63 | SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB} 64 | ${G2O_CORE_LIB} 65 | ${G2O_STUFF_LIB} 66 | ${G2O_TYPES_SLAM3D_LIB} 67 | ${G2O_SOLVER_CHOLMOD_LIB} 68 | ${G2O_SOLVER_PCG_LIB} 69 | ${G2O_SOLVER_CSPARSE_LIB} 70 | ${G2O_INCREMENTAL_LIB} 71 | ) 72 | 73 | IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 74 | SET(G2O_FOUND "YES") 75 | IF(NOT G2O_FIND_QUIETLY) 76 | MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") 77 | ENDIF(NOT G2O_FIND_QUIETLY) 78 | ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 79 | IF(NOT G2O_LIBRARIES) 80 | IF(G2O_FIND_REQUIRED) 81 | message(FATAL_ERROR "Could not find libg2o!") 82 | ENDIF(G2O_FIND_REQUIRED) 83 | ENDIF(NOT G2O_LIBRARIES) 84 | 85 | IF(NOT G2O_INCLUDE_DIR) 86 | IF(G2O_FIND_REQUIRED) 87 | message(FATAL_ERROR "Could not find g2o include directory!") 88 | ENDIF(G2O_FIND_REQUIRED) 89 | ENDIF(NOT G2O_INCLUDE_DIR) 90 | ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) 91 | 92 | ENDIF(UNIX) 93 | 94 | -------------------------------------------------------------------------------- /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/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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | KeyFrame(){}; 54 | KeyFrame( const StVO::StereoFrame* sf ); 55 | KeyFrame( const StVO::StereoFrame* sf, int kf_idx_ ); 56 | ~KeyFrame(){}; 57 | 58 | Mat plotKeyFrame(); 59 | 60 | bool local; 61 | 62 | int kf_idx; 63 | Matrix4d T_kf_w; 64 | Vector6d x_kf_w; 65 | Matrix6d xcov_kf_w; 66 | 67 | DBoW2::BowVector descDBoW_P, descDBoW_L; 68 | 69 | StVO::StereoFrame* stereo_frame; 70 | 71 | 72 | 73 | }; 74 | 75 | } 76 | -------------------------------------------------------------------------------- /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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | 79 | MapHandler(PinholeStereoCamera* cam_); 80 | ~MapHandler(){}; 81 | 82 | void initialize(KeyFrame* kf0); 83 | void finishSLAM(); 84 | void addKeyFrame(KeyFrame *curr_kf); 85 | 86 | void addKeyFrame_multiThread(KeyFrame *curr_kf); 87 | void loopClosureThread(); 88 | void localMappingThread(); 89 | 90 | void lookForCommonMatches(KeyFrame *kf0, KeyFrame *&kf1); 91 | void expandGraphs(); 92 | void formLocalMap(); 93 | void formLocalMap_old(); 94 | void fuseLandmarksFromLocalMap(); 95 | void removeBadMapLandmarks(); 96 | void removeRedundantKFs(); 97 | void loopClosure(); 98 | bool lookForLoopCandidates(int kf_idx_curr, int &kf_idx_prev); 99 | void insertKFBowVectorP(KeyFrame *&kf); 100 | void insertKFBowVectorL(KeyFrame *&kf); 101 | void insertKFBowVectorPL(KeyFrame *&kf); 102 | bool isLoopClosure(KeyFrame* kf0, KeyFrame* kf1, Vector6d &pose_inc, 103 | vector &lc_pt_idx, vector &lc_ls_idx, 104 | vector &lc_points, vector &lc_lines); 105 | bool computeRelativePoseGN( vector &lc_points, vector &lc_lines, 106 | vector &lc_pt_idx, vector &lc_ls_idx, 107 | Vector6d &pose_inc ); 108 | bool computeRelativePoseRobustGN( vector &lc_points, vector &lc_lines, 109 | vector &lc_pt_idx, vector &lc_ls_idx, 110 | Vector6d &pose_inc ); 111 | bool loopClosureOptimizationEssGraphG2O(); 112 | bool loopClosureOptimizationCovGraphG2O(); 113 | void loopClosureFuseLandmarks(); 114 | 115 | void localBundleAdjustment(); 116 | void levMarquardtOptimizationLBA( vector X_aux, vector kf_list, vector pt_list, vector ls_list, vector pt_obs_list, vector ls_obs_list ); 117 | 118 | void globalBundleAdjustment(); 119 | void levMarquardtOptimizationGBA( vector X_aux, vector kf_list, vector pt_list, vector ls_list, vector pt_obs_list, vector ls_obs_list ); 120 | 121 | 122 | 123 | PinholeStereoCamera* cam; 124 | 125 | vector map_keyframes; 126 | vector map_points; 127 | vector map_lines; 128 | 129 | map> map_points_kf_idx; // base KF list from which the LM is observed 130 | map> map_lines_kf_idx; 131 | 132 | vector< vector > full_graph; 133 | 134 | vector< vector > conf_matrix; 135 | Vocabulary dbow_voc_p, dbow_voc_l; 136 | 137 | unsigned int max_pt_idx, max_ls_idx, max_kf_idx ; 138 | 139 | // experiment variables 140 | Vector7f time; 141 | // mrpt::utils::CTicTac clock; 142 | 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 | private: 41 | const int width, height; 42 | double fx, fy; 43 | double cx, cy; 44 | const double b; 45 | Matrix3d K; 46 | bool dist; 47 | Matrix d; 48 | Mat Kl, Kr, Dl, Dr, Rl, Rr, Pl, Pr, R, t, Q; 49 | Mat undistmap1l, undistmap2l, undistmap1r, undistmap2r; 50 | 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | PinholeStereoCamera( const int & width_, const int & height_, 54 | const double & fx_, const double & fy_, const double & cx_, const double & cy_, 55 | const double & b_, 56 | const double & d0 = 0.0, const double & d1 = 0.0, const double & d2 = 0.0, 57 | const double & d3 = 0.0, const double & d4 = 0.0); 58 | PinholeStereoCamera( const int & width_, const int & height_, 59 | const double & fx_, const double & fy_, const double & cx_, const double & cy_, 60 | const double & b_, 61 | const Mat & Rl, const Mat & Rr, 62 | const double & d0 = 0.0, const double & d1 = 0.0, const double & d2 = 0.0, 63 | const double & d3 = 0.0, const double & d4 = 0.0); 64 | //PinholeStereoCamera(int width_, int height_, double b_, Mat Kl_, Mat Kr_, Mat Rl_, Mat Rr_, Mat Dl_, Mat Dr_, bool equi ); 65 | PinholeStereoCamera( const int & width_, const int & height_, 66 | const double & b_, 67 | const Mat & Kl_, const Mat & Kr_, 68 | const Mat & R_, const Mat & t_, 69 | const Mat & Dl_, const Mat & Dr_, 70 | const bool & equi); 71 | 72 | ~PinholeStereoCamera(); 73 | 74 | // Image rectification 75 | void rectifyImage( const Mat& img_src, Mat& img_rec); 76 | void rectifyImagesLR( const Mat& img_src_l, Mat& img_rec_l, const Mat& img_src_r, Mat& img_rec_r ); 77 | 78 | // Proyection and Back-projection 79 | Vector3d backProjection_unit(const double &u, const double &v, const double &disp, double &depth); 80 | Vector3d backProjection(const double &u, const double &v, const double &disp); 81 | Vector2d projection(const Vector3d & P); 82 | Vector3d projectionNH(const Vector3d & P); 83 | Vector2d nonHomogeneous(const Vector3d & x); 84 | 85 | Vector2d normalizedCoordinates(const Vector2d & P ); 86 | double getDisparity( const double pZ ); 87 | Vector2d projectionNormalized(const Vector3d & P ); 88 | Vector2d projectionDistorted(const Vector3d & P, const double & K1, const double & K2, const double & K3 ); 89 | 90 | // Getters 91 | inline const int getWidth() const { return width; }; 92 | inline const int getHeight() const { return height; }; 93 | inline const Matrix3d& getK() const { return K; }; 94 | inline const double getB() const { return b; }; 95 | inline const Matrix getD() const { return d; }; 96 | inline const double getFx() const { return fx; }; 97 | inline const double getFy() const { return fy; }; 98 | inline const double getCx() const { return cx; }; 99 | inline const double getCy() const { return cy; }; 100 | // 101 | inline const cv::Mat getKl() const { return Kl; }; 102 | 103 | }; 104 | 105 | -------------------------------------------------------------------------------- /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 | #include 31 | using namespace mrpt; 32 | using namespace mrpt::gui; 33 | using namespace mrpt::poses; 34 | using namespace mrpt::utils; 35 | using namespace mrpt::math; 36 | using namespace mrpt::opengl; 37 | 38 | #include 39 | using namespace cv; 40 | 41 | #include 42 | using namespace Eigen; 43 | 44 | #include 45 | 46 | namespace PLSLAM{ 47 | 48 | class slamScene{ 49 | 50 | public: 51 | 52 | slamScene(); 53 | slamScene(string configFile); 54 | ~slamScene(); 55 | 56 | void initializeScene(Matrix4d x_0); 57 | void initViewports(int W, int H); 58 | bool updateScene(); 59 | bool updateScene(const MapHandler* map); 60 | void updateSceneGraphs(const MapHandler* map); 61 | 62 | void setText(int frame_, float time_, int nPoints_, int nPointsH_, int nLines_, int nLinesH_); 63 | void setText(int frame_, float time_, int nPoints_, int nLines_ ); 64 | void setPose(Matrix4d x_); 65 | void setGT(Matrix4d xgt_); 66 | void setComparison(Matrix4d xcomp_); 67 | void setImage(Mat image_); 68 | void setImage(string image_); 69 | void setLegend(); 70 | void setHelp(); 71 | void setPoints(CMatrixFloat pData_); 72 | void setLines(CMatrixFloat lData_); 73 | void setStereoCalibration(Matrix3d K_, float b_); 74 | void setKF(Matrix4d Tfw); 75 | 76 | bool waitUntilClose(); 77 | bool isOpen(); 78 | bool getYPR(float &yaw, float &pitch, float &roll); 79 | bool getPose(Matrix4d &T); 80 | 81 | //private: 82 | 83 | CMatrixDouble getPoseFormat(Matrix4d T); 84 | CMatrixDouble33 getCovFormat(MatrixXd cov_); 85 | CPose3D getPoseXYZ(VectorXd x); 86 | 87 | CDisplayWindow3D* win; 88 | COpenGLScenePtr theScene; 89 | COpenGLViewportPtr image, legend, help; 90 | opengl::CSetOfObjectsPtr bbObj, bbObj1, srefObj, srefObj1, gtObj, srefObjGT, elliObjL, elliObjP; 91 | opengl::CEllipsoidPtr elliObj; 92 | opengl::CFrustumPtr frustObj, frustObj1; 93 | opengl::CAxisPtr axesObj; 94 | 95 | opengl::CSetOfLinesPtr lineObj, lineObj_local, kfsLinesObj; 96 | opengl::CPointCloudPtr pointObj, pointObj_local; 97 | opengl::CSetOfObjectsPtr kfsObj; 98 | 99 | 100 | float sbb, saxis, srad, sref, sline, sfreq, szoom, selli, selev, sazim, sfrust, slinef; 101 | CVectorDouble v_aux, v_aux_, v_aux1, v_aux1_, v_auxgt, gt_aux_, v_auxgt_; 102 | CPose3D pose, pose_0, pose_gt, pose_ini, ellPose, pose1, change, frustumL_, frustumR_; 103 | Matrix4d x_ini; 104 | mrptKeyModifier kmods; 105 | int key; 106 | CMatrixDouble33 cov3D; 107 | bool hasText, hasCov, hasGT, hasImg, hasLines, hasPoints, hasFrustum, hasComparison, hasLegend, hasHelp, hasAxes, hasTraj, isKitti; 108 | 109 | Matrix4d x, xgt, xcomp; 110 | MatrixXd cov, W; 111 | unsigned int frame, nPoints, nPointsH, nLines, nLinesH; 112 | float time; 113 | string img, img_legend, img_help; 114 | CMatrixFloat lData, pData; 115 | CImage img_mrpt_legend, img_mrpt_image, img_mrpt_help; 116 | 117 | float b, sigmaP, sigmaL, f, cx, cy, bsigmaL, bsigmaP; 118 | 119 | }; 120 | 121 | } 122 | -------------------------------------------------------------------------------- /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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 | ** 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 | 26 | typedef Matrix Matrix6d; 27 | typedef Matrix Vector6d; 28 | 29 | 30 | #define DO_VIZ 31 | // #define SIMU_MOTION_BLUR 32 | 33 | 34 | class StereoFrame; 35 | 36 | namespace StVO { 37 | 38 | class StereoFrameHandler 39 | { 40 | 41 | public: 42 | 43 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 44 | 45 | StereoFrameHandler( PinholeStereoCamera* cam_ ); 46 | ~StereoFrameHandler(); 47 | 48 | void initialize( const Mat & img_l_, const Mat & img_r_, 49 | const int idx_, const double time_stamp_); 50 | void updateFrame(); 51 | void updateFrame_ECCV18( const Matrix4d T_base ); 52 | void insertStereoPair(const Mat & img_l_, const Mat & img_r_, 53 | const int idx_, const double time_stamp_); 54 | // 55 | void crossFrameMatching_Brute(); 56 | void crossFrameMatching_Hybrid(); 57 | void crossFrameMatching_Proj(); 58 | // 59 | void optimizePose(); 60 | void optimizePose(Matrix4d DT_ini); 61 | double lineSegmentOverlap( Vector2d spl_obs, Vector2d epl_obs, 62 | Vector2d spl_proj, Vector2d epl_proj ); 63 | 64 | // 65 | /* parameter setup / update */ 66 | void setLeftCamExtrinCalib(const double cam_rx, 67 | const double cam_ry, 68 | const double cam_rz, 69 | const double cam_tx, 70 | const double cam_ty, 71 | const double cam_tz); 72 | void predictFramePose(); 73 | void setFramePose(const cv::Matx44f Tfw_curr); 74 | // 75 | /* line uncertainty modeling */ 76 | void estimateProjUncertainty_greedy(); 77 | void estimateProjUncertainty_descent(const double stepCutRatio, 78 | const double rngCutRatio[2]); 79 | void estimateProjUncertainty_submodular(const double stepCutRatio, 80 | const double rngCutRatio[2]); 81 | // 82 | void getPoseCovMatrixOnLine(const int fst_idx, 83 | const int lst_idx, 84 | const Matrix rigframe_T_world, 85 | const list::iterator it, 86 | Matrix & cov_pose_inv); 87 | 88 | void getPoseCovOnLine(const Matrix4d DT_inv, 89 | const Vector2d J_loss, 90 | const StVO::LineFeature * line, 91 | const double cutRatio[2], 92 | Matrix & cov_pose); 93 | 94 | void getPoseInfoOnLine(const Matrix4d DT_inv, 95 | const Vector2d J_loss, 96 | const StVO::LineFeature * line, 97 | const double cutRatio[2], 98 | Matrix & info_pose); 99 | 100 | void getPoseInfoPoint(const Matrix4d DT_inv, 101 | const StVO::PointFeature * point, 102 | Matrix & info_pose); 103 | 104 | void updateEndPointByRatio(StVO::LineFeature * line); 105 | 106 | void greedySolveLineCut_P3L(const Matrix4d DT_inv, 107 | const double stepCutRatio, 108 | const double rngCutRatio[2], 109 | vector line_P3L); 110 | 111 | // 112 | /* viz functions */ 113 | void plotLine3D( bool save_screen_shot ); 114 | void plotMatchedLines( bool vizUncertainty, bool save_screen_shot ); 115 | void plotMatchedPointLine(bool save_screen_shot); 116 | void appendPlotCutLine(bool save_screen_shot); 117 | void appendPlotOptLine(bool save_screen_shot); 118 | 119 | 120 | cv::viz::Viz3d * viz_3D_lines; 121 | cv::Mat canvas_match_frames; 122 | 123 | // adaptative fast 124 | int orb_fast_th; 125 | 126 | // slam-specific functions 127 | bool needNewKF(); 128 | void currFrameIsKF(); 129 | 130 | /* variables used in cross-frame line matching */ 131 | list matched_pt; 132 | list matched_ls; 133 | 134 | StereoFrame* prev_frame; 135 | StereoFrame* curr_frame; 136 | PinholeStereoCamera* cam; 137 | 138 | ORB_SLAM2::ORBextractor * mpORBextractor_l; 139 | ORB_SLAM2::ORBextractor * mpORBextractor_r; 140 | 141 | Matrix left_cam_T_rigframe; 142 | double NUM_PTS_SAMPLE = 50.0; // 20.0; 143 | 144 | int n_inliers, n_inliers_pt, n_inliers_ls; 145 | 146 | int numFrameLoss = 0; 147 | int numFrameSinceKeyframe = 0; 148 | 149 | // slam-specific variables 150 | bool prev_f_iskf; 151 | double entropy_first_prevKF; 152 | Matrix4d T_prevKF; 153 | Matrix6d cov_prevKF_currF; 154 | 155 | // 156 | /* variables used in debug and figure generation */ 157 | bool save_screen_shot = true; // false; // 158 | bool print_frame_info = false; // true; // 159 | 160 | // vector< std::pair > T_allF; 161 | vector> vec_all_frame_pose; 162 | 163 | private: 164 | 165 | void removeOutliers( Matrix4d DT ); 166 | void gaussNewtonOptimization(Matrix4d &DT, Matrix6d &DT_cov, double &err_, int max_iters); 167 | void optimizeFunctions(Matrix4d DT, Matrix6d &H, Vector6d &g, double &e); 168 | 169 | Vector6d prior_inc; 170 | Matrix6d prior_cov; 171 | std::vector line_projection_end; 172 | std::vector line_projection_start; 173 | 174 | }; 175 | 176 | } 177 | -------------------------------------------------------------------------------- /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 | #include 33 | using namespace mrpt; 34 | using namespace mrpt::gui; 35 | using namespace mrpt::poses; 36 | using namespace mrpt::utils; 37 | using namespace mrpt::math; 38 | using namespace mrpt::opengl; 39 | using namespace mrpt::maps; 40 | 41 | #include 42 | using namespace cv; 43 | 44 | #include 45 | using namespace Eigen; 46 | 47 | #include 48 | using namespace StVO; 49 | 50 | class voScene{ 51 | 52 | public: 53 | 54 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 55 | 56 | voScene(); 57 | voScene(string configFile); 58 | ~voScene(); 59 | void initialize3DScene(Matrix4d x_0); 60 | void initialize3DSceneLines(Matrix4d x_0); 61 | void initialize3DSceneImg(Matrix4d x_0); 62 | void initialize3DSceneGT(Matrix4d x_0); 63 | 64 | void initializeScene(Matrix4d x_0, bool has_gt); 65 | void initializeScene(Matrix4d x_0, Matrix4d x_0gt); 66 | 67 | bool updateScene(); 68 | bool updateScene(list matched_pt); 69 | void plotPointsCovariances(); 70 | void plotLinesCovariances(); 71 | 72 | void setText(int frame_, float time_, int nPoints_, int nPointsH_, int nLines_, int nLinesH_); 73 | void setCov(MatrixXd cov_); 74 | void setPose(Matrix4d x_); 75 | void setGT(Matrix4d xgt_); 76 | void setComparison(Matrix4d xcomp_); 77 | void setImage(Mat image_); 78 | void setImage(string image_); 79 | void setLegend(); 80 | void setHelp(); 81 | void setPoints(CMatrixFloat pData_); 82 | void setLines(CMatrixFloat lData_); 83 | void setStereoCalibration(Matrix3d K_, float b_); 84 | void setKF(); 85 | void setKF(Matrix4d Tfw); 86 | 87 | bool waitUntilClose(); 88 | bool isOpen(); 89 | bool getYPR(float &yaw, float &pitch, float &roll); 90 | bool getPose(Matrix4d &T); 91 | 92 | private: 93 | 94 | CMatrixDouble getPoseFormat(Matrix4d T); 95 | CMatrixDouble33 getCovFormat(MatrixXd cov_); 96 | CPose3D getPoseXYZ(VectorXd x); 97 | 98 | CDisplayWindow3D* win; 99 | COpenGLScenePtr theScene; 100 | COpenGLViewportPtr image, legend, help; 101 | opengl::CSetOfObjectsPtr bbObj, bbObj1, srefObj, srefObj1, gtObj, srefObjGT, elliObjL, elliObjP; 102 | opengl::CEllipsoidPtr elliObj; 103 | opengl::CSetOfLinesPtr lineObj; 104 | opengl::CPointCloudPtr pointObj; 105 | 106 | //CPointsMapPtr pointsObj; 107 | 108 | opengl::CFrustumPtr frustObj, frustObj1; 109 | opengl::CAxisPtr axesObj; 110 | 111 | 112 | float sbb, saxis, srad, sref, sline, sfreq, szoom, selli, selev, sazim, sfrust, slinef; 113 | CVectorDouble v_aux, v_aux_, v_aux1, v_aux1_, v_auxgt, gt_aux_, v_auxgt_; 114 | CPose3D pose, pose_0, pose_gt, pose_ini, ellPose, pose1, change, frustumL_, frustumR_; 115 | Matrix4d x_ini; 116 | mrptKeyModifier kmods; 117 | int key; 118 | CMatrixDouble33 cov3D; 119 | bool hasCamFix, hasText, hasCov, hasGT, hasChange, hasImg, hasLines, hasPoints, hasFrustum, hasComparison, hasLegend, hasHelp, hasAxes, hasTraj, isKitti; 120 | 121 | Matrix4d x, xgt, xcomp; 122 | MatrixXd cov, W; 123 | unsigned int frame, nPoints, nPointsH, nLines, nLinesH; 124 | float time; 125 | string img, img_legend, img_help; 126 | CMatrixFloat lData, pData; 127 | CImage img_mrpt_legend, img_mrpt_image, img_mrpt_help; 128 | 129 | float b, sigmaP, sigmaL, f, cx, cy, bsigmaL, bsigmaP; 130 | 131 | }; 132 | 133 | -------------------------------------------------------------------------------- /simu/line_debug.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | 6 | # To read images 7 | from scipy.misc import imread,imsave,imresize 8 | 9 | # import pcaflow.parameters as defaults 10 | # from pcaflow.features.FeatureMatcherLibviso import FeatureMatcherLibviso 11 | # from pcaflow.features.FeatureMatcherORB import FeatureMatcherORB 12 | # from pcaflow.utils.show_correspondences import show_correspondences 13 | 14 | def drawlines(img1,img2,lines,pts1,pts2): 15 | ''' img1 - image on which we draw the epilines for the points in img2 16 | lines - corresponding epilines ''' 17 | r,c,l = img1.shape 18 | # img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR) 19 | # img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR) 20 | for r,pt1,pt2 in zip(lines,pts1,pts2): 21 | color = tuple(np.random.randint(0,255,3).tolist()) 22 | x0,y0 = map(int, [0, -r[2]/r[1] ]) 23 | x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ]) 24 | img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1) 25 | img1 = cv2.circle(img1,tuple(pt1),5,color,-1) 26 | img2 = cv2.circle(img2,tuple(pt2),5,color,-1) 27 | return img1,img2 28 | 29 | def inverseMap(mapx, mapy): 30 | assert mapx.shape == mapy.shape 31 | mapx_inv = np.zeros(mapx.shape) 32 | mapy_inv = np.zeros(mapy.shape) 33 | for row in range(mapx.shape[0]): 34 | for col in range(mapx.shape[1]): 35 | dx = mapx[row][col] 36 | dy = mapy[row][col] 37 | print dx, dy 38 | mapx_inv[dx][dy] = row 39 | mapy_inv[dx][dy] = col 40 | return mapx_inv, mapy_inv 41 | 42 | # load text log 43 | f = open('/home/pang/workspace/leapcore/malleus/build/line_number.log', 'r') 44 | 45 | i = 0 46 | frame_idx = np.zeros((10000, 1)) 47 | num_line_prev = np.zeros((10000, 1)) 48 | num_line_curr = np.zeros((10000, 1)) 49 | num_line_match = np.zeros((10000, 1)) 50 | 51 | for line in f: 52 | # print line 53 | data_tmp = line.split() 54 | frame_idx[i] = data_tmp[0] 55 | num_line_prev[i] = data_tmp[1] 56 | num_line_curr[i] = data_tmp[2] 57 | num_line_match[i] = data_tmp[3] 58 | i = i + 1 59 | 60 | 61 | # plot trend of line number 62 | # plt.plot(frame_idx, num_line_match) 63 | plt.plot(frame_idx[:i], num_line_curr[:i], 'bs--', label="number of stereo line") 64 | plt.plot(frame_idx[:i], num_line_match[:i], 'rs--', label="number of matched line") 65 | plt.legend(shadow=True, fancybox=True) 66 | plt.show() -------------------------------------------------------------------------------- /simu/line_detect_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | from matplotlib import gridspec 6 | import matplotlib.lines as lines 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | # load text log - prev 12 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/lsd/frame_line_info.log', 'r') 13 | 14 | i = 0 15 | frame_idx_1 = np.zeros((1000000, 1)) 16 | match_line_stereo_1 = np.zeros((1000000, 1)) 17 | left_line_detect_1 = np.zeros((1000000, 1)) 18 | right_line_detect_1 = np.zeros((1000000, 1)) 19 | 20 | for line in f: 21 | # print line 22 | data_tmp = line.split() 23 | # print data_tmp 24 | frame_idx_1[i] = data_tmp[0] 25 | match_line_stereo_1[i] = data_tmp[2] 26 | left_line_detect_1[i] = data_tmp[4] 27 | right_line_detect_1[i] = data_tmp[5] 28 | i = i + 1 29 | 30 | # load text log - curr 31 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/polyline/frame_line_info.log', 'r') 32 | 33 | j = 0 34 | frame_idx_2 = np.zeros((1000000, 1)) 35 | match_line_stereo_2 = np.zeros((1000000, 1)) 36 | left_line_detect_2 = np.zeros((1000000, 1)) 37 | right_line_detect_2 = np.zeros((1000000, 1)) 38 | 39 | for line in f: 40 | # print line 41 | data_tmp = line.split() 42 | # print data_tmp 43 | frame_idx_2[j] = data_tmp[0] 44 | match_line_stereo_2[j] = data_tmp[2] 45 | left_line_detect_2[j] = data_tmp[4] 46 | right_line_detect_2[j] = data_tmp[5] 47 | j = j + 1 48 | 49 | 50 | plot_range = 50 51 | 52 | fig = plt.figure() 53 | gs = gridspec.GridSpec(1, 2, width_ratios=[3, 1]) 54 | 55 | ax1 = plt.subplot(gs[0]) 56 | # plot trend of line number 57 | ax1.plot(frame_idx_1[:i], left_line_detect_1[:i], color='r', marker='*', alpha=.4, label="lines detected with LSD") 58 | ax1.plot(frame_idx_2[:j], left_line_detect_2[:j], color='g', marker='*', alpha=.4, label="lines detected with PolyLine") 59 | ax1.legend(shadow=True, fancybox=True) 60 | ax1.set_xlabel('frame index') 61 | ax1.set_ylabel('number of lines') 62 | ax1.set_xlim(0, frame_idx_2[j-1]) 63 | # ax1.set_ylim(-plot_range, plot_range) 64 | 65 | ax2 = plt.subplot(gs[1]) 66 | data=[left_line_detect_1[:i], left_line_detect_2[:j]] 67 | ax2.boxplot(data) 68 | ax2.set_ylabel('number of lines') 69 | 70 | plt.show() 71 | 72 | -------------------------------------------------------------------------------- /simu/line_match_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | from matplotlib import gridspec 6 | import matplotlib.lines as lines 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | # load text log - prev 12 | # f = open('/home/yzhao/Data/22fa0d84-433c-482a-9aff-5b9caa4cb839/debug/line_no_projection/frame_line_info.log', 'r') 13 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/matching/lbd/frame_line_info.log', 'r') 14 | 15 | i = 0 16 | frame_idx_1 = np.zeros((1000000, 1)) 17 | match_line_frame_1 = np.zeros((1000000, 1)) 18 | left_line_detect_1 = np.zeros((1000000, 1)) 19 | right_line_detect_1 = np.zeros((1000000, 1)) 20 | 21 | for line in f: 22 | # print line 23 | data_tmp = line.split() 24 | # print data_tmp 25 | frame_idx_1[i] = data_tmp[0] 26 | match_line_frame_1[i] = data_tmp[3] 27 | left_line_detect_1[i] = data_tmp[4] 28 | right_line_detect_1[i] = data_tmp[5] 29 | i = i + 1 30 | 31 | # load text log - curr 32 | # f = open('/home/yzhao/Data/22fa0d84-433c-482a-9aff-5b9caa4cb839/debug/line_full_pipeline/frame_line_info.log', 'r') 33 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/matching/proj/frame_line_info.log', 'r') 34 | 35 | i = 0 36 | frame_idx_2 = np.zeros((1000000, 1)) 37 | match_line_frame_2 = np.zeros((1000000, 1)) 38 | left_line_detect_2 = np.zeros((1000000, 1)) 39 | right_line_detect_2 = np.zeros((1000000, 1)) 40 | 41 | for line in f: 42 | # print line 43 | data_tmp = line.split() 44 | # print data_tmp 45 | frame_idx_2[i] = data_tmp[0] 46 | match_line_frame_2[i] = data_tmp[3] 47 | left_line_detect_2[i] = data_tmp[4] 48 | right_line_detect_2[i] = data_tmp[5] 49 | i = i + 1 50 | 51 | 52 | plot_range = 50 53 | 54 | fig = plt.figure() 55 | gs = gridspec.GridSpec(1, 2, width_ratios=[3, 1]) 56 | 57 | ax1 = fig.add_subplot(gs[0]) 58 | # plot trend of line number 59 | # plt.plot(frame_idx, num_line_match) 60 | ax1.plot(frame_idx_1[:i], match_line_frame_1[:i], color='r', marker='.', alpha=.4, label="Bidirectional Best LBD") 61 | # ax1.plot(frame_idx_1[:i], right_line_detect_1[:i], color='r', marker='.', alpha=.4) 62 | ax1.plot(frame_idx_2[:i], match_line_frame_2[:i], color='g', marker='.', alpha=.4, label="Prediction + LBD + Distance Check") 63 | # ax1.plot(frame_idx_2[:i], right_line_detect_2[:i], color='g', marker='.', alpha=.4) 64 | # ax1.plot(frame_idx[:i], prev_line_stereo[:i], color='r', marker='.', alpha=.4, label="prev. num. stereo lines") 65 | # ax1.plot(frame_idx[:i], curr_line_stereo[:i], color='g', marker='.', alpha=.4, label="curr. num. stereo lines") 66 | # ax1.plot(frame_idx[:i], match_line_stereo[:i], color='b', marker='.', alpha=.4, label="num. matched stereo lines") 67 | # ax1.set_yscale('log') 68 | ax1.legend(shadow=True, fancybox=True) 69 | ax1.set_xlabel('frame index') 70 | ax1.set_ylabel('number of lines being matched cross-frame') 71 | # ax1.set_xlim(-plot_range, plot_range) 72 | # ax1.set_ylim(-plot_range, plot_range) 73 | 74 | ax2 = fig.add_subplot(gs[1]) 75 | data=[match_line_frame_1[:i], match_line_frame_2[:i]] 76 | ax2.boxplot(data) 77 | ax2.set_ylabel('number of lines being matched cross-frame') 78 | 79 | plt.show() 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /simu/line_merge_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | import matplotlib.lines as lines 6 | 7 | # To read images 8 | from scipy.misc import imread,imsave,imresize 9 | 10 | # load text log - prev 11 | f = open('/home/yzhao/Data/22fa0d84-433c-482a-9aff-5b9caa4cb839/debug/line_no_merging/frame_line_info.log', 'r') 12 | 13 | i = 0 14 | frame_idx_1 = np.zeros((1000000, 1)) 15 | match_line_stereo_1 = np.zeros((1000000, 1)) 16 | left_line_detect_1 = np.zeros((1000000, 1)) 17 | right_line_detect_1 = np.zeros((1000000, 1)) 18 | 19 | for line in f: 20 | # print line 21 | data_tmp = line.split() 22 | # print data_tmp 23 | frame_idx_1[i] = data_tmp[0] 24 | match_line_stereo_1[i] = data_tmp[2] 25 | left_line_detect_1[i] = data_tmp[4] 26 | right_line_detect_1[i] = data_tmp[5] 27 | i = i + 1 28 | 29 | # load text log - curr 30 | f = open('/home/yzhao/Data/22fa0d84-433c-482a-9aff-5b9caa4cb839/debug/line_no_SSD/frame_line_info.log', 'r') 31 | 32 | i = 0 33 | frame_idx_2 = np.zeros((1000000, 1)) 34 | match_line_stereo_2 = np.zeros((1000000, 1)) 35 | left_line_detect_2 = np.zeros((1000000, 1)) 36 | right_line_detect_2 = np.zeros((1000000, 1)) 37 | 38 | for line in f: 39 | # print line 40 | data_tmp = line.split() 41 | # print data_tmp 42 | frame_idx_2[i] = data_tmp[0] 43 | match_line_stereo_2[i] = data_tmp[2] 44 | left_line_detect_2[i] = data_tmp[4] 45 | right_line_detect_2[i] = data_tmp[5] 46 | i = i + 1 47 | 48 | 49 | plot_range = 50 50 | 51 | # fig = plt.figure() 52 | 53 | # ax1 = fig.add_subplot(111) 54 | # # plot trend of line number 55 | # # plt.plot(frame_idx, num_line_match) 56 | # ax1.plot(frame_idx_1[:i], left_line_detect_1[:i], color='r', marker='.', alpha=.4, label="without line merging") 57 | # # ax1.plot(frame_idx_1[:i], right_line_detect_1[:i], color='r', marker='.', alpha=.4) 58 | # ax1.plot(frame_idx_2[:i], left_line_detect_2[:i], color='g', marker='.', alpha=.4, label="with line merging") 59 | # # ax1.plot(frame_idx_2[:i], right_line_detect_2[:i], color='g', marker='.', alpha=.4) 60 | # # ax1.plot(frame_idx[:i], prev_line_stereo[:i], color='r', marker='.', alpha=.4, label="prev. num. stereo lines") 61 | # # ax1.plot(frame_idx[:i], curr_line_stereo[:i], color='g', marker='.', alpha=.4, label="curr. num. stereo lines") 62 | # # ax1.plot(frame_idx[:i], match_line_stereo[:i], color='b', marker='.', alpha=.4, label="num. matched stereo lines") 63 | # # ax1.set_yscale('log') 64 | # ax1.legend(shadow=True, fancybox=True) 65 | # ax1.set_xlabel('frame index') 66 | # ax1.set_ylabel('number of lines being detected (left frame only)') 67 | # # ax1.set_xlim(-plot_range, plot_range) 68 | # # ax1.set_ylim(-plot_range, plot_range) 69 | 70 | # plt.show() 71 | 72 | 73 | 74 | 75 | fig = plt.figure() 76 | 77 | ax1 = fig.add_subplot(111) 78 | ax1.plot(frame_idx_1[:i], left_line_detect_1[:i], color='r', marker='.', alpha=.4, label="without line merging") 79 | ax1.plot(frame_idx_2[:i], left_line_detect_2[:i], color='g', marker='.', alpha=.4, label="with line merging") 80 | ax1.legend(shadow=True, fancybox=True) 81 | ax1.set_xlabel('frame index') 82 | ax1.set_ylabel('number of lines being stereo matched') 83 | plt.show() -------------------------------------------------------------------------------- /simu/line_stereo_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | from matplotlib import gridspec 6 | import matplotlib.lines as lines 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | # load text log - prev 12 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/lbd/frame_line_info.log', 'r') 13 | 14 | i = 0 15 | frame_idx_1 = np.zeros((1000000, 1)) 16 | match_line_stereo_1 = np.zeros((1000000, 1)) 17 | left_line_detect_1 = np.zeros((1000000, 1)) 18 | right_line_detect_1 = np.zeros((1000000, 1)) 19 | 20 | for line in f: 21 | # print line 22 | data_tmp = line.split() 23 | # print data_tmp 24 | frame_idx_1[i] = data_tmp[0] 25 | match_line_stereo_1[i] = data_tmp[2] 26 | left_line_detect_1[i] = data_tmp[4] 27 | right_line_detect_1[i] = data_tmp[5] 28 | i = i + 1 29 | 30 | # load text log - curr 31 | f = open('/home/yzhao/Data/96edfbf2-d999-490c-b1c8-d10291c6cce4/debug/ssd/frame_line_info.log', 'r') 32 | 33 | j = 0 34 | frame_idx_2 = np.zeros((1000000, 1)) 35 | match_line_stereo_2 = np.zeros((1000000, 1)) 36 | left_line_detect_2 = np.zeros((1000000, 1)) 37 | right_line_detect_2 = np.zeros((1000000, 1)) 38 | 39 | for line in f: 40 | # print line 41 | data_tmp = line.split() 42 | # print data_tmp 43 | frame_idx_2[j] = data_tmp[0] 44 | match_line_stereo_2[j] = data_tmp[2] 45 | left_line_detect_2[j] = data_tmp[4] 46 | right_line_detect_2[j] = data_tmp[5] 47 | j = j + 1 48 | 49 | 50 | plot_range = 50 51 | 52 | fig = plt.figure() 53 | gs = gridspec.GridSpec(1, 2, width_ratios=[3, 1]) 54 | 55 | ax3 = plt.subplot(gs[0]) 56 | # plot trend of line number 57 | ax3.plot(frame_idx_1[:i], match_line_stereo_1[:i], color='r', marker='.', alpha=.4, label="lines stereo matched with LBD") 58 | ax3.plot(frame_idx_2[:j], match_line_stereo_2[:j], color='g', marker='.', alpha=.4, label="lines stereo matched with SSD") 59 | ax3.legend(shadow=True, fancybox=True) 60 | ax3.set_xlabel('frame index') 61 | ax3.set_ylabel('number of lines') 62 | ax3.set_xlim(0, frame_idx_2[j-1]) 63 | # ax1.set_ylim(-plot_range, plot_range) 64 | 65 | ax4 = plt.subplot(gs[1]) 66 | data=[match_line_stereo_1[:i], match_line_stereo_2[:j]] 67 | ax4.boxplot(data) 68 | ax4.set_ylabel('number of lines') 69 | 70 | 71 | plt.show() 72 | 73 | -------------------------------------------------------------------------------- /simu/project_error_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | import matplotlib.lines as lines 6 | from mpl_toolkits.mplot3d import Axes3D 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | 12 | # load text log 13 | f = open('/home/yzhao/Data/75c27f4e-56f0-4d5d-b06b-405e85dede71/debug/evaluation_GT/proj_eval_res.log', 'r') 14 | 15 | i = 0 16 | frame_idx = np.zeros((1000000, 1)) 17 | line_prev_idx = np.zeros((1000000, 1)) 18 | line_curr_idx = np.zeros((1000000, 1)) 19 | sPt_u = np.zeros((1000000, 1)) 20 | sPt_v = np.zeros((1000000, 1)) 21 | ePt_u = np.zeros((1000000, 1)) 22 | ePt_v = np.zeros((1000000, 1)) 23 | sdist_Dline2Ppt = np.zeros((1000000, 1)) 24 | edist_Dline2Ppt = np.zeros((1000000, 1)) 25 | sdist_Dline2Tpt = np.zeros((1000000, 1)) 26 | edist_Dline2Tpt = np.zeros((1000000, 1)) 27 | sdist_Dpt2Pline = np.zeros((1000000, 1)) 28 | edist_Dpt2Pline = np.zeros((1000000, 1)) 29 | sdist_Dpt2Tline = np.zeros((1000000, 1)) 30 | edist_Dpt2Tline = np.zeros((1000000, 1)) 31 | 32 | for line in f: 33 | # print line 34 | data_tmp = line.split() 35 | # print data_tmp 36 | 37 | # print data_tmp[0] 38 | # if data_tmp[0] > 200: 39 | # break 40 | 41 | frame_idx[i] = data_tmp[0] 42 | line_prev_idx[i] = data_tmp[1] 43 | line_curr_idx[i] = data_tmp[2] 44 | sPt_u[i] = data_tmp[3] 45 | sPt_v[i] = data_tmp[4] 46 | ePt_u[i] = data_tmp[5] 47 | ePt_v[i] = data_tmp[6] 48 | sdist_Dline2Ppt[i] = data_tmp[7] 49 | edist_Dline2Ppt[i] = data_tmp[8] 50 | sdist_Dline2Tpt[i] = data_tmp[9] 51 | edist_Dline2Tpt[i] = data_tmp[10] 52 | sdist_Dpt2Pline[i] = data_tmp[11] 53 | edist_Dpt2Pline[i] = data_tmp[12] 54 | sdist_Dpt2Tline[i] = data_tmp[13] 55 | edist_Dpt2Tline[i] = data_tmp[14] 56 | 57 | i = i + 1 58 | 59 | print i 60 | 61 | plot_range = 50 # 200 # 62 | 63 | sample_interval = 20 64 | 65 | fig = plt.figure() 66 | 67 | # ax1 = fig.add_subplot(121) 68 | # ax1.scatter(frame_idx[:i], sdist_Dline2Tpt[:i], color='r', s=2, marker='.', alpha=.4, label="proj. err. of sPt") 69 | # ax1.scatter(frame_idx[:i], edist_Dline2Tpt[:i], color='g', s=2, marker='.', alpha=.4, label="proj. err. of ePt") 70 | # ax1.set_ylim(-plot_range, plot_range) 71 | 72 | # ax2 = fig.add_subplot(122) 73 | # ax2.scatter(frame_idx[:i], sdist_Dpt2Tline[:i], color='r', s=2, marker='.', alpha=.4, label="proj. err. of sPt") 74 | # ax2.scatter(frame_idx[:i], edist_Dpt2Tline[:i], color='g', s=2, marker='.', alpha=.4, label="proj. err. of ePt") 75 | # ax2.set_ylim(-plot_range, plot_range) 76 | 77 | ax1 = fig.add_subplot(121) 78 | ax1.plot([-plot_range, plot_range], [-plot_range, plot_range], 'k--', picker=2) 79 | # plot trend of line number 80 | # plt.plot(frame_idx, num_line_match) 81 | ax1.scatter(sdist_Dline2Tpt[:i], sdist_Dline2Ppt[:i], color='r', s=10, marker='.', alpha=.4, label="proj. err. of sPt") 82 | ax1.scatter(edist_Dline2Tpt[:i], edist_Dline2Ppt[:i], color='g', s=10, marker='.', alpha=.4, label="proj. err. of ePt") 83 | ax1.legend(shadow=True, fancybox=True) 84 | ax1.set_xlabel('ground truth point to detected line (pixel)') 85 | ax1.set_ylabel('projected point to detected line (pixel)') 86 | ax1.set_xlim(-plot_range, plot_range) 87 | ax1.set_ylim(-plot_range, plot_range) 88 | 89 | ax2 = fig.add_subplot(122) 90 | ax2.plot([-plot_range, plot_range], [-plot_range, plot_range], 'k--', picker=2) 91 | ax2.scatter(sdist_Dpt2Tline[:i], sdist_Dpt2Pline[:i], color='r', s=10, marker='.', alpha=.4, label="proj. err. of sPt") 92 | ax2.scatter(edist_Dpt2Tline[:i], edist_Dpt2Pline[:i], color='g', s=10, marker='.', alpha=.4, label="proj. err. of ePt") 93 | ax2.legend(shadow=True, fancybox=True) 94 | ax2.set_xlabel('detected point to ground truth line (pixel)') 95 | ax2.set_ylabel('detected point to projected line (pixel)') 96 | ax2.set_xlim(-plot_range, plot_range) 97 | ax2.set_ylim(-plot_range, plot_range) 98 | 99 | # ax3 = fig.add_subplot(111, projection='3d') 100 | # ax3.scatter(sPt_u[0:i:sample_interval], sPt_v[0:i:sample_interval], sdist_Dline2Ppt[0:i:sample_interval], color='r', marker='.', alpha=.4, label="proj. err. of sPt") 101 | # ax3.scatter(ePt_u[0:i:sample_interval], ePt_v[0:i:sample_interval], edist_Dline2Ppt[0:i:sample_interval], color='g', marker='.', alpha=.4, label="proj. err. of ePt") 102 | # ax3.legend(shadow=True, fancybox=True) 103 | # ax3.set_xlabel('u (pixel)') 104 | # ax3.set_ylabel('v (pixel)') 105 | # ax3.set_zlabel('projected point to detected line (pixel)') 106 | 107 | plt.show() -------------------------------------------------------------------------------- /simu/simu_line_vol_assessment.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | from matplotlib import gridspec 6 | from mpl_toolkits.mplot3d import Axes3D 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | def fit_z(z): 12 | return 525.0*0.12/z * 0.2 13 | # return np.exp(-(t-1)*0.4) * 20 14 | 15 | # load text log 16 | f = open('./simulate_line_vol.log', 'r') 17 | 18 | i = 0 19 | sP = np.zeros((100000, 3)) 20 | eP = np.zeros((100000, 3)) 21 | t = np.zeros((100000, 3)) 22 | q = np.zeros((100000, 4)) 23 | vol_st = np.zeros((100000, 49)) 24 | vol_ed = np.zeros((100000, 49)) 25 | 26 | for line in f: 27 | # print line 28 | data_tmp = line.split() 29 | # print data_tmp 30 | sP[i, 0:3] = data_tmp[0:3] 31 | eP[i, 0:3] = data_tmp[3:6] 32 | t[i, 0:3] = data_tmp[6:9] 33 | q[i, 0:4] = data_tmp[9:13] 34 | vol_st[i, 0:49] = data_tmp[13:62] 35 | vol_ed[i, 0:49] = data_tmp[62:111] 36 | 37 | i = i + 1 38 | 39 | 40 | # i = 5000 41 | 42 | sample_rate = 1 43 | 44 | fig = plt.figure() 45 | gs = gridspec.GridSpec(2, 2, width_ratios=[1, 1]) 46 | 47 | ax1 = plt.subplot(gs[0]) 48 | for pn in range(0, i, sample_rate): 49 | # ax1.scatter(np.ones((i, 1)) * pn, vol[:i, pn], color='r', s=2, marker='.', alpha=.4) 50 | # ax1.plot(range(0, 100, 2), vol[pn, :], color='r', marker='.', alpha=.4) 51 | if vol_st[pn, 0] > vol_st[pn, 1]: 52 | ax1.plot(range(0, 98, 2), vol_st[pn, :], color='r', marker='.', alpha=.4) 53 | else: 54 | ax1.plot(range(0, 98, 2), vol_st[pn, :], color='g', marker='.', alpha=.4) 55 | # ax1.set_yscale('log') 56 | # ax1.set_ylim(-plot_range, plot_range) 57 | ax1.legend(shadow=True, fancybox=True) 58 | ax1.set_xlabel('Portion of Line Being Cut (%)') 59 | ax1.set_ylabel('Volume of Pose Covariance Matrix') 60 | ax1.set_xlim(0, 100) 61 | 62 | ax2 = plt.subplot(gs[1]) 63 | # labels = list('ABCD') 64 | # data = [] 65 | # for cn in range(0, 20, 1): 66 | # data = [data, vol[0:i:sample_rate, cn]] 67 | ax2.boxplot(vol_st[0:i:sample_rate, :]) 68 | ax2.set_yscale('log') 69 | # ax2.set_xticklabels(['0', '5', '10', '15', '20', '25', '30', '35', '40', '45', '50', '55', '60', '65', '70', '75', '80', '85', '90', '95']) 70 | # print data 71 | # dataArr = np.asarray(data) 72 | # print dataArr.shape 73 | # ax2.boxplot(dataArr) 74 | ax2.set_xlabel('Portion of Line Being Cut (%)') 75 | ax2.set_ylabel('Volume of Pose Covariance Matrix') 76 | 77 | ax3 = plt.subplot(gs[2]) 78 | for pn in range(0, i, sample_rate): 79 | if vol_ed[pn, 0] > vol_ed[pn, 1]: 80 | ax3.plot(range(0, 98, 2), vol_ed[pn, :], color='r', marker='.', alpha=.4) 81 | else: 82 | ax3.plot(range(0, 98, 2), vol_ed[pn, :], color='g', marker='.', alpha=.4) 83 | # ax1.set_yscale('log') 84 | ax3.legend(shadow=True, fancybox=True) 85 | ax3.set_xlabel('Portion of Line Being Cut (%)') 86 | ax3.set_ylabel('Volume of Pose Covariance Matrix') 87 | ax3.set_xlim(0, 100) 88 | 89 | ax4 = plt.subplot(gs[3]) 90 | ax4.boxplot(vol_ed[0:i:sample_rate, :]) 91 | ax4.set_yscale('log') 92 | # ax2.set_xticklabels(['0', '5', '10', '15', '20', '25', '30', '35', '40', '45', '50', '55', '60', '65', '70', '75', '80', '85', '90', '95']) 93 | ax2.set_xlabel('Portion of Line Being Cut (%)') 94 | ax2.set_ylabel('Volume of Pose Covariance Matrix') 95 | 96 | plt.show() 97 | -------------------------------------------------------------------------------- /simu/simu_point_line_loss.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import sys,os 4 | from matplotlib import pyplot as plt 5 | from matplotlib import gridspec 6 | from mpl_toolkits.mplot3d import Axes3D 7 | 8 | # To read images 9 | from scipy.misc import imread,imsave,imresize 10 | 11 | def fit_z(z): 12 | return 525.0*0.12/z * 0.2 13 | # return np.exp(-(t-1)*0.4) * 20 14 | 15 | # load text log 16 | f = open('/home/yzhao/Eval/simulate_pl_loss_tX.log', 'r') 17 | 18 | nPL = 0 19 | nPP = 0 20 | vPL = 0 21 | vPP = 0 22 | p3D = np.zeros((100000, 3)) 23 | lossPP = np.zeros((100000, 41)) 24 | lossPL = np.zeros((100000, 41)) 25 | gradPP = np.zeros((100000, 40)) 26 | gradPL = np.zeros((100000, 40)) 27 | varPP = np.zeros((100000, 40)) 28 | varPL = np.zeros((100000, 40)) 29 | 30 | for line in f: 31 | # print line 32 | data_tmp = line.split() 33 | # print data_tmp 34 | flg = int(data_tmp[0]) 35 | # p3D[i/2, :] = data_tmp[1:4] 36 | 37 | if flg == 1: 38 | # check the flag as 1 39 | # feed pl loss 40 | lossPL[nPL, 0:41] = data_tmp[4:45] 41 | lossPL[nPL, 0:41] = lossPL[nPL, 0:41] / 10000.0 42 | gradPL[nPL, :] = lossPL[nPL, 1:41] - lossPL[nPL, 0:40] 43 | nPL = nPL + 1 44 | elif flg == 0: 45 | # check the flag as 0 46 | # feed pl loss 47 | lossPP[nPP, 0:41] = data_tmp[4:45] 48 | lossPP[nPP, 0:41] = lossPP[nPP, 0:41] / 10000.0 49 | gradPP[nPP, :] = lossPP[nPP, 1:41] - lossPP[nPP, 0:40] 50 | nPP = nPP + 1 51 | elif flg == 30: 52 | varPP[vPP, 0:20] = data_tmp[4:24] 53 | varPP[vPP, 20:40] = data_tmp[25:45] 54 | varPP[vPP, :] = 1.0 / (varPP[vPP, :] * 10000) 55 | vPP = vPP + 1 56 | else: 57 | varPL[vPL, 0:20] = data_tmp[4:24] 58 | varPL[vPL, 20:40] = data_tmp[25:45] 59 | varPL[vPL, :] = 1.0 / (varPL[vPL, :] * 10000 * 100) 60 | vPL = vPL + 1 61 | 62 | # print varPP[:vPP, :] 63 | # print varPL[:vPL, :] 64 | 65 | i = 100 66 | 67 | sample_rate = 1 68 | 69 | fig = plt.figure() 70 | gs = gridspec.GridSpec(3, 2, width_ratios=[1, 1]) 71 | 72 | ax1 = plt.subplot(gs[0]) 73 | for pn in range(0, i, sample_rate): 74 | # ax1.scatter(np.ones((i, 1)) * pn, vol[:i, pn], color='r', s=2, marker='.', alpha=.4) 75 | ax1.plot(range(-20, 21, 1), lossPL[pn, :], color='r', marker='.', alpha=.4) 76 | ax1.plot(range(-20, 21, 1), lossPP[pn, :], color='g', marker='.', alpha=.4) 77 | # ax1.set_yscale('log') 78 | # ax1.set_ylim(-plot_range, plot_range) 79 | ax1.legend(shadow=True, fancybox=True) 80 | ax1.set_xlabel('Pose Error (%)') 81 | ax1.set_ylabel('Residual') 82 | ax1.set_xlim(-20, 21) 83 | ax1.legend(['point-to-line residual', 'point-to-point residual']) 84 | 85 | ax2 = plt.subplot(gs[1]) 86 | # labels = list('ABCD') 87 | # data = [] 88 | # for cn in range(0, 20, 1): 89 | # data = [data, vol[0:i:sample_rate, cn]] 90 | bp1 = ax2.boxplot(lossPL[0:i:sample_rate, :]) 91 | plt.setp(bp1['boxes'], color='red') 92 | bp2 = ax2.boxplot(lossPP[0:i:sample_rate, :]) 93 | # for patch in bp2['boxes']: 94 | # patch.set_facecolor('green') 95 | plt.setp(bp2['boxes'], color='green') 96 | # ax2.set_xticklabels(['0', '5', '10', '15', '20', '25', '30', '35', '40', '45', '50', '55', '60', '65', '70', '75', '80', '85', '90', '95']) 97 | # print data 98 | # dataArr = np.asarray(data) 99 | # print dataArr.shape 100 | # ax2.boxplot(dataArr) 101 | ax2.set_xlabel('Pose Error (%)') 102 | ax2.set_ylabel('Residual') 103 | ax2.legend(['point-to-line residual', 'point-to-point residual']) 104 | 105 | # gradient 106 | ax3 = plt.subplot(gs[2]) 107 | for pn in range(0, i, sample_rate): 108 | ax3.plot(range(-20, 20, 1), gradPL[pn, :], color='r', marker='.', alpha=.4) 109 | ax3.plot(range(-20, 20, 1), gradPP[pn, :], color='g', marker='.', alpha=.4) 110 | ax3.legend(shadow=True, fancybox=True) 111 | ax3.set_xlabel('Pose Error (%)') 112 | ax3.set_ylabel('Gradient of Residual') 113 | ax3.set_xlim(-20, 20) 114 | ax3.legend(['gradient of point-to-line residual', 'gradient of point-to-point residual']) 115 | 116 | ax4 = plt.subplot(gs[3]) 117 | bp1 = ax4.boxplot(gradPL[0:i:sample_rate, :]) 118 | plt.setp(bp1['boxes'], color='red') 119 | bp2 = ax4.boxplot(gradPP[0:i:sample_rate, :]) 120 | plt.setp(bp2['boxes'], color='green') 121 | ax4.set_xlabel('Pose Error (%)') 122 | ax4.set_ylabel('Gradient of Residual') 123 | ax4.legend(['gradient of point-to-line residual', 'gradient of point-to-point residual']) 124 | 125 | 126 | ax5 = plt.subplot(gs[4]) 127 | for pn in range(0, i, sample_rate): 128 | # ax1.scatter(np.ones((i, 1)) * pn, vol[:i, pn], color='r', s=2, marker='.', alpha=.4) 129 | ax5.plot(range(-20, 20, 1), varPL[pn, :], color='r', marker='.', alpha=.4) 130 | ax5.plot(range(-20, 20, 1), varPP[pn, :], color='g', marker='.', alpha=.4) 131 | # ax1.set_yscale('log') 132 | # ax1.set_ylim(-plot_range, plot_range) 133 | ax5.legend(shadow=True, fancybox=True) 134 | ax5.set_xlabel('Pose Error (%)') 135 | ax5.set_ylabel('Variance of Residual') 136 | ax5.set_xlim(-20, 21) 137 | ax5.set_ylim(0, 1) 138 | ax5.legend(['point-to-line residual', 'point-to-point residual']) 139 | 140 | ax6 = plt.subplot(gs[5]) 141 | bp1 = ax6.boxplot(varPL[0:i:sample_rate, :], showfliers=False) 142 | plt.setp(bp1['boxes'], color='red') 143 | bp2 = ax6.boxplot(varPP[0:i:sample_rate, :], showfliers=False) 144 | plt.setp(bp2['boxes'], color='green') 145 | ax6.set_ylim(0, 1) 146 | ax6.set_xlabel('Pose Error (%)') 147 | ax6.set_ylabel('Variance of Residual') 148 | ax6.legend(['point-to-line residual', 'point-to-point residual']) 149 | 150 | 151 | plt.show() 152 | -------------------------------------------------------------------------------- /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 | KeyFrame::KeyFrame( const StereoFrame* sf ) 27 | { 28 | kf_idx = -1; 29 | T_kf_w = sf->Tfw; 30 | x_kf_w = logmap_se3( T_kf_w ); 31 | xcov_kf_w = sf->Tfw_cov; 32 | 33 | stereo_frame = new StereoFrame( sf->rgbImg_l, sf->rgbImg_r, kf_idx, sf->cam, sf->time_stamp ); 34 | stereo_frame->pdesc_l = sf->pdesc_l; 35 | stereo_frame->pdesc_r = sf->pdesc_r; 36 | stereo_frame->ldesc_l = sf->ldesc_l; 37 | stereo_frame->ldesc_r = sf->ldesc_r; 38 | stereo_frame->stereo_pt = sf->stereo_pt; 39 | stereo_frame->stereo_ls = sf->stereo_ls; 40 | 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 | 60 | Mat KeyFrame::plotKeyFrame() 61 | { 62 | // create new image to modify it 63 | Mat img_l_aux; 64 | stereo_frame->rgbImg_l.copyTo( img_l_aux ); 65 | // Variables 66 | unsigned int r = 0, g, b = 0; 67 | Point2f p,q; 68 | double thick = 1.5; 69 | int k = 0, radius = 3; 70 | // plot point features 71 | for( vector::iterator pt_it = stereo_frame->stereo_pt.begin(); pt_it != stereo_frame->stereo_pt.end(); pt_it++) 72 | { 73 | if( (*pt_it)->idx != -1 ) 74 | { 75 | g = 200; 76 | p = cv::Point( int((*pt_it)->pl(0)), int((*pt_it)->pl(1)) ); 77 | circle( img_l_aux, p, radius, Scalar(b,g,r), thick); 78 | } 79 | } 80 | // plot line segment features 81 | for( vector::iterator ls_it = stereo_frame->stereo_ls.begin(); ls_it != stereo_frame->stereo_ls.end(); ls_it++) 82 | { 83 | if( (*ls_it)->idx != -1 ) 84 | { 85 | g = 200; 86 | p = cv::Point( int((*ls_it)->spl(0)), int((*ls_it)->spl(1)) ); 87 | q = cv::Point( int((*ls_it)->epl(0)), int((*ls_it)->epl(1)) ); 88 | line( img_l_aux, p, q, Scalar(b,g,r), thick); 89 | } 90 | } 91 | return img_l_aux; 92 | } 93 | 94 | } 95 | -------------------------------------------------------------------------------- /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 | // Point features 27 | 28 | MapPoint::MapPoint(int idx_, Vector3d point3D_, Mat desc_, int kf_obs_, Vector2d obs_, Vector3d dir_, double sigma2_ ) : 29 | 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 | 94 | // Line segment features 95 | 96 | MapLine::MapLine(int idx_, Vector6d line3D_, Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_) : 97 | idx(idx_), line3D(line3D_), inlier(true) 98 | { 99 | desc_list.push_back( desc_ ); 100 | obs_list.push_back( obs_ ); 101 | kf_obs_list.push_back( kf_obs_ ); 102 | dir_list.push_back( dir_ ); 103 | pts_list.push_back(pts_); 104 | sigma_list.push_back(sigma2_); 105 | med_obs_dir = dir_; 106 | med_desc = desc_; 107 | } 108 | 109 | void MapLine::addMapLineObservation(Mat desc_, int kf_obs_, Vector3d obs_, Vector3d dir_, Vector4d pts_, double sigma2_) 110 | { 111 | desc_list.push_back( desc_ ); 112 | obs_list.push_back( obs_ ); 113 | kf_obs_list.push_back( kf_obs_ ); 114 | dir_list.push_back( dir_ ); 115 | pts_list.push_back(pts_); 116 | sigma_list.push_back(sigma2_); 117 | updateAverageDescDir(); 118 | } 119 | 120 | void MapLine::updateAverageDescDir() 121 | { 122 | 123 | // descriptor 124 | // - check distances between all the observed descriptors 125 | int n = desc_list.size(); 126 | MatrixXf conf_desc(n,n); 127 | for(int i = 0; i < n; i++ ) 128 | { 129 | conf_desc(i,i) = 0; 130 | for(int j = i+1 ; j < n; j++ ) 131 | { 132 | int d = norm(desc_list[i],desc_list[j],NORM_HAMMING); 133 | conf_desc(i,j) = d; 134 | conf_desc(j,i) = d; 135 | } 136 | } 137 | 138 | // - select the one with least mean distance to the rest 139 | int max_dist = 99999; 140 | int max_idx = 0; 141 | for(int i = 0; i < n; i++) 142 | { 143 | vector dist_idx; 144 | for(int j = 0; j < n; j++) 145 | dist_idx.push_back( conf_desc(i,j) ); 146 | sort( dist_idx.begin(), dist_idx.end() ); 147 | int idx_median = dist_idx[ int(1+0.5*(n-1)) ]; 148 | if( idx_median < max_dist ) 149 | { 150 | max_dist = idx_median; 151 | max_idx = i; 152 | } 153 | } 154 | med_desc = desc_list[max_idx]; 155 | 156 | // direction 157 | Vector3d med_dir; 158 | for(int i = 0; i < n; i++) 159 | med_dir += dir_list[i]; 160 | med_obs_dir = med_dir / n; 161 | 162 | } 163 | 164 | } 165 | -------------------------------------------------------------------------------- /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 | // Point feature 27 | 28 | PointFeature::PointFeature( const Vector3d & P_, const Vector2d & pl_obs_) : 29 | P(P_), pl_obs(pl_obs_), level(0) 30 | {} 31 | 32 | PointFeature::PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_ ) : 33 | pl(pl_), disp(disp_), P(P_), inlier(true), level(0) 34 | {} 35 | 36 | PointFeature::PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_, 37 | const int & idx_ ) : 38 | pl(pl_), disp(disp_), P(P_), inlier(true), idx(idx_), level(0) 39 | {} 40 | 41 | PointFeature::PointFeature( const Vector2d & pl_, const double & disp_, const Vector3d & P_, 42 | const int & idx_, const int & level_ ) : 43 | 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_ ) : 52 | pl(pl_), disp(disp_), P(P_), pl_obs(pl_obs_), inlier(true), level(0) 53 | {} 54 | 55 | 56 | // Line segment feature 57 | 58 | LineFeature::LineFeature( const Vector3d & sP_, const Vector3d & eP_, const Vector3d & le_obs_) : 59 | sP(sP_), eP(eP_), le_obs(le_obs_), level(0) 60 | {} 61 | 62 | 63 | LineFeature::LineFeature( const Vector3d & sP_, const Vector3d & eP_, const Vector3d & le_obs_, 64 | const Vector2d & spl_obs_, const Vector2d & epl_obs_) : 65 | sP(sP_), eP(eP_), le_obs(le_obs_), spl_obs(spl_obs_), epl_obs(epl_obs_), level(0) 66 | {} 67 | 68 | 69 | LineFeature::LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 70 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 71 | const Vector3d & le_) : 72 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), level(0) 73 | {} 74 | 75 | LineFeature::LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 76 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 77 | const Vector3d & le_, const Vector3d & le_obs_) : 78 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), le_obs(le_obs_), inlier(true), level(0) 79 | {} 80 | 81 | LineFeature::LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 82 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 83 | const Vector3d & le_, const int & idx_) : 84 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), level(0) 85 | {} 86 | 87 | LineFeature::LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 88 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 89 | const Vector3d & le_, const double & angle_, const int & idx_) : 90 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(0) 91 | {} 92 | 93 | LineFeature::LineFeature( const Vector2d & spl_, const double & sdisp_, const Vector3d & sP_, 94 | const Vector2d & epl_, const double & edisp_, const Vector3d & eP_, 95 | const Vector3d & le_, const double & angle_, const int & idx_, const int & level_) : 96 | spl(spl_), sdisp(sdisp_), sP(sP_), epl(epl_), edisp(edisp_), eP(eP_), le(le_), inlier(true), idx(idx_), angle(angle_), level(level_) 97 | { 98 | for( int i = 0; i < level+1; i++ ) 99 | sigma2 *= Config::lsdScale(); 100 | sigma2 = 1.f / (sigma2*sigma2); 101 | } 102 | 103 | } 104 | -------------------------------------------------------------------------------- /src/test_line_matching.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main() { 8 | cv::Mat K = cv::Mat::eye(3,3,CV_32F); 9 | K.at (0,0) = 525.0; 10 | K.at (0,2) = 319.5; 11 | K.at (1,1) = 525.0; 12 | K.at (1,2) = 239.5; 13 | 14 | cv::Mat D = cv::Mat::eye(1,5,CV_32F); 15 | D.at (0,0) = float(0.0); 16 | D.at (0,1) = float(0.0); 17 | D.at (0,2) = float(0.0); 18 | D.at (0,3) = float(0.0); 19 | D.at (0,4) = float(0.0); 20 | 21 | cv::Mat R = cv::Mat::eye(3, 3, CV_32F); 22 | cv::Mat t = cv::Mat::zeros(3, 1, CV_32F); 23 | t.at(2, 0) = 1.0; 24 | 25 | PinholeStereoCamera* camera = 26 | new PinholeStereoCamera (int(640),int(480),double(0.12),K,K,R,t,D,D,false); 27 | StVO::StereoFrameHandler* Stvo = 28 | new StVO::StereoFrameHandler(camera); 29 | 30 | Stvo->setLeftCamExtrinCalib(0, 0, 0, 0, 0, 0); 31 | 32 | std::string path = "/home/pang/data/dataset/euroc/MH_01_easy/mav0/cam0/data"; 33 | std::vector img_fname_list; 34 | DIR* dirp = opendir(path.c_str()); 35 | struct dirent * dp; 36 | while ((dp = readdir(dirp)) != NULL) { 37 | if (dp->d_type == DT_REG) { 38 | img_fname_list.push_back(dp->d_name); 39 | } 40 | } 41 | closedir(dirp); 42 | 43 | // 44 | std::sort(img_fname_list.begin(), img_fname_list.end()); 45 | 46 | for (int i=0; iinitialize(img, img, i, uint64_t(i)); 55 | else { 56 | Stvo->insertStereoPair(img, img, i, uint64_t(i)); 57 | cv::imshow("line detection", Stvo->curr_frame->stereo_frame_detect); 58 | cv::imshow("line stereo", Stvo->curr_frame->stereo_frame_match); 59 | cv::imshow("line matching", Stvo->canvas_match_frames); 60 | // cv::waitKey(0); 61 | } 62 | Stvo->updateFrame(); 63 | } 64 | } 65 | 66 | -------------------------------------------------------------------------------- /test/main.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ::testing::InitGoogleTest(&argc, argv); 6 | int ret = RUN_ALL_TESTS(); 7 | return ret; 8 | } 9 | --------------------------------------------------------------------------------