Merge branch 'config' into dev

Author: panxkun

cmake/depends/cereal.cmake

@@ -0,0 +1,23 @@
+if(NOT TARGET depends::cereal)
+  if(NOT TARGET options::cpp17)
+    message(FATAL_ERROR "depends::cereal expects options::cpp17")
+  endif()
+  FetchContent_Declare(
+    depends-cereal
+    GIT_REPOSITORY https://github.com/USCiLab/cereal.git
+    GIT_TAG        v1.3.2
+  )
+  FetchContent_GetProperties(depends-cereal)
+  if(NOT depends-cereal_POPULATED)
+    message(STATUS "Fetching cereal sources")
+    FetchContent_Populate(depends-cereal)
+    message(STATUS "Fetching cereal sources - done")
+  endif()
+  
+  add_library(depends::cereal INTERFACE IMPORTED GLOBAL)
+  target_include_directories(depends::cereal INTERFACE ${depends-cereal_SOURCE_DIR}/include)
+  target_link_libraries(depends::cereal INTERFACE options::cpp17)
+
+  set(depends-cereal-source-dir ${depends-cereal_SOURCE_DIR} CACHE INTERNAL "" FORCE)
+  mark_as_advanced(depends-cereal-source-dir)
+endif()

configs/euroc_slam.yaml

@@ -41,3 +41,12 @@ parsac:
   threshold: 1.0
   norm_scale: 1.0
   keyframe_check_size: 1
+
+backend:
+  backend_flag: true
+  voc_file: Vocabulary/ORBvoc.txt
+  orb_n_features: 1000
+  orb_scale_factor: 1.2
+  orb_n_levels: 8
+  orb_init_threshold_fast: 20
+  orb_min_threshold_fast: 7

xrslam-extra/include/xrslam/extra/yaml_config.h

@@ -88,6 +88,15 @@ class YamlConfig : public Config {
     double rotation_misalignment_threshold() const override;
     double rotation_ransac_threshold() const override;
 
+    bool backend_flag() const override;
+    std::string backend_voc_file() const override;
+    size_t backend_orb_n_features() const override;
+    double backend_orb_scale_factor() const override;
+    size_t backend_orb_n_levels() const override;
+    size_t backend_orb_init_threshold_fast() const override;
+    size_t backend_orb_min_threshold_fast() const override;
+
+
   private:
     vector<2> m_camera_resolution;
     matrix<3> m_camera_intrinsic;
@@ -144,6 +153,15 @@ class YamlConfig : public Config {
 
     double m_rotation_misalignment_threshold;
     double m_rotation_ransac_threshold;
+
+    bool m_backend_flag;
+    std::string m_backend_voc_file;
+    size_t m_backend_orb_n_features;
+    double m_backend_orb_scale_factor;
+    size_t m_backend_orb_n_levels;
+    size_t m_backend_orb_init_threshold_fast;
+    size_t m_backend_orb_min_threshold_fast;
+    
 };
 
 } // namespace xrslam::extra

xrslam-extra/src/xrslam/extra/yaml_config.cpp

