diff --git a/.gitignore b/.gitignore index 611b5adb2c..7bbb1324e2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ CMakeLists.txt.user +Examples/OdomMono/odom_mono_nv Examples/Monocular/mono_euroc Examples/Monocular/mono_kitti Examples/Monocular/mono_tum @@ -21,4 +22,4 @@ Vocabulary/ORBvoc.txt build/ *~ lib/ - +Debug/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 08a8af468e..c5037aaf8f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,7 @@ src/KeyFrameDatabase.cc src/Sim3Solver.cc src/Initializer.cc src/Viewer.cc +src/Se2.cpp ) target_link_libraries(${PROJECT_NAME} @@ -80,34 +81,40 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so # Build examples -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) +#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D) -add_executable(rgbd_tum -Examples/RGB-D/rgbd_tum.cc) -target_link_libraries(rgbd_tum ${PROJECT_NAME}) +#add_executable(rgbd_tum +#Examples/RGB-D/rgbd_tum.cc) +#target_link_libraries(rgbd_tum ${PROJECT_NAME}) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) +#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo) -add_executable(stereo_kitti -Examples/Stereo/stereo_kitti.cc) -target_link_libraries(stereo_kitti ${PROJECT_NAME}) +#add_executable(stereo_kitti +#Examples/Stereo/stereo_kitti.cc) +#target_link_libraries(stereo_kitti ${PROJECT_NAME}) -add_executable(stereo_euroc -Examples/Stereo/stereo_euroc.cc) -target_link_libraries(stereo_euroc ${PROJECT_NAME}) +#add_executable(stereo_euroc +#Examples/Stereo/stereo_euroc.cc) +#target_link_libraries(stereo_euroc ${PROJECT_NAME}) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) +#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular) -add_executable(mono_tum -Examples/Monocular/mono_tum.cc) -target_link_libraries(mono_tum ${PROJECT_NAME}) +#add_executable(mono_tum +#Examples/Monocular/mono_tum.cc) +#target_link_libraries(mono_tum ${PROJECT_NAME}) -add_executable(mono_kitti -Examples/Monocular/mono_kitti.cc) -target_link_libraries(mono_kitti ${PROJECT_NAME}) +#add_executable(mono_kitti +#Examples/Monocular/mono_kitti.cc) +#target_link_libraries(mono_kitti ${PROJECT_NAME}) -add_executable(mono_euroc -Examples/Monocular/mono_euroc.cc) -target_link_libraries(mono_euroc ${PROJECT_NAME}) +#add_executable(mono_euroc +#Examples/Monocular/mono_euroc.cc) +#target_link_libraries(mono_euroc ${PROJECT_NAME}) + +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/OdomMono) + +add_executable(odom_mono_nv +Examples/OdomMono/odom_mono_nv.cc) +target_link_libraries(odom_mono_nv ${PROJECT_NAME}) diff --git a/Examples/OdomMono/odom_mono_nv.cc b/Examples/OdomMono/odom_mono_nv.cc new file mode 100644 index 0000000000..2075aa8e1a --- /dev/null +++ b/Examples/OdomMono/odom_mono_nv.cc @@ -0,0 +1,165 @@ +/** +* This file is part of ORB-SLAM2. +* +* Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) +* For more information see +* +* ORB-SLAM2 is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM2 is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM2. If not, see . +*/ + + +#include +#include +#include +#include + +#include + +using namespace std; +void LoadImages(const string &strFile, vector &vstrImageFilenames, + vector &vTimestamps); + +int main(int argc, char **argv) +{ + if(argc != 4) + { + cerr << endl << "Usage: ./odom_mono_nv path_to_vocabulary path_to_settings path_to_sequence" << endl; + return 1; + } + string orbBinFile = argv[1]; + string settingFile = argv[2]; + string dataPath = argv[3]; + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + + ORB_SLAM2::System SLAM(orbBinFile, settingFile, ORB_SLAM2::System::MONOCULAR,true); + + cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ); + + // Number of images + int nImages = (int)fsSettings["Images.Number"]; + + // Between-frame duration + double T = (double)fsSettings["Images.FrameDuration"]; + + // Camera parameters for pre-process of undistortion + cv::Mat preK, preD; + fsSettings["Camera.Pre.K"] >> preK; + fsSettings["Camera.Pre.D"] >> preD; + + cout << endl << "-------" << endl; + cout << "Start processing sequence ..." << endl; + + string fullOdomName = dataPath + "/odo_raw.txt"; + ifstream odomFile(fullOdomName); + float x, y, theta; + string odomLine; + + // Vector for tracking time statistics + vector vTimesTrack; + vTimesTrack.resize(nImages); + + // Main loop + cv::Mat im, imraw; + for(int ni=0; ni> x >> y >> theta; + Se2 odom(x, y, theta); + + double tframe = 0; + +#ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); +#else + std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now(); +#endif + + SLAM.TrackOdomMono(im, odom, tframe); + +#ifdef COMPILEDWITHC11 + std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); +#else + std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now(); +#endif + + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); + + vTimesTrack[ni]=ttrack; + + // Wait to load the next frame + if(ttrack &vstrImageFilenames, vector &vTimestamps) +{ + ifstream f; + f.open(strFile.c_str()); + + // skip first three lines + string s0; + getline(f,s0); + getline(f,s0); + getline(f,s0); + + while(!f.eof()) + { + string s; + getline(f,s); + if(!s.empty()) + { + stringstream ss; + ss << s; + double t; + string sRGB; + ss >> t; + vTimestamps.push_back(t); + ss >> sRGB; + vstrImageFilenames.push_back(sRGB); + } + } +} diff --git a/Examples/OdomMono/vn1.yaml b/Examples/OdomMono/vn1.yaml new file mode 100644 index 0000000000..bc8dc79d89 --- /dev/null +++ b/Examples/OdomMono/vn1.yaml @@ -0,0 +1,92 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 231.976033627644090 +Camera.fy: 232.157224036901510 +Camera.cx: 326.92392097053931 +Camera.cy: 227.83848839534838 + +Camera.k1: 0 +Camera.k2: 0 +Camera.p1: 0 +Camera.p2: 0 +Camera.k3: 0 + +Camera.Pre.D: !!opencv-matrix + rows: 5 + cols: 1 + dt: f + data: [ -0.207406506100898 , 0.032194071349429 , 0.001120166051888 , 0.000859411522110 , 0.000000000000000 ] +Camera.Pre.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: f + data: [ 231.976033627644090, 0., 326.923920970539310, 0., + 232.157224036901510, 227.838488395348380, 0., 0., 1. ] + +# Camera frames per second +Camera.fps: 30.0 + +# Camera extrinsics +Tbc.tbc.x: 123.91 +Tbc.tbc.y: 399.1023 +Tbc.tbc.z: 0.0 +Tbc.rbc.x: -0.0121 +Tbc.rbc.y: 0.0362 +Tbc.rbc.z: -1.6077 + +# +Images.Number: 3106 +Images.FrameDuration: 0.03 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 +# Visual.KeyFrameSize: 0.4 +# Visual.KeyFrameLineWidth: 1 +# Visual.GraphLineWidth: 0.9 +# Visual.PointSize: 2 +# Visual.CameraSize: 0.6 +# Visual.CameraLineWidth: 3 +# Visual.ViewpointX: 0 +# Visual.ViewpointY: -0.7 +# Visual.ViewpointZ: -1.8 +# Visual.ViewpointF: 500 +Visual.Ratio: 0.002 diff --git a/Examples/OdomMono/vn2.yaml b/Examples/OdomMono/vn2.yaml new file mode 100644 index 0000000000..5f7b0f114d --- /dev/null +++ b/Examples/OdomMono/vn2.yaml @@ -0,0 +1,92 @@ +%YAML:1.0 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +Camera.fx: 231.976033627644090 +Camera.fy: 232.157224036901510 +Camera.cx: 326.92392097053931 +Camera.cy: 227.83848839534838 + +Camera.k1: 0 +Camera.k2: 0 +Camera.p1: 0 +Camera.p2: 0 +Camera.k3: 0 + +Camera.Pre.D: !!opencv-matrix + rows: 5 + cols: 1 + dt: f + data: [ -0.207406506100898 , 0.032194071349429 , 0.001120166051888 , 0.000859411522110 , 0.000000000000000 ] +Camera.Pre.K: !!opencv-matrix + rows: 3 + cols: 3 + dt: f + data: [ 231.976033627644090, 0., 326.923920970539310, 0., + 232.157224036901510, 227.838488395348380, 0., 0., 1. ] + +# Camera frames per second +Camera.fps: 30.0 + +# Camera extrinsics +Tbc.tbc.x: 87.0240 +Tbc.tbc.y: 412.2315 +Tbc.tbc.z: 0.0 +Tbc.rbc.x: -0.0438 +Tbc.rbc.y: -0.0019 +Tbc.rbc.z: 0.8016 + +# +Images.Number: 13520 +Images.FrameDuration: 0.03 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize: 2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 +# Visual.KeyFrameSize: 0.4 +# Visual.KeyFrameLineWidth: 1 +# Visual.GraphLineWidth: 0.9 +# Visual.PointSize: 2 +# Visual.CameraSize: 0.6 +# Visual.CameraLineWidth: 3 +# Visual.ViewpointX: 0 +# Visual.ViewpointY: -0.7 +# Visual.ViewpointZ: -1.8 +# Visual.ViewpointF: 500 +Visual.Ratio: 0.002 diff --git a/README.md b/README.md index 122db22296..4b8dc8dd3c 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,13 @@ +Adaptation with odometry assistance. See `Examples/OdomMono`. + +If you use this repo for academic publication, please cite: + +- Fan Zheng, Hengbo Tang, Yun-Hui Liu. "Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses". IEEE Transactions on Cybernetics, vol. 49, no. 7, 2019. \[[code](https://github.com/izhengfan/se2clam)\] +- Fan Zheng, Yun-Hui Liu. "Visual-Odometric Localization and Mapping for Ground Vehicles Using SE(2)-XYZ Constraints". Proc. IEEE International Conference on Robotics and Automation (ICRA), 2019. \[[code](https://github.com/izhengfan/se2lam)\] + + +--- + # ORB-SLAM2 **Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)) diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt index 0eb512600a..4da56b2bcd 100644 --- a/Thirdparty/DBoW2/CMakeLists.txt +++ b/Thirdparty/DBoW2/CMakeLists.txt @@ -24,13 +24,7 @@ set(SRCS_DUTILS DUtils/Random.cpp DUtils/Timestamp.cpp) -find_package(OpenCV 3.0 QUIET) -if(NOT OpenCV_FOUND) - find_package(OpenCV 2.4.3 QUIET) - if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 2.4.3 not found.") - endif() -endif() +find_package(OpenCV REQUIRED) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) diff --git a/Thirdparty/DBoW2/DBoW2/BowVector.cpp b/Thirdparty/DBoW2/DBoW2/BowVector.cpp index 1337fa3e5b..0ff2e6c73a 100644 --- a/Thirdparty/DBoW2/DBoW2/BowVector.cpp +++ b/Thirdparty/DBoW2/DBoW2/BowVector.cpp @@ -1,130 +1,130 @@ -/** - * File: BowVector.cpp - * Date: March 2011 - * Author: Dorian Galvez-Lopez - * Description: bag of words vector - * License: see the LICENSE.txt file - * - */ - -#include -#include -#include -#include -#include - -#include "BowVector.h" - -namespace DBoW2 { - -// -------------------------------------------------------------------------- - -BowVector::BowVector(void) -{ -} - -// -------------------------------------------------------------------------- - -BowVector::~BowVector(void) -{ -} - -// -------------------------------------------------------------------------- - -void BowVector::addWeight(WordId id, WordValue v) -{ - BowVector::iterator vit = this->lower_bound(id); - - if(vit != this->end() && !(this->key_comp()(id, vit->first))) - { - vit->second += v; - } - else - { - this->insert(vit, BowVector::value_type(id, v)); - } -} - -// -------------------------------------------------------------------------- - -void BowVector::addIfNotExist(WordId id, WordValue v) -{ - BowVector::iterator vit = this->lower_bound(id); - - if(vit == this->end() || (this->key_comp()(id, vit->first))) - { - this->insert(vit, BowVector::value_type(id, v)); - } -} - -// -------------------------------------------------------------------------- - -void BowVector::normalize(LNorm norm_type) -{ - double norm = 0.0; - BowVector::iterator it; - - if(norm_type == DBoW2::L1) - { - for(it = begin(); it != end(); ++it) - norm += fabs(it->second); - } - else - { - for(it = begin(); it != end(); ++it) - norm += it->second * it->second; - norm = sqrt(norm); - } - - if(norm > 0.0) - { - for(it = begin(); it != end(); ++it) - it->second /= norm; - } -} - -// -------------------------------------------------------------------------- - -std::ostream& operator<< (std::ostream &out, const BowVector &v) -{ - BowVector::const_iterator vit; - std::vector::const_iterator iit; - unsigned int i = 0; - const unsigned int N = v.size(); - for(vit = v.begin(); vit != v.end(); ++vit, ++i) - { - out << "<" << vit->first << ", " << vit->second << ">"; - - if(i < N-1) out << ", "; - } - return out; -} - -// -------------------------------------------------------------------------- - -void BowVector::saveM(const std::string &filename, size_t W) const -{ - std::fstream f(filename.c_str(), std::ios::out); - - WordId last = 0; - BowVector::const_iterator bit; - for(bit = this->begin(); bit != this->end(); ++bit) - { - for(; last < bit->first; ++last) - { - f << "0 "; - } - f << bit->second << " "; - - last = bit->first + 1; - } - for(; last < (WordId)W; ++last) - f << "0 "; - - f.close(); -} - -// -------------------------------------------------------------------------- - -} // namespace DBoW2 - +/** + * File: BowVector.cpp + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#include +#include +#include +#include +#include + +#include "BowVector.h" + +namespace DBoW2 { + +// -------------------------------------------------------------------------- + +BowVector::BowVector(void) +{ +} + +// -------------------------------------------------------------------------- + +BowVector::~BowVector(void) +{ +} + +// -------------------------------------------------------------------------- + +void BowVector::addWeight(WordId id, WordValue v) +{ + BowVector::iterator vit = this->lower_bound(id); + + if(vit != this->end() && !(this->key_comp()(id, vit->first))) + { + vit->second += v; + } + else + { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::addIfNotExist(WordId id, WordValue v) +{ + BowVector::iterator vit = this->lower_bound(id); + + if(vit == this->end() || (this->key_comp()(id, vit->first))) + { + this->insert(vit, BowVector::value_type(id, v)); + } +} + +// -------------------------------------------------------------------------- + +void BowVector::normalize(LNorm norm_type) +{ + double norm = 0.0; + BowVector::iterator it; + + if(norm_type == DBoW2::L1) + { + for(it = begin(); it != end(); ++it) + norm += fabs(it->second); + } + else + { + for(it = begin(); it != end(); ++it) + norm += it->second * it->second; + norm = sqrt(norm); + } + + if(norm > 0.0) + { + for(it = begin(); it != end(); ++it) + it->second /= norm; + } +} + +// -------------------------------------------------------------------------- + +std::ostream& operator<< (std::ostream &out, const BowVector &v) +{ + BowVector::const_iterator vit; + std::vector::const_iterator iit; + unsigned int i = 0; + const unsigned int N = v.size(); + for(vit = v.begin(); vit != v.end(); ++vit, ++i) + { + out << "<" << vit->first << ", " << vit->second << ">"; + + if(i < N-1) out << ", "; + } + return out; +} + +// -------------------------------------------------------------------------- + +void BowVector::saveM(const std::string &filename, size_t W) const +{ + std::fstream f(filename.c_str(), std::ios::out); + + WordId last = 0; + BowVector::const_iterator bit; + for(bit = this->begin(); bit != this->end(); ++bit) + { + for(; last < bit->first; ++last) + { + f << "0 "; + } + f << bit->second << " "; + + last = bit->first + 1; + } + for(; last < (WordId)W; ++last) + f << "0 "; + + f.close(); +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW2 + diff --git a/Thirdparty/DBoW2/DBoW2/BowVector.h b/Thirdparty/DBoW2/DBoW2/BowVector.h index f559811deb..4f3f2007a0 100644 --- a/Thirdparty/DBoW2/DBoW2/BowVector.h +++ b/Thirdparty/DBoW2/DBoW2/BowVector.h @@ -1,109 +1,112 @@ -/** - * File: BowVector.h - * Date: March 2011 - * Author: Dorian Galvez-Lopez - * Description: bag of words vector - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_BOW_VECTOR__ -#define __D_T_BOW_VECTOR__ - -#include -#include -#include - -namespace DBoW2 { - -/// Id of words -typedef unsigned int WordId; - -/// Value of a word -typedef double WordValue; - -/// Id of nodes in the vocabulary treee -typedef unsigned int NodeId; - -/// L-norms for normalization -enum LNorm -{ - L1, - L2 -}; - -/// Weighting type -enum WeightingType -{ - TF_IDF, - TF, - IDF, - BINARY -}; - -/// Scoring type -enum ScoringType -{ - L1_NORM, - L2_NORM, - CHI_SQUARE, - KL, - BHATTACHARYYA, - DOT_PRODUCT, -}; - -/// Vector of words to represent images -class BowVector: - public std::map -{ -public: - - /** - * Constructor - */ - BowVector(void); - - /** - * Destructor - */ - ~BowVector(void); - - /** - * Adds a value to a word value existing in the vector, or creates a new - * word with the given value - * @param id word id to look for - * @param v value to create the word with, or to add to existing word - */ - void addWeight(WordId id, WordValue v); - - /** - * Adds a word with a value to the vector only if this does not exist yet - * @param id word id to look for - * @param v value to give to the word if this does not exist - */ - void addIfNotExist(WordId id, WordValue v); - - /** - * L1-Normalizes the values in the vector - * @param norm_type norm used - */ - void normalize(LNorm norm_type); - - /** - * Prints the content of the bow vector - * @param out stream - * @param v - */ - friend std::ostream& operator<<(std::ostream &out, const BowVector &v); - - /** - * Saves the bow vector as a vector in a matlab file - * @param filename - * @param W number of words in the vocabulary - */ - void saveM(const std::string &filename, size_t W) const; -}; - -} // namespace DBoW2 - -#endif +/** + * File: BowVector.h + * Date: March 2011 + * Author: Dorian Galvez-Lopez + * Description: bag of words vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_BOW_VECTOR__ +#define __D_T_BOW_VECTOR__ + +#include +#include +#include + +#include "../DUtils/config.h" + +namespace DBoW2 { + +/// Id of words +typedef unsigned int WordId; + +/// Value of a word +typedef double WordValue; + +/// Id of nodes in the vocabulary treee +typedef unsigned int NodeId; + +/// L-norms for normalization +EXPORT typedef enum LNorm +{ + L1, + L2 +} LNorm; + +/// Weighting type +EXPORT typedef enum WeightingType +{ + TF_IDF, + TF, + IDF, + BINARY +} WeightingType; + +/// Scoring type +EXPORT typedef enum ScoringType +{ + L1_NORM, + L2_NORM, + CHI_SQUARE, + KL, + BHATTACHARYYA, + DOT_PRODUCT, +} ScoringType; + +/// Vector of words to represent images +/// stl的map结构，key为wordId，value为tfidf中的tf +class EXPORT BowVector: + public std::map +{ +public: + + /** + * Constructor + */ + BowVector(void); + + /** + * Destructor + */ + ~BowVector(void); + + /** + * Adds a value to a word value existing in the vector, or creates a new + * word with the given value + * @param id word id to look for + * @param v value to create the word with, or to add to existing word + */ + void addWeight(WordId id, WordValue v); + + /** + * Adds a word with a value to the vector only if this does not exist yet + * @param id word id to look for + * @param v value to give to the word if this does not exist + */ + void addIfNotExist(WordId id, WordValue v); + + /** + * L1-Normalizes the values in the vector + * @param norm_type norm used + */ + void normalize(LNorm norm_type); + + /** + * Prints the content of the bow vector + * @param out stream + * @param v + */ + friend std::ostream& operator<<(std::ostream &out, const BowVector &v); + + /** + * Saves the bow vector as a vector in a matlab file + * @param filename + * @param W number of words in the vocabulary + */ + void saveM(const std::string &filename, size_t W) const; +}; + +} // namespace DBoW2 + +#endif diff --git a/Thirdparty/DBoW2/DBoW2/FClass.h b/Thirdparty/DBoW2/DBoW2/FClass.h index 13be53d753..5a0481609b 100644 --- a/Thirdparty/DBoW2/DBoW2/FClass.h +++ b/Thirdparty/DBoW2/DBoW2/FClass.h @@ -1,71 +1,73 @@ -/** - * File: FClass.h - * Date: November 2011 - * Author: Dorian Galvez-Lopez - * Description: generic FClass to instantiate templated classes - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_FCLASS__ -#define __D_T_FCLASS__ - -#include -#include -#include - -namespace DBoW2 { - -/// Generic class to encapsulate functions to manage descriptors. -/** - * This class must be inherited. Derived classes can be used as the - * parameter F when creating Templated structures - * (TemplatedVocabulary, TemplatedDatabase, ...) - */ -class FClass -{ - class TDescriptor; - typedef const TDescriptor *pDescriptor; - - /** - * Calculates the mean value of a set of descriptors - * @param descriptors - * @param mean mean descriptor - */ - virtual void meanValue(const std::vector &descriptors, - TDescriptor &mean) = 0; - - /** - * Calculates the distance between two descriptors - * @param a - * @param b - * @return distance - */ - static double distance(const TDescriptor &a, const TDescriptor &b); - - /** - * Returns a string version of the descriptor - * @param a descriptor - * @return string version - */ - static std::string toString(const TDescriptor &a); - - /** - * Returns a descriptor from a string - * @param a descriptor - * @param s string version - */ - static void fromString(TDescriptor &a, const std::string &s); - - /** - * Returns a mat with the descriptors in float format - * @param descriptors - * @param mat (out) NxL 32F matrix - */ - static void toMat32F(const std::vector &descriptors, - cv::Mat &mat); -}; - -} // namespace DBoW2 - -#endif +/** + * File: FClass.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: generic FClass to instantiate templated classes + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_FCLASS__ +#define __D_T_FCLASS__ + +#include +#include +#include + +#include "../DUtils/config.h" + +namespace DBoW2 { + +/// Generic class to encapsulate functions to manage descriptors. +/** + * This class must be inherited. Derived classes can be used as the + * parameter F when creating Templated structures + * (TemplatedVocabulary, TemplatedDatabase, ...) + */ +class EXPORT FClass +{ + class TDescriptor; + typedef const TDescriptor *pDescriptor; + + /** + * Calculates the mean value of a set of descriptors + * @param descriptors + * @param mean mean descriptor + */ + virtual void meanValue(const std::vector &descriptors, + TDescriptor &mean) = 0; + + /** + * Calculates the distance between two descriptors + * @param a + * @param b + * @return distance + */ + static double distance(const TDescriptor &a, const TDescriptor &b); + + /** + * Returns a string version of the descriptor + * @param a descriptor + * @return string version + */ + static std::string toString(const TDescriptor &a); + + /** + * Returns a descriptor from a string + * @param a descriptor + * @param s string version + */ + static void fromString(TDescriptor &a, const std::string &s); + + /** + * Returns a mat with the descriptors in float format + * @param descriptors + * @param mat (out) NxL 32F matrix + */ + static void toMat32F(const std::vector &descriptors, + cv::Mat &mat); +}; + +} // namespace DBoW2 + +#endif diff --git a/Thirdparty/DBoW2/DBoW2/FORB.cpp b/Thirdparty/DBoW2/DBoW2/FORB.cpp index 1f1990c2f7..00de7e4dee 100644 --- a/Thirdparty/DBoW2/DBoW2/FORB.cpp +++ b/Thirdparty/DBoW2/DBoW2/FORB.cpp @@ -1,193 +1,194 @@ -/** - * File: FORB.cpp - * Date: June 2012 - * Author: Dorian Galvez-Lopez - * Description: functions for ORB descriptors - * License: see the LICENSE.txt file - * - * Distance function has been modified - * - */ - - -#include -#include -#include -#include - -#include "FORB.h" - -using namespace std; - -namespace DBoW2 { - -// -------------------------------------------------------------------------- - -const int FORB::L=32; - -void FORB::meanValue(const std::vector &descriptors, - FORB::TDescriptor &mean) -{ - if(descriptors.empty()) - { - mean.release(); - return; - } - else if(descriptors.size() == 1) - { - mean = descriptors[0]->clone(); - } - else - { - vector sum(FORB::L * 8, 0); - - for(size_t i = 0; i < descriptors.size(); ++i) - { - const cv::Mat &d = *descriptors[i]; - const unsigned char *p = d.ptr(); - - for(int j = 0; j < d.cols; ++j, ++p) - { - if(*p & (1 << 7)) ++sum[ j*8 ]; - if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; - if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; - if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; - if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; - if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; - if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; - if(*p & (1)) ++sum[ j*8 + 7 ]; - } - } - - mean = cv::Mat::zeros(1, FORB::L, CV_8U); - unsigned char *p = mean.ptr(); - - const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; - for(size_t i = 0; i < sum.size(); ++i) - { - if(sum[i] >= N2) - { - // set bit - *p |= 1 << (7 - (i % 8)); - } - - if(i % 8 == 7) ++p; - } - } -} - -// -------------------------------------------------------------------------- - -int FORB::distance(const FORB::TDescriptor &a, - const FORB::TDescriptor &b) -{ - // Bit set count operation from - // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel - - const int *pa = a.ptr(); - const int *pb = b.ptr(); - - int dist=0; - - for(int i=0; i<8; i++, pa++, pb++) - { - unsigned int v = *pa ^ *pb; - v = v - ((v >> 1) & 0x55555555); - v = (v & 0x33333333) + ((v >> 2) & 0x33333333); - dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; - } - - return dist; -} - -// -------------------------------------------------------------------------- - -std::string FORB::toString(const FORB::TDescriptor &a) -{ - stringstream ss; - const unsigned char *p = a.ptr(); - - for(int i = 0; i < a.cols; ++i, ++p) - { - ss << (int)*p << " "; - } - - return ss.str(); -} - -// -------------------------------------------------------------------------- - -void FORB::fromString(FORB::TDescriptor &a, const std::string &s) -{ - a.create(1, FORB::L, CV_8U); - unsigned char *p = a.ptr(); - - stringstream ss(s); - for(int i = 0; i < FORB::L; ++i, ++p) - { - int n; - ss >> n; - - if(!ss.fail()) - *p = (unsigned char)n; - } - -} - -// -------------------------------------------------------------------------- - -void FORB::toMat32F(const std::vector &descriptors, - cv::Mat &mat) -{ - if(descriptors.empty()) - { - mat.release(); - return; - } - - const size_t N = descriptors.size(); - - mat.create(N, FORB::L*8, CV_32F); - float *p = mat.ptr(); - - for(size_t i = 0; i < N; ++i) - { - const int C = descriptors[i].cols; - const unsigned char *desc = descriptors[i].ptr(); - - for(int j = 0; j < C; ++j, p += 8) - { - p[0] = (desc[j] & (1 << 7) ? 1 : 0); - p[1] = (desc[j] & (1 << 6) ? 1 : 0); - p[2] = (desc[j] & (1 << 5) ? 1 : 0); - p[3] = (desc[j] & (1 << 4) ? 1 : 0); - p[4] = (desc[j] & (1 << 3) ? 1 : 0); - p[5] = (desc[j] & (1 << 2) ? 1 : 0); - p[6] = (desc[j] & (1 << 1) ? 1 : 0); - p[7] = desc[j] & (1); - } - } -} - -// -------------------------------------------------------------------------- - -void FORB::toMat8U(const std::vector &descriptors, - cv::Mat &mat) -{ - mat.create(descriptors.size(), 32, CV_8U); - - unsigned char *p = mat.ptr(); - - for(size_t i = 0; i < descriptors.size(); ++i, p += 32) - { - const unsigned char *d = descriptors[i].ptr(); - std::copy(d, d+32, p); - } - -} - -// -------------------------------------------------------------------------- - -} // namespace DBoW2 - - +/** + * File: FORB.cpp + * Date: June 2012 + * Author: Dorian Galvez-Lopez + * Description: functions for ORB descriptors + * License: see the LICENSE.txt file + * + * Distance function has been modified + * + */ + + +#include +#include +#include +#include +#include + +#include "FORB.h" + +using namespace std; + +namespace DBoW2 { + +// -------------------------------------------------------------------------- + +// const int FORB::L=32; + +void FORB::meanValue(const std::vector &descriptors, + FORB::TDescriptor &mean) +{ + if(descriptors.empty()) + { + mean.release(); + return; + } + else if(descriptors.size() == 1) + { + mean = descriptors[0]->clone(); + } + else + { + vector sum(FORB::L * 8, 0); + + for(size_t i = 0; i < descriptors.size(); ++i) + { + const cv::Mat &d = *descriptors[i]; + const unsigned char *p = d.ptr(); + + for(int j = 0; j < d.cols; ++j, ++p) + { + if(*p & (1 << 7)) ++sum[ j*8 ]; + if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; + if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; + if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; + if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; + if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; + if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; + if(*p & (1)) ++sum[ j*8 + 7 ]; + } + } + + mean = cv::Mat::zeros(1, FORB::L, CV_8U); + unsigned char *p = mean.ptr(); + + const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; + for(size_t i = 0; i < sum.size(); ++i) + { + if(sum[i] >= N2) + { + // set bit + *p |= 1 << (7 - (i % 8)); + } + + if(i % 8 == 7) ++p; + } + } +} + +// -------------------------------------------------------------------------- + +int FORB::distance(const FORB::TDescriptor &a, + const FORB::TDescriptor &b) +{ + // Bit set count operation from + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + + const int *pa = a.ptr(); + const int *pb = b.ptr(); + + int dist=0; + + for(int i=0; i<8; i++, pa++, pb++) + { + unsigned int v = *pa ^ *pb; + v = v - ((v >> 1) & 0x55555555); + v = (v & 0x33333333) + ((v >> 2) & 0x33333333); + dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; + } + + return dist; +} + +// -------------------------------------------------------------------------- + +std::string FORB::toString(const FORB::TDescriptor &a) +{ + stringstream ss; + const unsigned char *p = a.ptr(); + + for(int i = 0; i < a.cols; ++i, ++p) + { + ss << (int)*p << " "; + } + + return ss.str(); +} + +// -------------------------------------------------------------------------- + +void FORB::fromString(FORB::TDescriptor &a, const std::string &s) +{ + a.create(1, FORB::L, CV_8U); + unsigned char *p = a.ptr(); + + stringstream ss(s); + for(int i = 0; i < FORB::L; ++i, ++p) + { + int n; + ss >> n; + + if(!ss.fail()) + *p = (unsigned char)n; + } + +} + +// -------------------------------------------------------------------------- + +void FORB::toMat32F(const std::vector &descriptors, + cv::Mat &mat) +{ + if(descriptors.empty()) + { + mat.release(); + return; + } + + const size_t N = descriptors.size(); + + mat.create(N, FORB::L*8, CV_32F); + float *p = mat.ptr(); + + for(size_t i = 0; i < N; ++i) + { + const int C = descriptors[i].cols; + const unsigned char *desc = descriptors[i].ptr(); + + for(int j = 0; j < C; ++j, p += 8) + { + p[0] = (desc[j] & (1 << 7) ? 1 : 0); + p[1] = (desc[j] & (1 << 6) ? 1 : 0); + p[2] = (desc[j] & (1 << 5) ? 1 : 0); + p[3] = (desc[j] & (1 << 4) ? 1 : 0); + p[4] = (desc[j] & (1 << 3) ? 1 : 0); + p[5] = (desc[j] & (1 << 2) ? 1 : 0); + p[6] = (desc[j] & (1 << 1) ? 1 : 0); + p[7] = desc[j] & (1); + } + } +} + +// -------------------------------------------------------------------------- + +void FORB::toMat8U(const std::vector &descriptors, + cv::Mat &mat) +{ + mat.create(descriptors.size(), 32, CV_8U); + + unsigned char *p = mat.ptr(); + + for(size_t i = 0; i < descriptors.size(); ++i, p += 32) + { + const unsigned char *d = descriptors[i].ptr(); + std::copy(d, d+32, p); + } + +} + +// -------------------------------------------------------------------------- + +} // namespace DBoW2 + + diff --git a/Thirdparty/DBoW2/DBoW2/FORB.h b/Thirdparty/DBoW2/DBoW2/FORB.h index a39599f20e..0f169c4ba2 100644 --- a/Thirdparty/DBoW2/DBoW2/FORB.h +++ b/Thirdparty/DBoW2/DBoW2/FORB.h @@ -1,79 +1,81 @@ -/** - * File: FORB.h - * Date: June 2012 - * Author: Dorian Galvez-Lopez - * Description: functions for ORB descriptors - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_F_ORB__ -#define __D_T_F_ORB__ - -#include -#include -#include - -#include "FClass.h" - -namespace DBoW2 { - -/// Functions to manipulate ORB descriptors -class FORB: protected FClass -{ -public: - - /// Descriptor type - typedef cv::Mat TDescriptor; // CV_8U - /// Pointer to a single descriptor - typedef const TDescriptor *pDescriptor; - /// Descriptor length (in bytes) - static const int L; - - /** - * Calculates the mean value of a set of descriptors - * @param descriptors - * @param mean mean descriptor - */ - static void meanValue(const std::vector &descriptors, - TDescriptor &mean); - - /** - * Calculates the distance between two descriptors - * @param a - * @param b - * @return distance - */ - static int distance(const TDescriptor &a, const TDescriptor &b); - - /** - * Returns a string version of the descriptor - * @param a descriptor - * @return string version - */ - static std::string toString(const TDescriptor &a); - - /** - * Returns a descriptor from a string - * @param a descriptor - * @param s string version - */ - static void fromString(TDescriptor &a, const std::string &s); - - /** - * Returns a mat with the descriptors in float format - * @param descriptors - * @param mat (out) NxL 32F matrix - */ - static void toMat32F(const std::vector &descriptors, - cv::Mat &mat); - - static void toMat8U(const std::vector &descriptors, - cv::Mat &mat); - -}; - -} // namespace DBoW2 - -#endif - +/** + * File: FORB.h + * Date: June 2012 + * Author: Dorian Galvez-Lopez + * Description: functions for ORB descriptors + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_F_ORB__ +#define __D_T_F_ORB__ + +#include +#include +#include + +#include "FClass.h" + +#include "../DUtils/config.h" + +namespace DBoW2 { + +/// Functions to manipulate ORB descriptors +class EXPORT FORB : protected FClass +{ +public: + + /// Descriptor type + typedef cv::Mat TDescriptor; // CV_8U + /// Pointer to a single descriptor + typedef const TDescriptor *pDescriptor; + /// Descriptor length (in bytes) + static const int L = 32; + + /** + * Calculates the mean value of a set of descriptors + * @param descriptors + * @param mean mean descriptor + */ + static void meanValue(const std::vector &descriptors, + TDescriptor &mean); + + /** + * Calculates the distance between two descriptors + * @param a + * @param b + * @return distance + */ + static int distance(const TDescriptor &a, const TDescriptor &b); + + /** + * Returns a string version of the descriptor + * @param a descriptor + * @return string version + */ + static std::string toString(const TDescriptor &a); + + /** + * Returns a descriptor from a string + * @param a descriptor + * @param s string version + */ + static void fromString(TDescriptor &a, const std::string &s); + + /** + * Returns a mat with the descriptors in float format + * @param descriptors + * @param mat (out) NxL 32F matrix + */ + static void toMat32F(const std::vector &descriptors, + cv::Mat &mat); + + static void toMat8U(const std::vector &descriptors, + cv::Mat &mat); + +}; + +} // namespace DBoW2 + +#endif + diff --git a/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp b/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp index c055a15767..ebe3e3510e 100644 --- a/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp +++ b/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp @@ -1,85 +1,85 @@ -/** - * File: FeatureVector.cpp - * Date: November 2011 - * Author: Dorian Galvez-Lopez - * Description: feature vector - * License: see the LICENSE.txt file - * - */ - -#include "FeatureVector.h" -#include -#include -#include - -namespace DBoW2 { - -// --------------------------------------------------------------------------- - -FeatureVector::FeatureVector(void) -{ -} - -// --------------------------------------------------------------------------- - -FeatureVector::~FeatureVector(void) -{ -} - -// --------------------------------------------------------------------------- - -void FeatureVector::addFeature(NodeId id, unsigned int i_feature) -{ - FeatureVector::iterator vit = this->lower_bound(id); - - if(vit != this->end() && vit->first == id) - { - vit->second.push_back(i_feature); - } - else - { - vit = this->insert(vit, FeatureVector::value_type(id, - std::vector() )); - vit->second.push_back(i_feature); - } -} - -// --------------------------------------------------------------------------- - -std::ostream& operator<<(std::ostream &out, - const FeatureVector &v) -{ - if(!v.empty()) - { - FeatureVector::const_iterator vit = v.begin(); - - const std::vector* f = &vit->second; - - out << "<" << vit->first << ": ["; - if(!f->empty()) out << (*f)[0]; - for(unsigned int i = 1; i < f->size(); ++i) - { - out << ", " << (*f)[i]; - } - out << "]>"; - - for(++vit; vit != v.end(); ++vit) - { - f = &vit->second; - - out << ", <" << vit->first << ": ["; - if(!f->empty()) out << (*f)[0]; - for(unsigned int i = 1; i < f->size(); ++i) - { - out << ", " << (*f)[i]; - } - out << "]>"; - } - } - - return out; -} - -// --------------------------------------------------------------------------- - -} // namespace DBoW2 +/** + * File: FeatureVector.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#include "FeatureVector.h" +#include +#include +#include + +namespace DBoW2 { + +// --------------------------------------------------------------------------- + +FeatureVector::FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +FeatureVector::~FeatureVector(void) +{ +} + +// --------------------------------------------------------------------------- + +void FeatureVector::addFeature(NodeId id, unsigned int i_feature) +{ + FeatureVector::iterator vit = this->lower_bound(id); + + if(vit != this->end() && vit->first == id) + { + vit->second.push_back(i_feature); + } + else + { + vit = this->insert(vit, FeatureVector::value_type(id, + std::vector() )); + vit->second.push_back(i_feature); + } +} + +// --------------------------------------------------------------------------- + +std::ostream& operator<<(std::ostream &out, + const FeatureVector &v) +{ + if(!v.empty()) + { + FeatureVector::const_iterator vit = v.begin(); + + const std::vector* f = &vit->second; + + out << "<" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + + for(++vit; vit != v.end(); ++vit) + { + f = &vit->second; + + out << ", <" << vit->first << ": ["; + if(!f->empty()) out << (*f)[0]; + for(unsigned int i = 1; i < f->size(); ++i) + { + out << ", " << (*f)[i]; + } + out << "]>"; + } + } + + return out; +} + +// --------------------------------------------------------------------------- + +} // namespace DBoW2 diff --git a/Thirdparty/DBoW2/DBoW2/FeatureVector.h b/Thirdparty/DBoW2/DBoW2/FeatureVector.h index 08a91def7c..a9259d86bd 100644 --- a/Thirdparty/DBoW2/DBoW2/FeatureVector.h +++ b/Thirdparty/DBoW2/DBoW2/FeatureVector.h @@ -1,56 +1,58 @@ -/** - * File: FeatureVector.h - * Date: November 2011 - * Author: Dorian Galvez-Lopez - * Description: feature vector - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_FEATURE_VECTOR__ -#define __D_T_FEATURE_VECTOR__ - -#include "BowVector.h" -#include -#include -#include - -namespace DBoW2 { - -/// Vector of nodes with indexes of local features -class FeatureVector: - public std::map > -{ -public: - - /** - * Constructor - */ - FeatureVector(void); - - /** - * Destructor - */ - ~FeatureVector(void); - - /** - * Adds a feature to an existing node, or adds a new node with an initial - * feature - * @param id node id to add or to modify - * @param i_feature index of feature to add to the given node - */ - void addFeature(NodeId id, unsigned int i_feature); - - /** - * Sends a string versions of the feature vector through the stream - * @param out stream - * @param v feature vector - */ - friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); - -}; - -} // namespace DBoW2 - -#endif - +/** + * File: FeatureVector.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: feature vector + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_FEATURE_VECTOR__ +#define __D_T_FEATURE_VECTOR__ + +#include "BowVector.h" +#include +#include +#include + +#include "../DUtils/config.h" + +namespace DBoW2 { + +/// Vector of nodes with indexes of local features +class EXPORT FeatureVector: + public std::map > +{ +public: + + /** + * Constructor + */ + FeatureVector(void); + + /** + * Destructor + */ + ~FeatureVector(void); + + /** + * Adds a feature to an existing node, or adds a new node with an initial + * feature + * @param id node id to add or to modify + * @param i_feature index of feature to add to the given node + */ + void addFeature(NodeId id, unsigned int i_feature); + + /** + * Sends a string versions of the feature vector through the stream + * @param out stream + * @param v feature vector + */ + friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); + +}; + +} // namespace DBoW2 + +#endif + diff --git a/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp b/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp index 063a96e87d..2e6514e3d1 100644 --- a/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp +++ b/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp @@ -1,315 +1,315 @@ -/** - * File: ScoringObject.cpp - * Date: November 2011 - * Author: Dorian Galvez-Lopez - * Description: functions to compute bow scores - * License: see the LICENSE.txt file - * - */ - -#include -#include "TemplatedVocabulary.h" -#include "BowVector.h" - -using namespace DBoW2; - -// If you change the type of WordValue, make sure you change also the -// epsilon value (this is needed by the KL method) -const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double L1Scoring::score(const BowVector &v1, const BowVector &v2) const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - score += fabs(vi - wi) - fabs(vi) - fabs(wi); - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - v1_it = v1.lower_bound(v2_it->first); - // v1_it = (first element >= v2_it.id) - } - else - { - // move v2 forward - v2_it = v2.lower_bound(v1_it->first); - // v2_it = (first element >= v1_it.id) - } - } - - // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) - // for all i | v_i != 0 and w_i != 0 - // (Nister, 2006) - // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} - score = -score/2.0; - - return score; // [0..1] -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double L2Scoring::score(const BowVector &v1, const BowVector &v2) const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - score += vi * wi; - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - v1_it = v1.lower_bound(v2_it->first); - // v1_it = (first element >= v2_it.id) - } - else - { - // move v2 forward - v2_it = v2.lower_bound(v1_it->first); - // v2_it = (first element >= v1_it.id) - } - } - - // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) - // for all i | v_i != 0 and w_i != 0 ) - // (Nister, 2006) - if(score >= 1) // rounding errors - score = 1.0; - else - score = 1.0 - sqrt(1.0 - score); // [0..1] - - return score; -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) - const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - // all the items are taken into account - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) - // we move the -4 out - if(vi + wi != 0.0) score += vi * wi / (vi + wi); - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - v1_it = v1.lower_bound(v2_it->first); - } - else - { - // move v2 forward - v2_it = v2.lower_bound(v1_it->first); - } - } - - // this takes the -4 into account - score = 2. * score; // [0..1] - - return score; -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double KLScoring::score(const BowVector &v1, const BowVector &v2) const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - // all the items or v are taken into account - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - if(vi != 0 && wi != 0) score += vi * log(vi/wi); - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - score += vi * (log(vi) - LOG_EPS); - ++v1_it; - } - else - { - // move v2_it forward, do not add any score - v2_it = v2.lower_bound(v1_it->first); - // v2_it = (first element >= v1_it.id) - } - } - - // sum rest of items of v - for(; v1_it != v1_end; ++v1_it) - if(v1_it->second != 0) - score += v1_it->second * (log(v1_it->second) - LOG_EPS); - - return score; // cannot be scaled -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double BhattacharyyaScoring::score(const BowVector &v1, - const BowVector &v2) const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - score += sqrt(vi * wi); - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - v1_it = v1.lower_bound(v2_it->first); - // v1_it = (first element >= v2_it.id) - } - else - { - // move v2 forward - v2_it = v2.lower_bound(v1_it->first); - // v2_it = (first element >= v1_it.id) - } - } - - return score; // already scaled -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - -double DotProductScoring::score(const BowVector &v1, - const BowVector &v2) const -{ - BowVector::const_iterator v1_it, v2_it; - const BowVector::const_iterator v1_end = v1.end(); - const BowVector::const_iterator v2_end = v2.end(); - - v1_it = v1.begin(); - v2_it = v2.begin(); - - double score = 0; - - while(v1_it != v1_end && v2_it != v2_end) - { - const WordValue& vi = v1_it->second; - const WordValue& wi = v2_it->second; - - if(v1_it->first == v2_it->first) - { - score += vi * wi; - - // move v1 and v2 forward - ++v1_it; - ++v2_it; - } - else if(v1_it->first < v2_it->first) - { - // move v1 forward - v1_it = v1.lower_bound(v2_it->first); - // v1_it = (first element >= v2_it.id) - } - else - { - // move v2 forward - v2_it = v2.lower_bound(v1_it->first); - // v2_it = (first element >= v1_it.id) - } - } - - return score; // cannot scale -} - -// --------------------------------------------------------------------------- -// --------------------------------------------------------------------------- - +/** + * File: ScoringObject.cpp + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#include +#include "TemplatedVocabulary.h" +#include "BowVector.h" + +using namespace DBoW2; + +// If you change the type of WordValue, make sure you change also the +// epsilon value (this is needed by the KL method) +const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L1Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += fabs(vi - wi) - fabs(vi) - fabs(wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|) + // for all i | v_i != 0 and w_i != 0 + // (Nister, 2006) + // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1} + score = -score/2.0; + + return score; // [0..1] +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double L2Scoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) ) + // for all i | v_i != 0 and w_i != 0 ) + // (Nister, 2006) + if(score >= 1) // rounding errors + score = 1.0; + else + score = 1.0 - sqrt(1.0 - score); // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2) + const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + // (v-w)^2/(v+w) - v - w = -4 vw/(v+w) + // we move the -4 out + if(vi + wi != 0.0) score += vi * wi / (vi + wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + } + } + + // this takes the -4 into account + score = 2. * score; // [0..1] + + return score; +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double KLScoring::score(const BowVector &v1, const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + // all the items or v are taken into account + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + if(vi != 0 && wi != 0) score += vi * log(vi/wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + score += vi * (log(vi) - LOG_EPS); + ++v1_it; + } + else + { + // move v2_it forward, do not add any score + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + // sum rest of items of v + for(; v1_it != v1_end; ++v1_it) + if(v1_it->second != 0) + score += v1_it->second * (log(v1_it->second) - LOG_EPS); + + return score; // cannot be scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double BhattacharyyaScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += sqrt(vi * wi); + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // already scaled +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + +double DotProductScoring::score(const BowVector &v1, + const BowVector &v2) const +{ + BowVector::const_iterator v1_it, v2_it; + const BowVector::const_iterator v1_end = v1.end(); + const BowVector::const_iterator v2_end = v2.end(); + + v1_it = v1.begin(); + v2_it = v2.begin(); + + double score = 0; + + while(v1_it != v1_end && v2_it != v2_end) + { + const WordValue& vi = v1_it->second; + const WordValue& wi = v2_it->second; + + if(v1_it->first == v2_it->first) + { + score += vi * wi; + + // move v1 and v2 forward + ++v1_it; + ++v2_it; + } + else if(v1_it->first < v2_it->first) + { + // move v1 forward + v1_it = v1.lower_bound(v2_it->first); + // v1_it = (first element >= v2_it.id) + } + else + { + // move v2 forward + v2_it = v2.lower_bound(v1_it->first); + // v2_it = (first element >= v1_it.id) + } + } + + return score; // cannot scale +} + +// --------------------------------------------------------------------------- +// --------------------------------------------------------------------------- + diff --git a/Thirdparty/DBoW2/DBoW2/ScoringObject.h b/Thirdparty/DBoW2/DBoW2/ScoringObject.h index 8d5b82192a..2d2bbad2f5 100644 --- a/Thirdparty/DBoW2/DBoW2/ScoringObject.h +++ b/Thirdparty/DBoW2/DBoW2/ScoringObject.h @@ -1,96 +1,98 @@ -/** - * File: ScoringObject.h - * Date: November 2011 - * Author: Dorian Galvez-Lopez - * Description: functions to compute bow scores - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_SCORING_OBJECT__ -#define __D_T_SCORING_OBJECT__ - -#include "BowVector.h" - -namespace DBoW2 { - -/// Base class of scoring functions -class GeneralScoring -{ -public: - /** - * Computes the score between two vectors. Vectors must be sorted and - * normalized if necessary - * @param v (in/out) - * @param w (in/out) - * @return score - */ - virtual double score(const BowVector &v, const BowVector &w) const = 0; - - /** - * Returns whether a vector must be normalized before scoring according - * to the scoring scheme - * @param norm norm to use - * @return true iff must normalize - */ - virtual bool mustNormalize(LNorm &norm) const = 0; - - /// Log of epsilon - static const double LOG_EPS; - // If you change the type of WordValue, make sure you change also the - // epsilon value (this is needed by the KL method) - - virtual ~GeneralScoring() {} //!< Required for virtual base classes - -}; - -/** - * Macro for defining Scoring classes - * @param NAME name of class - * @param MUSTNORMALIZE if vectors must be normalized to compute the score - * @param NORM type of norm to use when MUSTNORMALIZE - */ -#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ - NAME: public GeneralScoring \ - { public: \ - /** \ - * Computes score between two vectors \ - * @param v \ - * @param w \ - * @return score between v and w \ - */ \ - virtual double score(const BowVector &v, const BowVector &w) const; \ - \ - /** \ - * Says if a vector must be normalized according to the scoring function \ - * @param norm (out) if true, norm to use - * @return true iff vectors must be normalized \ - */ \ - virtual inline bool mustNormalize(LNorm &norm) const \ - { norm = NORM; return MUSTNORMALIZE; } \ - } - -/// L1 Scoring object -class __SCORING_CLASS(L1Scoring, true, L1); - -/// L2 Scoring object -class __SCORING_CLASS(L2Scoring, true, L2); - -/// Chi square Scoring object -class __SCORING_CLASS(ChiSquareScoring, true, L1); - -/// KL divergence Scoring object -class __SCORING_CLASS(KLScoring, true, L1); - -/// Bhattacharyya Scoring object -class __SCORING_CLASS(BhattacharyyaScoring, true, L1); - -/// Dot product Scoring object -class __SCORING_CLASS(DotProductScoring, false, L1); - -#undef __SCORING_CLASS - -} // namespace DBoW2 - -#endif - +/** + * File: ScoringObject.h + * Date: November 2011 + * Author: Dorian Galvez-Lopez + * Description: functions to compute bow scores + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_SCORING_OBJECT__ +#define __D_T_SCORING_OBJECT__ + +#include "BowVector.h" + +#include "../DUtils/config.h" + +namespace DBoW2 { + +/// Base class of scoring functions +class EXPORT GeneralScoring +{ +public: + /** + * Computes the score between two vectors. Vectors must be sorted and + * normalized if necessary + * @param v (in/out) + * @param w (in/out) + * @return score + */ + virtual double score(const BowVector &v, const BowVector &w) const = 0; + + /** + * Returns whether a vector must be normalized before scoring according + * to the scoring scheme + * @param norm norm to use + * @return true iff must normalize + */ + virtual bool mustNormalize(LNorm &norm) const = 0; + + /// Log of epsilon + static const double LOG_EPS; + // If you change the type of WordValue, make sure you change also the + // epsilon value (this is needed by the KL method) + + virtual ~GeneralScoring() {} //!< Required for virtual base classes + +}; + +/** + * Macro for defining Scoring classes + * @param NAME name of class + * @param MUSTNORMALIZE if vectors must be normalized to compute the score + * @param NORM type of norm to use when MUSTNORMALIZE + */ +#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ + NAME: public GeneralScoring \ + { public: \ + /** \ + * Computes score between two vectors \ + * @param v \ + * @param w \ + * @return score between v and w \ + */ \ + virtual double score(const BowVector &v, const BowVector &w) const; \ + \ + /** \ + * Says if a vector must be normalized according to the scoring function \ + * @param norm (out) if true, norm to use + * @return true iff vectors must be normalized \ + */ \ + virtual inline bool mustNormalize(LNorm &norm) const \ + { norm = NORM; return MUSTNORMALIZE; } \ + } + +/// L1 Scoring object +class EXPORT __SCORING_CLASS(L1Scoring, true, L1); + +/// L2 Scoring object +class EXPORT __SCORING_CLASS(L2Scoring, true, L2); + +/// Chi square Scoring object +class EXPORT __SCORING_CLASS(ChiSquareScoring, true, L1); + +/// KL divergence Scoring object +class EXPORT __SCORING_CLASS(KLScoring, true, L1); + +/// Bhattacharyya Scoring object +class EXPORT __SCORING_CLASS(BhattacharyyaScoring, true, L1); + +/// Dot product Scoring object +class EXPORT __SCORING_CLASS(DotProductScoring, false, L1); + +#undef __SCORING_CLASS + +} // namespace DBoW2 + +#endif + diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index 01959344ed..8729d91587 100644 --- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -1,1665 +1,1764 @@ -/** - * This is a modified version of TemplatedVocabulary.h from DBoW2 (see below). - * Added functions: Save and Load from text files without using cv::FileStorage. - * Date: August 2015 - * Raúl Mur-Artal - */ - -/** - * File: TemplatedVocabulary.h - * Date: February 2011 - * Author: Dorian Galvez-Lopez - * Description: templated vocabulary - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_T_TEMPLATED_VOCABULARY__ -#define __D_T_TEMPLATED_VOCABULARY__ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "FeatureVector.h" -#include "BowVector.h" -#include "ScoringObject.h" - -#include "../DUtils/Random.h" - -using namespace std; - -namespace DBoW2 { - -/// @param TDescriptor class of descriptor -/// @param F class of descriptor functions -template -/// Generic Vocabulary -class TemplatedVocabulary -{ -public: - - /** - * Initiates an empty vocabulary - * @param k branching factor - * @param L depth levels - * @param weighting weighting type - * @param scoring scoring type - */ - TemplatedVocabulary(int k = 10, int L = 5, - WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM); - - /** - * Creates the vocabulary by loading a file - * @param filename - */ - TemplatedVocabulary(const std::string &filename); - - /** - * Creates the vocabulary by loading a file - * @param filename - */ - TemplatedVocabulary(const char *filename); - - /** - * Copy constructor - * @param voc - */ - TemplatedVocabulary(const TemplatedVocabulary &voc); - - /** - * Destructor - */ - virtual ~TemplatedVocabulary(); - - /** - * Assigns the given vocabulary to this by copying its data and removing - * all the data contained by this vocabulary before - * @param voc - * @return reference to this vocabulary - */ - TemplatedVocabulary& operator=( - const TemplatedVocabulary &voc); - - /** - * Creates a vocabulary from the training features with the already - * defined parameters - * @param training_features - */ - virtual void create - (const std::vector > &training_features); - - /** - * Creates a vocabulary from the training features, setting the branching - * factor and the depth levels of the tree - * @param training_features - * @param k branching factor - * @param L depth levels - */ - virtual void create - (const std::vector > &training_features, - int k, int L); - - /** - * Creates a vocabulary from the training features, setting the branching - * factor nad the depth levels of the tree, and the weighting and scoring - * schemes - */ - virtual void create - (const std::vector > &training_features, - int k, int L, WeightingType weighting, ScoringType scoring); - - /** - * Returns the number of words in the vocabulary - * @return number of words - */ - virtual inline unsigned int size() const; - - /** - * Returns whether the vocabulary is empty (i.e. it has not been trained) - * @return true iff the vocabulary is empty - */ - virtual inline bool empty() const; - - /** - * Transforms a set of descriptores into a bow vector - * @param features - * @param v (out) bow vector of weighted words - */ - virtual void transform(const std::vector& features, BowVector &v) - const; - - /** - * Transform a set of descriptors into a bow vector and a feature vector - * @param features - * @param v (out) bow vector - * @param fv (out) feature vector of nodes and feature indexes - * @param levelsup levels to go up the vocabulary tree to get the node index - */ - virtual void transform(const std::vector& features, - BowVector &v, FeatureVector &fv, int levelsup) const; - - /** - * Transforms a single feature into a word (without weight) - * @param feature - * @return word id - */ - virtual WordId transform(const TDescriptor& feature) const; - - /** - * Returns the score of two vectors - * @param a vector - * @param b vector - * @return score between vectors - * @note the vectors must be already sorted and normalized if necessary - */ - inline double score(const BowVector &a, const BowVector &b) const; - - /** - * Returns the id of the node that is "levelsup" levels from the word given - * @param wid word id - * @param levelsup 0..L - * @return node id. if levelsup is 0, returns the node id associated to the - * word id - */ - virtual NodeId getParentNode(WordId wid, int levelsup) const; - - /** - * Returns the ids of all the words that are under the given node id, - * by traversing any of the branches that goes down from the node - * @param nid starting node id - * @param words ids of words - */ - void getWordsFromNode(NodeId nid, std::vector &words) const; - - /** - * Returns the branching factor of the tree (k) - * @return k - */ - inline int getBranchingFactor() const { return m_k; } - - /** - * Returns the depth levels of the tree (L) - * @return L - */ - inline int getDepthLevels() const { return m_L; } - - /** - * Returns the real depth levels of the tree on average - * @return average of depth levels of leaves - */ - float getEffectiveLevels() const; - - /** - * Returns the descriptor of a word - * @param wid word id - * @return descriptor - */ - virtual inline TDescriptor getWord(WordId wid) const; - - /** - * Returns the weight of a word - * @param wid word id - * @return weight - */ - virtual inline WordValue getWordWeight(WordId wid) const; - - /** - * Returns the weighting method - * @return weighting method - */ - inline WeightingType getWeightingType() const { return m_weighting; } - - /** - * Returns the scoring method - * @return scoring method - */ - inline ScoringType getScoringType() const { return m_scoring; } - - /** - * Changes the weighting method - * @param type new weighting type - */ - inline void setWeightingType(WeightingType type); - - /** - * Changes the scoring method - * @param type new scoring type - */ - void setScoringType(ScoringType type); - - /** - * Loads the vocabulary from a text file - * @param filename - */ - bool loadFromTextFile(const std::string &filename); - - /** - * Saves the vocabulary into a text file - * @param filename - */ - void saveToTextFile(const std::string &filename) const; - - /** - * Saves the vocabulary into a file - * @param filename - */ - void save(const std::string &filename) const; - - /** - * Loads the vocabulary from a file - * @param filename - */ - void load(const std::string &filename); - - /** - * Saves the vocabulary to a file storage structure - * @param fn node in file storage - */ - virtual void save(cv::FileStorage &fs, - const std::string &name = "vocabulary") const; - - /** - * Loads the vocabulary from a file storage node - * @param fn first node - * @param subname name of the child node of fn where the tree is stored. - * If not given, the fn node is used instead - */ - virtual void load(const cv::FileStorage &fs, - const std::string &name = "vocabulary"); - - /** - * Stops those words whose weight is below minWeight. - * Words are stopped by setting their weight to 0. There are not returned - * later when transforming image features into vectors. - * Note that when using IDF or TF_IDF, the weight is the idf part, which - * is equivalent to -log(f), where f is the frequency of the word - * (f = Ni/N, Ni: number of training images where the word is present, - * N: number of training images). - * Note that the old weight is forgotten, and subsequent calls to this - * function with a lower minWeight have no effect. - * @return number of words stopped now - */ - virtual int stopWords(double minWeight); - -protected: - - /// Pointer to descriptor - typedef const TDescriptor *pDescriptor; - - /// Tree node - struct Node - { - /// Node id - NodeId id; - /// Weight if the node is a word - WordValue weight; - /// Children - vector children; - /// Parent node (undefined in case of root) - NodeId parent; - /// Node descriptor - TDescriptor descriptor; - - /// Word id if the node is a word - WordId word_id; - - /** - * Empty constructor - */ - Node(): id(0), weight(0), parent(0), word_id(0){} - - /** - * Constructor - * @param _id node id - */ - Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){} - - /** - * Returns whether the node is a leaf node - * @return true iff the node is a leaf - */ - inline bool isLeaf() const { return children.empty(); } - }; - -protected: - - /** - * Creates an instance of the scoring object accoring to m_scoring - */ - void createScoringObject(); - - /** - * Returns a set of pointers to descriptores - * @param training_features all the features - * @param features (out) pointers to the training features - */ - void getFeatures( - const vector > &training_features, - vector &features) const; - - /** - * Returns the word id associated to a feature - * @param feature - * @param id (out) word id - * @param weight (out) word weight - * @param nid (out) if given, id of the node "levelsup" levels up - * @param levelsup - */ - virtual void transform(const TDescriptor &feature, - WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const; - - /** - * Returns the word id associated to a feature - * @param feature - * @param id (out) word id - */ - virtual void transform(const TDescriptor &feature, WordId &id) const; - - /** - * Creates a level in the tree, under the parent, by running kmeans with - * a descriptor set, and recursively creates the subsequent levels too - * @param parent_id id of parent node - * @param descriptors descriptors to run the kmeans on - * @param current_level current level in the tree - */ - void HKmeansStep(NodeId parent_id, const vector &descriptors, - int current_level); - - /** - * Creates k clusters from the given descriptors with some seeding algorithm. - * @note In this class, kmeans++ is used, but this function should be - * overriden by inherited classes. - */ - virtual void initiateClusters(const vector &descriptors, - vector &clusters) const; - - /** - * Creates k clusters from the given descriptor sets by running the - * initial step of kmeans++ - * @param descriptors - * @param clusters resulting clusters - */ - void initiateClustersKMpp(const vector &descriptors, - vector &clusters) const; - - /** - * Create the words of the vocabulary once the tree has been built - */ - void createWords(); - - /** - * Sets the weights of the nodes of tree according to the given features. - * Before calling this function, the nodes and the words must be already - * created (by calling HKmeansStep and createWords) - * @param features - */ - void setNodeWeights(const vector > &features); - -protected: - - /// Branching factor - int m_k; - - /// Depth levels - int m_L; - - /// Weighting method - WeightingType m_weighting; - - /// Scoring method - ScoringType m_scoring; - - /// Object for computing scores - GeneralScoring* m_scoring_object; - - /// Tree nodes - std::vector m_nodes; - - /// Words of the vocabulary (tree leaves) - /// this condition holds: m_words[wid]->word_id == wid - std::vector m_words; - -}; - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary::TemplatedVocabulary - (int k, int L, WeightingType weighting, ScoringType scoring) - : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring), - m_scoring_object(NULL) -{ - createScoringObject(); -} - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary::TemplatedVocabulary - (const std::string &filename): m_scoring_object(NULL) -{ - load(filename); -} - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary::TemplatedVocabulary - (const char *filename): m_scoring_object(NULL) -{ - load(filename); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::createScoringObject() -{ - delete m_scoring_object; - m_scoring_object = NULL; - - switch(m_scoring) - { - case L1_NORM: - m_scoring_object = new L1Scoring; - break; - - case L2_NORM: - m_scoring_object = new L2Scoring; - break; - - case CHI_SQUARE: - m_scoring_object = new ChiSquareScoring; - break; - - case KL: - m_scoring_object = new KLScoring; - break; - - case BHATTACHARYYA: - m_scoring_object = new BhattacharyyaScoring; - break; - - case DOT_PRODUCT: - m_scoring_object = new DotProductScoring; - break; - - } -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::setScoringType(ScoringType type) -{ - m_scoring = type; - createScoringObject(); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::setWeightingType(WeightingType type) -{ - this->m_weighting = type; -} - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary::TemplatedVocabulary( - const TemplatedVocabulary &voc) - : m_scoring_object(NULL) -{ - *this = voc; -} - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary::~TemplatedVocabulary() -{ - delete m_scoring_object; -} - -// -------------------------------------------------------------------------- - -template -TemplatedVocabulary& -TemplatedVocabulary::operator= - (const TemplatedVocabulary &voc) -{ - this->m_k = voc.m_k; - this->m_L = voc.m_L; - this->m_scoring = voc.m_scoring; - this->m_weighting = voc.m_weighting; - - this->createScoringObject(); - - this->m_nodes.clear(); - this->m_words.clear(); - - this->m_nodes = voc.m_nodes; - this->createWords(); - - return *this; -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::create( - const std::vector > &training_features) -{ - m_nodes.clear(); - m_words.clear(); - - // expected_nodes = Sum_{i=0..L} ( k^i ) - int expected_nodes = - (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); - - m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree - - - vector features; - getFeatures(training_features, features); - - - // create root - m_nodes.push_back(Node(0)); // root - - // create the tree - HKmeansStep(0, features, 1); - - // create the words - createWords(); - - // and set the weight of each node of the tree - setNodeWeights(training_features); - -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::create( - const std::vector > &training_features, - int k, int L) -{ - m_k = k; - m_L = L; - - create(training_features); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::create( - const std::vector > &training_features, - int k, int L, WeightingType weighting, ScoringType scoring) -{ - m_k = k; - m_L = L; - m_weighting = weighting; - m_scoring = scoring; - createScoringObject(); - - create(training_features); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::getFeatures( - const vector > &training_features, - vector &features) const -{ - features.resize(0); - - typename vector >::const_iterator vvit; - typename vector::const_iterator vit; - for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit) - { - features.reserve(features.size() + vvit->size()); - for(vit = vvit->begin(); vit != vvit->end(); ++vit) - { - features.push_back(&(*vit)); - } - } -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::HKmeansStep(NodeId parent_id, - const vector &descriptors, int current_level) -{ - if(descriptors.empty()) return; - - // features associated to each cluster - vector clusters; - vector > groups; // groups[i] = [j1, j2, ...] - // j1, j2, ... indices of descriptors associated to cluster i - - clusters.reserve(m_k); - groups.reserve(m_k); - - //const int msizes[] = { m_k, descriptors.size() }; - //cv::SparseMat assoc(2, msizes, CV_8U); - //cv::SparseMat last_assoc(2, msizes, CV_8U); - //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated - - if((int)descriptors.size() <= m_k) - { - // trivial case: one cluster per feature - groups.resize(descriptors.size()); - - for(unsigned int i = 0; i < descriptors.size(); i++) - { - groups[i].push_back(i); - clusters.push_back(*descriptors[i]); - } - } - else - { - // select clusters and groups with kmeans - - bool first_time = true; - bool goon = true; - - // to check if clusters move after iterations - vector last_association, current_association; - - while(goon) - { - // 1. Calculate clusters - - if(first_time) - { - // random sample - initiateClusters(descriptors, clusters); - } - else - { - // calculate cluster centres - - for(unsigned int c = 0; c < clusters.size(); ++c) - { - vector cluster_descriptors; - cluster_descriptors.reserve(groups[c].size()); - - /* - for(unsigned int d = 0; d < descriptors.size(); ++d) - { - if( assoc.find(c, d) ) - { - cluster_descriptors.push_back(descriptors[d]); - } - } - */ - - vector::const_iterator vit; - for(vit = groups[c].begin(); vit != groups[c].end(); ++vit) - { - cluster_descriptors.push_back(descriptors[*vit]); - } - - - F::meanValue(cluster_descriptors, clusters[c]); - } - - } // if(!first_time) - - // 2. Associate features with clusters - - // calculate distances to cluster centers - groups.clear(); - groups.resize(clusters.size(), vector()); - current_association.resize(descriptors.size()); - - //assoc.clear(); - - typename vector::const_iterator fit; - //unsigned int d = 0; - for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d) - { - double best_dist = F::distance(*(*fit), clusters[0]); - unsigned int icluster = 0; - - for(unsigned int c = 1; c < clusters.size(); ++c) - { - double dist = F::distance(*(*fit), clusters[c]); - if(dist < best_dist) - { - best_dist = dist; - icluster = c; - } - } - - //assoc.ref(icluster, d) = 1; - - groups[icluster].push_back(fit - descriptors.begin()); - current_association[ fit - descriptors.begin() ] = icluster; - } - - // kmeans++ ensures all the clusters has any feature associated with them - - // 3. check convergence - if(first_time) - { - first_time = false; - } - else - { - //goon = !eqUChar(last_assoc, assoc); - - goon = false; - for(unsigned int i = 0; i < current_association.size(); i++) - { - if(current_association[i] != last_association[i]){ - goon = true; - break; - } - } - } - - if(goon) - { - // copy last feature-cluster association - last_association = current_association; - //last_assoc = assoc.clone(); - } - - } // while(goon) - - } // if must run kmeans - - // create nodes - for(unsigned int i = 0; i < clusters.size(); ++i) - { - NodeId id = m_nodes.size(); - m_nodes.push_back(Node(id)); - m_nodes.back().descriptor = clusters[i]; - m_nodes.back().parent = parent_id; - m_nodes[parent_id].children.push_back(id); - } - - // go on with the next level - if(current_level < m_L) - { - // iterate again with the resulting clusters - const vector &children_ids = m_nodes[parent_id].children; - for(unsigned int i = 0; i < clusters.size(); ++i) - { - NodeId id = children_ids[i]; - - vector child_features; - child_features.reserve(groups[i].size()); - - vector::const_iterator vit; - for(vit = groups[i].begin(); vit != groups[i].end(); ++vit) - { - child_features.push_back(descriptors[*vit]); - } - - if(child_features.size() > 1) - { - HKmeansStep(id, child_features, current_level + 1); - } - } - } -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::initiateClusters - (const vector &descriptors, vector &clusters) const -{ - initiateClustersKMpp(descriptors, clusters); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::initiateClustersKMpp( - const vector &pfeatures, vector &clusters) const -{ - // Implements kmeans++ seeding algorithm - // Algorithm: - // 1. Choose one center uniformly at random from among the data points. - // 2. For each data point x, compute D(x), the distance between x and the nearest - // center that has already been chosen. - // 3. Add one new data point as a center. Each point x is chosen with probability - // proportional to D(x)^2. - // 4. Repeat Steps 2 and 3 until k centers have been chosen. - // 5. Now that the initial centers have been chosen, proceed using standard k-means - // clustering. - - DUtils::Random::SeedRandOnce(); - - clusters.resize(0); - clusters.reserve(m_k); - vector min_dists(pfeatures.size(), std::numeric_limits::max()); - - // 1. - - int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1); - - // create first cluster - clusters.push_back(*pfeatures[ifeature]); - - // compute the initial distances - typename vector::const_iterator fit; - vector::iterator dit; - dit = min_dists.begin(); - for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) - { - *dit = F::distance(*(*fit), clusters.back()); - } - - while((int)clusters.size() < m_k) - { - // 2. - dit = min_dists.begin(); - for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) - { - if(*dit > 0) - { - double dist = F::distance(*(*fit), clusters.back()); - if(dist < *dit) *dit = dist; - } - } - - // 3. - double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0); - - if(dist_sum > 0) - { - double cut_d; - do - { - cut_d = DUtils::Random::RandomValue(0, dist_sum); - } while(cut_d == 0.0); - - double d_up_now = 0; - for(dit = min_dists.begin(); dit != min_dists.end(); ++dit) - { - d_up_now += *dit; - if(d_up_now >= cut_d) break; - } - - if(dit == min_dists.end()) - ifeature = pfeatures.size()-1; - else - ifeature = dit - min_dists.begin(); - - clusters.push_back(*pfeatures[ifeature]); - - } // if dist_sum > 0 - else - break; - - } // while(used_clusters < m_k) - -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::createWords() -{ - m_words.resize(0); - - if(!m_nodes.empty()) - { - m_words.reserve( (int)pow((double)m_k, (double)m_L) ); - - typename vector::iterator nit; - - nit = m_nodes.begin(); // ignore root - for(++nit; nit != m_nodes.end(); ++nit) - { - if(nit->isLeaf()) - { - nit->word_id = m_words.size(); - m_words.push_back( &(*nit) ); - } - } - } -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::setNodeWeights - (const vector > &training_features) -{ - const unsigned int NWords = m_words.size(); - const unsigned int NDocs = training_features.size(); - - if(m_weighting == TF || m_weighting == BINARY) - { - // idf part must be 1 always - for(unsigned int i = 0; i < NWords; i++) - m_words[i]->weight = 1; - } - else if(m_weighting == IDF || m_weighting == TF_IDF) - { - // IDF and TF-IDF: we calculte the idf path now - - // Note: this actually calculates the idf part of the tf-idf score. - // The complete tf-idf score is calculated in ::transform - - vector Ni(NWords, 0); - vector counted(NWords, false); - - typename vector >::const_iterator mit; - typename vector::const_iterator fit; - - for(mit = training_features.begin(); mit != training_features.end(); ++mit) - { - fill(counted.begin(), counted.end(), false); - - for(fit = mit->begin(); fit < mit->end(); ++fit) - { - WordId word_id; - transform(*fit, word_id); - - if(!counted[word_id]) - { - Ni[word_id]++; - counted[word_id] = true; - } - } - } - - // set ln(N/Ni) - for(unsigned int i = 0; i < NWords; i++) - { - if(Ni[i] > 0) - { - m_words[i]->weight = log((double)NDocs / (double)Ni[i]); - }// else // This cannot occur if using kmeans++ - } - - } - -} - -// -------------------------------------------------------------------------- - -template -inline unsigned int TemplatedVocabulary::size() const -{ - return m_words.size(); -} - -// -------------------------------------------------------------------------- - -template -inline bool TemplatedVocabulary::empty() const -{ - return m_words.empty(); -} - -// -------------------------------------------------------------------------- - -template -float TemplatedVocabulary::getEffectiveLevels() const -{ - long sum = 0; - typename std::vector::const_iterator wit; - for(wit = m_words.begin(); wit != m_words.end(); ++wit) - { - const Node *p = *wit; - - for(; p->id != 0; sum++) p = &m_nodes[p->parent]; - } - - return (float)((double)sum / (double)m_words.size()); -} - -// -------------------------------------------------------------------------- - -template -TDescriptor TemplatedVocabulary::getWord(WordId wid) const -{ - return m_words[wid]->descriptor; -} - -// -------------------------------------------------------------------------- - -template -WordValue TemplatedVocabulary::getWordWeight(WordId wid) const -{ - return m_words[wid]->weight; -} - -// -------------------------------------------------------------------------- - -template -WordId TemplatedVocabulary::transform - (const TDescriptor& feature) const -{ - if(empty()) - { - return 0; - } - - WordId wid; - transform(feature, wid); - return wid; -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::transform( - const std::vector& features, BowVector &v) const -{ - v.clear(); - - if(empty()) - { - return; - } - - // normalize - LNorm norm; - bool must = m_scoring_object->mustNormalize(norm); - - typename vector::const_iterator fit; - - if(m_weighting == TF || m_weighting == TF_IDF) - { - for(fit = features.begin(); fit < features.end(); ++fit) - { - WordId id; - WordValue w; - // w is the idf value if TF_IDF, 1 if TF - - transform(*fit, id, w); - - // not stopped - if(w > 0) v.addWeight(id, w); - } - - if(!v.empty() && !must) - { - // unnecessary when normalizing - const double nd = v.size(); - for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) - vit->second /= nd; - } - - } - else // IDF || BINARY - { - for(fit = features.begin(); fit < features.end(); ++fit) - { - WordId id; - WordValue w; - // w is idf if IDF, or 1 if BINARY - - transform(*fit, id, w); - - // not stopped - if(w > 0) v.addIfNotExist(id, w); - - } // if add_features - } // if m_weighting == ... - - if(must) v.normalize(norm); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::transform( - const std::vector& features, - BowVector &v, FeatureVector &fv, int levelsup) const -{ - v.clear(); - fv.clear(); - - if(empty()) // safe for subclasses - { - return; - } - - // normalize - LNorm norm; - bool must = m_scoring_object->mustNormalize(norm); - - typename vector::const_iterator fit; - - if(m_weighting == TF || m_weighting == TF_IDF) - { - unsigned int i_feature = 0; - for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) - { - WordId id; - NodeId nid; - WordValue w; - // w is the idf value if TF_IDF, 1 if TF - - transform(*fit, id, w, &nid, levelsup); - - if(w > 0) // not stopped - { - v.addWeight(id, w); - fv.addFeature(nid, i_feature); - } - } - - if(!v.empty() && !must) - { - // unnecessary when normalizing - const double nd = v.size(); - for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) - vit->second /= nd; - } - - } - else // IDF || BINARY - { - unsigned int i_feature = 0; - for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) - { - WordId id; - NodeId nid; - WordValue w; - // w is idf if IDF, or 1 if BINARY - - transform(*fit, id, w, &nid, levelsup); - - if(w > 0) // not stopped - { - v.addIfNotExist(id, w); - fv.addFeature(nid, i_feature); - } - } - } // if m_weighting == ... - - if(must) v.normalize(norm); -} - -// -------------------------------------------------------------------------- - -template -inline double TemplatedVocabulary::score - (const BowVector &v1, const BowVector &v2) const -{ - return m_scoring_object->score(v1, v2); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::transform - (const TDescriptor &feature, WordId &id) const -{ - WordValue weight; - transform(feature, id, weight); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::transform(const TDescriptor &feature, - WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const -{ - // propagate the feature down the tree - vector nodes; - typename vector::const_iterator nit; - - // level at which the node must be stored in nid, if given - const int nid_level = m_L - levelsup; - if(nid_level <= 0 && nid != NULL) *nid = 0; // root - - NodeId final_id = 0; // root - int current_level = 0; - - do - { - ++current_level; - nodes = m_nodes[final_id].children; - final_id = nodes[0]; - - double best_d = F::distance(feature, m_nodes[final_id].descriptor); - - for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit) - { - NodeId id = *nit; - double d = F::distance(feature, m_nodes[id].descriptor); - if(d < best_d) - { - best_d = d; - final_id = id; - } - } - - if(nid != NULL && current_level == nid_level) - *nid = final_id; - - } while( !m_nodes[final_id].isLeaf() ); - - // turn node id into word id - word_id = m_nodes[final_id].word_id; - weight = m_nodes[final_id].weight; -} - -// -------------------------------------------------------------------------- - -template -NodeId TemplatedVocabulary::getParentNode - (WordId wid, int levelsup) const -{ - NodeId ret = m_words[wid]->id; // node id - while(levelsup > 0 && ret != 0) // ret == 0 --> root - { - --levelsup; - ret = m_nodes[ret].parent; - } - return ret; -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::getWordsFromNode - (NodeId nid, std::vector &words) const -{ - words.clear(); - - if(m_nodes[nid].isLeaf()) - { - words.push_back(m_nodes[nid].word_id); - } - else - { - words.reserve(m_k); // ^1, ^2, ... - - vector parents; - parents.push_back(nid); - - while(!parents.empty()) - { - NodeId parentid = parents.back(); - parents.pop_back(); - - const vector &child_ids = m_nodes[parentid].children; - vector::const_iterator cit; - - for(cit = child_ids.begin(); cit != child_ids.end(); ++cit) - { - const Node &child_node = m_nodes[*cit]; - - if(child_node.isLeaf()) - words.push_back(child_node.word_id); - else - parents.push_back(*cit); - - } // for each child - } // while !parents.empty - } -} - -// -------------------------------------------------------------------------- - -template -int TemplatedVocabulary::stopWords(double minWeight) -{ - int c = 0; - typename vector::iterator wit; - for(wit = m_words.begin(); wit != m_words.end(); ++wit) - { - if((*wit)->weight < minWeight) - { - ++c; - (*wit)->weight = 0; - } - } - return c; -} - -// -------------------------------------------------------------------------- - -template -bool TemplatedVocabulary::loadFromTextFile(const std::string &filename) -{ - ifstream f; - f.open(filename.c_str()); - - if(f.eof()) - return false; - - m_words.clear(); - m_nodes.clear(); - - string s; - getline(f,s); - stringstream ss; - ss << s; - ss >> m_k; - ss >> m_L; - int n1, n2; - ss >> n1; - ss >> n2; - - if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3) - { - std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl; - return false; - } - - m_scoring = (ScoringType)n1; - m_weighting = (WeightingType)n2; - createScoringObject(); - - // nodes - int expected_nodes = - (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); - m_nodes.reserve(expected_nodes); - - m_words.reserve(pow((double)m_k, (double)m_L + 1)); - - m_nodes.resize(1); - m_nodes[0].id = 0; - while(!f.eof()) - { - string snode; - getline(f,snode); - stringstream ssnode; - ssnode << snode; - - int nid = m_nodes.size(); - m_nodes.resize(m_nodes.size()+1); - m_nodes[nid].id = nid; - - int pid ; - ssnode >> pid; - m_nodes[nid].parent = pid; - m_nodes[pid].children.push_back(nid); - - int nIsLeaf; - ssnode >> nIsLeaf; - - stringstream ssd; - for(int iD=0;iD> sElement; - ssd << sElement << " "; - } - F::fromString(m_nodes[nid].descriptor, ssd.str()); - - ssnode >> m_nodes[nid].weight; - - if(nIsLeaf>0) - { - int wid = m_words.size(); - m_words.resize(wid+1); - - m_nodes[nid].word_id = wid; - m_words[wid] = &m_nodes[nid]; - } - else - { - m_nodes[nid].children.reserve(m_k); - } - } - - return true; - -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::saveToTextFile(const std::string &filename) const -{ - fstream f; - f.open(filename.c_str(),ios_base::out); - f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl; - - for(size_t i=1; i -void TemplatedVocabulary::save(const std::string &filename) const -{ - cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); - if(!fs.isOpened()) throw string("Could not open file ") + filename; - - save(fs); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::load(const std::string &filename) -{ - cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); - if(!fs.isOpened()) throw string("Could not open file ") + filename; - - this->load(fs); -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::save(cv::FileStorage &f, - const std::string &name) const -{ - // Format YAML: - // vocabulary - // { - // k: - // L: - // scoringType: - // weightingType: - // nodes - // [ - // { - // nodeId: - // parentId: - // weight: - // descriptor: - // } - // ] - // words - // [ - // { - // wordId: - // nodeId: - // } - // ] - // } - // - // The root node (index 0) is not included in the node vector - // - - f << name << "{"; - - f << "k" << m_k; - f << "L" << m_L; - f << "scoringType" << m_scoring; - f << "weightingType" << m_weighting; - - // tree - f << "nodes" << "["; - vector parents, children; - vector::const_iterator pit; - - parents.push_back(0); // root - - while(!parents.empty()) - { - NodeId pid = parents.back(); - parents.pop_back(); - - const Node& parent = m_nodes[pid]; - children = parent.children; - - for(pit = children.begin(); pit != children.end(); pit++) - { - const Node& child = m_nodes[*pit]; - - // save node data - f << "{:"; - f << "nodeId" << (int)child.id; - f << "parentId" << (int)pid; - f << "weight" << (double)child.weight; - f << "descriptor" << F::toString(child.descriptor); - f << "}"; - - // add to parent list - if(!child.isLeaf()) - { - parents.push_back(*pit); - } - } - } - - f << "]"; // nodes - - // words - f << "words" << "["; - - typename vector::const_iterator wit; - for(wit = m_words.begin(); wit != m_words.end(); wit++) - { - WordId id = wit - m_words.begin(); - f << "{:"; - f << "wordId" << (int)id; - f << "nodeId" << (int)(*wit)->id; - f << "}"; - } - - f << "]"; // words - - f << "}"; - -} - -// -------------------------------------------------------------------------- - -template -void TemplatedVocabulary::load(const cv::FileStorage &fs, - const std::string &name) -{ - m_words.clear(); - m_nodes.clear(); - - cv::FileNode fvoc = fs[name]; - - m_k = (int)fvoc["k"]; - m_L = (int)fvoc["L"]; - m_scoring = (ScoringType)((int)fvoc["scoringType"]); - m_weighting = (WeightingType)((int)fvoc["weightingType"]); - - createScoringObject(); - - // nodes - cv::FileNode fn = fvoc["nodes"]; - - m_nodes.resize(fn.size() + 1); // +1 to include root - m_nodes[0].id = 0; - - for(unsigned int i = 0; i < fn.size(); ++i) - { - NodeId nid = (int)fn[i]["nodeId"]; - NodeId pid = (int)fn[i]["parentId"]; - WordValue weight = (WordValue)fn[i]["weight"]; - string d = (string)fn[i]["descriptor"]; - - m_nodes[nid].id = nid; - m_nodes[nid].parent = pid; - m_nodes[nid].weight = weight; - m_nodes[pid].children.push_back(nid); - - F::fromString(m_nodes[nid].descriptor, d); - } - - // words - fn = fvoc["words"]; - - m_words.resize(fn.size()); - - for(unsigned int i = 0; i < fn.size(); ++i) - { - NodeId wid = (int)fn[i]["wordId"]; - NodeId nid = (int)fn[i]["nodeId"]; - - m_nodes[nid].word_id = wid; - m_words[wid] = &m_nodes[nid]; - } -} - -// -------------------------------------------------------------------------- - -/** - * Writes printable information of the vocabulary - * @param os stream to write to - * @param voc - */ -template -std::ostream& operator<<(std::ostream &os, - const TemplatedVocabulary &voc) -{ - os << "Vocabulary: k = " << voc.getBranchingFactor() - << ", L = " << voc.getDepthLevels() - << ", Weighting = "; - - switch(voc.getWeightingType()) - { - case TF_IDF: os << "tf-idf"; break; - case TF: os << "tf"; break; - case IDF: os << "idf"; break; - case BINARY: os << "binary"; break; - } - - os << ", Scoring = "; - switch(voc.getScoringType()) - { - case L1_NORM: os << "L1-norm"; break; - case L2_NORM: os << "L2-norm"; break; - case CHI_SQUARE: os << "Chi square distance"; break; - case KL: os << "KL-divergence"; break; - case BHATTACHARYYA: os << "Bhattacharyya coefficient"; break; - case DOT_PRODUCT: os << "Dot product"; break; - } - - os << ", Number of words = " << voc.size(); - - return os; -} - -} // namespace DBoW2 - -#endif +/** + * This is a modified version of TemplatedVocabulary.h from DBoW2 (see below). + * Added functions: Save and Load from text files without using cv::FileStorage. + * Date: August 2015 + * Raúl Mur-Artal + */ + +/** + * File: TemplatedVocabulary.h + * Date: February 2011 + * Author: Dorian Galvez-Lopez + * Description: templated vocabulary + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_T_TEMPLATED_VOCABULARY__ +#define __D_T_TEMPLATED_VOCABULARY__ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "FeatureVector.h" +#include "BowVector.h" +#include "ScoringObject.h" + +#include "../DUtils/Random.h" + +#include "../DUtils/config.h" + +using namespace std; + +namespace DBoW2 { + +/// @param TDescriptor class of descriptor +/// @param F class of descriptor functions +template +/// Generic Vocabulary +class EXPORT TemplatedVocabulary +{ +public: + + /** + * Initiates an empty vocabulary + * @param k branching factor + * @param L depth levels + * @param weighting weighting type + * @param scoring scoring type + */ + TemplatedVocabulary(int k = 10, int L = 5, + WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + TemplatedVocabulary(const std::string &filename); + + /** + * Creates the vocabulary by loading a file + * @param filename + */ + TemplatedVocabulary(const char *filename); + + /** + * Copy constructor + * @param voc + */ + TemplatedVocabulary(const TemplatedVocabulary &voc); + + /** + * Destructor + */ + virtual ~TemplatedVocabulary(); + + /** + * Assigns the given vocabulary to this by copying its data and removing + * all the data contained by this vocabulary before + * @param voc + * @return reference to this vocabulary + */ + TemplatedVocabulary& operator=( + const TemplatedVocabulary &voc); + + /** + * Creates a vocabulary from the training features with the already + * defined parameters + * @param training_features + */ + virtual void create + (const std::vector > &training_features); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor and the depth levels of the tree + * @param training_features + * @param k branching factor + * @param L depth levels + */ + virtual void create + (const std::vector > &training_features, + int k, int L); + + /** + * Creates a vocabulary from the training features, setting the branching + * factor nad the depth levels of the tree, and the weighting and scoring + * schemes + */ + virtual void create + (const std::vector > &training_features, + int k, int L, WeightingType weighting, ScoringType scoring); + + /** + * Returns the number of words in the vocabulary + * @return number of words + */ + virtual inline unsigned int size() const; + + /** + * Returns whether the vocabulary is empty (i.e. it has not been trained) + * @return true iff the vocabulary is empty + */ + virtual inline bool empty() const; + + /** + * Transforms a set of descriptores into a bow vector + * @param features + * @param v (out) bow vector of weighted words + */ + virtual void transform(const std::vector& features, BowVector &v) + const; + + /** + * Transform a set of descriptors into a bow vector and a feature vector + * @param features + * @param v (out) bow vector + * @param fv (out) feature vector of nodes and feature indexes + * @param levelsup levels to go up the vocabulary tree to get the node index + */ + virtual void transform(const std::vector& features, + BowVector &v, FeatureVector &fv, int levelsup) const; + + /** + * Transforms a single feature into a word (without weight) + * @param feature + * @return word id + */ + virtual WordId transform(const TDescriptor& feature) const; + + /** + * Returns the score of two vectors + * @param a vector + * @param b vector + * @return score between vectors + * @note the vectors must be already sorted and normalized if necessary + */ + inline double score(const BowVector &a, const BowVector &b) const; + + /** + * Returns the id of the node that is "levelsup" levels from the word given + * @param wid word id + * @param levelsup 0..L + * @return node id. if levelsup is 0, returns the node id associated to the + * word id + */ + virtual NodeId getParentNode(WordId wid, int levelsup) const; + + /** + * Returns the ids of all the words that are under the given node id, + * by traversing any of the branches that goes down from the node + * @param nid starting node id + * @param words ids of words + */ + void getWordsFromNode(NodeId nid, std::vector &words) const; + + /** + * Returns the branching factor of the tree (k) + * @return k + */ + inline int getBranchingFactor() const { return m_k; } + + /** + * Returns the depth levels of the tree (L) + * @return L + */ + inline int getDepthLevels() const { return m_L; } + + /** + * Returns the real depth levels of the tree on average + * @return average of depth levels of leaves + */ + float getEffectiveLevels() const; + + /** + * Returns the descriptor of a word + * @param wid word id + * @return descriptor + */ + virtual inline TDescriptor getWord(WordId wid) const; + + /** + * Returns the weight of a word + * @param wid word id + * @return weight + */ + virtual inline WordValue getWordWeight(WordId wid) const; + + /** + * Returns the weighting method + * @return weighting method + */ + inline WeightingType getWeightingType() const { return m_weighting; } + + /** + * Returns the scoring method + * @return scoring method + */ + inline ScoringType getScoringType() const { return m_scoring; } + + /** + * Changes the weighting method + * @param type new weighting type + */ + inline void setWeightingType(WeightingType type); + + /** + * Changes the scoring method + * @param type new scoring type + */ + void setScoringType(ScoringType type); + + /** + * Loads the vocabulary from a text file + * @param filename + */ + bool loadFromTextFile(const std::string &filename); + + /** + * Saves the vocabulary into a text file + * @param filename + */ + void saveToTextFile(const std::string &filename) const; + + /** + * Loads the vocabulary from a binary file + * @param filename + */ + bool loadFromBinaryFile(const std::string &filename); + + /** + * Saves the vocabulary into a binary file + * @param filename + */ + void saveToBinaryFile(const std::string &filename) const; + + + /** + * Saves the vocabulary into a file + * @param filename + */ + void save(const std::string &filename) const; + + /** + * Loads the vocabulary from a file + * @param filename + */ + void load(const std::string &filename); + + /** + * Saves the vocabulary to a file storage structure + * @param fn node in file storage + */ + virtual void save(cv::FileStorage &fs, + const std::string &name = "vocabulary") const; + + /** + * Loads the vocabulary from a file storage node + * @param fn first node + * @param subname name of the child node of fn where the tree is stored. + * If not given, the fn node is used instead + */ + virtual void load(const cv::FileStorage &fs, + const std::string &name = "vocabulary"); + + /** + * Stops those words whose weight is below minWeight. + * Words are stopped by setting their weight to 0. There are not returned + * later when transforming image features into vectors. + * Note that when using IDF or TF_IDF, the weight is the idf part, which + * is equivalent to -log(f), where f is the frequency of the word + * (f = Ni/N, Ni: number of training images where the word is present, + * N: number of training images). + * Note that the old weight is forgotten, and subsequent calls to this + * function with a lower minWeight have no effect. + * @return number of words stopped now + */ + virtual int stopWords(double minWeight); + +protected: + + /// Pointer to descriptor + typedef const TDescriptor *pDescriptor; + + /// Tree node + struct Node + { + /// Node id + NodeId id; + /// Weight if the node is a word + WordValue weight; + /// Children + vector children; + /// Parent node (undefined in case of root) + NodeId parent; + /// Node descriptor + TDescriptor descriptor; + + /// Word id if the node is a word + WordId word_id; + + /** + * Empty constructor + */ + Node(): id(0), weight(0), parent(0), word_id(0){} + + /** + * Constructor + * @param _id node id + */ + Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){} + + /** + * Returns whether the node is a leaf node + * @return true iff the node is a leaf + */ + inline bool isLeaf() const { return children.empty(); } + }; + +protected: + + /** + * Creates an instance of the scoring object accoring to m_scoring + */ + void createScoringObject(); + + /** + * Returns a set of pointers to descriptores + * @param training_features all the features + * @param features (out) pointers to the training features + */ + void getFeatures( + const vector > &training_features, + vector &features) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + * @param weight (out) word weight + * @param nid (out) if given, id of the node "levelsup" levels up + * @param levelsup + */ + virtual void transform(const TDescriptor &feature, + WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const; + + /** + * Returns the word id associated to a feature + * @param feature + * @param id (out) word id + */ + virtual void transform(const TDescriptor &feature, WordId &id) const; + + /** + * Creates a level in the tree, under the parent, by running kmeans with + * a descriptor set, and recursively creates the subsequent levels too + * @param parent_id id of parent node + * @param descriptors descriptors to run the kmeans on + * @param current_level current level in the tree + */ + void HKmeansStep(NodeId parent_id, const vector &descriptors, + int current_level); + + /** + * Creates k clusters from the given descriptors with some seeding algorithm. + * @note In this class, kmeans++ is used, but this function should be + * overriden by inherited classes. + */ + virtual void initiateClusters(const vector &descriptors, + vector &clusters) const; + + /** + * Creates k clusters from the given descriptor sets by running the + * initial step of kmeans++ + * @param descriptors + * @param clusters resulting clusters + */ + void initiateClustersKMpp(const vector &descriptors, + vector &clusters) const; + + /** + * Create the words of the vocabulary once the tree has been built + */ + void createWords(); + + /** + * Sets the weights of the nodes of tree according to the given features. + * Before calling this function, the nodes and the words must be already + * created (by calling HKmeansStep and createWords) + * @param features + */ + void setNodeWeights(const vector > &features); + +protected: + + /// Branching factor + int m_k; + + /// Depth levels + int m_L; + + /// Weighting method + WeightingType m_weighting; + + /// Scoring method + ScoringType m_scoring; + + /// Object for computing scores + GeneralScoring* m_scoring_object; + + /// Tree nodes + std::vector m_nodes; + + /// Words of the vocabulary (tree leaves) + /// this condition holds: m_words[wid]->word_id == wid + std::vector m_words; + +}; + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (int k, int L, WeightingType weighting, ScoringType scoring) + : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring), + m_scoring_object(NULL) +{ + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (const std::string &filename): m_scoring_object(NULL) +{ + load(filename); +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary + (const char *filename): m_scoring_object(NULL) +{ + load(filename); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::createScoringObject() +{ + delete m_scoring_object; + m_scoring_object = NULL; + + switch(m_scoring) + { + case L1_NORM: + m_scoring_object = new L1Scoring; + break; + + case L2_NORM: + m_scoring_object = new L2Scoring; + break; + + case CHI_SQUARE: + m_scoring_object = new ChiSquareScoring; + break; + + case KL: + m_scoring_object = new KLScoring; + break; + + case BHATTACHARYYA: + m_scoring_object = new BhattacharyyaScoring; + break; + + case DOT_PRODUCT: + m_scoring_object = new DotProductScoring; + break; + + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setScoringType(ScoringType type) +{ + m_scoring = type; + createScoringObject(); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setWeightingType(WeightingType type) +{ + this->m_weighting = type; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::TemplatedVocabulary( + const TemplatedVocabulary &voc) + : m_scoring_object(NULL) +{ + *this = voc; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary::~TemplatedVocabulary() +{ + delete m_scoring_object; +} + +// -------------------------------------------------------------------------- + +template +TemplatedVocabulary& +TemplatedVocabulary::operator= + (const TemplatedVocabulary &voc) +{ + this->m_k = voc.m_k; + this->m_L = voc.m_L; + this->m_scoring = voc.m_scoring; + this->m_weighting = voc.m_weighting; + + this->createScoringObject(); + + this->m_nodes.clear(); + this->m_words.clear(); + + this->m_nodes = voc.m_nodes; + this->createWords(); + + return *this; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features) +{ + m_nodes.clear(); + m_words.clear(); + + // expected_nodes = Sum_{i=0..L} ( k^i ) + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); + + m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree + + + vector features; + getFeatures(training_features, features); + + + // create root + m_nodes.push_back(Node(0)); // root + + // create the tree + HKmeansStep(0, features, 1); + + // create the words 即tree的叶子结点 + createWords(); + + // and set the weight of each node of the tree + setNodeWeights(training_features); + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features, + int k, int L) +{ + m_k = k; + m_L = L; + + create(training_features); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::create( + const std::vector > &training_features, + int k, int L, WeightingType weighting, ScoringType scoring) +{ + m_k = k; + m_L = L; + m_weighting = weighting; + m_scoring = scoring; + createScoringObject(); + + create(training_features); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::getFeatures( + const vector > &training_features, + vector &features) const +{ + features.resize(0); + + typename vector >::const_iterator vvit; + typename vector::const_iterator vit; + for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit) + { + features.reserve(features.size() + vvit->size()); + for(vit = vvit->begin(); vit != vvit->end(); ++vit) + { + features.push_back(&(*vit)); + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::HKmeansStep(NodeId parent_id, + const vector &descriptors, int current_level) +{ + if(descriptors.empty()) return; + + // features associated to each cluster + vector clusters; + vector > groups; // groups[i] = [j1, j2, ...] + // j1, j2, ... indices of descriptors associated to cluster i + + clusters.reserve(m_k); + groups.reserve(m_k); + + //const int msizes[] = { m_k, descriptors.size() }; + //cv::SparseMat assoc(2, msizes, CV_8U); + //cv::SparseMat last_assoc(2, msizes, CV_8U); + //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated + + if((int)descriptors.size() <= m_k) + { + // trivial case: one cluster per feature + groups.resize(descriptors.size()); + + for(unsigned int i = 0; i < descriptors.size(); i++) + { + groups[i].push_back(i); + clusters.push_back(*descriptors[i]); + } + } + else + { + // select clusters and groups with kmeans + + bool first_time = true; + bool goon = true; + + // to check if clusters move after iterations + vector last_association, current_association; + + while(goon) + { + // 1. Calculate clusters + + if(first_time) + { + // random sample + initiateClusters(descriptors, clusters); + } + else + { + // calculate cluster centres 计算聚类中心 + + for(unsigned int c = 0; c < clusters.size(); ++c) + { + vector cluster_descriptors; + cluster_descriptors.reserve(groups[c].size()); + + /* + for(unsigned int d = 0; d < descriptors.size(); ++d) + { + if( assoc.find(c, d) ) + { + cluster_descriptors.push_back(descriptors[d]); + } + } + */ + + vector::const_iterator vit; + for(vit = groups[c].begin(); vit != groups[c].end(); ++vit) + { + cluster_descriptors.push_back(descriptors[*vit]); + } + + + F::meanValue(cluster_descriptors, clusters[c]); + } + + } // if(!first_time) + + // 2. Associate features with clusters + + // calculate distances to cluster centers + groups.clear(); + groups.resize(clusters.size(), vector()); + current_association.resize(descriptors.size()); + + //assoc.clear(); + + typename vector::const_iterator fit; + //unsigned int d = 0; + for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d) + { + double best_dist = F::distance(*(*fit), clusters[0]); + unsigned int icluster = 0; + + for(unsigned int c = 1; c < clusters.size(); ++c) + { + double dist = F::distance(*(*fit), clusters[c]); + if(dist < best_dist) + { + best_dist = dist; + icluster = c; + } + } + + //assoc.ref(icluster, d) = 1; + + groups[icluster].push_back(fit - descriptors.begin()); + current_association[ fit - descriptors.begin() ] = icluster; + } + + // kmeans++ ensures all the clusters has any feature associated with them + + // 3. check convergence + if(first_time) + { + first_time = false; + } + else + { + //goon = !eqUChar(last_assoc, assoc); + + goon = false; + for(unsigned int i = 0; i < current_association.size(); i++) + { + // 比较上一次和这一次聚类的结果，如果不同则再迭代 + if(current_association[i] != last_association[i]){ + goon = true; + break; + } + } + } + + if(goon) + { + // copy last feature-cluster association + last_association = current_association; + //last_assoc = assoc.clone(); + } + + } // while(goon) + + } // if must run kmeans + + // create nodes + for(unsigned int i = 0; i < clusters.size(); ++i) + { + NodeId id = m_nodes.size(); + m_nodes.push_back(Node(id)); + m_nodes.back().descriptor = clusters[i]; + m_nodes.back().parent = parent_id; + m_nodes[parent_id].children.push_back(id); + } + + // go on with the next level + if(current_level < m_L) + { + // iterate again with the resulting clusters + const vector &children_ids = m_nodes[parent_id].children; + for(unsigned int i = 0; i < clusters.size(); ++i) + { + NodeId id = children_ids[i]; + + vector child_features; + child_features.reserve(groups[i].size()); + + vector::const_iterator vit; + for(vit = groups[i].begin(); vit != groups[i].end(); ++vit) + { + child_features.push_back(descriptors[*vit]); + } + + if(child_features.size() > 1) + { + HKmeansStep(id, child_features, current_level + 1); + } + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::initiateClusters + (const vector &descriptors, vector &clusters) const +{ + initiateClustersKMpp(descriptors, clusters); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::initiateClustersKMpp( + const vector &pfeatures, vector &clusters) const +{ + // Implements kmeans++ seeding algorithm + // Algorithm: + // 1. Choose one center uniformly at random from among the data points. + // 2. For each data point x, compute D(x), the distance between x and the nearest + // center that has already been chosen. + // 3. Add one new data point as a center. Each point x is chosen with probability + // proportional to D(x)^2. + // 4. Repeat Steps 2 and 3 until k centers have been chosen. + // 5. Now that the initial centers have been chosen, proceed using standard k-means + // clustering. + + // 1. 从输入的数据点集合中随机选择一个点作为第一个聚类中心 + // 2. 对于数据集中的每一个点x，计算它与最近聚类中心(指已选择的聚类中心)的距离D(x)并保存在一个数组里， + // 然后把这些距离加起来得到Sum(D(x))。 + // 3. 选择一个新的数据点作为新的聚类中心，选择的原则是：D(x)较大的点，被选取作为聚类中心的概率较大 + // 实际做法：取一个0～Sum(D(x))之间的随机值Random，计算Sum(D(0)，D(1)...D(j))>=Random，第j个点为种子点 + // 4. 重复2和3直到k个聚类中心被选出来 + // 5. 利用这k个初始的聚类中心来运行标准的k-means算法 + + DUtils::Random::SeedRandOnce(); + + clusters.resize(0); + clusters.reserve(m_k); + vector min_dists(pfeatures.size(), std::numeric_limits::max()); + + // 1. + + int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1); + + // create first cluster + clusters.push_back(*pfeatures[ifeature]); + + // compute the initial distances + typename vector::const_iterator fit; + vector::iterator dit; + dit = min_dists.begin(); + for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) + { + *dit = F::distance(*(*fit), clusters.back()); + } + + while((int)clusters.size() < m_k) + { + // 2. + dit = min_dists.begin(); + for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit) + { + if(*dit > 0) + { + double dist = F::distance(*(*fit), clusters.back()); + if(dist < *dit) *dit = dist; // 仅保存最近的距离，即与最近聚类中心的距离 + } + } + + // 3. + double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0); + + if(dist_sum > 0) + { + double cut_d; + do + { + cut_d = DUtils::Random::RandomValue(0, dist_sum); + } while(cut_d == 0.0); + + double d_up_now = 0; + for(dit = min_dists.begin(); dit != min_dists.end(); ++dit) + { + d_up_now += *dit; + if(d_up_now >= cut_d) break; + } + + if(dit == min_dists.end()) + ifeature = pfeatures.size()-1; + else + ifeature = dit - min_dists.begin(); + + clusters.push_back(*pfeatures[ifeature]); + + } // if dist_sum > 0 + else + break; + + } // while(used_clusters < m_k) + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::createWords() +{ + m_words.resize(0); + + if(!m_nodes.empty()) + { + m_words.reserve( (int)pow((double)m_k, (double)m_L) ); + + typename vector::iterator nit; + + nit = m_nodes.begin(); // ignore root + for(++nit; nit != m_nodes.end(); ++nit) + { + if(nit->isLeaf()) + { + nit->word_id = m_words.size(); + m_words.push_back( &(*nit) ); + } + } + } +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::setNodeWeights + (const vector > &training_features) +{ + const unsigned int NWords = m_words.size(); + const unsigned int NDocs = training_features.size(); + + if(m_weighting == TF || m_weighting == BINARY) + { + // idf part must be 1 always + for(unsigned int i = 0; i < NWords; i++) + m_words[i]->weight = 1; + } + else if(m_weighting == IDF || m_weighting == TF_IDF) + { + // IDF and TF-IDF: we calculte the idf path now + + // Note: this actually calculates the idf part of the tf-idf score. + // The complete tf-idf score is calculated in ::transform + + vector Ni(NWords, 0); // 统计词频，范围为0～training_features.size() + vector counted(NWords, false); + + typename vector >::const_iterator mit; + typename vector::const_iterator fit; + + for(mit = training_features.begin(); mit != training_features.end(); ++mit) + { + fill(counted.begin(), counted.end(), false); + + for(fit = mit->begin(); fit < mit->end(); ++fit) + { + WordId word_id; + transform(*fit, word_id); + + if(!counted[word_id]) + { + Ni[word_id]++; + counted[word_id] = true; + } + } + } + + // set ln(N/Ni) iDf是一个词语普遍重要性的度量, Ni越小，重要程度越高 + for(unsigned int i = 0; i < NWords; i++) + { + if(Ni[i] > 0) + { + m_words[i]->weight = log((double)NDocs / (double)Ni[i]); + }// else // This cannot occur if using kmeans++ + } + + } + +} + +// -------------------------------------------------------------------------- + +template +inline unsigned int TemplatedVocabulary::size() const +{ + return m_words.size(); +} + +// -------------------------------------------------------------------------- + +template +inline bool TemplatedVocabulary::empty() const +{ + return m_words.empty(); +} + +// -------------------------------------------------------------------------- + +template +float TemplatedVocabulary::getEffectiveLevels() const +{ + long sum = 0; + typename std::vector::const_iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); ++wit) + { + const Node *p = *wit; + + for(; p->id != 0; sum++) p = &m_nodes[p->parent]; + } + + return (float)((double)sum / (double)m_words.size()); +} + +// -------------------------------------------------------------------------- + +template +TDescriptor TemplatedVocabulary::getWord(WordId wid) const +{ + return m_words[wid]->descriptor; +} + +// -------------------------------------------------------------------------- + +template +WordValue TemplatedVocabulary::getWordWeight(WordId wid) const +{ + return m_words[wid]->weight; +} + +// -------------------------------------------------------------------------- + +template +WordId TemplatedVocabulary::transform + (const TDescriptor& feature) const +{ + if(empty()) + { + return 0; + } + + WordId wid; + transform(feature, wid); + return wid; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform( + const std::vector& features, BowVector &v) const +{ + v.clear(); + + if(empty()) + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + typename vector::const_iterator fit; + + if(m_weighting == TF || m_weighting == TF_IDF) + { + for(fit = features.begin(); fit < features.end(); ++fit) + { + WordId id; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w); + + // not stopped + if(w > 0) v.addWeight(id, w); + } + + if(!v.empty() && !must) + { + // unnecessary when normalizing + const double nd = v.size(); + for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } + else // IDF || BINARY + { + for(fit = features.begin(); fit < features.end(); ++fit) + { + WordId id; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w); + + // not stopped + if(w > 0) v.addIfNotExist(id, w); + + } // if add_features + } // if m_weighting == ... + + if(must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform( + const std::vector& features, + BowVector &v, FeatureVector &fv, int levelsup) const +{ + v.clear(); + fv.clear(); + + if(empty()) // safe for subclasses + { + return; + } + + // normalize + LNorm norm; + bool must = m_scoring_object->mustNormalize(norm); + + typename vector::const_iterator fit; + + if(m_weighting == TF || m_weighting == TF_IDF) + { + unsigned int i_feature = 0; + for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) + { + WordId id; + NodeId nid; + WordValue w; + // w is the idf value if TF_IDF, 1 if TF + + transform(*fit, id, w, &nid, levelsup); + + if(w > 0) // not stopped + { + v.addWeight(id, w); + fv.addFeature(nid, i_feature); + } + } + + if(!v.empty() && !must) + { + // unnecessary when normalizing + const double nd = v.size(); + for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++) + vit->second /= nd; + } + + } + else // IDF || BINARY + { + unsigned int i_feature = 0; + for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature) + { + WordId id; + NodeId nid; + WordValue w; + // w is idf if IDF, or 1 if BINARY + + transform(*fit, id, w, &nid, levelsup); + + if(w > 0) // not stopped + { + v.addIfNotExist(id, w); + fv.addFeature(nid, i_feature); + } + } + } // if m_weighting == ... + + if(must) v.normalize(norm); +} + +// -------------------------------------------------------------------------- + +template +inline double TemplatedVocabulary::score + (const BowVector &v1, const BowVector &v2) const +{ + return m_scoring_object->score(v1, v2); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform + (const TDescriptor &feature, WordId &id) const +{ + WordValue weight; + transform(feature, id, weight); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::transform(const TDescriptor &feature, + WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const +{ + // propagate the feature down the tree + vector nodes; + typename vector::const_iterator nit; + + // level at which the node must be stored in nid, if given + const int nid_level = m_L - levelsup; + if(nid_level <= 0 && nid != NULL) *nid = 0; // root + + NodeId final_id = 0; // root + int current_level = 0; + + do + { + ++current_level; + nodes = m_nodes[final_id].children; + final_id = nodes[0]; + + double best_d = F::distance(feature, m_nodes[final_id].descriptor); + + for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit) + { + NodeId id = *nit; + double d = F::distance(feature, m_nodes[id].descriptor); + if(d < best_d) + { + best_d = d; + final_id = id; + } + } + + if(nid != NULL && current_level == nid_level) + *nid = final_id; + + } while( !m_nodes[final_id].isLeaf() ); + + // turn node id into word id + word_id = m_nodes[final_id].word_id; + weight = m_nodes[final_id].weight; +} + +// -------------------------------------------------------------------------- + +template +NodeId TemplatedVocabulary::getParentNode + (WordId wid, int levelsup) const +{ + NodeId ret = m_words[wid]->id; // node id + while(levelsup > 0 && ret != 0) // ret == 0 --> root + { + --levelsup; + ret = m_nodes[ret].parent; + } + return ret; +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::getWordsFromNode + (NodeId nid, std::vector &words) const +{ + words.clear(); + + if(m_nodes[nid].isLeaf()) + { + words.push_back(m_nodes[nid].word_id); + } + else + { + words.reserve(m_k); // ^1, ^2, ... + + vector parents; + parents.push_back(nid); + + while(!parents.empty()) + { + NodeId parentid = parents.back(); + parents.pop_back(); + + const vector &child_ids = m_nodes[parentid].children; + vector::const_iterator cit; + + for(cit = child_ids.begin(); cit != child_ids.end(); ++cit) + { + const Node &child_node = m_nodes[*cit]; + + if(child_node.isLeaf()) + words.push_back(child_node.word_id); + else + parents.push_back(*cit); + + } // for each child + } // while !parents.empty + } +} + +// -------------------------------------------------------------------------- + +template +int TemplatedVocabulary::stopWords(double minWeight) +{ + int c = 0; + typename vector::iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); ++wit) + { + if((*wit)->weight < minWeight) + { + ++c; + (*wit)->weight = 0; + } + } + return c; +} + +// -------------------------------------------------------------------------- + +template +bool TemplatedVocabulary::loadFromTextFile(const std::string &filename) +{ + ifstream f; + f.open(filename.c_str()); + + if(f.eof()) + return false; + + m_words.clear(); + m_nodes.clear(); + + string s; + getline(f,s); + stringstream ss; + ss << s; + ss >> m_k; + ss >> m_L; + int n1, n2; + ss >> n1; + ss >> n2; + + if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3) + { + std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl; + return false; + } + + m_scoring = (ScoringType)n1; + m_weighting = (WeightingType)n2; + createScoringObject(); + + // nodes + int expected_nodes = + (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1)); + m_nodes.reserve(expected_nodes); + + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + + m_nodes.resize(1); + m_nodes[0].id = 0; + while(!f.eof()) + { + string snode; + getline(f,snode); + stringstream ssnode; + ssnode << snode; + + int nid = m_nodes.size(); + m_nodes.resize(m_nodes.size()+1); + m_nodes[nid].id = nid; + + int pid ; + ssnode >> pid; + m_nodes[nid].parent = pid; + m_nodes[pid].children.push_back(nid); + + int nIsLeaf; + ssnode >> nIsLeaf; + + stringstream ssd; + for(int iD=0;iD> sElement; + ssd << sElement << " "; + } + F::fromString(m_nodes[nid].descriptor, ssd.str()); + + ssnode >> m_nodes[nid].weight; + + if(nIsLeaf>0) + { + int wid = m_words.size(); + m_words.resize(wid+1); + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } + else + { + m_nodes[nid].children.reserve(m_k); + } + } + + return true; + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::saveToTextFile(const std::string &filename) const +{ + fstream f; + f.open(filename.c_str(),ios_base::out); + f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl; + + for(size_t i=1; i +bool TemplatedVocabulary::loadFromBinaryFile(const std::string &filename) { + fstream f; + f.open(filename.c_str(), ios_base::in|ios::binary); + unsigned int nb_nodes, size_node; + f.read((char*)&nb_nodes, sizeof(nb_nodes)); + f.read((char*)&size_node, sizeof(size_node)); + f.read((char*)&m_k, sizeof(m_k)); + f.read((char*)&m_L, sizeof(m_L)); + f.read((char*)&m_scoring, sizeof(m_scoring)); + f.read((char*)&m_weighting, sizeof(m_weighting)); + createScoringObject(); + + m_words.clear(); + m_words.reserve(pow((double)m_k, (double)m_L + 1)); + m_nodes.clear(); + m_nodes.resize(nb_nodes+1); + m_nodes[0].id = 0; + char* buf = new char [size_node]; + int nid = 1; + while (!f.eof()) { + f.read(buf, size_node); + m_nodes[nid].id = nid; + // FIXME + const int* ptr=(int*)buf; + m_nodes[nid].parent = *ptr; + //m_nodes[nid].parent = *(const int*)buf; + m_nodes[m_nodes[nid].parent].children.push_back(nid); + m_nodes[nid].descriptor = cv::Mat(1, F::L, CV_8U); //F::L + memcpy(m_nodes[nid].descriptor.data, buf+4, F::L); //F::L + m_nodes[nid].weight = *(float*)(buf+4+F::L); // F::L + if (buf[8+F::L]) { // is leaf //F::L + int wid = m_words.size(); + m_words.resize(wid+1); + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } + else + m_nodes[nid].children.reserve(m_k); + nid+=1; + } + f.close(); + + delete[] buf; + return true; +} + + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::saveToBinaryFile(const std::string &filename) const { + fstream f; + f.open(filename.c_str(), ios_base::out|ios::binary); + unsigned int nb_nodes = m_nodes.size(); + float _weight; + unsigned int size_node = sizeof(m_nodes[0].parent) + F::L*sizeof(char) + sizeof(_weight) + sizeof(bool); //F::L + f.write((char*)&nb_nodes, sizeof(nb_nodes)); + f.write((char*)&size_node, sizeof(size_node)); + f.write((char*)&m_k, sizeof(m_k)); + f.write((char*)&m_L, sizeof(m_L)); + f.write((char*)&m_scoring, sizeof(m_scoring)); + f.write((char*)&m_weighting, sizeof(m_weighting)); + for(size_t i=1; i +void TemplatedVocabulary::save(const std::string &filename) const +{ + cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE); + if(!fs.isOpened()) throw string("Could not open file ") + filename; + + save(fs); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::load(const std::string &filename) +{ + cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ); + if(!fs.isOpened()) throw string("Could not open file ") + filename; + + this->load(fs); +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::save(cv::FileStorage &f, + const std::string &name) const +{ + // Format YAML: + // vocabulary + // { + // k: + // L: + // scoringType: + // weightingType: + // nodes + // [ + // { + // nodeId: + // parentId: + // weight: + // descriptor: + // } + // ] + // words + // [ + // { + // wordId: + // nodeId: + // } + // ] + // } + // + // The root node (index 0) is not included in the node vector + // + + f << name << "{"; + + f << "k" << m_k; + f << "L" << m_L; + f << "scoringType" << m_scoring; + f << "weightingType" << m_weighting; + + // tree + f << "nodes" << "["; + vector parents, children; + vector::const_iterator pit; + + parents.push_back(0); // root + + while(!parents.empty()) + { + NodeId pid = parents.back(); + parents.pop_back(); + + const Node& parent = m_nodes[pid]; + children = parent.children; + + for(pit = children.begin(); pit != children.end(); pit++) + { + const Node& child = m_nodes[*pit]; + + // save node data + f << "{:"; + f << "nodeId" << (int)child.id; + f << "parentId" << (int)pid; + f << "weight" << (double)child.weight; + f << "descriptor" << F::toString(child.descriptor); + f << "}"; + + // add to parent list + if(!child.isLeaf()) + { + parents.push_back(*pit); + } + } + } + + f << "]"; // nodes + + // words + f << "words" << "["; + + typename vector::const_iterator wit; + for(wit = m_words.begin(); wit != m_words.end(); wit++) + { + WordId id = wit - m_words.begin(); + f << "{:"; + f << "wordId" << (int)id; + f << "nodeId" << (int)(*wit)->id; + f << "}"; + } + + f << "]"; // words + + f << "}"; + +} + +// -------------------------------------------------------------------------- + +template +void TemplatedVocabulary::load(const cv::FileStorage &fs, + const std::string &name) +{ + m_words.clear(); + m_nodes.clear(); + + cv::FileNode fvoc = fs[name]; + + m_k = (int)fvoc["k"]; + m_L = (int)fvoc["L"]; + m_scoring = (ScoringType)((int)fvoc["scoringType"]); + m_weighting = (WeightingType)((int)fvoc["weightingType"]); + + createScoringObject(); + + // nodes + cv::FileNode fn = fvoc["nodes"]; + + m_nodes.resize(fn.size() + 1); // +1 to include root + m_nodes[0].id = 0; + + for(unsigned int i = 0; i < fn.size(); ++i) + { + NodeId nid = (int)fn[i]["nodeId"]; + NodeId pid = (int)fn[i]["parentId"]; + WordValue weight = (WordValue)fn[i]["weight"]; + string d = (string)fn[i]["descriptor"]; + + m_nodes[nid].id = nid; + m_nodes[nid].parent = pid; + m_nodes[nid].weight = weight; + m_nodes[pid].children.push_back(nid); + + F::fromString(m_nodes[nid].descriptor, d); + } + + // words + fn = fvoc["words"]; + + m_words.resize(fn.size()); + + for(unsigned int i = 0; i < fn.size(); ++i) + { + NodeId wid = (int)fn[i]["wordId"]; + NodeId nid = (int)fn[i]["nodeId"]; + + m_nodes[nid].word_id = wid; + m_words[wid] = &m_nodes[nid]; + } +} + +// -------------------------------------------------------------------------- + +/** + * Writes printable information of the vocabulary + * @param os stream to write to + * @param voc + */ +template +std::ostream& operator<<(std::ostream &os, + const TemplatedVocabulary &voc) +{ + os << "Vocabulary: k = " << voc.getBranchingFactor() + << ", L = " << voc.getDepthLevels() + << ", Weighting = "; + + switch(voc.getWeightingType()) + { + case TF_IDF: os << "tf-idf"; break; + case TF: os << "tf"; break; + case IDF: os << "idf"; break; + case BINARY: os << "binary"; break; + } + + os << ", Scoring = "; + switch(voc.getScoringType()) + { + case L1_NORM: os << "L1-norm"; break; + case L2_NORM: os << "L2-norm"; break; + case CHI_SQUARE: os << "Chi square distance"; break; + case KL: os << "KL-divergence"; break; + case BHATTACHARYYA: os << "Bhattacharyya coefficient"; break; + case DOT_PRODUCT: os << "Dot product"; break; + } + + os << ", Number of words = " << voc.size(); + + return os; +} + +} // namespace DBoW2 + +#endif diff --git a/Thirdparty/DBoW2/DUtils/Random.h b/Thirdparty/DBoW2/DUtils/Random.h index 5115dc47c2..974ad5f36d 100644 --- a/Thirdparty/DBoW2/DUtils/Random.h +++ b/Thirdparty/DBoW2/DUtils/Random.h @@ -1,184 +1,184 @@ -/* - * File: Random.h - * Project: DUtils library - * Author: Dorian Galvez-Lopez - * Date: April 2010, November 2011 - * Description: manages pseudo-random numbers - * License: see the LICENSE.txt file - * - */ - -#pragma once -#ifndef __D_RANDOM__ -#define __D_RANDOM__ - -#include -#include - -namespace DUtils { - -/// Functions to generate pseudo-random numbers -class Random -{ -public: - class UnrepeatedRandomizer; - -public: - /** - * Sets the random number seed to the current time - */ - static void SeedRand(); - - /** - * Sets the random number seed to the current time only the first - * time this function is called - */ - static void SeedRandOnce(); - - /** - * Sets the given random number seed - * @param seed - */ - static void SeedRand(int seed); - - /** - * Sets the given random number seed only the first time this function - * is called - * @param seed - */ - static void SeedRandOnce(int seed); - - /** - * Returns a random number in the range [0..1] - * @return random T number in [0..1] - */ - template - static T RandomValue(){ - return (T)rand()/(T)RAND_MAX; - } - - /** - * Returns a random number in the range [min..max] - * @param min - * @param max - * @return random T number in [min..max] - */ - template - static T RandomValue(T min, T max){ - return Random::RandomValue() * (max - min) + min; - } - - /** - * Returns a random int in the range [min..max] - * @param min - * @param max - * @return random int in [min..max] - */ - static int RandomInt(int min, int max); - - /** - * Returns a random number from a gaussian distribution - * @param mean - * @param sigma standard deviation - */ - template - static T RandomGaussianValue(T mean, T sigma) - { - // Box-Muller transformation - T x1, x2, w, y1; - - do { - x1 = (T)2. * RandomValue() - (T)1.; - x2 = (T)2. * RandomValue() - (T)1.; - w = x1 * x1 + x2 * x2; - } while ( w >= (T)1. || w == (T)0. ); - - w = sqrt( ((T)-2.0 * log( w ) ) / w ); - y1 = x1 * w; - - return( mean + y1 * sigma ); - } - -private: - - /// If SeedRandOnce() or SeedRandOnce(int) have already been called - static bool m_already_seeded; - -}; - -// --------------------------------------------------------------------------- - -/// Provides pseudo-random numbers with no repetitions -class Random::UnrepeatedRandomizer -{ -public: - - /** - * Creates a randomizer that returns numbers in the range [min, max] - * @param min - * @param max - */ - UnrepeatedRandomizer(int min, int max); - ~UnrepeatedRandomizer(){} - - /** - * Copies a randomizer - * @param rnd - */ - UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); - - /** - * Copies a randomizer - * @param rnd - */ - UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); - - /** - * Returns a random number not given before. If all the possible values - * were already given, the process starts again - * @return unrepeated random number - */ - int get(); - - /** - * Returns whether all the possible values between min and max were - * already given. If get() is called when empty() is true, the behaviour - * is the same than after creating the randomizer - * @return true iff all the values were returned - */ - inline bool empty() const { return m_values.empty(); } - - /** - * Returns the number of values still to be returned - * @return amount of values to return - */ - inline unsigned int left() const { return m_values.size(); } - - /** - * Resets the randomizer as it were just created - */ - void reset(); - -protected: - - /** - * Creates the vector with available values - */ - void createValues(); - -protected: - - /// Min of range of values - int m_min; - /// Max of range of values - int m_max; - - /// Available values - std::vector m_values; - -}; - -} - -#endif - +/* + * File: Random.h + * Project: DUtils library + * Author: Dorian Galvez-Lopez + * Date: April 2010, November 2011 + * Description: manages pseudo-random numbers + * License: see the LICENSE.txt file + * + */ + +#pragma once +#ifndef __D_RANDOM__ +#define __D_RANDOM__ + +#include +#include + +namespace DUtils { + +/// Functions to generate pseudo-random numbers +class Random +{ +public: + class UnrepeatedRandomizer; + +public: + /** + * Sets the random number seed to the current time + */ + static void SeedRand(); + + /** + * Sets the random number seed to the current time only the first + * time this function is called + */ + static void SeedRandOnce(); + + /** + * Sets the given random number seed + * @param seed + */ + static void SeedRand(int seed); + + /** + * Sets the given random number seed only the first time this function + * is called + * @param seed + */ + static void SeedRandOnce(int seed); + + /** + * Returns a random number in the range [0..1] + * @return random T number in [0..1] + */ + template + static T RandomValue(){ + return (T)rand()/(T)RAND_MAX; + } + + /** + * Returns a random number in the range [min..max] + * @param min + * @param max + * @return random T number in [min..max] + */ + template + static T RandomValue(T min, T max){ + return Random::RandomValue() * (max - min) + min; + } + + /** + * Returns a random int in the range [min..max] + * @param min + * @param max + * @return random int in [min..max] + */ + static int RandomInt(int min, int max); + + /** + * Returns a random number from a gaussian distribution + * @param mean + * @param sigma standard deviation + */ + template + static T RandomGaussianValue(T mean, T sigma) + { + // Box-Muller transformation + T x1, x2, w, y1; + + do { + x1 = (T)2. * RandomValue() - (T)1.; + x2 = (T)2. * RandomValue() - (T)1.; + w = x1 * x1 + x2 * x2; + } while ( w >= (T)1. || w == (T)0. ); + + w = sqrt( ((T)-2.0 * log( w ) ) / w ); + y1 = x1 * w; + + return( mean + y1 * sigma ); + } + +private: + + /// If SeedRandOnce() or SeedRandOnce(int) have already been called + static bool m_already_seeded; + +}; + +// --------------------------------------------------------------------------- + +/// Provides pseudo-random numbers with no repetitions +class Random::UnrepeatedRandomizer +{ +public: + + /** + * Creates a randomizer that returns numbers in the range [min, max] + * @param min + * @param max + */ + UnrepeatedRandomizer(int min, int max); + ~UnrepeatedRandomizer(){} + + /** + * Copies a randomizer + * @param rnd + */ + UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); + + /** + * Copies a randomizer + * @param rnd + */ + UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); + + /** + * Returns a random number not given before. If all the possible values + * were already given, the process starts again + * @return unrepeated random number + */ + int get(); + + /** + * Returns whether all the possible values between min and max were + * already given. If get() is called when empty() is true, the behaviour + * is the same than after creating the randomizer + * @return true iff all the values were returned + */ + inline bool empty() const { return m_values.empty(); } + + /** + * Returns the number of values still to be returned + * @return amount of values to return + */ + inline unsigned int left() const { return m_values.size(); } + + /** + * Resets the randomizer as it were just created + */ + void reset(); + +protected: + + /** + * Creates the vector with available values + */ + void createValues(); + +protected: + + /// Min of range of values + int m_min; + /// Max of range of values + int m_max; + + /// Available values + std::vector m_values; + +}; + +} + +#endif + diff --git a/Thirdparty/DBoW2/DUtils/Timestamp.h b/Thirdparty/DBoW2/DUtils/Timestamp.h index b92f89f872..66531187ea 100644 --- a/Thirdparty/DBoW2/DUtils/Timestamp.h +++ b/Thirdparty/DBoW2/DUtils/Timestamp.h @@ -1,204 +1,204 @@ -/* - * File: Timestamp.h - * Author: Dorian Galvez-Lopez - * Date: March 2009 - * Description: timestamping functions - * License: see the LICENSE.txt file - * - */ - -#ifndef __D_TIMESTAMP__ -#define __D_TIMESTAMP__ - -#include -using namespace std; - -namespace DUtils { - -/// Timestamp -class Timestamp -{ -public: - - /// Options to initiate a timestamp - enum tOptions - { - NONE = 0, - CURRENT_TIME = 0x1, - ZERO = 0x2 - }; - -public: - - /** - * Creates a timestamp - * @param option option to set the initial time stamp - */ - Timestamp(Timestamp::tOptions option = NONE); - - /** - * Destructor - */ - virtual ~Timestamp(void); - - /** - * Says if the timestamp is "empty": seconds and usecs are both 0, as - * when initiated with the ZERO flag - * @return true iif secs == usecs == 0 - */ - bool empty() const; - - /** - * Sets this instance to the current time - */ - void setToCurrentTime(); - - /** - * Sets the timestamp from seconds and microseconds - * @param secs: seconds - * @param usecs: microseconds - */ - inline void setTime(unsigned long secs, unsigned long usecs){ - m_secs = secs; - m_usecs = usecs; - } - - /** - * Returns the timestamp in seconds and microseconds - * @param secs seconds - * @param usecs microseconds - */ - inline void getTime(unsigned long &secs, unsigned long &usecs) const - { - secs = m_secs; - usecs = m_usecs; - } - - /** - * Sets the timestamp from a string with the time in seconds - * @param stime: string such as "1235603336.036609" - */ - void setTime(const string &stime); - - /** - * Sets the timestamp from a number of seconds from the epoch - * @param s seconds from the epoch - */ - void setTime(double s); - - /** - * Returns this timestamp as the number of seconds in (long) float format - */ - double getFloatTime() const; - - /** - * Returns this timestamp as the number of seconds in fixed length string format - */ - string getStringTime() const; - - /** - * Returns the difference in seconds between this timestamp (greater) and t (smaller) - * If the order is swapped, a negative number is returned - * @param t: timestamp to subtract from this timestamp - * @return difference in seconds - */ - double operator- (const Timestamp &t) const; - - /** - * Returns a copy of this timestamp + s seconds + us microseconds - * @param s seconds - * @param us microseconds - */ - Timestamp plus(unsigned long s, unsigned long us) const; - - /** - * Returns a copy of this timestamp - s seconds - us microseconds - * @param s seconds - * @param us microseconds - */ - Timestamp minus(unsigned long s, unsigned long us) const; - - /** - * Adds s seconds to this timestamp and returns a reference to itself - * @param s seconds - * @return reference to this timestamp - */ - Timestamp& operator+= (double s); - - /** - * Substracts s seconds to this timestamp and returns a reference to itself - * @param s seconds - * @return reference to this timestamp - */ - Timestamp& operator-= (double s); - - /** - * Returns a copy of this timestamp + s seconds - * @param s: seconds - */ - Timestamp operator+ (double s) const; - - /** - * Returns a copy of this timestamp - s seconds - * @param s: seconds - */ - Timestamp operator- (double s) const; - - /** - * Returns whether this timestamp is at the future of t - * @param t - */ - bool operator> (const Timestamp &t) const; - - /** - * Returns whether this timestamp is at the future of (or is the same as) t - * @param t - */ - bool operator>= (const Timestamp &t) const; - - /** - * Returns whether this timestamp and t represent the same instant - * @param t - */ - bool operator== (const Timestamp &t) const; - - /** - * Returns whether this timestamp is at the past of t - * @param t - */ - bool operator< (const Timestamp &t) const; - - /** - * Returns whether this timestamp is at the past of (or is the same as) t - * @param t - */ - bool operator<= (const Timestamp &t) const; - - /** - * Returns the timestamp in a human-readable string - * @param machine_friendly if true, the returned string is formatted - * to yyyymmdd_hhmmss, without weekday or spaces - * @note This has not been tested under Windows - * @note The timestamp is truncated to seconds - */ - string Format(bool machine_friendly = false) const; - - /** - * Returns a string version of the elapsed time in seconds, with the format - * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us - * @param s: elapsed seconds (given by getFloatTime) to format - */ - static string Format(double s); - - -protected: - /// Seconds - unsigned long m_secs; // seconds - /// Microseconds - unsigned long m_usecs; // microseconds -}; - -} - -#endif - +/* + * File: Timestamp.h + * Author: Dorian Galvez-Lopez + * Date: March 2009 + * Description: timestamping functions + * License: see the LICENSE.txt file + * + */ + +#ifndef __D_TIMESTAMP__ +#define __D_TIMESTAMP__ + +#include +using namespace std; + +namespace DUtils { + +/// Timestamp +class Timestamp +{ +public: + + /// Options to initiate a timestamp + enum tOptions + { + NONE = 0, + CURRENT_TIME = 0x1, + ZERO = 0x2 + }; + +public: + + /** + * Creates a timestamp + * @param option option to set the initial time stamp + */ + Timestamp(Timestamp::tOptions option = NONE); + + /** + * Destructor + */ + virtual ~Timestamp(void); + + /** + * Says if the timestamp is "empty": seconds and usecs are both 0, as + * when initiated with the ZERO flag + * @return true iif secs == usecs == 0 + */ + bool empty() const; + + /** + * Sets this instance to the current time + */ + void setToCurrentTime(); + + /** + * Sets the timestamp from seconds and microseconds + * @param secs: seconds + * @param usecs: microseconds + */ + inline void setTime(unsigned long secs, unsigned long usecs){ + m_secs = secs; + m_usecs = usecs; + } + + /** + * Returns the timestamp in seconds and microseconds + * @param secs seconds + * @param usecs microseconds + */ + inline void getTime(unsigned long &secs, unsigned long &usecs) const + { + secs = m_secs; + usecs = m_usecs; + } + + /** + * Sets the timestamp from a string with the time in seconds + * @param stime: string such as "1235603336.036609" + */ + void setTime(const string &stime); + + /** + * Sets the timestamp from a number of seconds from the epoch + * @param s seconds from the epoch + */ + void setTime(double s); + + /** + * Returns this timestamp as the number of seconds in (long) float format + */ + double getFloatTime() const; + + /** + * Returns this timestamp as the number of seconds in fixed length string format + */ + string getStringTime() const; + + /** + * Returns the difference in seconds between this timestamp (greater) and t (smaller) + * If the order is swapped, a negative number is returned + * @param t: timestamp to subtract from this timestamp + * @return difference in seconds + */ + double operator- (const Timestamp &t) const; + + /** + * Returns a copy of this timestamp + s seconds + us microseconds + * @param s seconds + * @param us microseconds + */ + Timestamp plus(unsigned long s, unsigned long us) const; + + /** + * Returns a copy of this timestamp - s seconds - us microseconds + * @param s seconds + * @param us microseconds + */ + Timestamp minus(unsigned long s, unsigned long us) const; + + /** + * Adds s seconds to this timestamp and returns a reference to itself + * @param s seconds + * @return reference to this timestamp + */ + Timestamp& operator+= (double s); + + /** + * Substracts s seconds to this timestamp and returns a reference to itself + * @param s seconds + * @return reference to this timestamp + */ + Timestamp& operator-= (double s); + + /** + * Returns a copy of this timestamp + s seconds + * @param s: seconds + */ + Timestamp operator+ (double s) const; + + /** + * Returns a copy of this timestamp - s seconds + * @param s: seconds + */ + Timestamp operator- (double s) const; + + /** + * Returns whether this timestamp is at the future of t + * @param t + */ + bool operator> (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the future of (or is the same as) t + * @param t + */ + bool operator>= (const Timestamp &t) const; + + /** + * Returns whether this timestamp and t represent the same instant + * @param t + */ + bool operator== (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the past of t + * @param t + */ + bool operator< (const Timestamp &t) const; + + /** + * Returns whether this timestamp is at the past of (or is the same as) t + * @param t + */ + bool operator<= (const Timestamp &t) const; + + /** + * Returns the timestamp in a human-readable string + * @param machine_friendly if true, the returned string is formatted + * to yyyymmdd_hhmmss, without weekday or spaces + * @note This has not been tested under Windows + * @note The timestamp is truncated to seconds + */ + string Format(bool machine_friendly = false) const; + + /** + * Returns a string version of the elapsed time in seconds, with the format + * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us + * @param s: elapsed seconds (given by getFloatTime) to format + */ + static string Format(double s); + + +protected: + /// Seconds + unsigned long m_secs; // seconds + /// Microseconds + unsigned long m_usecs; // microseconds +}; + +} + +#endif + diff --git a/Thirdparty/DBoW2/DUtils/config.h b/Thirdparty/DBoW2/DUtils/config.h new file mode 100644 index 0000000000..352ae5ad50 --- /dev/null +++ b/Thirdparty/DBoW2/DUtils/config.h @@ -0,0 +1,17 @@ +#ifndef __CONFIG_H +#define __CONFIG_H + +#ifdef WIN32 + #ifndef DBoW2_EXPORTS + #define DBoW2_EXPORTS + #endif + #ifdef DBoW2_EXPORTS + #define EXPORT //__declspec(dllexport) + #else + #define EXPORT //__declspec(dllimport) + #endif +#else + #define EXPORT +#endif + +#endif // CONFIG_H diff --git a/Thirdparty/DBoW2/README.txt b/Thirdparty/DBoW2/README.txt index 71827f06cf..64761beea4 100644 --- a/Thirdparty/DBoW2/README.txt +++ b/Thirdparty/DBoW2/README.txt @@ -1,4 +1,4 @@ -You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). +You should have received this DBoW2 version along with ORB-SLAM (https://github.com/raulmur/ORB_SLAM). See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 All files included in this version are BSD, see LICENSE.txt diff --git a/include/Frame.h b/include/Frame.h index a6a8032f57..25481d16cb 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -23,12 +23,11 @@ #include -#include "MapPoint.h" #include "Thirdparty/DBoW2/DBoW2/BowVector.h" #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" #include "ORBVocabulary.h" -#include "KeyFrame.h" #include "ORBextractor.h" +#include "Se2.h" #include @@ -57,6 +56,9 @@ class Frame // Constructor for Monocular cameras. Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth); + // Constructor for Odometry-Monocular + Frame(const cv::Mat &imGray, const Se2 &odo, const double timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, cv::Mat &extParaBc, const float &bf, const float &thDepth); + // Extract ORB on the image. 0 for left image and 1 for right image. void ExtractORB(int flag, const cv::Mat &im); @@ -99,6 +101,13 @@ class Frame cv::Mat UnprojectStereo(const int &i); public: + // Odometry measurement + Se2 odom; + + // Extrinsic parameter + cv::Mat Tbc; + cv::Mat Tcb; + // Vocabulary used for relocalization. ORBVocabulary* mpORBvocabulary; diff --git a/include/Initializer.h b/include/Initializer.h index 09b27a65bb..2f474a5fab 100644 --- a/include/Initializer.h +++ b/include/Initializer.h @@ -43,6 +43,16 @@ class Initializer cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated); + // LETS BEGIN HERE + bool InitializeWithOdom(const Frame &CurrentFrame, const vector &vMatches12, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated); + + Se2 odom1; + Se2 odom2; + cv::Mat Tbc; + // END + + private: void FindHomography(vector &vbMatchesInliers, float &score, cv::Mat &H21); @@ -61,6 +71,11 @@ class Initializer bool ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); + // LETS BEGIN HERE + bool ReconstructWithOdom(vector &vbMatchesInliers, cv::Mat &K, + cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated); + // END + void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D); void Normalize(const vector &vKeys, vector &vNormalizedPoints, cv::Mat &T); diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 67f4348273..4a122b34eb 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -120,6 +120,10 @@ class KeyFrame // The following variables are accesed from only 1 thread or never change (no mutex needed). public: + Se2 odom; + cv::Mat Tbc; + + static long unsigned int nNextId; long unsigned int mnId; const long unsigned int mnFrameId; @@ -189,6 +193,12 @@ class KeyFrame const cv::Mat mK; + // LETS BEGIN HERE + KeyFrame* mpLastKF; + void SetPoseByOdomTo(KeyFrame* refKF); + // END + + // The following variables need to be accessed trough a mutex to be thread safe. protected: @@ -231,6 +241,8 @@ class KeyFrame std::mutex mMutexPose; std::mutex mMutexConnections; std::mutex mMutexFeatures; + + }; } //namespace ORB_SLAM diff --git a/include/Map.h b/include/Map.h index a75339feea..88986abceb 100644 --- a/include/Map.h +++ b/include/Map.h @@ -26,8 +26,8 @@ #include #include - - +#include +#include namespace ORB_SLAM2 { @@ -59,16 +59,33 @@ class Map void clear(); - vector mvpKeyFrameOrigins; + std::vector mvpKeyFrameOrigins; std::mutex mMutexMapUpdate; // This avoid that two points are created simultaneously in separate threads (id conflict) std::mutex mMutexPointCreation; + // LETS BEGIN + std::atomic_int newestChangedKeyFrameId; + std::atomic_int newestChangedEssentialId; + std::vector GetKeyFramesIdNoLesserThan(int minId); + KeyFrame* GetLastestKeyFrameIdLessThan(unsigned long id); + KeyFrame* GetKeyFrameById(int id); + // END + protected: + + // LETS BEGIN HERE + struct KfIdLess + { + bool operator() (const KeyFrame* kf1, const KeyFrame* kf2) const; + + }; + // END + std::set mspMapPoints; - std::set mspKeyFrames; + std::set mspKeyFrames; std::vector mvpReferenceMapPoints; diff --git a/include/MapDrawer.h b/include/MapDrawer.h index 83b282676d..8323b44fa0 100644 --- a/include/MapDrawer.h +++ b/include/MapDrawer.h @@ -53,6 +53,7 @@ class MapDrawer float mPointSize; float mCameraSize; float mCameraLineWidth; + float mRatio; cv::Mat mCameraPose; diff --git a/include/Se2.h b/include/Se2.h new file mode 100755 index 0000000000..d1339a3055 --- /dev/null +++ b/include/Se2.h @@ -0,0 +1,44 @@ +#ifndef UTIL_SE2_H +#define UTIL_SE2_H +#include + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + + +class Se2{ +public: + float x; + float y; + float theta; + Se2(); + Se2(float _x, float _y ,float _theta); + ~Se2(); + Se2 inv() const; + Se2 operator -(const Se2& that) const; + Se2 operator +(const Se2& that) const; + cv::Mat toCvSE3() const; + Se2& fromCvSE3(const cv::Mat& mat); +}; + + +inline double normalize_angle(double theta) +{ + if (theta >= -M_PI && theta < M_PI) + return theta; + + double multiplier = floor(theta / (2*M_PI)); + theta = theta - multiplier*2*M_PI; + if (theta >= M_PI) + theta -= 2*M_PI; + if (theta < -M_PI) + theta += 2*M_PI; + + return theta; +} + + + +#endif // SE2_H diff --git a/include/System.h b/include/System.h index b377b453d1..12e870d588 100644 --- a/include/System.h +++ b/include/System.h @@ -53,7 +53,7 @@ class System enum eSensor{ MONOCULAR=0, STEREO=1, - RGBD=2 + RGBD=2, }; public: @@ -61,22 +61,14 @@ class System // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true); - // Proccess the given stereo frame. Images must be synchronized and rectified. - // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. - // Returns the camera pose (empty if tracking fails). - cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp); - - // Process the given rgbd frame. Depthmap must be registered to the RGB frame. - // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. - // Input depthmap: Float (CV_32F). - // Returns the camera pose (empty if tracking fails). - cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp); - // Proccess the given monocular frame // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp); + + cv::Mat TrackOdomMono(const cv::Mat &im, const Se2 &odo, const double timestamp); + // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); // This resumes local mapping thread and performs SLAM again. @@ -106,6 +98,10 @@ class System // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset void SaveKeyFrameTrajectoryTUM(const string &filename); + // LETS BEGIN HERE + void SaveKeyFrameTrajectoryVN(const string &filename); + // END + // Save camera trajectory in the KITTI dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() diff --git a/include/Tracking.h b/include/Tracking.h index 5aaa93ef26..a0d816b327 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -58,10 +58,10 @@ class Tracking KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); // Preprocess the input and call Track(). Extract features and performs stereo matching. - cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); - cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp); + cv::Mat GrabImageOdomMono(const cv::Mat &im, const Se2 &odo, const double timestamp); + void SetLocalMapper(LocalMapping* pLocalMapper); void SetLoopClosing(LoopClosing* pLoopClosing); void SetViewer(Viewer* pViewer); @@ -120,9 +120,6 @@ class Tracking // Main tracking function. It is independent of the input sensor. void Track(); - // Map initialization for stereo and RGB-D - void StereoInitialization(); - // Map initialization for monocular void MonocularInitialization(); void CreateInitialMapMonocular(); @@ -132,6 +129,12 @@ class Tracking void UpdateLastFrame(); bool TrackWithMotionModel(); + // LETS BEGIN HERE + bool TrackWithMotionModelOdom(); + bool TrackReferenceKeyFrameOdom(); + int lastNewestChangedKeyFrameId = 0; + // END + bool Relocalization(); void UpdateLocalMap(); @@ -181,6 +184,10 @@ class Tracking //Map Map* mpMap; + // Camera Extrinsics + cv::Mat mTbc; + cv::Mat mTcb; + //Calibration matrix cv::Mat mK; cv::Mat mDistCoef; @@ -210,6 +217,11 @@ class Tracking //Motion Model cv::Mat mVelocity; + // LETS BEGIN HERE + cv::Mat mVelocityOdom; + + // END + //Color order (true RGB, false BGR, ignored if grayscale) bool mbRGB; diff --git a/src/Frame.cc b/src/Frame.cc index 0e37d49335..108e488414 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -49,6 +49,9 @@ Frame::Frame(const Frame &frame) mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2) { + frame.Tbc.copyTo(Tbc); + odom = frame.odom; + for(int i=0;i(NULL)), + mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) +{ + + odom = odo; + extParaBc.copyTo(Tbc); + Tcb = Tbc.clone(); + Tcb.rowRange(0,3).colRange(0,3) = Tbc.rowRange(0,3).colRange(0,3).t(); + Tcb.rowRange(0,3).col(3) = -Tbc.rowRange(0,3).colRange(0,3).t()*Tbc.rowRange(0,3).col(3); + + // Frame ID + mnId=nNextId++; + + // Scale Level Info + mnScaleLevels = mpORBextractorLeft->GetLevels(); + mfScaleFactor = mpORBextractorLeft->GetScaleFactor(); + mfLogScaleFactor = log(mfScaleFactor); + mvScaleFactors = mpORBextractorLeft->GetScaleFactors(); + mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors(); + mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares(); + mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares(); + + // ORB extraction + ExtractORB(0,imGray); + + N = mvKeys.size(); + + if(mvKeys.empty()) + return; + + UndistortKeyPoints(); + + // Set no stereo information + mvuRight = vector(N,-1); + mvDepth = vector(N,-1); + + mvpMapPoints = vector(N,static_cast(NULL)); + mvbOutlier = vector(N,false); + + // This is done only for the first Frame (or after a change in the calibration) + if(mbInitialComputations) + { + ComputeImageBounds(imGray); + + mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX); + mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY); + + fx = K.at(0,0); + fy = K.at(1,1); + cx = K.at(0,2); + cy = K.at(1,2); + invfx = 1.0f/fx; + invfy = 1.0f/fy; + + mbInitialComputations=false; + } + + mb = mbf/fx; + + AssignFeaturesToGrid(); +} + void Frame::AssignFeaturesToGrid() { int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS); diff --git a/src/Initializer.cc b/src/Initializer.cc index 6094b8f51b..7fbfbc825b 100644 --- a/src/Initializer.cc +++ b/src/Initializer.cc @@ -39,8 +39,98 @@ Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iteration mSigma = sigma; mSigma2 = sigma*sigma; mMaxIterations = iterations; + + odom1 = ReferenceFrame.odom; +} + +bool Initializer::InitializeWithOdom(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21, + vector &vP3D, vector &vbTriangulated) +{ + // Fill structures with current keypoints and matches with reference frame + // Reference Frame: 1, Current Frame: 2 + mvKeys2 = CurrentFrame.mvKeysUn; + odom2 = CurrentFrame.odom; + CurrentFrame.Tbc.copyTo(Tbc); + + mvMatches12.clear(); + mvMatches12.reserve(mvKeys2.size()); + mvbMatched1.resize(mvKeys1.size()); + for(size_t i=0, iend=vMatches12.size();i=0) + { + mvMatches12.push_back(make_pair(i,vMatches12[i])); + mvbMatched1[i]=true; + } + else + mvbMatched1[i]=false; + } + + const int N = mvMatches12.size(); + + // Indices for minimum set selection + vector vAllIndices; + vAllIndices.reserve(N); + vector vAvailableIndices; + + for(int i=0; i >(mMaxIterations,vector(8,0)); + + DUtils::Random::SeedRandOnce(0); + + for(int it=0; it vbMatchesInliersH, vbMatchesInliersF; + float SH, SF; + cv::Mat H, F; + + thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H)); + thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F)); + + // Wait until both threads have finished + threadH.join(); + threadF.join(); + + // Compute ratio of scores + float RH = SH/(SH+SF); + + // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45) + if(RH>0.40) + { + //return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); + return ReconstructWithOdom(vbMatchesInliersH,mK,R21,t21,vP3D,vbTriangulated,1.0,50); + } + else //if(pF_HF>0.6) + { + //return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); + return ReconstructWithOdom(vbMatchesInliersH,mK,R21,t21,vP3D,vbTriangulated,1.0,50); + } + + return false; } + bool Initializer::Initialize(const Frame &CurrentFrame, const vector &vMatches12, cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated) { @@ -569,6 +659,45 @@ bool Initializer::ReconstructF(vector &vbMatchesInliers, cv::Mat &F21, cv: return false; } +bool Initializer::ReconstructWithOdom(vector &vbMatchesInliers, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, + vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) +{ + int N=0; + for(size_t i=0, iend = vbMatchesInliers.size() ; i vP3D1; + vector vbTriangulated1; + float parallax1; + + int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1); + + R21 = cv::Mat(); + t21 = cv::Mat(); + + int nMinGood = max(static_cast(0.7*N),minTriangulated); + if(parallax1 > minParallax && nGood1 > nMinGood) + { + vP3D = vP3D1; + vbTriangulated = vbTriangulated1; + R1.copyTo(R21); + t1.copyTo(t21); + return true; + } else + { + return false; + } +} + bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated) { @@ -731,6 +860,7 @@ bool Initializer::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv: return false; } + void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) { cv::Mat A(4,4,CV_32F); diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index 4ef1e78e0f..4930b196fa 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -43,6 +43,10 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap) { + odom = F.odom; + F.Tbc.copyTo(Tbc); + + mnId=nNextId++; mGrid.resize(mnGridCols); @@ -83,6 +87,15 @@ void KeyFrame::SetPose(const cv::Mat &Tcw_) Cw = Twc*center; } +// LETS BEGIN HERE +void KeyFrame::SetPoseByOdomTo(KeyFrame *refKF) +{ + Se2 dOdom = refKF->odom - odom; + cv::Mat dCvOdom = Tbc.inv() * dOdom.toCvSE3() * Tbc; + SetPose(dCvOdom * refKF->GetPose()); +} +// END + cv::Mat KeyFrame::GetPose() { unique_lock lock(mMutexPose); @@ -321,7 +334,22 @@ void KeyFrame::UpdateConnections() // This should not happen if(KFcounter.empty()) - return; + { + // LETS BEGIN HERE + //return; + // END + } + + // LETS BEGIN HERE + + mpLastKF = mpMap->GetLastestKeyFrameIdLessThan(mnId); + + if(mnId != 0 && !KFcounter.count(mpLastKF)) + { + KFcounter[mpLastKF] = 0; + } + // END + //If the counter is greater than threshold add connection //In case no keyframe counter is over threshold add the one with maximum counter @@ -338,18 +366,28 @@ void KeyFrame::UpdateConnections() nmax=mit->second; pKFmax=mit->first; } - if(mit->second>=th) + // LETS BEGIN HERE + if(mit->second>=th || mit->first->mnId == mpLastKF->mnId) { vPairs.push_back(make_pair(mit->second,mit->first)); (mit->first)->AddConnection(this,mit->second); } + // END } + // LETS BEGIN HERE + // This should not happen. if(vPairs.empty()) { - vPairs.push_back(make_pair(nmax,pKFmax)); - pKFmax->AddConnection(this,nmax); + // LETS BEGIN HERE + if(pKFmax) + { + vPairs.push_back(make_pair(nmax,pKFmax)); + pKFmax->AddConnection(this,nmax); + } + // END } + // END sort(vPairs.begin(),vPairs.end()); list lKFs; @@ -464,7 +502,20 @@ void KeyFrame::SetBadFlag() } for(map::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) - mit->first->EraseConnection(this); + { + // LETS BEGIN HERE + KeyFrame* other = mit->first; + other->EraseConnection(this); + if(other->mnId == 0) + continue; + if(other->mpLastKF->mnId == mnId) + { + other->mpLastKF = mpLastKF; + other->AddConnection(mpLastKF, other->GetWeight(mpLastKF)); + mpLastKF->AddConnection(other, mpLastKF->GetWeight(other)); + } + // END + } for(size_t i=0; iAddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpMatchedKF); + +// // LETS BEGIN HERE +// std::vector vKfsToAdjust = mpMap->GetKeyFramesIdNoLesserThan(mpMap->newestChangedEssentialId); +// for(int i = 1, iend = vKfsToAdjust.size(); i < iend; i++) +// { +// KeyFrame* kfi = vKfsToAdjust[i-1]; +// KeyFrame* kfj = vKfsToAdjust[i]; +// kfj->SetPoseByOdomTo(kfi); +// } +// if(!std::count(vKfsToAdjust.begin(), vKfsToAdjust.end(), mpCurrentKF)) +// { +// mpCurrentKF->SetPoseByOdomTo(vKfsToAdjust.back()); +// } +// // END + // Launch a new thread to perform Global Bundle Adjustment mbRunningGBA = true; mbFinishedGBA = false; @@ -676,6 +691,7 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) // Correct keyframes starting at map first keyframe list lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end()); + int largestChangedId = 0; while(!lpKFtoCheck.empty()) { KeyFrame* pKF = lpKFtoCheck.front(); @@ -697,7 +713,10 @@ void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) pKF->mTcwBefGBA = pKF->GetPose(); pKF->SetPose(pKF->mTcwGBA); lpKFtoCheck.pop_front(); + if((int)(pKF->mnId) > largestChangedId) + largestChangedId = pKF->mnId; } + mpMap->newestChangedKeyFrameId = largestChangedId; // Correct MapPoints const vector vpMPs = mpMap->GetAllMapPoints(); diff --git a/src/Map.cc b/src/Map.cc index 15fcd86914..5aa7225b1e 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -27,6 +27,10 @@ namespace ORB_SLAM2 Map::Map():mnMaxKFid(0),mnBigChangeIdx(0) { + // LETS BEGIN + newestChangedKeyFrameId = 0; + newestChangedEssentialId = 0; + // END } void Map::AddKeyFrame(KeyFrame *pKF) @@ -130,4 +134,59 @@ void Map::clear() mvpKeyFrameOrigins.clear(); } +// LETS BEGIN HERE +std::vector Map::GetKeyFramesIdNoLesserThan(int minId) +{ + unique_lock lock(mMutexMap); + std::vector vKfs; + for(auto crit = mspKeyFrames.crbegin(); crit != mspKeyFrames.crend(); crit++) + { + KeyFrame* kf = *crit; + if((int)(kf->mnId) >= minId) + { + vKfs.push_back(kf); + } else + { + break; + } + } + std::sort(vKfs.begin(), vKfs.end(), KeyFrame::lId); + return vKfs; +} + +KeyFrame* Map::GetKeyFrameById(int id) +{ + unique_lock lock(mMutexMap); + for(auto crit = mspKeyFrames.crbegin(); crit != mspKeyFrames.crend(); crit++) + { + KeyFrame* kf = *crit; + if((int)(kf->mnId) == id) + { + return kf; + } + } + return static_cast(nullptr); +} + +KeyFrame* Map::GetLastestKeyFrameIdLessThan(long unsigned int id) +{ + unique_lock lock(mMutexMap); + for(auto crit = mspKeyFrames.crbegin(); crit != mspKeyFrames.crend(); crit++) + { + KeyFrame* kf = *crit; + if(kf->mnId < id) + { + return kf; + } + } + return static_cast(nullptr); +} + +bool Map::KfIdLess::operator ()(const KeyFrame* kf1, const KeyFrame* kf2) const +{ + return kf1->mnId < kf2->mnId; +} + +// END + } //namespace ORB_SLAM diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc index 4d9990bc8a..86265160af 100644 --- a/src/MapDrawer.cc +++ b/src/MapDrawer.cc @@ -38,6 +38,7 @@ MapDrawer::MapDrawer(Map* pMap, const string &strSettingPath):mpMap(pMap) mPointSize = fSettings["Viewer.PointSize"]; mCameraSize = fSettings["Viewer.CameraSize"]; mCameraLineWidth = fSettings["Viewer.CameraLineWidth"]; + mRatio = fSettings["Visual.Ratio"]; } @@ -60,6 +61,9 @@ void MapDrawer::DrawMapPoints() if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i])) continue; cv::Mat pos = vpMPs[i]->GetWorldPos(); + // LETS BEGIN HERE + pos *= mRatio; + // glVertex3f(pos.at(0),pos.at(1),pos.at(2)); } glEnd(); @@ -93,7 +97,13 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) for(size_t i=0; iGetPoseInverse().t(); + + // LETS START HERE + cv::Mat Twc_ = pKF->GetPoseInverse(); + Twc_.rowRange(0,3).col(3) *= mRatio; + //cv::Mat Twc = pKF->GetPoseInverse().t(); + cv::Mat Twc = Twc_.t(); + // END glPushMatrix(); @@ -139,6 +149,11 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) // Covisibility Graph const vector vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100); cv::Mat Ow = vpKFs[i]->GetCameraCenter(); + + // LETS BEGIN HERE + Ow *= mRatio; + // END + if(!vCovKFs.empty()) { for(vector::const_iterator vit=vCovKFs.begin(), vend=vCovKFs.end(); vit!=vend; vit++) @@ -146,6 +161,9 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) if((*vit)->mnIdmnId) continue; cv::Mat Ow2 = (*vit)->GetCameraCenter(); + // LETS BEGIN HERE + Ow2 *= mRatio; + // END glVertex3f(Ow.at(0),Ow.at(1),Ow.at(2)); glVertex3f(Ow2.at(0),Ow2.at(1),Ow2.at(2)); } @@ -156,6 +174,9 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) if(pParent) { cv::Mat Owp = pParent->GetCameraCenter(); + // LETS BEGIN HERE + Owp *= mRatio; + // END glVertex3f(Ow.at(0),Ow.at(1),Ow.at(2)); glVertex3f(Owp.at(0),Owp.at(1),Owp.at(2)); } @@ -167,6 +188,9 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph) if((*sit)->mnIdmnId) continue; cv::Mat Owl = (*sit)->GetCameraCenter(); + // LETS BEGIN HERE + Owl *= mRatio; + // END glVertex3f(Ow.at(0),Ow.at(1),Ow.at(2)); glVertex3f(Owl.at(0),Owl.at(1),Owl.at(2)); } @@ -222,7 +246,12 @@ void MapDrawer::DrawCurrentCamera(pangolin::OpenGlMatrix &Twc) void MapDrawer::SetCurrentCameraPose(const cv::Mat &Tcw) { unique_lock lock(mMutexCamera); - mCameraPose = Tcw.clone(); + // LETS BEGIN HERE + cv::Mat Twc = Tcw.inv(); + Twc.rowRange(0,3).col(3) *= mRatio; + mCameraPose = Twc.inv(); + // END + } void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M) diff --git a/src/Optimizer.cc b/src/Optimizer.cc index 83d9264546..4a46d419ad 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -989,6 +989,9 @@ void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* p unique_lock lock(pMap->mMutexMapUpdate); // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1] + // LETS BEGIN HERE + int largestChangedKfId = 0; + // END for(size_t i=0;iSetPose(Tiw); + // LETS BEGIN HERE + if(pKFi->mnId > largestChangedKfId) + { + largestChangedKfId = pKFi->mnId; + } + // END } + // LETS BEGIN HERE + pMap->newestChangedEssentialId = largestChangedKfId; + // END // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose for(size_t i=0, iend=vpMPs.size(); i + +Se2::Se2(){} +Se2::Se2(float _x, float _y ,float _theta): + x(_x), y(_y), theta(normalize_angle(_theta)){} +Se2::~Se2(){} + +Se2 Se2::inv() const +{ + float c = std::cos(theta); + float s = std::sin(theta); + return Se2(-c*x-s*y, s*x-c*y, -theta); +} + +Se2 Se2::operator +(const Se2& that) const{ + float c = std::cos(theta); + float s = std::sin(theta); + float _x = x + that.x*c - that.y*s; + float _y = y + that.x*s + that.y*c; + float _theta = normalize_angle(theta + that.theta); + return Se2(_x, _y, _theta); +} + +// Same as: that.inv() + *this +Se2 Se2::operator -(const Se2& that) const{ + float dx = x - that.x; + float dy = y - that.y; + float dth = normalize_angle(theta - that.theta); + + float c = std::cos(that.theta); + float s = std::sin(that.theta); + return Se2(c*dx+s*dy, -s*dx+c*dy, dth); +} + +cv::Mat Se2::toCvSE3() const +{ + float c = cos(theta); + float s = sin(theta); + + return (cv::Mat_(4,4) << + c,-s, 0, x, + s, c, 0, y, + 0, 0, 1, 0, + 0, 0, 0, 1); +} + +Se2& Se2::fromCvSE3(const cv::Mat &mat) +{ + float yaw = std::atan2(mat.at(1,0), mat.at(0,0)); + theta = normalize_angle(yaw); + x = mat.at(0,3); + y = mat.at(1,3); + return *this; +} diff --git a/src/System.cc b/src/System.cc index 8df4157095..8ea7ee824e 100644 --- a/src/System.cc +++ b/src/System.cc @@ -62,7 +62,8 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; mpVocabulary = new ORBVocabulary(); - bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + //bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); + bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile); if(!bVocLoad) { cerr << "Wrong path to vocabulary. " << endl; @@ -113,65 +114,14 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS mpLoopCloser->SetLocalMapper(mpLocalMapper); } -cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) +cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) { - if(mSensor!=STEREO) + if(mSensor!=MONOCULAR) { - cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl; + cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl; exit(-1); - } - - // Check mode change - { - unique_lock lock(mMutexMode); - if(mbActivateLocalizationMode) - { - mpLocalMapper->RequestStop(); - - // Wait until Local Mapping has effectively stopped - while(!mpLocalMapper->isStopped()) - { - usleep(1000); - } - - mpTracker->InformOnlyTracking(true); - mbActivateLocalizationMode = false; - } - if(mbDeactivateLocalizationMode) - { - mpTracker->InformOnlyTracking(false); - mpLocalMapper->Release(); - mbDeactivateLocalizationMode = false; - } - } - - // Check reset - { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } } - cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp); - - unique_lock lock2(mMutexState); - mTrackingState = mpTracker->mState; - mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; - mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; - return Tcw; -} - -cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) -{ - if(mSensor!=RGBD) - { - cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl; - exit(-1); - } - // Check mode change { unique_lock lock(mMutexMode); @@ -206,20 +156,21 @@ cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const doub } } - cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp); + cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; + return Tcw; } -cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) +cv::Mat System::TrackOdomMono(const cv::Mat &im, const Se2 &odo, const double timestamp) { if(mSensor!=MONOCULAR) { - cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl; + cerr << "ERROR: you called TrackOdomMono but input sensor was not set to ODOM_Monocular." << endl; exit(-1); } @@ -247,17 +198,7 @@ cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) } } - // Check reset - { - unique_lock lock(mMutexReset); - if(mbReset) - { - mpTracker->Reset(); - mbReset = false; - } - } - - cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp); + cv::Mat Tcw = mpTracker->GrabImageOdomMono(im, odo, timestamp); unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -416,6 +357,48 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) cout << endl << "trajectory saved!" << endl; } +// LETS BEGIN HERE +void System::SaveKeyFrameTrajectoryVN(const string &filename) +{ + cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; + + vector vpKFs = mpMap->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + //cv::Mat Two = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for(size_t i=0; iisBad()) + continue; + //cv::Mat Twb = (pKF->Tbc * pKF->GetPose()).inv(); + cv::Mat Twc = pKF->GetPoseInverse(); + cv::Mat Twb = pKF->Tbc * Twc * pKF->Tbc.inv(); + Se2 se2wb; + se2wb.fromCvSE3(Twb); + float zwb = Twb.at(2,3); + + f << pKF->mnFrameId << " " << setprecision(7) + << se2wb.x << " " << se2wb.y << " " << zwb << " " << se2wb.theta + << endl; + + } + + f.close(); + cout << endl << "trajectory saved!" << endl; +} +// END + + void System::SaveTrajectoryKITTI(const string &filename) { cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; diff --git a/src/Tracking.cc b/src/Tracking.cc index 2273b2ce48..cae7ce61ce 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -76,6 +76,25 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, } DistCoef.copyTo(mDistCoef); + + cv::Mat tbc(3,1,CV_32F); + tbc.at(0) = fSettings["Tbc.tbc.x"]; + tbc.at(1) = fSettings["Tbc.tbc.y"]; + tbc.at(2) = fSettings["Tbc.tbc.z"]; + cv::Mat rbc(3,1,CV_32F); + rbc.at(0) = fSettings["Tbc.rbc.x"]; + rbc.at(1) = fSettings["Tbc.rbc.y"]; + rbc.at(2) = fSettings["Tbc.rbc.z"]; + cv::Mat Rbc(3,3,CV_32F); + cv::Rodrigues(rbc, Rbc); + mTbc = cv::Mat::eye(4,4,CV_32F); + Rbc.copyTo(mTbc.rowRange(0,3).colRange(0,3)); + tbc.copyTo(mTbc.rowRange(0,3).col(3)); + mTcb = mTbc.clone(); + mTcb.rowRange(0,3).colRange(0,3) = Rbc.t(); + mTcb.rowRange(0,3).col(3) = -Rbc.t()*tbc; + + mbf = fSettings["Camera.bf"]; float fps = fSettings["Camera.fps"]; @@ -164,50 +183,9 @@ void Tracking::SetViewer(Viewer *pViewer) } -cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp) -{ - mImGray = imRectLeft; - cv::Mat imGrayRight = imRectRight; - - if(mImGray.channels()==3) - { - if(mbRGB) - { - cvtColor(mImGray,mImGray,CV_RGB2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY); - } - else - { - cvtColor(mImGray,mImGray,CV_BGR2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY); - } - } - else if(mImGray.channels()==4) - { - if(mbRGB) - { - cvtColor(mImGray,mImGray,CV_RGBA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY); - } - else - { - cvtColor(mImGray,mImGray,CV_BGRA2GRAY); - cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY); - } - } - - mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); - - Track(); - - return mCurrentFrame.mTcw.clone(); -} - - -cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp) +cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) { - mImGray = imRGB; - cv::Mat imDepth = imD; + mImGray = im; if(mImGray.channels()==3) { @@ -224,18 +202,17 @@ cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const d cvtColor(mImGray,mImGray,CV_BGRA2GRAY); } - if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F) - imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor); - - mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) + mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + else + mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); Track(); return mCurrentFrame.mTcw.clone(); } - -cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) +cv::Mat Tracking::GrabImageOdomMono(const cv::Mat &im, const Se2 &odo, const double timestamp) { mImGray = im; @@ -255,9 +232,9 @@ cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp) } if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) - mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + mCurrentFrame = Frame(mImGray,odo,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mTbc,mbf,mThDepth); else - mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth); + mCurrentFrame = Frame(mImGray,odo,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mTbc,mbf,mThDepth); Track(); @@ -279,7 +256,9 @@ void Tracking::Track() if(mState==NOT_INITIALIZED) { if(mSensor==System::STEREO || mSensor==System::RGBD) - StereoInitialization(); + { + //StereoInitialization(); + } else MonocularInitialization(); @@ -304,6 +283,8 @@ void Tracking::Track() // Local Mapping might have changed some MapPoints tracked in last frame CheckReplacedInLastFrame(); + // LETS BEGIN HERE + /* if(mVelocity.empty() || mCurrentFrame.mnIdUpdate(this); + if(mState!=OK) + return; + // END } } else { // Localization Mode: Local Mapping is deactivated - if(mState==LOST) { bOK = Relocalization(); @@ -412,13 +403,19 @@ void Tracking::Track() if(bOK) mState = OK; else - mState=LOST; + { + // LETS BEGIN HERE + // We will never lose track with odometry + mState = OK; + //mState=LOST; + // END + } // Update drawer mpFrameDrawer->Update(this); // If tracking were good, check if we insert a keyframe - if(bOK) + if(mState==OK) { // Update motion model if(!mLastFrame.mTcw.empty()) @@ -468,6 +465,7 @@ void Tracking::Track() } } + /* // Reset if the camera get lost soon after initialization if(mState==LOST) { @@ -478,6 +476,8 @@ void Tracking::Track() return; } } + */ + // END if(!mCurrentFrame.mpReferenceKF) mCurrentFrame.mpReferenceKF = mpReferenceKF; @@ -505,61 +505,6 @@ void Tracking::Track() } - -void Tracking::StereoInitialization() -{ - if(mCurrentFrame.N>500) - { - // Set Frame pose to the origin - mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F)); - - // Create KeyFrame - KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); - - // Insert KeyFrame in the map - mpMap->AddKeyFrame(pKFini); - - // Create MapPoints and asscoiate to KeyFrame - for(int i=0; i0) - { - cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); - MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap); - pNewMP->AddObservation(pKFini,i); - pKFini->AddMapPoint(pNewMP,i); - pNewMP->ComputeDistinctiveDescriptors(); - pNewMP->UpdateNormalAndDepth(); - mpMap->AddMapPoint(pNewMP); - - mCurrentFrame.mvpMapPoints[i]=pNewMP; - } - } - - cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl; - - mpLocalMapper->InsertKeyFrame(pKFini); - - mLastFrame = Frame(mCurrentFrame); - mnLastKeyFrameId=mCurrentFrame.mnId; - mpLastKeyFrame = pKFini; - - mvpLocalKeyFrames.push_back(pKFini); - mvpLocalMapPoints=mpMap->GetAllMapPoints(); - mpReferenceKF = pKFini; - mCurrentFrame.mpReferenceKF = pKFini; - - mpMap->SetReferenceMapPoints(mvpLocalMapPoints); - - mpMap->mvpKeyFrameOrigins.push_back(pKFini); - - mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); - - mState=OK; - } -} - void Tracking::MonocularInitialization() { @@ -611,7 +556,7 @@ void Tracking::MonocularInitialization() cv::Mat tcw; // Current Camera Translation vector vbTriangulated; // Triangulated Correspondences (mvIniMatches) - if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) + if(mpInitializer->InitializeWithOdom(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) { for(size_t i=0, iend=mvIniMatches.size(); impLastKF = pKFini; + // END + pKFini->ComputeBoW(); pKFcur->ComputeBoW(); @@ -691,9 +641,9 @@ void Tracking::CreateInitialMapMonocular() if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100) { - cout << "Wrong initialization, reseting..." << endl; - Reset(); - return; + //cout << "Wrong initialization, reseting..." << endl; + //Reset(); + //return; } // Scale initial baseline @@ -770,7 +720,7 @@ bool Tracking::TrackReferenceKeyFrame() return false; mCurrentFrame.mvpMapPoints = vpMapPointMatches; - mCurrentFrame.SetPose(mLastFrame.mTcw); + mCurrentFrame.SetPose(mVelocityOdom * mLastFrame.mTcw); Optimizer::PoseOptimization(&mCurrentFrame); @@ -798,6 +748,53 @@ bool Tracking::TrackReferenceKeyFrame() return nmatchesMap>=10; } + + +bool Tracking::TrackReferenceKeyFrameOdom() +{ + // Compute Bag of Words vector + mCurrentFrame.ComputeBoW(); + + // We perform first an ORB matching with the reference keyframe + // If enough matches are found we setup a PnP solver + ORBmatcher matcher(0.7,true); + vector vpMapPointMatches; + + int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches); + + if(nmatches<15) + return false; + + mCurrentFrame.mvpMapPoints = vpMapPointMatches; + mCurrentFrame.SetPose(mVelocityOdom*mLastFrame.mTcw); + + //Optimizer::PoseOptimization(&mCurrentFrame); + + // Discard outliers + int nmatchesMap = 0; + for(int i =0; i(NULL); + mCurrentFrame.mvbOutlier[i]=false; + pMP->mbTrackInView = false; + pMP->mnLastFrameSeen = mCurrentFrame.mnId; + nmatches--; + } + else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + nmatchesMap++; + } + } + + return nmatchesMap>=10; +} + + void Tracking::UpdateLastFrame() { // Update pose according to reference keyframe @@ -927,6 +924,89 @@ bool Tracking::TrackWithMotionModel() return nmatchesMap>=10; } +bool Tracking::TrackWithMotionModelOdom() +{ + ORBmatcher matcher(0.9,true); + + // LETS BEGIN HERE + int newestChangedKfId = mpMap->newestChangedEssentialId; + if(newestChangedKfId != lastNewestChangedKeyFrameId) + { + lastNewestChangedKeyFrameId = newestChangedKfId; + std::vector vKfs = mpMap->GetKeyFramesIdNoLesserThan(newestChangedKfId); + for(int i = 1, iend = vKfs.size(); i < iend; i++) + { + KeyFrame* kfi = vKfs[i-1]; + KeyFrame* kfj = vKfs[i]; + kfj->SetPoseByOdomTo(kfi); + } + KeyFrame* lastKf = vKfs[vKfs.size()-1]; + Se2 lastOdom = lastKf->odom; + Se2 lastFrameOdom = mLastFrame.odom; + Se2 dOdom = lastOdom - lastFrameOdom; + cv::Mat dCvOdom = mTcb * dOdom.toCvSE3() * mTbc; + mLastFrame.SetPose(dCvOdom * lastKf->GetPose()); + } + Se2 dOdom = mLastFrame.odom - mCurrentFrame.odom; + mVelocityOdom = mTcb * dOdom.toCvSE3() * mTbc; + //mCurrentFrame.SetPose(mLastFrame.mTcw); + mCurrentFrame.SetPose(mVelocityOdom*mLastFrame.mTcw); + // END + + + fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + + // Project points seen in previous frame + int th; + if(mSensor!=System::STEREO) + th=15; + else + th=7; + int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR); + + // If few matches, uses a wider window search + if(nmatches<20) + { + fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); + nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); + } + + if(nmatches<20) + return false; + + // Optimize frame pose with all matches + //Optimizer::PoseOptimization(&mCurrentFrame); + + // Discard outliers + int nmatchesMap = 0; + for(int i =0; i(NULL); + mCurrentFrame.mvbOutlier[i]=false; + pMP->mbTrackInView = false; + pMP->mnLastFrameSeen = mCurrentFrame.mnId; + nmatches--; + } + else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) + nmatchesMap++; + } + } + + if(mbOnlyTracking) + { + mbVO = nmatchesMap<10; + return nmatches>20; + } + + return nmatchesMap>=10; +} + bool Tracking::TrackLocalMap() { // We have an estimation of the camera pose and some map points tracked in the frame. @@ -937,7 +1017,7 @@ bool Tracking::TrackLocalMap() SearchLocalPoints(); // Optimize Pose - Optimizer::PoseOptimization(&mCurrentFrame); + //Optimizer::PoseOptimization(&mCurrentFrame); mnMatchesInliers = 0; // Update MapPoints Statistics @@ -983,6 +1063,31 @@ bool Tracking::NeedNewKeyFrame() if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) return false; + + Se2 dOdom = mCurrentFrame.odom - mCurrentFrame.mpReferenceKF->odom; + bool c5 = dOdom.theta >= 0.0349f; // Larger than 2 degree + //cv::Mat cTc = Config::cTb * toT4x4(dOdo.x, dOdo.y, dOdo.theta) * Config::bTc; + cv::Mat Tcc = mTcb * dOdom.toCvSE3() * mTbc; + cv::Mat xy = Tcc.rowRange(0,2).col(3); + bool c6 = cv::norm(xy) >= ( 100.f ); + //bool c6 = cv::norm(xy) >= ( 0.0523f * 10000.f * 0.1f ); // 3 degree = 0.0523 rad + + // Local Mapping accept keyframes? + bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames(); + + bool bNeedKeyFrameByOdo = c5 || c6; + if(bNeedKeyFrameByOdo) + { + if(bLocalMappingIdle) + { + return true; + } + else + { + return false; + } + } + const int nKFs = mpMap->KeyFramesInMap(); // Do not insert keyframes if not enough frames have passed from last relocalisation @@ -995,9 +1100,6 @@ bool Tracking::NeedNewKeyFrame() nMinObs=2; int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs); - // Local Mapping accept keyframes? - bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames(); - // Check how many "close" points are being tracked and how many could be potentially created. int nNonTrackedClose = 0; int nTrackedClose= 0; @@ -1034,6 +1136,7 @@ bool Tracking::NeedNewKeyFrame() // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches. const bool c2 = ((mnMatchesInliers15); + if((c1a||c1b||c1c)&&c2) { // If the mapping accepts keyframes, insert keyframe. @@ -1067,9 +1170,15 @@ void Tracking::CreateNewKeyFrame() KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB); + //cout << "0" << pKF->mnId << endl; + //cout << "1" << pKF->mpReferenceKF->mnId << endl; + + mpReferenceKF = pKF; mCurrentFrame.mpReferenceKF = pKF; + //cout << "2" << mCurrentFrame.mpReferenceKF->mnId << endl; + if(mSensor!=System::MONOCULAR) { mCurrentFrame.UpdatePoseMatrices();