-
Notifications
You must be signed in to change notification settings - Fork 402
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Use forked version of DBoW2 * Add run_euroc_localization.cc * Fix missing fstream * Add FBoW to CI * Fix variable name fbow_LIBS * Add CI workflow for FBoW
- Loading branch information
1 parent
61b58ec
commit b6c40f6
Showing
16 changed files
with
321 additions
and
10 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,244 @@ | ||
| #include "util/euroc_util.h" | ||
|
|
||
| #ifdef USE_PANGOLIN_VIEWER | ||
| #include "pangolin_viewer/viewer.h" | ||
| #elif USE_SOCKET_PUBLISHER | ||
| #include "socket_publisher/publisher.h" | ||
| #endif | ||
|
|
||
| #include "openvslam/system.h" | ||
| #include "openvslam/config.h" | ||
| #include "openvslam/util/stereo_rectifier.h" | ||
| #include "openvslam/util/image_converter.h" | ||
|
|
||
| #include <iostream> | ||
| #include <algorithm> | ||
| #include <fstream> | ||
| #include <chrono> | ||
| #include <iomanip> | ||
| #include <numeric> | ||
|
|
||
| #include <opencv2/core/core.hpp> | ||
| #include <opencv2/imgcodecs.hpp> | ||
| #include <spdlog/spdlog.h> | ||
| #include <popl.hpp> | ||
|
|
||
| #ifdef USE_STACK_TRACE_LOGGER | ||
| #include <glog/logging.h> | ||
| #endif | ||
|
|
||
| #ifdef USE_GOOGLE_PERFTOOLS | ||
| #include <gperftools/profiler.h> | ||
| #endif | ||
|
|
||
| void mono_localization(const std::shared_ptr<openvslam::config>& cfg, | ||
| const std::string& vocab_file_path, const std::string& sequence_dir_path, | ||
| const unsigned int frame_skip, const bool no_sleep, const bool auto_term, | ||
| const bool eval_log, const std::string& map_db_path, const bool mapping, | ||
| const bool equal_hist) { | ||
| const euroc_sequence sequence(sequence_dir_path); | ||
| const auto frames = sequence.get_frames(); | ||
|
|
||
| // build a SLAM system | ||
| openvslam::system SLAM(cfg, vocab_file_path); | ||
| // load the prebuilt map | ||
| SLAM.load_map_database(map_db_path); | ||
| // startup the SLAM process (it does not need initialization of a map) | ||
| SLAM.startup(false); | ||
| // select to activate the mapping module or not | ||
| if (mapping) { | ||
| SLAM.enable_mapping_module(); | ||
| } | ||
| else { | ||
| SLAM.disable_mapping_module(); | ||
| } | ||
|
|
||
| // create a viewer object | ||
| // and pass the frame_publisher and the map_publisher | ||
| #ifdef USE_PANGOLIN_VIEWER | ||
| pangolin_viewer::viewer viewer(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher()); | ||
| #elif USE_SOCKET_PUBLISHER | ||
| socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher()); | ||
| #endif | ||
|
|
||
| std::vector<double> track_times; | ||
| track_times.reserve(frames.size()); | ||
|
|
||
| // run the SLAM in another thread | ||
| std::thread thread([&]() { | ||
| for (unsigned int i = 0; i < frames.size(); ++i) { | ||
| const auto& frame = frames.at(i); | ||
| cv::Mat img; | ||
| if (equal_hist) { | ||
| img = cv::imread(frame.left_img_path_, cv::IMREAD_UNCHANGED); | ||
| openvslam::util::equalize_histogram(img); | ||
| } | ||
| else { | ||
| img = cv::imread(frame.left_img_path_, cv::IMREAD_GRAYSCALE); | ||
| } | ||
|
|
||
| const auto tp_1 = std::chrono::steady_clock::now(); | ||
|
|
||
| if (!img.empty() && (i % frame_skip == 0)) { | ||
| // input the current frame and estimate the camera pose | ||
| SLAM.feed_monocular_frame(img, frame.timestamp_); | ||
| } | ||
|
|
||
| const auto tp_2 = std::chrono::steady_clock::now(); | ||
|
|
||
| const auto track_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count(); | ||
| if (i % frame_skip == 0) { | ||
| track_times.push_back(track_time); | ||
| } | ||
|
|
||
| // wait until the timestamp of the next frame | ||
| if (!no_sleep && i < frames.size() - 1) { | ||
| const auto wait_time = frames.at(i + 1).timestamp_ - (frame.timestamp_ + track_time); | ||
| if (0.0 < wait_time) { | ||
| std::this_thread::sleep_for(std::chrono::microseconds(static_cast<unsigned int>(wait_time * 1e6))); | ||
| } | ||
| } | ||
|
|
||
| // check if the termination of SLAM system is requested or not | ||
| if (SLAM.terminate_is_requested()) { | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| // wait until the loop BA is finished | ||
| while (SLAM.loop_BA_is_running()) { | ||
| std::this_thread::sleep_for(std::chrono::microseconds(5000)); | ||
| } | ||
|
|
||
| // automatically close the viewer | ||
| #ifdef USE_PANGOLIN_VIEWER | ||
| if (auto_term) { | ||
| viewer.request_terminate(); | ||
| } | ||
| #elif USE_SOCKET_PUBLISHER | ||
| if (auto_term) { | ||
| publisher.request_terminate(); | ||
| } | ||
| #endif | ||
| }); | ||
|
|
||
| // run the viewer in the current thread | ||
| #ifdef USE_PANGOLIN_VIEWER | ||
| viewer.run(); | ||
| #elif USE_SOCKET_PUBLISHER | ||
| publisher.run(); | ||
| #endif | ||
|
|
||
| thread.join(); | ||
|
|
||
| // shutdown the SLAM process | ||
| SLAM.shutdown(); | ||
|
|
||
| if (eval_log) { | ||
| // output the trajectories for evaluation | ||
| SLAM.save_frame_trajectory("frame_trajectory.txt", "TUM"); | ||
| SLAM.save_keyframe_trajectory("keyframe_trajectory.txt", "TUM"); | ||
| // output the tracking times for evaluation | ||
| std::ofstream ofs("track_times.txt", std::ios::out); | ||
| if (ofs.is_open()) { | ||
| for (const auto track_time : track_times) { | ||
| ofs << track_time << std::endl; | ||
| } | ||
| ofs.close(); | ||
| } | ||
| } | ||
|
|
||
| if (!map_db_path.empty()) { | ||
| // output the map database | ||
| SLAM.save_map_database(map_db_path); | ||
| } | ||
|
|
||
| std::sort(track_times.begin(), track_times.end()); | ||
| const auto total_track_time = std::accumulate(track_times.begin(), track_times.end(), 0.0); | ||
| std::cout << "median tracking time: " << track_times.at(track_times.size() / 2) << "[s]" << std::endl; | ||
| std::cout << "mean tracking time: " << total_track_time / track_times.size() << "[s]" << std::endl; | ||
| } | ||
|
|
||
| int main(int argc, char* argv[]) { | ||
| #ifdef USE_STACK_TRACE_LOGGER | ||
| google::InitGoogleLogging(argv[0]); | ||
| google::InstallFailureSignalHandler(); | ||
| #endif | ||
|
|
||
| // create options | ||
| popl::OptionParser op("Allowed options"); | ||
| auto help = op.add<popl::Switch>("h", "help", "produce help message"); | ||
| auto vocab_file_path = op.add<popl::Value<std::string>>("v", "vocab", "vocabulary file path"); | ||
| auto data_dir_path = op.add<popl::Value<std::string>>("d", "data-dir", "directory path which contains dataset"); | ||
| auto config_file_path = op.add<popl::Value<std::string>>("c", "config", "config file path"); | ||
| auto frame_skip = op.add<popl::Value<unsigned int>>("", "frame-skip", "interval of frame skip", 1); | ||
| auto no_sleep = op.add<popl::Switch>("", "no-sleep", "not wait for next frame in real time"); | ||
| auto auto_term = op.add<popl::Switch>("", "auto-term", "automatically terminate the viewer"); | ||
| auto debug_mode = op.add<popl::Switch>("", "debug", "debug mode"); | ||
| auto eval_log = op.add<popl::Switch>("", "eval-log", "store trajectory and tracking times for evaluation"); | ||
| auto map_db_path = op.add<popl::Value<std::string>>("p", "map-db", "path to a prebuilt map database", ""); | ||
| auto mapping = op.add<popl::Switch>("", "mapping", "perform mapping as well as localization"); | ||
| auto equal_hist = op.add<popl::Switch>("", "equal-hist", "apply histogram equalization"); | ||
|
|
||
| try { | ||
| op.parse(argc, argv); | ||
| } | ||
| catch (const std::exception& e) { | ||
| std::cerr << e.what() << std::endl; | ||
| std::cerr << std::endl; | ||
| std::cerr << op << std::endl; | ||
| return EXIT_FAILURE; | ||
| } | ||
|
|
||
| // check validness of options | ||
| if (help->is_set()) { | ||
| std::cerr << op << std::endl; | ||
| return EXIT_FAILURE; | ||
| } | ||
| if (!vocab_file_path->is_set() || !data_dir_path->is_set() || !config_file_path->is_set()) { | ||
| std::cerr << "invalid arguments" << std::endl; | ||
| std::cerr << std::endl; | ||
| std::cerr << op << std::endl; | ||
| return EXIT_FAILURE; | ||
| } | ||
|
|
||
| // setup logger | ||
| spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$"); | ||
| if (debug_mode->is_set()) { | ||
| spdlog::set_level(spdlog::level::debug); | ||
| } | ||
| else { | ||
| spdlog::set_level(spdlog::level::info); | ||
| } | ||
|
|
||
| // load configuration | ||
| std::shared_ptr<openvslam::config> cfg; | ||
| try { | ||
| cfg = std::make_shared<openvslam::config>(config_file_path->value()); | ||
| } | ||
| catch (const std::exception& e) { | ||
| std::cerr << e.what() << std::endl; | ||
| return EXIT_FAILURE; | ||
| } | ||
|
|
||
| #ifdef USE_GOOGLE_PERFTOOLS | ||
| ProfilerStart("slam.prof"); | ||
| #endif | ||
|
|
||
| // run tracking | ||
| if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) { | ||
| mono_localization(cfg, vocab_file_path->value(), data_dir_path->value(), | ||
| frame_skip->value(), no_sleep->is_set(), auto_term->is_set(), | ||
| eval_log->is_set(), map_db_path->value(), mapping->is_set(), | ||
| equal_hist->is_set()); | ||
| } | ||
| else { | ||
| throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string()); | ||
| } | ||
|
|
||
| #ifdef USE_GOOGLE_PERFTOOLS | ||
| ProfilerStop(); | ||
| #endif | ||
|
|
||
| return EXIT_SUCCESS; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.