@@ -124,6 +124,14 @@ YamlConfig::YamlConfig(const std::string &slam_config_filename,
     m_visual_localization_ip = Config::visual_localization_config_ip();
     m_visual_localization_port = Config::visual_localization_config_port();
 
+    m_backend_flag = Config::backend_flag();
+    m_backend_voc_file = Config::backend_voc_file();
+    m_backend_orb_n_features = Config::backend_orb_n_features();
+    m_backend_orb_scale_factor = Config::backend_orb_scale_factor();
+    m_backend_orb_n_levels = Config::backend_orb_n_levels();
+    m_backend_orb_init_threshold_fast = Config::backend_orb_init_threshold_fast();
+    m_backend_orb_min_threshold_fast = Config::backend_orb_min_threshold_fast();
+
     YAML::Node slam_config;
     YAML::Node device_config;
     try {
@@ -360,6 +368,35 @@ YamlConfig::YamlConfig(const std::string &slam_config_filename,
             find_node(slam_config, "rotation.ransac_threshold", false)) {
         assign(m_rotation_ransac_threshold, node);
     }
+
+    if (auto node = find_node(slam_config, "backend.backend_flag", false)) {
+        assign(m_backend_flag, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.voc_file", false)) {
+        assign(m_backend_voc_file, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.orb_n_features", false)) {
+        assign(m_backend_orb_n_features, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.orb_scale_factor", false)) {
+        assign(m_backend_orb_scale_factor, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.orb_n_levels", false)) {
+        assign(m_backend_orb_n_levels, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.orb_init_threshold_fast", false)) {
+        assign(m_backend_orb_init_threshold_fast, node);
+    }
+
+    if (auto node = find_node(slam_config, "backend.orb_min_threshold_fast", false)) {
+        assign(m_backend_orb_min_threshold_fast, node);
+    }
+
 }
 
 YamlConfig::~YamlConfig() = default;
@@ -534,4 +571,18 @@ double YamlConfig::rotation_ransac_threshold() const {
     return m_rotation_ransac_threshold;
 }
 
+bool YamlConfig::backend_flag() const { return m_backend_flag; }
+
+std::string YamlConfig::backend_voc_file() const { return m_backend_voc_file; }
+
+size_t YamlConfig::backend_orb_n_features() const { return m_backend_orb_n_features; }
+
+double YamlConfig::backend_orb_scale_factor() const { return m_backend_orb_scale_factor; }
+
+size_t YamlConfig::backend_orb_n_levels() const { return m_backend_orb_n_levels; }
+
+size_t YamlConfig::backend_orb_init_threshold_fast() const { return m_backend_orb_init_threshold_fast; }
+
+size_t YamlConfig::backend_orb_min_threshold_fast() const { return m_backend_orb_min_threshold_fast; }
+
 } // namespace xrslam::extra

xrslam/CMakeLists.txt

@@ -6,6 +6,7 @@ include(SuperBuildDepends)
 superbuild_depend(eigen)
 superbuild_depend(ceres-solver)
 superbuild_depend(spdlog)
+superbuild_depend(cereal)
 
 configure_file(cmake/version.h.in include/xrslam/version.h)
 
@@ -63,7 +64,11 @@ set(PRIVATE_HEADERS
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/worker.h
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/logger.h
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/parsac.h
+  ${XRSLAM_SOURCE_DIR}/xrslam/utility/serialize.h
+  ${XRSLAM_SOURCE_DIR}/xrslam/utility/potocal.h
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/imu_parsac.h
+  ${XRSLAM_SOURCE_DIR}/xrslam/utility/orb_extractor.h
+  ${XRSLAM_SOURCE_DIR}/xrslam/utility/client.h
   ${XRSLAM_SOURCE_DIR}/xrslam/ar/virtual_object_manager.h
   ${XRSLAM_SOURCE_DIR}/xrslam/localizer/base64.h
   ${XRSLAM_SOURCE_DIR}/xrslam/localizer/base64_convert.h
@@ -92,6 +97,7 @@ set(PRIVATE_SOURCES
   ${XRSLAM_SOURCE_DIR}/xrslam/xrslam.cpp
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/debug.cpp
   ${XRSLAM_SOURCE_DIR}/xrslam/utility/worker.cpp
+  ${XRSLAM_SOURCE_DIR}/xrslam/utility/orb_extractor.cpp
   ${XRSLAM_SOURCE_DIR}/xrslam/localizer/localizer.cpp
   ${XRSLAM_SOURCE_DIR}/xrslam/ar/virtual_object_manager.cpp
 )
@@ -139,6 +145,7 @@ PUBLIC
   depends::spdlog
   depends::ceres-solver
   depends::opencv
+  depends::cereal
 )
 
 set(XRSLAM_LIBRARY_DIR ${CMAKE_CURRENT_BINARY_DIR} CACHE INTERNAL "")

xrslam/include/xrslam/xrslam.h

@@ -7,47 +7,47 @@
 #include <opencv2/opencv.hpp>
 #include <vector>
 #include <xrslam/version.h>
-#include <chrono>
-#include <thread>
 
 namespace xrslam {
 
-template <int Rows = Eigen::Dynamic, int Cols = Rows, bool UseRowMajor = false, typename T = double>
+template <int Rows = Eigen::Dynamic, int Cols = Rows, bool UseRowMajor = false,
+          typename T = double>
 using matrix = typename std::conditional<
     Rows != 1 && Cols != 1,
-    Eigen::Matrix<T, Rows, Cols, UseRowMajor ? Eigen::RowMajor : Eigen::ColMajor>,
+    Eigen::Matrix<T, Rows, Cols,
+                  UseRowMajor ? Eigen::RowMajor : Eigen::ColMajor>,
     Eigen::Matrix<T, Rows, Cols>>::type;
 
-template <int Dimension = Eigen::Dynamic, bool RowVector = false, typename T = double>
+template <int Dimension = Eigen::Dynamic, bool RowVector = false,
+          typename T = double>
 using vector =
     typename std::conditional<RowVector, matrix<1, Dimension, false, T>,
                               matrix<Dimension, 1, false, T>>::type;
 
 using quaternion = Eigen::Quaternion<double>;
 
-using matrix6 = matrix<6, 6>;
-using matrix3 = matrix<3, 3>;
-using matrix2 = matrix<2, 2>;
-using matrix1 = matrix<1, 1>;
-using matrix6x3 = matrix<6, 3>;
-using matrix6x1 = matrix<6, 1>;
-using matrixX = matrix<-1, -1>;
-
-using vector6 = vector<6>;
-using vector5 = vector<5>;
-using vector4 = vector<4>;
-using vector3 = vector<3>;
-using vector2 = vector<2>;
-using vector1 = vector<1>;
-using vectorX = vector<-1>;
-
 struct Pose {
     Pose() {
         q.setIdentity();
         p.setZero();
     }
     quaternion q;
     vector<3> p;
+
+    matrix<4> to_matrix(){
+        matrix<4> T;
+        T.setIdentity();
+        T.block<3, 3>(0, 0) = q.toRotationMatrix();
+        T.block<3, 1>(0, 3) = p;
+        return T;
+    }
+
+    static Pose from_matrix(matrix<4> T){
+        Pose pose;
+        pose.q = quaternion(T.block<3, 3>(0, 0));
+        pose.p = T.block<3, 1>(0, 3);
+        return pose;
+    }
 };
 
 using OutputPose = Pose;
@@ -68,6 +68,7 @@ struct OutputObject {
     int isolated;
 };
 
+
 class Config {
   public:
     virtual ~Config();
@@ -131,6 +132,14 @@ class Config {
     virtual double parsac_norm_scale() const;
     virtual size_t parsac_keyframe_check_size() const;
 
+    virtual bool backend_flag() const;
+    virtual std::string backend_voc_file() const;
+    virtual size_t backend_orb_n_features() const;
+    virtual double backend_orb_scale_factor() const;
+    virtual size_t backend_orb_n_levels() const;
+    virtual size_t backend_orb_init_threshold_fast() const;
+    virtual size_t backend_orb_min_threshold_fast() const;
+    
     void log_config() const;
 };
 
@@ -189,25 +198,6 @@ class XRSLAM {
     std::unique_ptr<Detail> detail;
 };
 
-struct Timer{
-    Timer(){reset();}
-
-    void reset(){
-        t1 = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
-    }
-
-    double get_time(){
-        double t2 = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
-        return t2 - t1;
-    }
-
-    void sleep(double seconds){
-        std::this_thread::sleep_for(std::chrono::milliseconds((int)(seconds * 1000)));
-    }
-
-    double t1;
-};
-
 } // namespace xrslam
 
 #endif // XRSLAM_XRSLAM_H

xrslam/src/xrslam/config.cpp

@@ -77,6 +77,20 @@ size_t Config::parsac_keyframe_check_size() const { return 3; }
 
 size_t Config::sliding_window_tracker_frequent() const { return 1; }
 
+bool Config::backend_flag() const { return false; }
+
+std::string Config::backend_voc_file() const { return "Vocabulary/ORBvoc.txt"; }
+
+size_t Config::backend_orb_n_features() const { return 1000; }
+
+double Config::backend_orb_scale_factor() const { return 1.2; }
+
+size_t Config::backend_orb_n_levels() const { return 8; }
+
+size_t Config::backend_orb_init_threshold_fast() const { return 20; }
+
+size_t Config::backend_orb_min_threshold_fast() const { return 7; }
+
 void Config::log_config() const {
     std::stringstream ss;
     ss << std::scientific << std::boolalpha << std::setprecision(5);
@@ -215,6 +229,20 @@ void Config::log_config() const {
     ss << "Config::rotation_ransac_threshold: " << rotation_ransac_threshold()
        << std::endl;
 
+    ss << "Config::backend_flag: " << backend_flag() << std::endl;
+
+    ss << "Config::backend_voc_file: " << backend_voc_file() << std::endl;
+
+    ss << "Config::backend_orb_n_features: " << backend_orb_n_features() << std::endl;
+
+    ss << "Config::backend_orb_scale_factor: " << backend_orb_scale_factor() << std::endl;
+
+    ss << "Config::backend_orb_n_levels: " << backend_orb_n_levels() << std::endl;
+
+    ss << "Config::backend_orb_init_threshold_fast: " << backend_orb_init_threshold_fast() << std::endl;
+
+    ss << "Config::backend_orb_min_threshold_fast: " << backend_orb_min_threshold_fast() << std::endl;
+
 #if defined(XRSLAM_ENABLE_THREADING)
     ss << std::endl;
     ss << "THREADING ENABLE" << std::endl;