git update

src/ground_slam/include/read_configs.h

@@ -0,0 +1,137 @@
+#ifndef READ_CONFIGS_H_
+#define READ_CONFIGS_H_
+
+#include <iostream>
+#include <yaml-cpp/yaml.h>
+
+#include "utils.h"
+
+
+struct DatasetConfig{
+  std::string dataroot;
+  std::string image_dir_name;
+  std::string camera_file;
+};
+
+struct CFConfig{
+  int width;
+  int height;
+  float lambda;
+  int kernel;
+  float sigma;
+  float offset;
+  int power;
+  int rotation_divisor;
+  int rotation_channel;
+};
+
+struct KeyframeSelectionConfig{
+  double max_distance;
+  double max_angle;
+  double lower_response_thr;
+  double upper_response_thr;
+};
+
+struct MapConfig{
+  double grid_scale;
+};
+
+struct LoopClosureConfig{
+  bool to_find_loop;
+  double position_response_thr;
+  double angle_response_thr;
+  int frame_gap_thr;
+  double distance_thr;
+};
+
+struct MapStitcherConfig{
+  bool stitch_map;
+  int cell_size;
+};
+
+struct VisualizationConfig{
+  std::string frame_id;
+  std::string kcc_pose_topic;
+  std::string frame_pose_topic;
+  std::string map_topic;
+  std::string image_topic;
+};
+
+struct SavingConfig{
+  std::string saving_root;
+  bool save_pose;
+};
+
+struct Configs{
+  DatasetConfig dataset_config;
+  CFConfig cf_config;
+  KeyframeSelectionConfig keyframe_selection_config;
+  MapConfig map_config;
+  LoopClosureConfig loop_closure_config;
+  MapStitcherConfig map_stitcher_config;
+  VisualizationConfig visualization_config;
+  SavingConfig saving_config;
+
+  Configs(const std::string& config_file){
+    if(!FileExists(config_file)){
+      std::cout << "config file: " << config_file << " doesn't exist" << std::endl;
+      return;
+    }
+
+    YAML::Node file_node = YAML::LoadFile(config_file);
+    YAML::Node dataset_node = file_node["dataset"];
+    dataset_config.dataroot = dataset_node["dataroot"].as<std::string>();
+    dataset_config.image_dir_name = dataset_node["image_dir_name"].as<std::string>();
+    dataset_config.camera_file = dataset_node["camera_config"].as<std::string>();
+
+    YAML::Node cf_node = file_node["correlation_flow"];
+    cf_config.width = cf_node["width"].as<int>();
+    cf_config.height = cf_node["height"].as<int>();
+    cf_config.lambda = cf_node["lambda"].as<float>();
+    cf_config.rotation_divisor = cf_node["rotation_divisor"].as<int>();
+    cf_config.rotation_channel = cf_node["rotation_channel"].as<int>();
+    cf_config.kernel = cf_node["kernel"].as<int>();
+    cf_config.offset = cf_node["polynomial"]["offset"].as<float>();
+    cf_config.power = cf_node["polynomial"]["power"].as<int>();
+    cf_config.sigma = cf_node["gaussian"]["sigma"].as<float>();
+
+    YAML::Node kfs_node = file_node["keyframe_selection"];
+    keyframe_selection_config.max_distance = kfs_node["max_distance"].as<double>();
+    keyframe_selection_config.max_angle = kfs_node["max_angle"].as<double>();
+    keyframe_selection_config.lower_response_thr = kfs_node["lower_response_thr"].as<double>();
+    keyframe_selection_config.upper_response_thr = kfs_node["upper_response_thr"].as<double>();
+
+    YAML::Node map_node = file_node["map"];
+    map_config.grid_scale = map_node["grid_scale"].as<double>();
+
+    YAML::Node loop_closure_node = file_node["loop_closure"];
+    loop_closure_config.to_find_loop = 
+        loop_closure_node["to_find_loop"].as<bool>();
+    loop_closure_config.position_response_thr = 
+        loop_closure_node["position_response_thr"].as<double>();
+    loop_closure_config.angle_response_thr = 
+        loop_closure_node["angle_response_thr"].as<double>();
+    loop_closure_config.frame_gap_thr = 
+        loop_closure_node["frame_gap_thr"].as<int>();
+    loop_closure_config.distance_thr = 
+        loop_closure_node["distance_thr"].as<double>();
+
+    YAML::Node map_stitcher_node = file_node["map_sticther"];
+    map_stitcher_config.stitch_map = map_stitcher_node["stitch_map"].as<bool>();
+    map_stitcher_config.cell_size = map_stitcher_node["cell_size"].as<int>();
+
+    YAML::Node visualization_node = file_node["visualization"];
+    visualization_config.frame_id = visualization_node["frame_id"].as<std::string>();
+    visualization_config.kcc_pose_topic = visualization_node["topic"]["kcc_pose"].as<std::string>();
+    visualization_config.frame_pose_topic = visualization_node["topic"]["frame_pose"].as<std::string>();
+    visualization_config.map_topic = visualization_node["topic"]["map"].as<std::string>();
+    visualization_config.image_topic = visualization_node["topic"]["image"].as<std::string>();
+
+    YAML::Node saving_node = file_node["saving"];
+    saving_config.saving_root = saving_node["saving_root"].as<std::string>();
+    saving_config.save_pose = saving_node["save_pose"].as<bool>();
+  }
+};
+
+
+#endif  // READ_CONFIGS_H_