| 29 | } |
| 30 | |
| 31 | void Parameters::loadParameters(const std::string& config_file) { |
| 32 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); |
| 33 | |
| 34 | if (!fsSettings.isOpened()) { |
| 35 | LOG(FATAL) << "ERROR: Wrong path to settings" << std::endl; |
| 36 | } |
| 37 | |
| 38 | camera_visual_size_ = fsSettings["visualize_camera_size"]; |
| 39 | |
| 40 | std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("pose_graph"); |
| 41 | |
| 42 | vocabulary_file_ = pkg_share_dir + "/Vocabulary/brief_k10L6.bin"; |
| 43 | std::cout << "vocabulary_file" << vocabulary_file_ << std::endl; |
| 44 | |
| 45 | brief_pattern_file_ = pkg_share_dir + "/Vocabulary/brief_pattern.yml"; |
| 46 | |
| 47 | if (fsSettings["loop_closure_params"]["enable"].isInt()) { |
| 48 | loop_closure_params_.enabled = static_cast<int>(fsSettings["loop_closure_params"]["enable"]); |
| 49 | LOG(INFO) << "loop_closure_params.enable: " << loop_closure_params_.enabled; |
| 50 | |
| 51 | if (fsSettings["loop_closure_params"]["min_correspondences"].isInt() || |
| 52 | fsSettings["loop_closure_params"]["min_correspondences"].isReal()) { |
| 53 | loop_closure_params_.min_correspondences = |
| 54 | static_cast<int>(fsSettings["loop_closure_params"]["min_correspondences"]); |
| 55 | LOG(INFO) << "Num of matched keypoints for Loop Detection:" << loop_closure_params_.min_correspondences; |
| 56 | } |
| 57 | |
| 58 | if (fsSettings["loop_closure_params"]["pnp_reprojection_threshold"].isReal() || |
| 59 | fsSettings["loop_closure_params"]["pnp_reprojection_threshold"].isInt()) { |
| 60 | loop_closure_params_.pnp_reprojection_thresh = |
| 61 | static_cast<double>(fsSettings["loop_closure_params"]["pnp_reprojection_threshold"]); |
| 62 | LOG(INFO) << "PnP reprojection threshold: " << loop_closure_params_.pnp_reprojection_thresh; |
| 63 | } |
| 64 | |
| 65 | if (fsSettings["loop_closure_params"]["pnp_ransac_iterations"].isInt() || |
| 66 | fsSettings["loop_closure_params"]["pnp_ransac_iterations"].isReal()) { |
| 67 | loop_closure_params_.pnp_ransac_iterations = |
| 68 | static_cast<int>(fsSettings["loop_closure_params"]["pnp_ransac_iterations"]); |
| 69 | LOG(INFO) << "PnP ransac iterations: " << loop_closure_params_.pnp_ransac_iterations; |
| 70 | } |
| 71 | } |
| 72 | |
| 73 | // Determine source directory of this file |
| 74 | std::filesystem::path this_file(__FILE__); |
| 75 | std::filesystem::path package_src_dir = this_file.parent_path().parent_path().parent_path(); // Go up from src/pose_graph/ |
| 76 | |
| 77 | output_path_ = (package_src_dir).string(); |
| 78 | |
| 79 | if (fsSettings["output_params"]["output_dir"].isString()) { |
| 80 | try { |
| 81 | if (!std::filesystem::exists(output_path_)) { |
| 82 | std::filesystem::create_directory(output_path_); |
| 83 | } |
| 84 | } catch (std::filesystem::filesystem_error& e) { |
| 85 | LOG(ERROR) << e.what(); |
| 86 | output_path_ = "/tmp"; |
| 87 | } |
| 88 | debug_output_path_ = output_path_ + "/debug_output"; |