From 3e1eea2bf59f117a11c90154b04e51eddb3e17a2 Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Fri, 20 Aug 2021 22:57:24 -0700 Subject: [PATCH 01/55] Added initial classes needed for changing the verbosity level of open_vins --- ov_core/src/utils/print.cpp | 87 +++++++++++++++++++++++++++++++++++++ ov_core/src/utils/print.h | 77 ++++++++++++++++++++++++++++++++ 2 files changed, 164 insertions(+) create mode 100644 ov_core/src/utils/print.cpp create mode 100644 ov_core/src/utils/print.h diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp new file mode 100644 index 000000000..ae495476e --- /dev/null +++ b/ov_core/src/utils/print.cpp @@ -0,0 +1,87 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#include "print.h" +#include +#include +#include +#include + +using namespace ov_core; + +// Need to define the variable for everything to work +Printer::PrintLevel Printer::current_print_level = PrintLevel::ALL; + +void Printer::setPrintLevel(PrintLevel level) { + Printer::current_print_level = level; + + std::cout << "Setting printing level to: "; + switch (current_print_level) { + case PrintLevel::ALL: + std::cout << "ALL"; + break; + case PrintLevel::DEBUG: + std::cout << "DEBUG"; + break; + case PrintLevel::INFO: + std::cout << "INFO"; + break; + case PrintLevel::WARNING: + std::cout << "WARNING"; + break; + case PrintLevel::ERROR: + std::cout << "ERROR"; + break; + case PrintLevel::SILENT: + std::cout << "SILENT"; + break; + default: + // Can never get here + break; + } + + std::cout << std::endl; +} + +void Printer::debugPrint(PrintLevel level, const char location[], const char *format, ...) { + // Only print for the current debug level + if (static_cast(level) < static_cast(Printer::current_print_level)) { + return; + } + + // Print the location info first + if (strlen(location) > MAX_FILE_PATH_LEGTH) { + // Truncate the location length to the max size for the filepath + printf("%s", &(location[strlen(location) - MAX_FILE_PATH_LEGTH])); + } else { + // Print the full location + printf("%s", location); + } + + // Print the rest of the args + va_list args; + va_start(args, format); + vprintf(format, args); + va_end(args); + + // All prints get a new line! + printf("\r\n"); +} diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h new file mode 100644 index 000000000..209d06853 --- /dev/null +++ b/ov_core/src/utils/print.h @@ -0,0 +1,77 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef PRINT_H +#define PRINT_H + +#include + +namespace ov_core { + +class Printer { +public: + /** The different print levels possible + */ + enum PrintLevel { ALL = 0, DEBUG = 1, INFO = 2, WARNING = 3, ERROR = 4, SILENT = 5 }; + + /** @brief Set the print level to use for all future printing to stdout. + * + * @param level The debug level to use + */ + static void setPrintLevel(PrintLevel level); + + /** brief The print function that prints to stdout. + * + * @param level the print level for this print call + * @param location the location the print was made from + * @param format The printf format + */ + static void debugPrint(PrintLevel level, const char location[], const char *format, ...); + + /** brief The print level + */ + static PrintLevel current_print_level; + +private: + /// The max length for the file path. This is to avoid very long file paths from + static constexpr uint32_t MAX_FILE_PATH_LEGTH = 50; +}; + +} /* namespace ov_core */ + +/** Converts anything to a string + */ +#define STRINGIFY(x) #x +#define TOSTRING(x) STRINGIFY(x) + +/** Adds line numbers + */ +#define AT "(" __FILE__ ":" TOSTRING(__LINE__) "): " + +/** The different Types of print levels + */ +#define PRINT_ALL(x...) Debug::debugPrint(Debug::PrintLevel::ALL, AT, x); +#define PRINT_DEBUG(x...) Debug::debugPrint(Debug::PrintLevel::DEBUG, AT, x); +#define PRINT_INFO(x...) Debug::debugPrint(Debug::PrintLevel::INFO, AT, x); +#define PRINT_WARNING(x...) Debug::debugPrint(Debug::PrintLevel::WARNING, AT, x); +#define PRINT_ERROR(x...) Debug::debugPrint(Debug::PrintLevel::ERROR, AT, x); + +#endif /* PRINT_H */ From b9d2ee260e1646d1561fb6898f0b079d3c794c35 Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Sat, 21 Aug 2021 00:55:47 -0700 Subject: [PATCH 02/55] Converted all the printf, cout and cerr ouputs into PRINT_X outputs so that the verbosity level can be changed as needed --- ov_core/CMakeLists.txt | 1 + ov_core/src/feat/FeatureDatabase.h | 13 +-- ov_core/src/feat/FeatureInitializer.cpp | 16 ++- ov_core/src/feat/FeatureInitializerOptions.h | 26 ++--- ov_core/src/init/InertialInitializer.cpp | 15 +-- ov_core/src/sim/BsplineSE3.cpp | 21 ++-- ov_core/src/test_tracking.cpp | 31 +++--- ov_core/src/test_webcam.cpp | 17 ++-- ov_core/src/track/TrackAruco.cpp | 19 ++-- ov_core/src/track/TrackAruco.h | 4 +- ov_core/src/track/TrackDescriptor.cpp | 35 +++---- ov_core/src/track/TrackKLT.cpp | 43 ++++---- ov_core/src/track/TrackSIM.h | 5 +- ov_core/src/types/Landmark.h | 3 +- ov_core/src/utils/dataset_reader.h | 17 ++-- ov_core/src/utils/print.cpp | 3 - ov_core/src/utils/print.h | 10 +- ov_core/src/utils/quat_ops.h | 10 +- ov_eval/CMakeLists.txt | 6 +- ov_eval/package.xml | 2 + ov_eval/src/alignment/AlignTrajectory.cpp | 5 +- ov_eval/src/alignment/AlignUtils.cpp | 18 ++-- ov_eval/src/calc/ResultSimulation.cpp | 21 ++-- ov_eval/src/calc/ResultTrajectory.cpp | 30 +++--- ov_eval/src/error_comparison.cpp | 73 ++++++------- ov_eval/src/error_dataset.cpp | 45 ++++---- ov_eval/src/error_simulation.cpp | 13 +-- ov_eval/src/error_singlerun.cpp | 53 +++++----- ov_eval/src/format_converter.cpp | 34 ++++--- ov_eval/src/live_align_trajectory.cpp | 9 +- ov_eval/src/plot_trajectories.cpp | 17 ++-- ov_eval/src/timing_comparison.cpp | 21 ++-- ov_eval/src/timing_flamegraph.cpp | 13 +-- ov_eval/src/timing_percentages.cpp | 21 ++-- ov_eval/src/utils/Loader.cpp | 69 ++++++------- ov_eval/src/utils/Math.h | 10 +- ov_msckf/src/core/RosVisualizer.cpp | 45 ++++---- ov_msckf/src/core/VioManager.cpp | 102 ++++++++++--------- ov_msckf/src/core/VioManager.h | 19 ++-- ov_msckf/src/core/VioManagerOptions.h | 102 ++++++++++--------- ov_msckf/src/ros_serial_msckf.cpp | 2 +- ov_msckf/src/run_simulation.cpp | 5 +- ov_msckf/src/sim/Simulator.cpp | 44 ++++---- ov_msckf/src/state/Propagator.cpp | 31 +++--- ov_msckf/src/state/Propagator.h | 11 +- ov_msckf/src/state/StateHelper.cpp | 54 +++++----- ov_msckf/src/state/StateOptions.h | 32 +++--- ov_msckf/src/test_sim_meas.cpp | 8 +- ov_msckf/src/test_sim_repeat.cpp | 3 +- ov_msckf/src/update/UpdaterMSCKF.cpp | 26 +++-- ov_msckf/src/update/UpdaterOptions.h | 6 +- ov_msckf/src/update/UpdaterSLAM.cpp | 25 ++--- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 20 ++-- ov_msckf/src/utils/parse_cmd.h | 27 ++--- ov_msckf/src/utils/parse_ros.h | 33 +++--- 55 files changed, 720 insertions(+), 624 deletions(-) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 7fa4e6735..bba5c3e78 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -91,6 +91,7 @@ add_library(ov_core_lib SHARED src/types/Landmark.cpp src/feat/Feature.cpp src/feat/FeatureInitializer.cpp + src/utils/print.cpp ) target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC src) diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h index d272dc997..6580f3434 100644 --- a/ov_core/src/feat/FeatureDatabase.h +++ b/ov_core/src/feat/FeatureDatabase.h @@ -28,6 +28,7 @@ #include #include "Feature.h" +#include "utils/print.h" namespace ov_core { @@ -159,7 +160,7 @@ class FeatureDatabase { } // Debugging - // std::cout << "feature db size = " << features_idlookup.size() << std::endl; + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) // Return the old features return feats_old; @@ -206,7 +207,7 @@ class FeatureDatabase { } // Debugging - // std::cout << "feature db size = " << features_idlookup.size() << std::endl; + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) // Return the old features return feats_old; @@ -253,8 +254,8 @@ class FeatureDatabase { } // Debugging - // std::cout << "feature db size = " << features_idlookup.size() << std::endl; - // std::cout << "return vector = " << feats_has_timestamp.size() << std::endl; + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) + // PRINT_DEBUG("return vector = %u\n", feats_has_timestamp.size()) // Return the features return feats_has_timestamp; @@ -277,7 +278,7 @@ class FeatureDatabase { it++; } } - // std::cout << "feat db = " << sizebefore << " -> " << (int)features_idlookup.size() << std::endl; + // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; } /** @@ -386,7 +387,7 @@ class FeatureDatabase { features_idlookup[feat.first] = temp; } } - // std::cout << "feat db = " << sizebefore << " -> " << (int)features_idlookup.size() << std::endl; + // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; } protected: diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp index f176c5c98..5e92098bc 100644 --- a/ov_core/src/feat/FeatureInitializer.cpp +++ b/ov_core/src/feat/FeatureInitializer.cpp @@ -20,6 +20,9 @@ */ #include "FeatureInitializer.h" +#include "utils/print.h" + +#include using namespace ov_core; @@ -88,7 +91,10 @@ bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map< singularValues.resize(svd.singularValues().rows(), 1); singularValues = svd.singularValues(); double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0); - // std::cout << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl; + + // std::stringstream ss; + // ss << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl; + // PRINT_DEBUG(ss.str().c_str()); // If we have a bad condition number, or it is too close // Then set the flag for bad (i.e. set z-axis to nan) @@ -289,7 +295,9 @@ bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_mapp_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl; + // std::stringstream ss; + // ss << feat->featid << " - max base " << (feat->p_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl; + // PRINT_DEBUG(ss.str().c_str()); // Check if this feature is bad or not // 1. If the feature is too close diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h index bb83f04a7..352292b71 100644 --- a/ov_core/src/feat/FeatureInitializerOptions.h +++ b/ov_core/src/feat/FeatureInitializerOptions.h @@ -22,6 +22,8 @@ #ifndef OV_CORE_INITIALIZEROPTIONS_H #define OV_CORE_INITIALIZEROPTIONS_H +#include "utils/print.h" + namespace ov_core { /** @@ -67,18 +69,18 @@ struct FeatureInitializerOptions { /// Nice print function of what parameters we have loaded void print() { - printf("\t- triangulate_1d: %d\n", triangulate_1d); - printf("\t- refine_features: %d\n", refine_features); - printf("\t- max_runs: %d\n", max_runs); - printf("\t- init_lamda: %.3f\n", init_lamda); - printf("\t- max_lamda: %.3f\n", max_lamda); - printf("\t- min_dx: %.7f\n", min_dx); - printf("\t- min_dcost: %.7f\n", min_dcost); - printf("\t- lam_mult: %.3f\n", lam_mult); - printf("\t- min_dist: %.3f\n", min_dist); - printf("\t- max_dist: %.3f\n", max_dist); - printf("\t- max_baseline: %.3f\n", max_baseline); - printf("\t- max_cond_number: %.3f\n", max_cond_number); + PRINT_INFO("\t- triangulate_1d: %d\n", triangulate_1d); + PRINT_INFO("\t- refine_features: %d\n", refine_features); + PRINT_INFO("\t- max_runs: %d\n", max_runs); + PRINT_INFO("\t- init_lamda: %.3f\n", init_lamda); + PRINT_INFO("\t- max_lamda: %.3f\n", max_lamda); + PRINT_INFO("\t- min_dx: %.7f\n", min_dx); + PRINT_INFO("\t- min_dcost: %.7f\n", min_dcost); + PRINT_INFO("\t- lam_mult: %.3f\n", lam_mult); + PRINT_INFO("\t- min_dist: %.3f\n", min_dist); + PRINT_INFO("\t- max_dist: %.3f\n", max_dist); + PRINT_INFO("\t- max_baseline: %.3f\n", max_baseline); + PRINT_INFO("\t- max_cond_number: %.3f\n", max_cond_number); } }; diff --git a/ov_core/src/init/InertialInitializer.cpp b/ov_core/src/init/InertialInitializer.cpp index e7e471510..7de4ed81a 100644 --- a/ov_core/src/init/InertialInitializer.cpp +++ b/ov_core/src/init/InertialInitializer.cpp @@ -20,6 +20,7 @@ */ #include "InertialInitializer.h" +#include "utils/print.h" using namespace ov_core; @@ -50,7 +51,7 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix _imu_excite_threshold && wait_for_jerk) { - printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold); + PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold); return false; } // If it is above the threshold and we are not waiting for a jerk // Then we are not stationary (i.e. moving) so we should wait till we are if ((a_var_1to0 > _imu_excite_threshold || a_var_2to1 > _imu_excite_threshold) && !wait_for_jerk) { - printf(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, - _imu_excite_threshold); + PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, + _imu_excite_threshold); return false; } diff --git a/ov_core/src/sim/BsplineSE3.cpp b/ov_core/src/sim/BsplineSE3.cpp index 9069bbb8d..b2978cdfe 100644 --- a/ov_core/src/sim/BsplineSE3.cpp +++ b/ov_core/src/sim/BsplineSE3.cpp @@ -20,6 +20,9 @@ */ #include "BsplineSE3.h" +#include "utils/print.h" + +#include using namespace ov_core; @@ -32,7 +35,7 @@ void BsplineSE3::feed_trajectory(std::vector traj_points) { } dt = sumdt / (traj_points.size() - 1); dt = (dt < 0.05) ? 0.05 : dt; - printf("[B-SPLINE]: control point dt = %.3f (original dt of %.3f)\n", dt, sumdt / (traj_points.size() - 1)); + PRINT_DEBUG("[B-SPLINE]: control point dt = %.3f (original dt of %.3f)\n", dt, sumdt / (traj_points.size() - 1)); // convert all our trajectory points into SE(3) matrices // we are given [timestamp, p_IinG, q_GtoI] @@ -55,8 +58,8 @@ void BsplineSE3::feed_trajectory(std::vector traj_points) { timestamp_max = pose.first; } } - printf("[B-SPLINE]: trajectory start time = %.6f\n", timestamp_min); - printf("[B-SPLINE]: trajectory end time = %.6f\n", timestamp_max); + PRINT_DEBUG("[B-SPLINE]: trajectory start time = %.6f\n", timestamp_min); + PRINT_DEBUG("[B-SPLINE]: trajectory end time = %.6f\n", timestamp_max); // then create spline control points double timestamp_curr = timestamp_min; @@ -66,7 +69,7 @@ void BsplineSE3::feed_trajectory(std::vector traj_points) { double t0, t1; Eigen::Matrix4d pose0, pose1; bool success = find_bounding_poses(timestamp_curr, trajectory_points, t0, pose0, t1, pose1); - // printf("[SIM]: time curr = %.6f | lambda = %.3f | dt = %.3f | dtmeas = + // PRINT_DEBUG("[SIM]: time curr = %.6f | lambda = %.3f | dt = %.3f | dtmeas = // %.3f\n",timestamp_curr,(timestamp_curr-t0)/(t1-t0),dt,(t1-t0)); // If we didn't find a bounding pose, then that means we are at the end of the dataset @@ -79,12 +82,14 @@ void BsplineSE3::feed_trajectory(std::vector traj_points) { Eigen::Matrix4d pose_interp = exp_se3(lambda * log_se3(pose1 * Inv_se3(pose0))) * pose0; control_points.insert({timestamp_curr, pose_interp}); timestamp_curr += dt; - // std::cout << pose_interp(0,3) << "," << pose_interp(1,3) << "," << pose_interp(2,3) << std::endl; + // std::stringstream ss; + // ss << pose_interp(0,3) << "," << pose_interp(1,3) << "," << pose_interp(2,3) << std::endl; + // PRINT_DEBUG(ss.str().c_str()); } // The start time of the system is two dt in since we need at least two older control points timestamp_start = timestamp_min + 2 * dt; - printf("[B-SPLINE]: start trajectory time of %.6f\n", timestamp_start); + PRINT_DEBUG("[B-SPLINE]: start trajectory time of %.6f\n", timestamp_start); } bool BsplineSE3::get_pose(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vector3d &p_IinG) { @@ -93,7 +98,7 @@ bool BsplineSE3::get_pose(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vect double t0, t1, t2, t3; Eigen::Matrix4d pose0, pose1, pose2, pose3; bool success = find_bounding_control_points(timestamp, control_points, t0, pose0, t1, pose1, t2, pose2, t3, pose3); - // printf("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success = + // PRINT_DEBUG("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success = // %d\n",timestamp,t0-timestamp,t1-timestamp,t2-timestamp,t3-timestamp,(int)success); // Return failure if we can't get bounding poses @@ -129,7 +134,7 @@ bool BsplineSE3::get_velocity(double timestamp, Eigen::Matrix3d &R_GtoI, Eigen:: double t0, t1, t2, t3; Eigen::Matrix4d pose0, pose1, pose2, pose3; bool success = find_bounding_control_points(timestamp, control_points, t0, pose0, t1, pose1, t2, pose2, t3, pose3); - // printf("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success = + // PRINT_DEBUG("[SIM]: time curr = %.6f | dt1 = %.3f | dt2 = %.3f | dt3 = %.3f | dt4 = %.3f | success = // %d\n",timestamp,t0-timestamp,t1-timestamp,t2-timestamp,t3-timestamp,(int)success); // Return failure if we can't get bounding poses diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index fc3f8b26f..75ecdb83e 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -38,6 +38,7 @@ #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" +#include "utils/print.h" using namespace ov_core; @@ -72,7 +73,7 @@ int main(int argc, char **argv) { std::string path_to_bag; nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); // nh.param("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag"); - printf("ros bag path is: %s\n", path_to_bag.c_str()); + PRINT_INFO("ros bag path is: %s\n", path_to_bag.c_str()); // Get our start location and how much of the bag we want to play // Make the bag duration < 0 to just process to the end of the bag @@ -101,13 +102,13 @@ int main(int argc, char **argv) { nh.param("use_stereo", use_stereo, false); // Debug print! - printf("max features: %d\n", num_pts); - printf("max aruco: %d\n", num_aruco); - printf("clone states: %d\n", clone_states); - printf("grid size: %d x %d\n", grid_x, grid_y); - printf("fast threshold: %d\n", fast_threshold); - printf("min pixel distance: %d\n", min_px_dist); - printf("downsize aruco image: %d\n", do_downsizing); + PRINT_DEBUG("max features: %d\n", num_pts); + PRINT_DEBUG("max aruco: %d\n", num_aruco); + PRINT_DEBUG("clone states: %d\n", clone_states); + PRINT_DEBUG("grid size: %d x %d\n", grid_x, grid_y); + PRINT_DEBUG("fast threshold: %d\n", fast_threshold); + PRINT_DEBUG("min pixel distance: %d\n", min_px_dist); + PRINT_DEBUG("downsize aruco image: %d\n", do_downsizing); // Fake camera info (we don't need this, as we are not using the normalized coordinates for anything) std::unordered_map> cameras; @@ -120,7 +121,7 @@ int main(int argc, char **argv) { } // Lets make a feature extractor - //extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); + // extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist, // knn_ratio); extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing); @@ -144,13 +145,13 @@ int main(int argc, char **argv) { ros::Time time_init = view_full.getBeginTime(); time_init += ros::Duration(bag_start); ros::Time time_finish = (bag_durr < 0) ? view_full.getEndTime() : time_init + ros::Duration(bag_durr); - printf("time start = %.6f\n", time_init.toSec()); - printf("time end = %.6f\n", time_finish.toSec()); + PRINT_DEBUG("time start = %.6f\n", time_init.toSec()); + PRINT_DEBUG("time end = %.6f\n", time_finish.toSec()); view.addQuery(bag, time_init, time_finish); // Check to make sure we have data to play if (view.size() == 0) { - printf(RED "No messages to play on specified topics. Exiting.\n" RESET); + PRINT_ERROR(RED "No messages to play on specified topics. Exiting.\n" RESET); ros::shutdown(); return EXIT_FAILURE; } @@ -180,7 +181,7 @@ int main(int argc, char **argv) { try { cv_ptr = cv_bridge::toCvShare(s0, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - printf(RED "cv_bridge exception: %s\n" RESET, e.what()); + PRINT_ERROR(RED "cv_bridge exception: %s\n" RESET, e.what()); continue; } // Save to our temp variable @@ -198,7 +199,7 @@ int main(int argc, char **argv) { try { cv_ptr = cv_bridge::toCvShare(s1, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - printf(RED "cv_bridge exception: %s\n" RESET, e.what()); + PRINT_ERROR(RED "cv_bridge exception: %s\n" RESET, e.what()); continue; } // Save to our temp variable @@ -313,7 +314,7 @@ void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1) { double fpf = (double)featslengths / num_lostfeats; double mpf = (double)num_margfeats / frames; // DEBUG PRINT OUT - printf("fps = %.2f | lost_feats/frame = %.2f | track_length/lost_feat = %.2f | marg_tracks/frame = %.2f\n", fps, lpf, fpf, mpf); + PRINT_DEBUG("fps = %.2f | lost_feats/frame = %.2f | track_length/lost_feat = %.2f | marg_tracks/frame = %.2f\n", fps, lpf, fpf, mpf); // Reset variables frames = 0; time_start = time_curr; diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 3f5032c11..00af5b0f1 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -38,6 +38,7 @@ #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" #include "utils/CLI11.hpp" +#include "utils/print.h" using namespace ov_core; @@ -82,13 +83,13 @@ int main(int argc, char **argv) { } // Debug print! - printf("max features: %d\n", num_pts); - printf("max aruco: %d\n", num_aruco); - printf("clone states: %d\n", clone_states); - printf("grid size: %d x %d\n", grid_x, grid_y); - printf("fast threshold: %d\n", fast_threshold); - printf("min pixel distance: %d\n", min_px_dist); - printf("downsize aruco image: %d\n", do_downsizing); + PRINT_DEBUG("max features: %d\n", num_pts); + PRINT_DEBUG("max aruco: %d\n", num_aruco); + PRINT_DEBUG("clone states: %d\n", clone_states); + PRINT_DEBUG("grid size: %d x %d\n", grid_x, grid_y); + PRINT_DEBUG("fast threshold: %d\n", fast_threshold); + PRINT_DEBUG("min pixel distance: %d\n", min_px_dist); + PRINT_DEBUG("downsize aruco image: %d\n", do_downsizing); // Fake camera info (we don't need this, as we are not using the normalized coordinates for anything) std::unordered_map> cameras; @@ -112,7 +113,7 @@ int main(int argc, char **argv) { // Open the first webcam (0=laptop cam, 1=usb device) cv::VideoCapture cap; if (!cap.open(0)) { - printf(RED "Unable to open a webcam feed!\n" RESET); + PRINT_ERROR(RED "Unable to open a webcam feed!\n" RESET); return EXIT_FAILURE; } diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp index a7285feac..4ed35ce1d 100644 --- a/ov_core/src/track/TrackAruco.cpp +++ b/ov_core/src/track/TrackAruco.cpp @@ -20,6 +20,7 @@ */ #include "TrackAruco.h" +#include "utils/print.h" using namespace ov_core; @@ -27,10 +28,10 @@ void TrackAruco::feed_new_camera(const CameraData &message) { // Error check that we have all the data if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) { - printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); - printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); - printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); - printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); + PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); + PRINT_ERROR(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); + PRINT_ERROR(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); + PRINT_ERROR(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); std::exit(EXIT_FAILURE); } @@ -44,8 +45,8 @@ void TrackAruco::feed_new_camera(const CameraData &message) { } })); #else - printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET); - std::exit(EXIT_FAILURE); + PRINT_ERROR(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET); + std::exit(EXIT_FAILURE); #endif } @@ -147,9 +148,9 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t rT3 = boost::posix_time::microsec_clock::local_time(); // Timing information - // printf("[TIME-ARUCO]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); - // printf("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, - // (int)good_left.size()); printf("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6, + // (int)good_left.size()); PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6); } void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2) { diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index fd99430c7..431d4c9e0 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -27,6 +27,7 @@ #endif #include "TrackBase.h" +#include "utils/print.h" namespace ov_core { @@ -57,7 +58,7 @@ class TrackAruco : public TrackBase { // NOTE: people with newer opencv might fail here // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX; #else - printf(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET); + PRINT_ERROR(RED "[ERROR]: you have not compiled with aruco tag support!!!\n" RESET); std::exit(EXIT_FAILURE); #endif } @@ -107,7 +108,6 @@ class TrackAruco : public TrackBase { std::unordered_map> ids_aruco; std::unordered_map>> corners, rejects; #endif - }; } // namespace ov_core diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp index 890faa23f..6857dbf6e 100644 --- a/ov_core/src/track/TrackDescriptor.cpp +++ b/ov_core/src/track/TrackDescriptor.cpp @@ -20,6 +20,7 @@ */ #include "TrackDescriptor.h" +#include "utils/print.h" using namespace ov_core; @@ -27,10 +28,10 @@ void TrackDescriptor::feed_new_camera(const CameraData &message) { // Error check that we have all the data if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) { - printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); - printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); - printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); - printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); + PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); + PRINT_ERROR(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); + PRINT_ERROR(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); + PRINT_ERROR(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); std::exit(EXIT_FAILURE); } @@ -48,7 +49,7 @@ void TrackDescriptor::feed_new_camera(const CameraData &message) { } })); } else { - printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images); + PRINT_ERROR(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images); std::exit(EXIT_FAILURE); } } @@ -141,7 +142,7 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) { } // Debug info - // printf("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast); + // PRINT_DEBUG("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast); // Move forward in time img_last[cam_id] = img; @@ -152,11 +153,11 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) { rT5 = boost::posix_time::microsec_clock::local_time(); // Our timing information - // printf("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size()); - // printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, + // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); } void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) { @@ -297,7 +298,7 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, } // Debug info - // printf("LtoL = %d | RtoR = %d | LtoR = %d | good = %d | fromlast = %d\n", (int)matches_ll.size(), + // PRINT_DEBUG("LtoL = %d | RtoR = %d | LtoR = %d | good = %d | fromlast = %d\n", (int)matches_ll.size(), // (int)matches_rr.size(),(int)ids_left_new.size(),(int)good_left.size(),num_tracklast); // Move forward in time @@ -314,11 +315,11 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, rT5 = boost::posix_time::microsec_clock::local_time(); // Our timing information - // printf("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6); - // printf("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size()); - // printf("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, + // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); } void TrackDescriptor::perform_detection_monocular(const cv::Mat &img0, const cv::Mat &mask0, std::vector &pts0, diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp index a262015c9..80c3d0b40 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -20,6 +20,7 @@ */ #include "TrackKLT.h" +#include "utils/print.h" using namespace ov_core; @@ -27,10 +28,10 @@ void TrackKLT::feed_new_camera(const CameraData &message) { // Error check that we have all the data if (message.sensor_ids.empty() || message.sensor_ids.size() != message.images.size() || message.images.size() != message.masks.size()) { - printf(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); - printf(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); - printf(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); - printf(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); + PRINT_ERROR(RED "[ERROR]: MESSAGE DATA SIZES DO NOT MATCH OR EMPTY!!!\n" RESET); + PRINT_ERROR(RED "[ERROR]: - message.sensor_ids.size() = %zu\n" RESET, message.sensor_ids.size()); + PRINT_ERROR(RED "[ERROR]: - message.images.size() = %zu\n" RESET, message.images.size()); + PRINT_ERROR(RED "[ERROR]: - message.masks.size() = %zu\n" RESET, message.masks.size()); std::exit(EXIT_FAILURE); } @@ -48,7 +49,7 @@ void TrackKLT::feed_new_camera(const CameraData &message) { } })); } else { - printf(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images); + PRINT_ERROR(RED "[ERROR]: invalid number of images passed %zu, we only support mono or stereo tracking", num_images); std::exit(EXIT_FAILURE); } } @@ -114,7 +115,7 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { img_mask_last[cam_id] = mask; pts_last[cam_id].clear(); ids_last[cam_id].clear(); - printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET); + PRINT_ERROR(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET); return; } @@ -154,11 +155,11 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { rT5 = boost::posix_time::microsec_clock::local_time(); // Timing information - // printf("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, (int)good_left.size()); - // printf("[TIME-KLT]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6, + // (int)good_left.size()); PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6); } void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) { @@ -265,7 +266,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t pts_last[cam_id_right].clear(); ids_last[cam_id_left].clear(); ids_last[cam_id_right].clear(); - printf(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET); + PRINT_ERROR(RED "[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....\n" RESET); return; } @@ -300,11 +301,11 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t good_right.push_back(pts_right_new.at(index_right)); good_ids_left.push_back(ids_last[cam_id_left].at(i)); good_ids_right.push_back(ids_last[cam_id_right].at(index_right)); - // std::cout << "adding to stereo - " << ids_last[cam_id_left].at(i) << " , " << ids_last[cam_id_right].at(index_right) << std::endl; + // PRINT_DEBUG("adding to stereo - %u , %u\n", ids_last[cam_id_left].at(i), ids_last[cam_id_right].at(index_right)); } else if (mask_ll[i]) { good_left.push_back(pts_left_new.at(i)); good_ids_left.push_back(ids_last[cam_id_left].at(i)); - // std::cout << "adding to left - " << ids_last[cam_id_left].at(i) << std::endl; + // PRINT_DEBUG("adding to left - %u \n", ids_last[cam_id_left].at(i)); } } @@ -320,7 +321,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t if (mask_rr[i] && !added_already) { good_right.push_back(pts_right_new.at(i)); good_ids_right.push_back(ids_last[cam_id_right].at(i)); - // std::cout << "adding to right - " << ids_last[cam_id_right].at(i) << std::endl; + // PRINT_DEBUG("adding to right - %u \n", ids_last[cam_id_right].at(i)); } } @@ -350,12 +351,12 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t rT6 = boost::posix_time::microsec_clock::local_time(); // Timing information - // printf("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for stereo klt\n",(rT5-rT4).total_microseconds() * 1e-6); - // printf("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT6-rT5).total_microseconds() * 1e-6, (int)good_left.size()); - // printf("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for stereo klt\n",(rT5-rT4).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT6-rT5).total_microseconds() * 1e-6, + // (int)good_left.size()); PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6); } void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, const cv::Mat &mask0, std::vector &pts0, diff --git a/ov_core/src/track/TrackSIM.h b/ov_core/src/track/TrackSIM.h index 9ddd61e06..6fcd4dfb5 100644 --- a/ov_core/src/track/TrackSIM.h +++ b/ov_core/src/track/TrackSIM.h @@ -23,6 +23,7 @@ #define OV_CORE_TRACK_SIM_H #include "TrackBase.h" +#include "utils/print.h" namespace ov_core { @@ -54,8 +55,8 @@ class TrackSIM : public TrackBase { * @param message Contains our timestamp, images, and camera ids */ void feed_new_camera(const CameraData &message) override { - printf(RED "[SIM]: SIM TRACKER FEED NEW CAMERA CALLED!!!\n" RESET); - printf(RED "[SIM]: THIS SHOULD NEVER HAPPEN!\n" RESET); + PRINT_ERROR(RED "[SIM]: SIM TRACKER FEED NEW CAMERA CALLED!!!\n" RESET); + PRINT_ERROR(RED "[SIM]: THIS SHOULD NEVER HAPPEN!\n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_core/src/types/Landmark.h b/ov_core/src/types/Landmark.h index 2f9aa963d..c6fff6d44 100644 --- a/ov_core/src/types/Landmark.h +++ b/ov_core/src/types/Landmark.h @@ -25,6 +25,7 @@ #include "LandmarkRepresentation.h" #include "Vec.h" #include "utils/colors.h" +#include "utils/print.h" namespace ov_type { @@ -79,7 +80,7 @@ class Landmark : public Vec { set_value(_value + dx); // Ensure we are not near zero in the z-direction if (LandmarkRepresentation::is_relative_representation(_feat_representation) && _value(_value.rows() - 1) < 1e-8) { - printf(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1)); + PRINT_WARNING(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1)); should_marg = true; } } diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h index 9e3da37a1..49b2bd749 100644 --- a/ov_core/src/utils/dataset_reader.h +++ b/ov_core/src/utils/dataset_reader.h @@ -29,6 +29,7 @@ #include #include "colors.h" +#include "print.h" using namespace std; @@ -70,8 +71,8 @@ class DatasetReader { // Check that it was successfull if (!file) { - printf(RED "ERROR: Unable to open groundtruth file...\n" RESET); - printf(RED "ERROR: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "ERROR: Unable to open groundtruth file...\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -89,8 +90,8 @@ class DatasetReader { while (getline(s, field, ',')) { // Ensure we are in the range if (i > 16) { - printf(RED "ERROR: Invalid groudtruth line, too long!\n" RESET); - printf(RED "ERROR: %s\n" RESET, line.c_str()); + PRINT_ERROR(RED "ERROR: Invalid groudtruth line, too long!\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, line.c_str()); std::exit(EXIT_FAILURE); } // Save our groundtruth state value @@ -115,7 +116,7 @@ class DatasetReader { // Check that we even have groundtruth loaded if (gt_states.empty()) { - printf(RED "Groundtruth data loaded is empty, make sure you call load before asking for a state.\n" RESET); + PRINT_ERROR(RED "Groundtruth data loaded is empty, make sure you call load before asking for a state.\n" RESET); return false; } @@ -131,14 +132,14 @@ class DatasetReader { // If close to this timestamp, then use it if (std::abs(closest_time - timestep) < 0.10) { - // printf("init DT = %.4f\n", std::abs(closest_time-timestep)); - // printf("timestamp = %.15f\n", closest_time); + // PRINT_DEBUG("init DT = %.4f\n", std::abs(closest_time-timestep)); + // PRINT_DEBUG("timestamp = %.15f\n", closest_time); timestep = closest_time; } // Check that we have the timestamp in our GT file if (gt_states.find(timestep) == gt_states.end()) { - printf(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep); + PRINT_INFO(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep); return false; } diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp index ae495476e..39592bbed 100644 --- a/ov_core/src/utils/print.cpp +++ b/ov_core/src/utils/print.cpp @@ -81,7 +81,4 @@ void Printer::debugPrint(PrintLevel level, const char location[], const char *fo va_start(args, format); vprintf(format, args); va_end(args); - - // All prints get a new line! - printf("\r\n"); } diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h index 209d06853..eef3d5ba5 100644 --- a/ov_core/src/utils/print.h +++ b/ov_core/src/utils/print.h @@ -68,10 +68,10 @@ class Printer { /** The different Types of print levels */ -#define PRINT_ALL(x...) Debug::debugPrint(Debug::PrintLevel::ALL, AT, x); -#define PRINT_DEBUG(x...) Debug::debugPrint(Debug::PrintLevel::DEBUG, AT, x); -#define PRINT_INFO(x...) Debug::debugPrint(Debug::PrintLevel::INFO, AT, x); -#define PRINT_WARNING(x...) Debug::debugPrint(Debug::PrintLevel::WARNING, AT, x); -#define PRINT_ERROR(x...) Debug::debugPrint(Debug::PrintLevel::ERROR, AT, x); +#define PRINT_ALL(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ALL, AT, x); +#define PRINT_DEBUG(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::DEBUG, AT, x); +#define PRINT_INFO(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::INFO, AT, x); +#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, AT, x); +#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, AT, x); #endif /* PRINT_H */ diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index a8989e22c..adfadc588 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -61,6 +61,8 @@ * */ +#include "utils/print.h" + #include #include #include @@ -91,26 +93,26 @@ inline Eigen::Matrix rot_2_quat(const Eigen::Matrix Eigen::Matrix q; double T = rot.trace(); if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) { - // cout << "case 1- " << endl; + // PRINT_DEBUG("case 1- \n"); q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4); q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0)); q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0)); q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1)); } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) { - // cout << "case 2- " << endl; + // PRINT_DEBUG("case 2- \n"); q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4); q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0)); q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1)); q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2)); } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) { - // cout << "case 3- " << endl; + // PRINT_DEBUG("case 3- \n"); q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4); q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0)); q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1)); q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0)); } else { - // cout << "case 4- " << endl; + // PRINT_DEBUG("case 4- \n"); q(3) = sqrt((1 + T) / 4); q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1)); q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2)); diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 3ceae0906..ac3a547ae 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.8.8) project(ov_eval) # Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs) +find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core) # Include libraries find_package(Eigen3 REQUIRED) @@ -29,9 +29,9 @@ option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_definitions(-DROS_AVAILABLE=1) catkin_package( - CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs + CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core INCLUDE_DIRS src - LIBRARIES ov_core_lib + LIBRARIES ov_eval_lib ) else() add_definitions(-DROS_AVAILABLE=0) diff --git a/ov_eval/package.xml b/ov_eval/package.xml index 2af2b4dc1..ebcb85ae9 100644 --- a/ov_eval/package.xml +++ b/ov_eval/package.xml @@ -26,6 +26,7 @@ geometry_msgs nav_msgs sensor_msgs + ov_core roscpp @@ -33,6 +34,7 @@ geometry_msgs nav_msgs sensor_msgs + ov_core \ No newline at end of file diff --git a/ov_eval/src/alignment/AlignTrajectory.cpp b/ov_eval/src/alignment/AlignTrajectory.cpp index 99c8d8126..fa4f75b48 100644 --- a/ov_eval/src/alignment/AlignTrajectory.cpp +++ b/ov_eval/src/alignment/AlignTrajectory.cpp @@ -20,6 +20,7 @@ */ #include "AlignTrajectory.h" +#include "utils/print.h" using namespace ov_eval; @@ -48,8 +49,8 @@ void AlignTrajectory::align_trajectory(const std::vector using namespace ov_eval; @@ -86,8 +89,10 @@ void AlignUtils::align_umeyama(const std::vector> &d t.noalias() = mu_M - s * R * mu_D; // Debug printing - // std::cout << "R- " << std::endl << R << std::endl; - // std::cout << "t- " << std::endl << t << std::endl; + // std::stringstream ss; + // ss << "R- " << std::endl << R << std::endl; + // ss << "t- " << std::endl << t << std::endl; + // PRINT_DEBUG(ss.str().c_str()); } void AlignUtils::perform_association(double offset, double max_difference, std::vector &est_times, std::vector >_times, @@ -166,13 +171,14 @@ void AlignUtils::perform_association(double offset, double max_difference, std:: // Ensure that we have enough associations if (est_times.size() < 3) { - printf(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET); - printf(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size()); - printf(RED "[ALIGN]: Do the time of the files match??\n" RESET); + PRINT_ERROR(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET); + PRINT_ERROR(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size()); + PRINT_ERROR(RED "[ALIGN]: Do the time of the files match??\n" RESET); return; } assert(est_times_temp.size() == gt_times_temp.size()); - // printf("[TRAJ]: %d est poses and %d gt poses => %d matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size()); + // PRINT_DEBUG("[TRAJ]: %d est poses and %d gt poses => %d + // matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size()); // Overwrite with intersected values est_times = est_times_temp; diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 4d5874af4..f94502659 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -20,6 +20,7 @@ */ #include "ResultSimulation.h" +#include "utils/print.h" using namespace ov_eval; @@ -35,8 +36,8 @@ ResultSimulation::ResultSimulation(std::string path_est, std::string path_std, s assert(est_state.size() == gt_state.size()); // Debug print - printf("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size()); - printf("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18)); + PRINT_DEBUG("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size()); + PRINT_DEBUG("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18)); } void ResultSimulation::plot_state(bool doplotting, double max_time) { @@ -213,7 +214,7 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) { // If we are not calibrating then don't plot it! if (state_cov.at(i)(16) == 0.0) { - printf(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET); + PRINT_WARNING(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET); return; } @@ -229,7 +230,7 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) { return; #ifndef HAVE_PYTHONLIBS - printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); + PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; #endif #ifdef HAVE_PYTHONLIBS @@ -279,7 +280,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { // Check that we have cameras if ((int)est_state.at(0)(18) < 1) { - printf(YELLOW "You need at least one camera to plot intrinsics...\n" RESET); + PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET); return; } @@ -308,7 +309,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { // If we are not calibrating then don't plot it! if (state_cov.at(i)(18) == 0.0) { - printf(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET); + PRINT_WARNING(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET); return; } @@ -330,7 +331,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { return; #ifndef HAVE_PYTHONLIBS - printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); + PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; #endif #ifdef HAVE_PYTHONLIBS @@ -392,7 +393,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { // Check that we have cameras if ((int)est_state.at(0)(18) < 1) { - printf(YELLOW "You need at least one camera to plot intrinsics...\n" RESET); + PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET); return; } @@ -421,7 +422,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { // If we are not calibrating then don't plot it! if (state_cov.at(i)(26) == 0.0) { - printf(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET); + PRINT_WARNING(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET); return; } @@ -449,7 +450,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { return; #ifndef HAVE_PYTHONLIBS - printf(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); + PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; #endif #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp index e0bb7b375..e5d83c127 100644 --- a/ov_eval/src/calc/ResultTrajectory.cpp +++ b/ov_eval/src/calc/ResultTrajectory.cpp @@ -20,6 +20,7 @@ */ #include "ResultTrajectory.h" +#include "utils/print.h" using namespace ov_eval; @@ -32,16 +33,16 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st // Debug print amount // std::string base_filename1 = path_est.substr(path_est.find_last_of("/\\") + 1); // std::string base_filename2 = path_gt.substr(path_gt.find_last_of("/\\") + 1); - // printf("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str()); - // printf("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str()); + // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str()); + // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str()); // Intersect timestamps AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos); // Return failure if we didn't have any common timestamps if (est_poses.size() < 3) { - printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); - printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); + PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); + PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); std::exit(EXIT_FAILURE); } @@ -55,9 +56,9 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st // Debug print to the user Eigen::Vector4d q_ESTtoGT = Math::rot_2_quat(R_ESTtoGT); Eigen::Vector4d q_GTtoEST = Math::rot_2_quat(R_GTtoEST); - printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2), - q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); - // printf("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s = + PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), + q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); + // PRINT_DEBUG("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s = // %.2f\n",q_GTtoEST(0),q_GTtoEST(1),q_GTtoEST(2),q_GTtoEST(3),t_GTinEST(0),t_GTinEST(1),t_GTinEST(2),s_GTtoEST); // Finally lets calculate the aligned trajectories @@ -147,10 +148,11 @@ void ResultTrajectory::calculate_rpe(const std::vector &segment_lengths, // Warn if large pos difference double max_dist_diff = 0.5; if (average_pos_difference > max_dist_diff) { - printf(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference, - max_dist_diff); - printf(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET); - printf(YELLOW "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET); + PRINT_WARNING(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference, + max_dist_diff); + PRINT_WARNING(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET); + PRINT_WARNING(YELLOW + "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET); } // Loop through each segment length @@ -238,8 +240,8 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos // Check that we have our covariance matrices to normalize with if (est_times.size() != est_covori.size() || est_times.size() != est_covpos.size() || gt_times.size() != gt_covori.size() || gt_times.size() != gt_covpos.size()) { - printf(YELLOW "[TRAJ]: Normalized Estimation Error Squared called but trajectory does not have any covariances...\n" RESET); - printf(YELLOW "[TRAJ]: Did you record using a Odometry or PoseWithCovarianceStamped????\n" RESET); + PRINT_WARNING(YELLOW "[TRAJ]: Normalized Estimation Error Squared called but trajectory does not have any covariances...\n" RESET); + PRINT_WARNING(YELLOW "[TRAJ]: Did you record using a Odometry or PoseWithCovarianceStamped????\n" RESET); return; } @@ -265,7 +267,7 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos // Skip if nan error value if (std::isnan(ori_nees) || std::isnan(pos_nees)) { - printf(YELLOW "[TRAJ]: nees calculation had nan number (covariance is wrong?) skipping...\n" RESET); + PRINT_WARNING(YELLOW "[TRAJ]: nees calculation had nan number (covariance is wrong?) skipping...\n" RESET); continue; } diff --git a/ov_eval/src/error_comparison.cpp b/ov_eval/src/error_comparison.cpp index e4656480f..cc7596fdf 100644 --- a/ov_eval/src/error_comparison.cpp +++ b/ov_eval/src/error_comparison.cpp @@ -28,6 +28,7 @@ #include "calc/ResultTrajectory.h" #include "utils/Colors.h" #include "utils/Loader.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -42,9 +43,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 4) { - printf(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); - printf(RED "ERROR: ./error_comparison \n" RESET); - printf(RED "ERROR: rosrun ov_eval error_comparison \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); + PRINT_ERROR(RED "ERROR: ./error_comparison \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_comparison \n" RESET); std::exit(EXIT_FAILURE); } @@ -67,7 +68,7 @@ int main(int argc, char **argv) { ov_eval::Loader::load_data(path_groundtruths.at(i).string(), times, poses, cov_ori, cov_pos); // Print its length and stats double length = ov_eval::Loader::get_total_length(poses); - printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_groundtruths.at(i).filename().c_str(), length); + PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_groundtruths.at(i).filename().c_str(), length); } // Get the algorithms we will process @@ -120,8 +121,8 @@ int main(int argc, char **argv) { for (size_t i = 0; i < path_algorithms.size(); i++) { // Debug print - printf("======================================\n"); - printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); // Get the list of datasets this algorithm records std::map path_algo_datasets; @@ -136,14 +137,14 @@ int main(int argc, char **argv) { // Check if we have runs for this dataset if (path_algo_datasets.find(path_groundtruths.at(j).stem().string()) == path_algo_datasets.end()) { - printf(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), - path_groundtruths.at(j).stem().c_str()); + PRINT_ERROR(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), + path_groundtruths.at(j).stem().c_str()); continue; } // Debug print - printf("[COMP]: processing %s algorithm => %s dataset\n", path_algorithms.at(i).filename().c_str(), - path_groundtruths.at(j).stem().c_str()); + PRINT_DEBUG("[COMP]: processing %s algorithm => %s dataset\n", path_algorithms.at(i).filename().c_str(), + path_groundtruths.at(j).stem().c_str()); // Errors for this specific dataset (i.e. our averages over the total runs) ov_eval::Statistics ate_dataset_ori; @@ -198,17 +199,17 @@ int main(int argc, char **argv) { // Print stats for this specific dataset std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : ""; - printf("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, - (int)ate_dataset_pos.values.size()); - printf("\tATE: std_ori = %.3f | std_pos = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std); + PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, + ate_dataset_pos.mean, (int)ate_dataset_pos.values.size()); + PRINT_DEBUG("\tATE: std_ori = %.3f | std_pos = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std); for (auto &seg : rpe_dataset) { seg.second.first.calculate(); seg.second.second.calculate(); - // printf("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d + // PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d // samples)\n",(int)seg.first,seg.second.first.mean,seg.second.second.mean,(int)seg.second.second.values.size()); - printf("\tRPE: seg %d - median_ori = %.4f | median_pos = %.4f (%d samples)\n", (int)seg.first, seg.second.first.median, - seg.second.second.median, (int)seg.second.second.values.size()); - // printf("RPE: seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); + PRINT_DEBUG("\tRPE: seg %d - median_ori = %.4f | median_pos = %.4f (%d samples)\n", (int)seg.first, seg.second.first.median, + seg.second.second.median, (int)seg.second.second.values.size()); + // PRINT_DEBUG("RPE: seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); } // Update the global ATE error stats @@ -235,56 +236,56 @@ int main(int argc, char **argv) { //=============================================================================== // Finally print the ATE for all the runs - printf("============================================\n"); - printf("ATE LATEX TABLE\n"); - printf("============================================\n"); + PRINT_DEBUG("============================================\n"); + PRINT_DEBUG("ATE LATEX TABLE\n"); + PRINT_DEBUG("============================================\n"); for (size_t i = 0; i < path_groundtruths.size(); i++) { std::string gtname = path_groundtruths.at(i).stem().string(); boost::replace_all(gtname, "_", "\\_"); - printf(" & \\textbf{%s}", gtname.c_str()); + PRINT_DEBUG(" & \\textbf{%s}", gtname.c_str()); } - printf(" & \\textbf{Average} \\\\\\hline\n"); + PRINT_DEBUG(" & \\textbf{Average} \\\\\\hline\n"); for (auto &algo : algo_ate) { std::string algoname = algo.first; boost::replace_all(algoname, "_", "\\_"); - cout << algoname; + PRINT_DEBUG(algoname.c_str()); double sum_ori = 0.0; double sum_pos = 0.0; int sum_ct = 0; for (auto &seg : algo.second) { if (seg.first.values.empty() || seg.second.values.empty()) { - printf(" & - / -"); + PRINT_DEBUG(" & - / -"); } else { - printf(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse); + PRINT_DEBUG(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse); sum_ori += seg.first.rmse; sum_pos += seg.second.rmse; sum_ct++; } } - printf(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct); + PRINT_DEBUG(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct); } - printf("============================================\n"); + PRINT_DEBUG("============================================\n"); // Finally print the RPE for all the runs - printf("============================================\n"); - printf("RPE LATEX TABLE\n"); - printf("============================================\n"); + PRINT_DEBUG("============================================\n"); + PRINT_DEBUG("RPE LATEX TABLE\n"); + PRINT_DEBUG("============================================\n"); for (const auto &len : segments) { - printf(" & \\textbf{%dm}", (int)len); + PRINT_DEBUG(" & \\textbf{%dm}", (int)len); } - printf(" \\\\\\hline\n"); + PRINT_DEBUG(" \\\\\\hline\n"); for (auto &algo : algo_rpe) { std::string algoname = algo.first; boost::replace_all(algoname, "_", "\\_"); - cout << algoname; + PRINT_DEBUG(algoname.c_str()); for (auto &seg : algo.second) { seg.second.first.calculate(); seg.second.second.calculate(); - printf(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median); + PRINT_DEBUG(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median); } - printf(" \\\\\n"); + PRINT_DEBUG(" \\\\\n"); } - printf("============================================\n"); + PRINT_DEBUG("============================================\n"); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/error_dataset.cpp b/ov_eval/src/error_dataset.cpp index 3065dfc39..62188b143 100644 --- a/ov_eval/src/error_dataset.cpp +++ b/ov_eval/src/error_dataset.cpp @@ -26,6 +26,7 @@ #include "calc/ResultTrajectory.h" #include "utils/Colors.h" #include "utils/Loader.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -40,9 +41,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 4) { - printf(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); - printf(RED "ERROR: ./error_dataset \n" RESET); - printf(RED "ERROR: rosrun ov_eval error_dataset \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); + PRINT_ERROR(RED "ERROR: ./error_dataset \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_dataset \n" RESET); std::exit(EXIT_FAILURE); } @@ -54,7 +55,7 @@ int main(int argc, char **argv) { ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos); // Print its length and stats double length = ov_eval::Loader::get_total_length(poses); - printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length); + PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length); // Get the algorithms we will process // Also create empty statistic objects for each of our datasets @@ -83,8 +84,8 @@ int main(int argc, char **argv) { for (size_t i = 0; i < path_algorithms.size(); i++) { // Debug print - printf("======================================\n"); - printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); // Get the list of datasets this algorithm records std::map path_algo_datasets; @@ -96,8 +97,8 @@ int main(int argc, char **argv) { // Check if we have runs for our dataset if (path_algo_datasets.find(path_gt.stem().string()) == path_algo_datasets.end()) { - printf(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), - path_gt.stem().c_str()); + PRINT_DEBUG(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), + path_gt.stem().c_str()); continue; } @@ -123,7 +124,7 @@ int main(int argc, char **argv) { // Check if we have runs if (file_paths.empty()) { - printf(RED "\tERROR: No runs found for %s, is the folder structure right??\n" RESET, path_algorithms.at(i).filename().c_str()); + PRINT_ERROR(RED "\tERROR: No runs found for %s, is the folder structure right??\n" RESET, path_algorithms.at(i).filename().c_str()); continue; } @@ -188,18 +189,18 @@ int main(int argc, char **argv) { // Print stats for this specific dataset std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : ""; - printf("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, - (int)ate_dataset_ori.values.size()); - printf("\tATE: std_ori = %.5f | std_pos = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std); - printf("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean, - (int)ate_2d_dataset_ori.values.size()); - printf("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std); + PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, + (int)ate_dataset_ori.values.size()); + PRINT_DEBUG("\tATE: std_ori = %.5f | std_pos = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std); + PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean, + (int)ate_2d_dataset_ori.values.size()); + PRINT_DEBUG("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std); for (auto &seg : rpe_dataset) { seg.second.first.calculate(); seg.second.second.calculate(); - printf("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean, - seg.second.second.mean, (int)seg.second.second.values.size()); - // printf("RPE: seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); + PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean, + seg.second.second.mean, (int)seg.second.second.values.size()); + // PRINT_DEBUG("RPE: seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); } // RMSE: Convert into the right format (only use times where all runs have an error) @@ -216,7 +217,7 @@ int main(int argc, char **argv) { } rmse_ori.calculate(); rmse_pos.calculate(); - printf("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean); + PRINT_DEBUG("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean); // RMSE: Convert into the right format (only use times where all runs have an error) ov_eval::Statistics rmse_2d_ori, rmse_2d_pos; @@ -232,7 +233,7 @@ int main(int argc, char **argv) { } rmse_2d_ori.calculate(); rmse_2d_pos.calculate(); - printf("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean); + PRINT_DEBUG("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean); // NEES: Convert into the right format (only use times where all runs have an error) ov_eval::Statistics nees_ori, nees_pos; @@ -248,7 +249,7 @@ int main(int argc, char **argv) { } nees_ori.calculate(); nees_pos.calculate(); - printf("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); + PRINT_DEBUG("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); #ifdef HAVE_PYTHONLIBS @@ -315,7 +316,7 @@ int main(int argc, char **argv) { } // Final line for our printed stats - printf("============================================\n"); + PRINT_DEBUG("============================================\n"); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 757a6e247..2f732dcd2 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -21,6 +21,7 @@ #include "calc/ResultSimulation.h" #include "utils/Colors.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -35,8 +36,8 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 4) { - printf(RED "ERROR: ./error_simulation \n" RESET); - printf(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); + PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation \n" RESET); std::exit(EXIT_FAILURE); } @@ -44,19 +45,19 @@ int main(int argc, char **argv) { ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]); // Plot the state errors - printf("Plotting state variable errors...\n"); + PRINT_INFO("Plotting state variable errors...\n"); traj.plot_state(true); // Plot time offset - printf("Plotting time offset error...\n"); + PRINT_INFO("Plotting time offset error...\n"); traj.plot_timeoff(true, 10); // Plot camera intrinsics - printf("Plotting camera intrinsics...\n"); + PRINT_INFO("Plotting camera intrinsics...\n"); traj.plot_cam_instrinsics(true, 60); // Plot camera extrinsics - printf("Plotting camera extrinsics...\n"); + PRINT_INFO("Plotting camera extrinsics...\n"); traj.plot_cam_extrinsics(true, 60); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/error_singlerun.cpp b/ov_eval/src/error_singlerun.cpp index 179bec23c..90699360d 100644 --- a/ov_eval/src/error_singlerun.cpp +++ b/ov_eval/src/error_singlerun.cpp @@ -25,6 +25,7 @@ #include "calc/ResultTrajectory.h" #include "utils/Colors.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -85,9 +86,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 4) { - printf(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET); - printf(RED "ERROR: ./error_singlerun \n" RESET); - printf(RED "ERROR: rosrun ov_eval error_singlerun \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET); + PRINT_ERROR(RED "ERROR: ./error_singlerun \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval error_singlerun \n" RESET); std::exit(EXIT_FAILURE); } @@ -99,7 +100,7 @@ int main(int argc, char **argv) { ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos); // Print its length and stats double length = ov_eval::Loader::get_total_length(poses); - printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length); + PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length); // Create our trajectory object ov_eval::ResultTrajectory traj(argv[3], argv[2], argv[1]); @@ -113,14 +114,14 @@ int main(int argc, char **argv) { traj.calculate_ate(error_ori, error_pos); // Print it - printf("======================================\n"); - printf("Absolute Trajectory Error\n"); - printf("======================================\n"); - printf("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse); - printf("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean); - printf("min_ori = %.3f | min_pos = %.3f\n", error_ori.min, error_pos.min); - printf("max_ori = %.3f | max_pos = %.3f\n", error_ori.max, error_pos.max); - printf("std_ori = %.3f | std_pos = %.3f\n", error_ori.std, error_pos.std); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("Absolute Trajectory Error\n"); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse); + PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean); + PRINT_DEBUG("min_ori = %.3f | min_pos = %.3f\n", error_ori.min, error_pos.min); + PRINT_DEBUG("max_ori = %.3f | max_pos = %.3f\n", error_ori.max, error_pos.max); + PRINT_DEBUG("std_ori = %.3f | std_pos = %.3f\n", error_ori.std, error_pos.std); //=========================================================== // Relative pose error @@ -132,13 +133,13 @@ int main(int argc, char **argv) { traj.calculate_rpe(segments, error_rpe); // Print it - printf("======================================\n"); - printf("Relative Pose Error\n"); - printf("======================================\n"); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("Relative Pose Error\n"); + PRINT_DEBUG("======================================\n"); for (const auto &seg : error_rpe) { - printf("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median, - seg.second.second.median, (int)seg.second.second.values.size()); - // printf("seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); + PRINT_DEBUG("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median, + seg.second.second.median, (int)seg.second.second.values.size()); + // PRINT_DEBUG("seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); } #ifdef HAVE_PYTHONLIBS @@ -198,14 +199,14 @@ int main(int argc, char **argv) { traj.calculate_nees(nees_ori, nees_pos); // Print it - printf("======================================\n"); - printf("Normalized Estimation Error Squared\n"); - printf("======================================\n"); - printf("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); - printf("min_ori = %.3f | min_pos = %.3f\n", nees_ori.min, nees_pos.min); - printf("max_ori = %.3f | max_pos = %.3f\n", nees_ori.max, nees_pos.max); - printf("std_ori = %.3f | std_pos = %.3f\n", nees_ori.std, nees_pos.std); - printf("======================================\n"); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("Normalized Estimation Error Squared\n"); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); + PRINT_DEBUG("min_ori = %.3f | min_pos = %.3f\n", nees_ori.min, nees_pos.min); + PRINT_DEBUG("max_ori = %.3f | max_pos = %.3f\n", nees_ori.max, nees_pos.max); + PRINT_DEBUG("std_ori = %.3f | std_pos = %.3f\n", nees_ori.std, nees_pos.std); + PRINT_DEBUG("======================================\n"); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/format_converter.cpp b/ov_eval/src/format_converter.cpp index 38f9451ee..f4bc52186 100644 --- a/ov_eval/src/format_converter.cpp +++ b/ov_eval/src/format_converter.cpp @@ -24,9 +24,11 @@ #include #include #include +#include #include #include "utils/Colors.h" +#include "utils/print.h" /** * Given a CSV file this will convert it to our text file format. @@ -37,12 +39,12 @@ void process_csv(std::string infile) { std::ifstream file1; std::string line; file1.open(infile); - printf("Opening file %s\n", boost::filesystem::path(infile).filename().c_str()); + PRINT_INFO("Opening file %s\n", boost::filesystem::path(infile).filename().c_str()); // Check that it was successful if (!file1) { - printf(RED "ERROR: Unable to open input file...\n" RESET); - printf(RED "ERROR: %s\n" RESET, infile.c_str()); + PRINT_ERROR(RED "ERROR: Unable to open input file...\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str()); std::exit(EXIT_FAILURE); } @@ -74,7 +76,9 @@ void process_csv(std::string infile) { // Only a valid line if we have all the parameters if (i > 7) { traj_data.push_back(data); - // std::cout << std::setprecision(5) << data.transpose() << std::endl; + // std::stringstream ss; + // ss << std::setprecision(5) << data.transpose() << std::endl; + // PRINT_DEBUG(ss.str().c_str()); } } @@ -83,17 +87,17 @@ void process_csv(std::string infile) { // Error if we don't have any data if (traj_data.empty()) { - printf(RED "ERROR: Could not parse any data from the file!!\n" RESET); - printf(RED "ERROR: %s\n" RESET, infile.c_str()); + PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str()); std::exit(EXIT_FAILURE); } - printf("\t- Loaded %d poses from file\n", (int)traj_data.size()); + PRINT_INFO("\t- Loaded %d poses from file\n", (int)traj_data.size()); // If file exists already then crash std::string outfile = infile.substr(0, infile.find_last_of('.')) + ".txt"; if (boost::filesystem::exists(outfile)) { - printf(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET); - printf(RED "\t- ERROR: %s\n" RESET, outfile.c_str()); + PRINT_ERROR(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET); + PRINT_ERROR(RED "\t- ERROR: %s\n" RESET, outfile.c_str()); return; } @@ -101,8 +105,8 @@ void process_csv(std::string infile) { std::ofstream file2; file2.open(outfile.c_str()); if (file2.fail()) { - printf(RED "ERROR: Unable to open output file!!\n" RESET); - printf(RED "ERROR: %s\n" RESET, outfile.c_str()); + PRINT_ERROR(RED "ERROR: Unable to open output file!!\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, outfile.c_str()); std::exit(EXIT_FAILURE); } file2 << "# timestamp(s) tx ty tz qx qy qz qw" << std::endl; @@ -116,7 +120,7 @@ void process_csv(std::string infile) { file2 << traj_data.at(i)(1) << " " << traj_data.at(i)(2) << " " << traj_data.at(i)(3) << " " << traj_data.at(i)(5) << " " << traj_data.at(i)(6) << " " << traj_data.at(i)(7) << " " << traj_data.at(i)(4) << std::endl; } - printf("\t- Saved to file %s\n", boost::filesystem::path(outfile).filename().c_str()); + PRINT_INFO("\t- Saved to file %s\n", boost::filesystem::path(outfile).filename().c_str()); // Finally close the file file2.close(); @@ -126,9 +130,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 2) { - printf(RED "ERROR: Please specify a file to convert\n" RESET); - printf(RED "ERROR: ./format_convert \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a file to convert\n" RESET); + PRINT_ERROR(RED "ERROR: ./format_convert \n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_eval/src/live_align_trajectory.cpp b/ov_eval/src/live_align_trajectory.cpp index 628793d67..44cf45917 100644 --- a/ov_eval/src/live_align_trajectory.cpp +++ b/ov_eval/src/live_align_trajectory.cpp @@ -28,6 +28,7 @@ #include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Math.h" +#include "utils/print.h" ros::Publisher pub_path; void align_and_publish(const nav_msgs::Path::ConstPtr &msg); @@ -93,8 +94,8 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) { // Return failure if we didn't have any common timestamps if (poses_temp.size() < 3) { - printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); - printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); + PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); + PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); return; } @@ -104,8 +105,8 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) { double s_ESTtoGT; ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type); Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT); - printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2), - q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); + PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), + q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); // Finally lets calculate the aligned trajectories // TODO: maybe in the future we could live publish trajectory errors... diff --git a/ov_eval/src/plot_trajectories.cpp b/ov_eval/src/plot_trajectories.cpp index ea49c690f..7e8ac673b 100644 --- a/ov_eval/src/plot_trajectories.cpp +++ b/ov_eval/src/plot_trajectories.cpp @@ -32,6 +32,7 @@ #include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Math.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -87,9 +88,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 3) { - printf(RED "ERROR: Please specify a align mode and trajectory file\n" RESET); - printf(RED "ERROR: ./plot_trajectories ... \n" RESET); - printf(RED "ERROR: rosrun ov_eval plot_trajectories ... \n" RESET); + PRINT_DEBUG(RED "ERROR: Please specify a align mode and trajectory file\n" RESET); + PRINT_DEBUG(RED "ERROR: ./plot_trajectories ... \n" RESET); + PRINT_DEBUG(RED "ERROR: rosrun ov_eval plot_trajectories ... \n" RESET); std::exit(EXIT_FAILURE); } @@ -115,8 +116,8 @@ int main(int argc, char **argv) { // Return failure if we didn't have any common timestamps if (poses_temp.size() < 3) { - printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); - printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); + PRINT_DEBUG(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); + PRINT_DEBUG(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); std::exit(EXIT_FAILURE); } @@ -128,8 +129,8 @@ int main(int argc, char **argv) { // Debug print to the user Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT); - printf("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), - q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); + PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), + q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); // Finally lets calculate the aligned trajectories std::vector> est_poses_aignedtoGT; @@ -148,7 +149,7 @@ int main(int argc, char **argv) { boost::filesystem::path path(argv[i]); std::string name = path.stem().string(); double length = ov_eval::Loader::get_total_length(poses_temp); - printf("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length); + PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length); // Save this to our arrays names.push_back(name); diff --git a/ov_eval/src/timing_comparison.cpp b/ov_eval/src/timing_comparison.cpp index 41bd29288..f5b161d84 100644 --- a/ov_eval/src/timing_comparison.cpp +++ b/ov_eval/src/timing_comparison.cpp @@ -30,6 +30,7 @@ #include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -44,9 +45,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 2) { - printf(RED "ERROR: Please specify a timing file\n" RESET); - printf(RED "ERROR: ./timing_comparison ... \n" RESET); - printf(RED "ERROR: rosrun ov_eval timing_comparison ... \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET); + PRINT_ERROR(RED "ERROR: ./timing_comparison ... \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_comparison ... \n" RESET); std::exit(EXIT_FAILURE); } @@ -58,15 +59,15 @@ int main(int argc, char **argv) { // Parse the name of this timing boost::filesystem::path path(argv[z]); std::string name = path.stem().string(); - printf("======================================\n"); - printf("[TIME]: loading data for %s\n", name.c_str()); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("[TIME]: loading data for %s\n", name.c_str()); // Load it!! std::vector names_temp; std::vector times; std::vector timing_values; ov_eval::Loader::load_timing_flamegraph(argv[z], names_temp, times, timing_values); - printf("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size()); + PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size()); // Our categories std::vector stats; @@ -84,8 +85,8 @@ int main(int argc, char **argv) { // Now print the statistic for this run for (size_t i = 0; i < names_temp.size(); i++) { stats.at(i).calculate(); - printf("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, stats.at(i).ninetynine, - stats.at(i).max, names_temp.at(i).c_str()); + PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, + stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str()); } // Append the total stats to the big vector @@ -93,9 +94,9 @@ int main(int argc, char **argv) { names.push_back(name); total_times.push_back(stats.at(stats.size() - 1)); } else { - printf(RED "[TIME]: unable to load any data.....\n" RESET); + PRINT_DEBUG(RED "[TIME]: unable to load any data.....\n" RESET); } - printf("======================================\n"); + PRINT_DEBUG("======================================\n"); } #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/timing_flamegraph.cpp b/ov_eval/src/timing_flamegraph.cpp index 4dea53d6a..f374cb7af 100644 --- a/ov_eval/src/timing_flamegraph.cpp +++ b/ov_eval/src/timing_flamegraph.cpp @@ -30,6 +30,7 @@ #include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -44,9 +45,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 2) { - printf(RED "ERROR: Please specify a timing file\n" RESET); - printf(RED "ERROR: ./timing_flamegraph \n" RESET); - printf(RED "ERROR: rosrun ov_eval timing_flamegraph \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET); + PRINT_ERROR(RED "ERROR: ./timing_flamegraph \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_flamegraph \n" RESET); std::exit(EXIT_FAILURE); } @@ -55,7 +56,7 @@ int main(int argc, char **argv) { std::vector times; std::vector timing_values; ov_eval::Loader::load_timing_flamegraph(argv[1], names, times, timing_values); - printf("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size()); + PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size()); // Our categories std::vector stats; @@ -73,8 +74,8 @@ int main(int argc, char **argv) { // Now print the statistic for this run for (size_t i = 0; i < names.size(); i++) { stats.at(i).calculate(); - printf("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, stats.at(i).ninetynine, - stats.at(i).max, names.at(i).c_str()); + PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, + stats.at(i).ninetynine, stats.at(i).max, names.at(i).c_str()); } #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/timing_percentages.cpp b/ov_eval/src/timing_percentages.cpp index 79dc8d8bb..e163350fc 100644 --- a/ov_eval/src/timing_percentages.cpp +++ b/ov_eval/src/timing_percentages.cpp @@ -30,6 +30,7 @@ #include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -44,9 +45,9 @@ int main(int argc, char **argv) { // Ensure we have a path if (argc < 2) { - printf(RED "ERROR: Please specify a timing and memory percent folder\n" RESET); - printf(RED "ERROR: ./timing_percentages \n" RESET); - printf(RED "ERROR: rosrun ov_eval timing_percentages \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a timing and memory percent folder\n" RESET); + PRINT_ERROR(RED "ERROR: ./timing_percentages \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_percentages \n" RESET); std::exit(EXIT_FAILURE); } @@ -76,8 +77,8 @@ int main(int argc, char **argv) { for (size_t i = 0; i < path_algorithms.size(); i++) { // Debug print - printf("======================================\n"); - printf("[COMP]: processing %s algorithm\n", path_algorithms.at(i).stem().c_str()); + PRINT_DEBUG("======================================\n"); + PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).stem().c_str()); // our total summed values std::vector total_times; @@ -117,14 +118,14 @@ int main(int argc, char **argv) { } // Display for the user - printf("\tloaded %d timestamps from file!!\n", (int)algo_timings.at(algo).at(0).timestamps.size()); + PRINT_DEBUG("\tloaded %d timestamps from file!!\n", (int)algo_timings.at(algo).at(0).timestamps.size()); algo_timings.at(algo).at(0).calculate(); algo_timings.at(algo).at(1).calculate(); algo_timings.at(algo).at(2).calculate(); - printf("\tPREC: mean_cpu = %.3f +- %.3f\n", algo_timings.at(algo).at(0).mean, algo_timings.at(algo).at(0).std); - printf("\tPREC: mean_mem = %.3f +- %.3f\n", algo_timings.at(algo).at(1).mean, algo_timings.at(algo).at(1).std); - printf("\tTHR: mean_threads = %.3f +- %.3f\n", algo_timings.at(algo).at(2).mean, algo_timings.at(algo).at(2).std); - printf("======================================\n"); + PRINT_DEBUG("\tPREC: mean_cpu = %.3f +- %.3f\n", algo_timings.at(algo).at(0).mean, algo_timings.at(algo).at(0).std); + PRINT_DEBUG("\tPREC: mean_mem = %.3f +- %.3f\n", algo_timings.at(algo).at(1).mean, algo_timings.at(algo).at(1).std); + PRINT_DEBUG("\tTHR: mean_threads = %.3f +- %.3f\n", algo_timings.at(algo).at(2).mean, algo_timings.at(algo).at(2).std); + PRINT_DEBUG("======================================\n"); } //=============================================================================== diff --git a/ov_eval/src/utils/Loader.cpp b/ov_eval/src/utils/Loader.cpp index ab45d21aa..9f7e0edf3 100644 --- a/ov_eval/src/utils/Loader.cpp +++ b/ov_eval/src/utils/Loader.cpp @@ -20,6 +20,7 @@ */ #include "Loader.h" +#include "utils/print.h" using namespace ov_eval; @@ -29,8 +30,8 @@ void Loader::load_data(std::string path_traj, std::vector ×, std::v // Try to open our trajectory file std::ifstream file(path_traj); if (!file.is_open()) { - printf(RED "[LOAD]: Unable to open trajectory file...\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } @@ -82,28 +83,28 @@ void Loader::load_data(std::string path_traj, std::vector ×, std::v // Error if we don't have any data if (times.empty()) { - printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } // Assert that they are all equal if (times.size() != poses.size()) { - printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } // Assert that they are all equal if (!cov_ori.empty() && (times.size() != cov_ori.size() || times.size() != cov_pos.size())) { - printf(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } // Debug print amount // std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1); - // printf("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str()); + // PRINT_DEBUG("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str()); } void Loader::load_data_csv(std::string path_traj, std::vector ×, std::vector> &poses, @@ -112,8 +113,8 @@ void Loader::load_data_csv(std::string path_traj, std::vector ×, st // Try to open our trajectory file std::ifstream file(path_traj); if (!file.is_open()) { - printf(RED "[LOAD]: Unable to open trajectory file...\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } @@ -163,15 +164,15 @@ void Loader::load_data_csv(std::string path_traj, std::vector ×, st // Error if we don't have any data if (times.empty()) { - printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } // Assert that they are all equal if (times.size() != poses.size()) { - printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } } @@ -181,8 +182,8 @@ void Loader::load_simulation(std::string path, std::vector &val // Try to open our trajectory file std::ifstream file(path); if (!file.is_open()) { - printf(RED "[LOAD]: Unable to open file...\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -221,8 +222,8 @@ void Loader::load_simulation(std::string path, std::vector &val // Error if we don't have any data if (values.empty()) { - printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -230,8 +231,8 @@ void Loader::load_simulation(std::string path, std::vector &val int rowsize = values.at(0).rows(); for (size_t i = 0; i < values.size(); i++) { if (values.at(i).rows() != rowsize) { - printf(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)values.at(i).rows(), rowsize); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)values.at(i).rows(), rowsize); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } } @@ -243,8 +244,8 @@ void Loader::load_timing_flamegraph(std::string path, std::vector & // Try to open our trajectory file std::ifstream file(path); if (!file.is_open()) { - printf(RED "[LOAD]: Unable to open file...\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -301,8 +302,8 @@ void Loader::load_timing_flamegraph(std::string path, std::vector & // Error if we don't have any data if (timing_values.empty()) { - printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -310,9 +311,9 @@ void Loader::load_timing_flamegraph(std::string path, std::vector & int rowsize = names.size(); for (size_t i = 0; i < timing_values.size(); i++) { if (timing_values.at(i).rows() != rowsize) { - printf(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)timing_values.at(i).rows(), - rowsize); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)timing_values.at(i).rows(), + rowsize); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } } @@ -324,8 +325,8 @@ void Loader::load_timing_percent(std::string path, std::vector ×, s // Try to open our trajectory file std::ifstream file(path); if (!file.is_open()) { - printf(RED "[LOAD]: Unable to open timing file...\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Unable to open timing file...\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } @@ -372,15 +373,15 @@ void Loader::load_timing_percent(std::string path, std::vector ×, s // Error if we don't have any data if (times.empty()) { - printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } // Assert that they are all equal if (times.size() != summed_values.size() || times.size() != node_values.size()) { - printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); - printf(RED "[LOAD]: %s\n" RESET, path.c_str()); + PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET); + PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str()); std::exit(EXIT_FAILURE); } } diff --git a/ov_eval/src/utils/Math.h b/ov_eval/src/utils/Math.h index 6cc8fc81f..e37439a2d 100644 --- a/ov_eval/src/utils/Math.h +++ b/ov_eval/src/utils/Math.h @@ -27,6 +27,8 @@ #include #include +#include "utils/print.h" + using namespace std; namespace ov_eval { @@ -65,26 +67,26 @@ class Math { Eigen::Matrix q; double T = rot.trace(); if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) { - // cout << "case 1- " << endl; + // PRINT_DEBUG(("case 1- \n"); q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4); q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0)); q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0)); q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1)); } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) { - // cout << "case 2- " << endl; + // PRINT_DEBUG(("case 2- \n"); q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4); q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0)); q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1)); q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2)); } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) { - // cout << "case 3- " << endl; + // PRINT_DEBUG(("case 3- \n"); q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4); q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0)); q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1)); q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0)); } else { - // cout << "case 4- " << endl; + // PRINT_DEBUG(("case 4- \n"); q(3) = sqrt((1 + T) / 4); q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1)); q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2)); diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index e13524aad..000988c6f 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -20,6 +20,7 @@ */ #include "RosVisualizer.h" +#include "utils/print.h" using namespace ov_msckf; @@ -72,7 +73,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap if (nh.hasParam("path_gt") && _sim == nullptr) { std::string path_to_gt; nh.param("path_gt", path_to_gt, ""); - if(!path_to_gt.empty()) { + if (!path_to_gt.empty()) { DatasetReader::load_gt_file(path_to_gt, gt_states); ROS_INFO("gt file path is: %s", path_to_gt.c_str()); } @@ -155,7 +156,7 @@ void RosVisualizer::visualize() { // Print how much time it took to publish / displaying things rT0_2 = boost::posix_time::microsec_clock::local_time(); double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; - printf(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); } void RosVisualizer::visualize_odometry(double timestamp) { @@ -231,16 +232,16 @@ void RosVisualizer::visualize_final() { // Final time offset value if (_app->get_state()->_options.do_calib_camera_timeoffset) { - printf(REDPURPLE "camera-imu timeoffset = %.5f\n\n" RESET, _app->get_state()->_calib_dt_CAMtoIMU->value()(0)); + PRINT_INFO(REDPURPLE "camera-imu timeoffset = %.5f\n\n" RESET, _app->get_state()->_calib_dt_CAMtoIMU->value()(0)); } // Final camera intrinsics if (_app->get_state()->_options.do_calib_camera_intrinsics) { for (int i = 0; i < _app->get_state()->_options.num_cameras; i++) { std::shared_ptr calib = _app->get_state()->_cam_intrinsics.at(i); - printf(REDPURPLE "cam%d intrinsics:\n" RESET, (int)i); - printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n" RESET, calib->value()(0), calib->value()(1), calib->value()(2), calib->value()(3)); - printf(REDPURPLE "%.5f,%.5f,%.5f,%.5f\n\n" RESET, calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7)); + PRINT_INFO(REDPURPLE "cam%d intrinsics:\n" RESET, (int)i); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n" RESET, calib->value()(0), calib->value()(1), calib->value()(2), calib->value()(3)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,%.5f\n\n" RESET, calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7)); } } @@ -251,31 +252,31 @@ void RosVisualizer::visualize_final() { Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(calib->quat()).transpose(); T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * calib->pos(); - printf(REDPURPLE "T_C%dtoI:\n" RESET, i); - printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(0, 0), T_CtoI(0, 1), T_CtoI(0, 2), T_CtoI(0, 3)); - printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(1, 0), T_CtoI(1, 1), T_CtoI(1, 2), T_CtoI(1, 3)); - printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(2, 0), T_CtoI(2, 1), T_CtoI(2, 2), T_CtoI(2, 3)); - printf(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n\n" RESET, T_CtoI(3, 0), T_CtoI(3, 1), T_CtoI(3, 2), T_CtoI(3, 3)); + PRINT_INFO(REDPURPLE "T_C%dtoI:\n" RESET, i); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(0, 0), T_CtoI(0, 1), T_CtoI(0, 2), T_CtoI(0, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(1, 0), T_CtoI(1, 1), T_CtoI(1, 2), T_CtoI(1, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(2, 0), T_CtoI(2, 1), T_CtoI(2, 2), T_CtoI(2, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n\n" RESET, T_CtoI(3, 0), T_CtoI(3, 1), T_CtoI(3, 2), T_CtoI(3, 3)); } } // Publish RMSE if we have it if (!gt_states.empty()) { - printf(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); - printf(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); } // Publish RMSE and NEES if doing simulation if (_sim != nullptr) { - printf(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); - printf(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); - printf(REDPURPLE "NEES average: %.3f (deg) orientation\n" RESET, summed_nees_ori / summed_number); - printf(REDPURPLE "NEES average: %.3f (m) position\n\n" RESET, summed_nees_pos / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); + PRINT_INFO(REDPURPLE "NEES average: %.3f (deg) orientation\n" RESET, summed_nees_ori / summed_number); + PRINT_INFO(REDPURPLE "NEES average: %.3f (m) position\n\n" RESET, summed_nees_pos / summed_number); } // Print the total time rT2 = boost::posix_time::microsec_clock::local_time(); - printf(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); + PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); } void RosVisualizer::publish_state() { @@ -667,10 +668,10 @@ void RosVisualizer::publish_groundtruth() { } // Nice display for the user - printf(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | average error => %.3f, %.3f (deg,m) | called %d times\n" RESET, rmse_ori, rmse_pos, - summed_rmse_ori / summed_number, summed_rmse_pos / summed_number, (int)summed_number); - printf(REDPURPLE "nees => %.1f, %.1f (ori,pos) | average nees = %.1f, %.1f (ori,pos)\n" RESET, ori_nees, pos_nees, - summed_nees_ori / summed_number, summed_nees_pos / summed_number); + PRINT_INFO(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | average error => %.3f, %.3f (deg,m) | called %d times\n" RESET, rmse_ori, + rmse_pos, summed_rmse_ori / summed_number, summed_rmse_pos / summed_number, (int)summed_number); + PRINT_INFO(REDPURPLE "nees => %.1f, %.1f (ori,pos) | average nees = %.1f, %.1f (ori,pos)\n" RESET, ori_nees, pos_nees, + summed_nees_ori / summed_number, summed_nees_pos / summed_number); //========================================================================== //========================================================================== diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index f3c2a202c..abfe7e68c 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -20,9 +20,11 @@ */ #include "VioManager.h" - #include "types/Landmark.h" +#include "utils/print.h" + #include +#include using namespace ov_core; using namespace ov_type; @@ -31,9 +33,9 @@ using namespace ov_msckf; VioManager::VioManager(VioManagerOptions ¶ms_) { // Nice startup message - printf("=======================================\n"); - printf("OPENVINS ON-MANIFOLD EKF IS STARTING\n"); - printf("=======================================\n"); + PRINT_DEBUG("=======================================\n"); + PRINT_DEBUG("OPENVINS ON-MANIFOLD EKF IS STARTING\n"); + PRINT_DEBUG("=======================================\n"); // Nice debug this->params = params_; @@ -87,7 +89,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // If the file exists, then delete it if (boost::filesystem::exists(params.record_timing_filepath)) { boost::filesystem::remove(params.record_timing_filepath); - printf(YELLOW "[STATS]: found old file found, deleted...\n" RESET); + PRINT_INFO(YELLOW "[STATS]: found old file found, deleted...\n" RESET); } // Create the directory that we will open the file in boost::filesystem::path p(params.record_timing_filepath); @@ -196,7 +198,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector // Replace with the simulated tracker trackSIM = std::make_shared(state->_cam_intrinsics_cameras, state->_options.max_aruco_features); trackFEATS = trackSIM; - printf(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET); + PRINT_WARNING(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET); } trackSIM->set_width_height(params.camera_wh); @@ -240,8 +242,8 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector // If we do not have VIO initialization, then return an error if (!is_initialized_vio) { - printf(RED "[SIM]: your vio system should already be initialized before simulating features!!!\n" RESET); - printf(RED "[SIM]: initialize your system first before calling feed_measurement_simulation()!!!!\n" RESET); + PRINT_ERROR(RED "[SIM]: your vio system should already be initialized before simulating features!!!\n" RESET); + PRINT_ERROR(RED "[SIM]: initialize your system first before calling feed_measurement_simulation()!!!!\n" RESET); std::exit(EXIT_FAILURE); } @@ -355,7 +357,8 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // Return if the camera measurement is out of order if (state->_timestamp > message.timestamp) { - printf(YELLOW "image received out of order, unable to do anything (prop dt = %3f)\n" RESET, (message.timestamp - state->_timestamp)); + PRINT_WARNING(YELLOW "image received out of order, unable to do anything (prop dt = %3f)\n" RESET, + (message.timestamp - state->_timestamp)); return; } @@ -372,14 +375,15 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // This isn't super ideal, but it keeps the logic after this easier... // We can start processing things when we have at least 5 clones since we can start triangulating things... if ((int)state->_clones_IMU.size() < std::min(state->_options.max_clone_size, 5)) { - printf("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(), std::min(state->_options.max_clone_size, 5)); + PRINT_INFO("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(), + std::min(state->_options.max_clone_size, 5)); return; } // Return if we where unable to propagate if (state->_timestamp != message.timestamp) { - printf(RED "[PROP]: Propagator unable to propagate the state forward in time!\n" RESET); - printf(RED "[PROP]: It has been %.3f since last time we propagated\n" RESET, message.timestamp - state->_timestamp); + PRINT_WARNING(RED "[PROP]: Propagator unable to propagate the state forward in time!\n" RESET); + PRINT_WARNING(RED "[PROP]: It has been %.3f since last time we propagated\n" RESET, message.timestamp - state->_timestamp); return; } has_moved_since_zupt = true; @@ -425,7 +429,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) it1 = feats_lost.begin(); while (it1 != feats_lost.end()) { if (std::find(feats_marg.begin(), feats_marg.end(), (*it1)) != feats_marg.end()) { - // printf(YELLOW "FOUND FEATURE THAT WAS IN BOTH feats_lost and feats_marg!!!!!!\n" RESET); + // PRINT_WARNING(YELLOW "FOUND FEATURE THAT WAS IN BOTH feats_lost and feats_marg!!!!!!\n" RESET); it1 = feats_lost.erase(it1); } else { it1++; @@ -507,11 +511,11 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) for (size_t i = 0; i < feats_slam.size(); i++) { if (state->_features_SLAM.find(feats_slam.at(i)->featid) != state->_features_SLAM.end()) { feats_slam_UPDATE.push_back(feats_slam.at(i)); - // printf("[UPDATE-SLAM]: found old feature %d (%d + // PRINT_DEBUG("[UPDATE-SLAM]: found old feature %d (%d // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size()); } else { feats_slam_DELAYED.push_back(feats_slam.at(i)); - // printf("[UPDATE-SLAM]: new feature ready %d (%d + // PRINT_DEBUG("[UPDATE-SLAM]: new feature ready %d (%d // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size()); } } @@ -630,19 +634,22 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) double time_total = (rT7 - rT1).total_microseconds() * 1e-6; // Timing information - printf(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track); - printf(BLUE "[TIME]: %.4f seconds for propagation\n" RESET, time_prop); - printf(BLUE "[TIME]: %.4f seconds for MSCKF update (%d feats)\n" RESET, time_msckf, (int)featsup_MSCKF.size()); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for propagation\n" RESET, time_prop); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for MSCKF update (%d feats)\n" RESET, time_msckf, (int)featsup_MSCKF.size()); if (state->_options.max_slam_features > 0) { - printf(BLUE "[TIME]: %.4f seconds for SLAM update (%d feats)\n" RESET, time_slam_update, (int)state->_features_SLAM.size()); - printf(BLUE "[TIME]: %.4f seconds for SLAM delayed init (%d feats)\n" RESET, time_slam_delay, (int)feats_slam_DELAYED.size()); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for SLAM update (%d feats)\n" RESET, time_slam_update, (int)state->_features_SLAM.size()); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for SLAM delayed init (%d feats)\n" RESET, time_slam_delay, (int)feats_slam_DELAYED.size()); } - printf(BLUE "[TIME]: %.4f seconds for re-tri & marg (%d clones in state)\n" RESET, time_marg, (int)state->_clones_IMU.size()); - printf(BLUE "[TIME]: %.4f seconds for total (camera" RESET, time_total); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for re-tri & marg (%d clones in state)\n" RESET, time_marg, (int)state->_clones_IMU.size()); + + std::stringstream ss; + ss << "[TIME]: " << std::setprecision(4) << time_total << " seconds for total (camera"; for (const auto &id : message.sensor_ids) { - printf(BLUE " %d", id); + ss << " " << id; } - printf(")\n" RESET); + ss << ")" << std::endl; + PRINT_DEBUG(BLUE "%s" RESET, ss.str().c_str()); // Finally if we are saving stats to file, lets save it to file if (params.record_timing_information && of_statistics.is_open()) { @@ -668,22 +675,23 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) timelastupdate = message.timestamp; // Debug, print our current state - printf("q_GtoI = %.3f,%.3f,%.3f,%.3f | p_IinG = %.3f,%.3f,%.3f | dist = %.2f (meters)\n", state->_imu->quat()(0), state->_imu->quat()(1), - state->_imu->quat()(2), state->_imu->quat()(3), state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2), distance); - printf("bg = %.4f,%.4f,%.4f | ba = %.4f,%.4f,%.4f\n", state->_imu->bias_g()(0), state->_imu->bias_g()(1), state->_imu->bias_g()(2), - state->_imu->bias_a()(0), state->_imu->bias_a()(1), state->_imu->bias_a()(2)); + PRINT_INFO("q_GtoI = %.3f,%.3f,%.3f,%.3f | p_IinG = %.3f,%.3f,%.3f | dist = %.2f (meters)\n", state->_imu->quat()(0), + state->_imu->quat()(1), state->_imu->quat()(2), state->_imu->quat()(3), state->_imu->pos()(0), state->_imu->pos()(1), + state->_imu->pos()(2), distance); + PRINT_INFO("bg = %.4f,%.4f,%.4f | ba = %.4f,%.4f,%.4f\n", state->_imu->bias_g()(0), state->_imu->bias_g()(1), state->_imu->bias_g()(2), + state->_imu->bias_a()(0), state->_imu->bias_a()(1), state->_imu->bias_a()(2)); // Debug for camera imu offset if (state->_options.do_calib_camera_timeoffset) { - printf("camera-imu timeoffset = %.5f\n", state->_calib_dt_CAMtoIMU->value()(0)); + PRINT_INFO("camera-imu timeoffset = %.5f\n", state->_calib_dt_CAMtoIMU->value()(0)); } // Debug for camera intrinsics if (state->_options.do_calib_camera_intrinsics) { for (int i = 0; i < state->_options.num_cameras; i++) { std::shared_ptr calib = state->_cam_intrinsics.at(i); - printf("cam%d intrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f,%.3f\n", (int)i, calib->value()(0), calib->value()(1), - calib->value()(2), calib->value()(3), calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7)); + PRINT_INFO("cam%d intrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f,%.3f\n", (int)i, calib->value()(0), calib->value()(1), + calib->value()(2), calib->value()(3), calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7)); } } @@ -691,8 +699,8 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) if (state->_options.do_calib_camera_pose) { for (int i = 0; i < state->_options.num_cameras; i++) { std::shared_ptr calib = state->_calib_IMUtoCAM.at(i); - printf("cam%d extrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f\n", (int)i, calib->quat()(0), calib->quat()(1), calib->quat()(2), - calib->quat()(3), calib->pos()(0), calib->pos()(1), calib->pos()(2)); + PRINT_INFO("cam%d extrinsics = %.3f,%.3f,%.3f,%.3f | %.3f,%.3f,%.3f\n", (int)i, calib->quat()(0), calib->quat()(1), calib->quat()(2), + calib->quat()(3), calib->pos()(0), calib->pos()(1), calib->pos()(2)); } } } @@ -740,14 +748,14 @@ bool VioManager::try_to_initialize() { } // Else we are good to go, print out our stats - printf(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), - state->_imu->quat()(2), state->_imu->quat()(3)); - printf(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), - state->_imu->bias_g()(2)); - printf(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); - printf(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), - state->_imu->bias_a()(2)); - printf(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); + PRINT_INFO(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), + state->_imu->quat()(2), state->_imu->quat()(3)); + PRINT_INFO(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), + state->_imu->bias_g()(2)); + PRINT_INFO(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); + PRINT_INFO(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), + state->_imu->bias_a()(2)); + PRINT_INFO(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); return true; } @@ -905,7 +913,7 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) // Skip if not valid (i.e. negative depth, or outside of image) if (uv_dist(0) < 0 || (int)uv_dist(0) >= params.camera_wh.at(0).first || uv_dist(1) < 0 || (int)uv_dist(1) >= params.camera_wh.at(0).second) { - // printf("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1)); + // PRINT_DEBUG("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1)); continue; } @@ -917,9 +925,9 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) retri_rT5 = boost::posix_time::microsec_clock::local_time(); // Timing information - // printf(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6); - // printf(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6); - // printf(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6); - // printf(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6); - // printf(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6); } diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 43702c9d9..d8e33bb2b 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -40,6 +40,7 @@ #include "types/Landmark.h" #include "types/LandmarkRepresentation.h" #include "utils/lambda_body.h" +#include "utils/print.h" #include "utils/sensor_data.h" #include "state/Propagator.h" @@ -116,15 +117,15 @@ class VioManager { } // Print what we init'ed with - printf(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET); - printf(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), - state->_imu->quat()(2), state->_imu->quat()(3)); - printf(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), - state->_imu->bias_g()(2)); - printf(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); - printf(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), - state->_imu->bias_a()(2)); - printf(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); + PRINT_DEBUG(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET); + PRINT_DEBUG(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), + state->_imu->quat()(2), state->_imu->quat()(3)); + PRINT_DEBUG(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), + state->_imu->bias_g()(2)); + PRINT_DEBUG(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); + PRINT_DEBUG(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), + state->_imu->bias_a()(2)); + PRINT_DEBUG(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); } /// If we are initialized or not diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 70d336f0b..e009d5ff3 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -33,6 +34,7 @@ #include "track/TrackBase.h" #include "update/UpdaterOptions.h" #include "utils/colors.h" +#include "utils/print.h" #include "utils/quat_ops.h" using namespace std; @@ -89,18 +91,18 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_estimator() { - printf("ESTIMATOR PARAMETERS:\n"); + PRINT_INFO("ESTIMATOR PARAMETERS:\n"); state_options.print(); - printf("\t- dt_slam_delay: %.1f\n", dt_slam_delay); - printf("\t- init_window_time: %.2f\n", init_window_time); - printf("\t- init_imu_thresh: %.2f\n", init_imu_thresh); - printf("\t- zero_velocity_update: %d\n", try_zupt); - printf("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity); - printf("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); - printf("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity); - printf("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); - printf("\t- record timing?: %d\n", (int)record_timing_information); - printf("\t- record timing filepath: %s\n", record_timing_filepath.c_str()); + PRINT_INFO("\t- dt_slam_delay: %.1f\n", dt_slam_delay); + PRINT_INFO("\t- init_window_time: %.2f\n", init_window_time); + PRINT_INFO("\t- init_imu_thresh: %.2f\n", init_imu_thresh); + PRINT_INFO("\t- zero_velocity_update: %d\n", try_zupt); + PRINT_INFO("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity); + PRINT_INFO("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); + PRINT_INFO("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity); + PRINT_INFO("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); + PRINT_INFO("\t- record timing?: %d\n", (int)record_timing_information); + PRINT_INFO("\t- record timing filepath: %s\n", record_timing_filepath.c_str()); } // NOISE / CHI2 ============================ @@ -125,15 +127,15 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_noise() { - printf("NOISE PARAMETERS:\n"); + PRINT_INFO("NOISE PARAMETERS:\n"); imu_noises.print(); - printf("\tUpdater MSCKF Feats:\n"); + PRINT_INFO("\tUpdater MSCKF Feats:\n"); msckf_options.print(); - printf("\tUpdater SLAM Feats:\n"); + PRINT_INFO("\tUpdater SLAM Feats:\n"); slam_options.print(); - printf("\tUpdater ARUCO Tags:\n"); + PRINT_INFO("\tUpdater ARUCO Tags:\n"); aruco_options.print(); - printf("\tUpdater ZUPT:\n"); + PRINT_INFO("\tUpdater ZUPT:\n"); zupt_options.print(); } @@ -162,22 +164,24 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_state() { - printf("STATE PARAMETERS:\n"); - printf("\t- gravity_mag: %.4f\n", gravity_mag); - printf("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); - printf("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt); + PRINT_INFO("STATE PARAMETERS:\n"); + PRINT_INFO("\t- gravity_mag: %.4f\n", gravity_mag); + PRINT_INFO("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); + PRINT_INFO("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt); assert(state_options.num_cameras == (int)camera_fisheye.size()); for (int n = 0; n < state_options.num_cameras; n++) { - std::cout << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; - std::cout << "cam_" << n << "_wh:" << endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; - std::cout << "cam_" << n << "_intrinsic(0:3):" << endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; - std::cout << "cam_" << n << "_intrinsic(4:7):" << endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; - std::cout << "cam_" << n << "_extrinsic(0:3):" << endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; - std::cout << "cam_" << n << "_extrinsic(4:6):" << endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; + std::stringstream ss; + ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; + ss << "cam_" << n << "_wh:" << endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; + ss << "cam_" << n << "_intrinsic(0:3):" << endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_intrinsic(4:7):" << endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_extrinsic(0:3):" << endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_extrinsic(4:6):" << endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose(); T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1); - std::cout << "T_C" << n << "toI:" << endl << T_CtoI << std::endl << std::endl; + ss << "T_C" << n << "toI:" << endl << T_CtoI << std::endl << std::endl; + PRINT_INFO(ss.str().c_str()); } } @@ -235,20 +239,20 @@ struct VioManagerOptions { * @brief This function will print out all parameters releated to our visual trackers. */ void print_trackers() { - printf("FEATURE TRACKING PARAMETERS:\n"); - printf("\t- use_klt: %d\n", use_klt); - printf("\t- use_stereo: %d\n", use_stereo); - printf("\t- use_aruco: %d\n", use_aruco); - printf("\t- downsize aruco: %d\n", downsize_aruco); - printf("\t- downsize cameras: %d\n", downsample_cameras); - printf("\t- use multi-threading: %d\n", use_multi_threading); - printf("\t- num_pts: %d\n", num_pts); - printf("\t- fast threshold: %d\n", fast_threshold); - printf("\t- grid X by Y: %d by %d\n", grid_x, grid_y); - printf("\t- min px dist: %d\n", min_px_dist); - printf("\t- hist method: %d\n", (int)histogram_method); - printf("\t- knn ratio: %.3f\n", knn_ratio); - printf("\t- use mask?: %d\n", use_mask); + PRINT_INFO("FEATURE TRACKING PARAMETERS:\n"); + PRINT_INFO("\t- use_klt: %d\n", use_klt); + PRINT_INFO("\t- use_stereo: %d\n", use_stereo); + PRINT_INFO("\t- use_aruco: %d\n", use_aruco); + PRINT_INFO("\t- downsize aruco: %d\n", downsize_aruco); + PRINT_INFO("\t- downsize cameras: %d\n", downsample_cameras); + PRINT_INFO("\t- use multi-threading: %d\n", use_multi_threading); + PRINT_INFO("\t- num_pts: %d\n", num_pts); + PRINT_INFO("\t- fast threshold: %d\n", fast_threshold); + PRINT_INFO("\t- grid X by Y: %d by %d\n", grid_x, grid_y); + PRINT_INFO("\t- min px dist: %d\n", min_px_dist); + PRINT_INFO("\t- hist method: %d\n", (int)histogram_method); + PRINT_INFO("\t- knn ratio: %.3f\n", knn_ratio); + PRINT_INFO("\t- use mask?: %d\n", use_mask); featinit_options.print(); } @@ -285,14 +289,14 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_simulation() { - printf("SIMULATION PARAMETERS:\n"); - printf(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init); - printf(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb); - printf(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements); - printf("\t- traj path: %s\n", sim_traj_path.c_str()); - printf("\t- dist thresh: %.2f\n", sim_distance_threshold); - printf("\t- cam feq: %.2f\n", sim_freq_cam); - printf("\t- imu feq: %.2f\n", sim_freq_imu); + PRINT_INFO("SIMULATION PARAMETERS:\n"); + PRINT_INFO(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init); + PRINT_INFO(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb); + PRINT_INFO(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements); + PRINT_INFO("\t- traj path: %s\n", sim_traj_path.c_str()); + PRINT_INFO("\t- dist thresh: %.2f\n", sim_distance_threshold); + PRINT_INFO("\t- cam feq: %.2f\n", sim_freq_cam); + PRINT_INFO("\t- imu feq: %.2f\n", sim_freq_imu); } }; diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index 43753b619..952be7fee 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -91,7 +91,7 @@ int main(int argc, char **argv) { if (nh.hasParam("path_gt")) { std::string path_to_gt; nh.param("path_gt", path_to_gt, ""); - if(!path_to_gt.empty()) { + if (!path_to_gt.empty()) { DatasetReader::load_gt_file(path_to_gt, gt_states); ROS_INFO("gt file path is: %s", path_to_gt.c_str()); } diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index ff827bda2..6c481475a 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -27,6 +27,7 @@ #include "utils/colors.h" #include "utils/dataset_reader.h" #include "utils/parse_cmd.h" +#include "utils/print.h" #include "utils/sensor_data.h" #if ROS_AVAILABLE @@ -74,8 +75,8 @@ int main(int argc, char **argv) { Eigen::Matrix imustate; bool success = sim->get_state(sim->current_timestamp(), imustate); if (!success) { - printf(RED "[SIM]: Could not initialize the filter to the first state\n" RESET); - printf(RED "[SIM]: Did the simulator load properly???\n" RESET); + PRINT_ERROR(RED "[SIM]: Could not initialize the filter to the first state\n" RESET); + PRINT_ERROR(RED "[SIM]: Did the simulator load properly???\n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index ff7596cf8..01d7d04f7 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -21,6 +21,8 @@ #include "Simulator.h" +#include + using namespace ov_msckf; Simulator::Simulator(VioManagerOptions ¶ms_) { @@ -29,9 +31,9 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { //=============================================================== // Nice startup message - printf("=======================================\n"); - printf("VISUAL-INERTIAL SIMULATOR STARTING\n"); - printf("=======================================\n"); + PRINT_DEBUG("=======================================\n"); + PRINT_DEBUG("VISUAL-INERTIAL SIMULATOR STARTING\n"); + PRINT_DEBUG("=======================================\n"); // Store a copy of our params this->params = params_; @@ -43,8 +45,9 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Check that the max cameras matches the size of our cam matrices if (params.state_options.num_cameras != (int)params.camera_fisheye.size()) { - printf(RED "[SIM]: camera calib size does not match max cameras...\n" RESET); - printf(RED "[SIM]: got %d but expected %d max cameras\n" RESET, (int)params.camera_fisheye.size(), params.state_options.num_cameras); + PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET); + PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras\n" RESET, (int)params.camera_fisheye.size(), + params.state_options.num_cameras); std::exit(EXIT_FAILURE); } @@ -62,7 +65,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { Eigen::Vector3d p_IinG_init; bool success_pose_init = spline.get_pose(timestamp, R_GtoI_init, p_IinG_init); if (!success_pose_init) { - printf(RED "[SIM]: unable to find the first pose in the spline...\n" RESET); + PRINT_ERROR(RED "[SIM]: unable to find the first pose in the spline...\n" RESET); std::exit(EXIT_FAILURE); } @@ -78,7 +81,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Check if it fails if (!success_pose) { - printf(RED "[SIM]: unable to find jolt in the groundtruth data to initialize at\n" RESET); + PRINT_ERROR(RED "[SIM]: unable to find jolt in the groundtruth data to initialize at\n" RESET); std::exit(EXIT_FAILURE); } @@ -95,7 +98,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { timestamp_last_cam += 1.0 / params.sim_freq_cam; } } - printf("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time()); + PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time()); // Append the current true bias to our history hist_true_bias_time.push_back(timestamp_last_imu - 1.0 / params.sim_freq_imu); @@ -170,7 +173,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // double dt = 0.25/freq_cam; double dt = 0.25; size_t mapsize = featmap.size(); - printf("[SIM]: Generating map features at %d rate\n", (int)(1.0 / dt)); + PRINT_DEBUG("[SIM]: Generating map features at %d rate\n", (int)(1.0 / dt)); // Loop through each camera // NOTE: we loop through cameras here so that the feature map for camera 1 will always be the same @@ -204,8 +207,8 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { } // Debug print - printf("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize), - (int)((time_synth - spline.get_start_time()) / dt), i); + PRINT_DEBUG("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize), + (int)((time_synth - spline.get_start_time()) / dt), i); mapsize = featmap.size(); } @@ -349,8 +352,8 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, // If we do not have enough, generate more if ((int)uvs.size() < params.num_pts) { - printf(YELLOW "[SIM]: cam %d was unable to generate enough features (%d < %d projections)\n" RESET, (int)i, (int)uvs.size(), - params.num_pts); + PRINT_WARNING(YELLOW "[SIM]: cam %d was unable to generate enough features (%d < %d projections)\n" RESET, (int)i, (int)uvs.size(), + params.num_pts); } // If greater than only select the first set @@ -386,14 +389,14 @@ void Simulator::load_data(std::string path_traj) { std::ifstream file; file.open(path_traj); if (!file) { - printf(RED "ERROR: Unable to open simulation trajectory file...\n" RESET); - printf(RED "ERROR: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "ERROR: Unable to open simulation trajectory file...\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } // Debug print std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1); - printf("[SIM]: loaded trajectory %s\n", base_filename.c_str()); + PRINT_DEBUG("[SIM]: loaded trajectory %s\n", base_filename.c_str()); // Loop through each line of this file std::string current_line; @@ -422,7 +425,10 @@ void Simulator::load_data(std::string path_traj) { // Only a valid line if we have all the parameters if (i > 7) { traj_data.push_back(data); - // std::cout << std::setprecision(15) << data.transpose() << std::endl; + + // std::stringstream ss; + // ss << std::setprecision(15) << data.transpose() << std::endl; + // PRINT_DEBUG(ss.str().c_str()); } } @@ -431,8 +437,8 @@ void Simulator::load_data(std::string path_traj) { // Error if we don't have any data if (traj_data.empty()) { - printf(RED "ERROR: Could not parse any data from the file!!\n" RESET); - printf(RED "ERROR: %s\n" RESET, path_traj.c_str()); + PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str()); std::exit(EXIT_FAILURE); } } diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index b06170475..863e076dd 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -20,6 +20,7 @@ */ #include "Propagator.h" +#include "utils/print.h" using namespace ov_core; using namespace ov_msckf; @@ -29,14 +30,14 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest // If the difference between the current update time and state is zero // We should crash, as this means we would have two clones at the same time!!!! if (state->_timestamp == timestamp) { - printf(RED "Propagator::propagate_and_clone(): Propagation called again at same timestep at last update timestep!!!!\n" RESET); + PRINT_ERROR(RED "Propagator::propagate_and_clone(): Propagation called again at same timestep at last update timestep!!!!\n" RESET); std::exit(EXIT_FAILURE); } // We should crash if we are trying to propagate backwards if (state->_timestamp > timestamp) { - printf(RED "Propagator::propagate_and_clone(): Propagation called trying to propagate backwards in time!!!!\n" RESET); - printf(RED "Propagator::propagate_and_clone(): desired propagation = %.4f\n" RESET, (timestamp - state->_timestamp)); + PRINT_ERROR(RED "Propagator::propagate_and_clone(): Propagation called trying to propagate backwards in time!!!!\n" RESET); + PRINT_ERROR(RED "Propagator::propagate_and_clone(): desired propagation = %.4f\n" RESET, (timestamp - state->_timestamp)); std::exit(EXIT_FAILURE); } @@ -187,7 +188,7 @@ std::vector Propagator::select_imu_readings(const std::vector< // Ensure we have some measurements in the first place! if (imu_data.empty()) { if (warn) - printf(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET); + PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): No IMU measurements. IMU-CAMERA are likely messed up!!!\n" RESET); return prop_data; } @@ -202,7 +203,7 @@ std::vector Propagator::select_imu_readings(const std::vector< if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) { ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0); prop_data.push_back(data); - // printf("propagation #%d = CASE 1 = %.3f => %.3f\n", + // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp); continue; } @@ -212,7 +213,7 @@ std::vector Propagator::select_imu_readings(const std::vector< // Then we should just append the whole measurement time to our propagation vector if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) { prop_data.push_back(imu_data.at(i)); - // printf("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp); + // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp); continue; } @@ -234,11 +235,11 @@ std::vector Propagator::select_imu_readings(const std::vector< } else if (imu_data.at(i).timestamp > time1) { ov_core::ImuData data = interpolate_data(imu_data.at(i - 1), imu_data.at(i), time1); prop_data.push_back(data); - // printf("propagation #%d = CASE 3.1 = %.3f => %.3f\n", + // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n", // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); } else { prop_data.push_back(imu_data.at(i)); - // printf("propagation #%d = CASE 3.2 = %.3f => %.3f\n", + // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n", // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); } // If the added IMU message doesn't end exactly at the camera time @@ -246,7 +247,7 @@ std::vector Propagator::select_imu_readings(const std::vector< if (prop_data.at(prop_data.size() - 1).timestamp != time1) { ov_core::ImuData data = interpolate_data(imu_data.at(i), imu_data.at(i + 1), time1); prop_data.push_back(data); - // printf("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0); + // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0); } break; } @@ -255,7 +256,7 @@ std::vector Propagator::select_imu_readings(const std::vector< // Check that we have at least one measurement to propagate with if (prop_data.empty()) { if (warn) - printf( + PRINT_WARNING( YELLOW "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, (int)prop_data.size()); @@ -265,8 +266,8 @@ std::vector Propagator::select_imu_readings(const std::vector< // If we did not reach the whole integration period (i.e., the last inertial measurement we have is smaller then the time we want to // reach) Then we should just "stretch" the last measurement to be the whole period (case 3 in the above loop) // if(time1-imu_data.at(imu_data.size()-1).timestamp > 1e-3) { - // printf(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). IMU-CAMERA are - // likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data; + // PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). + // IMU-CAMERA are likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data; //} // Loop through and ensure we do not have an zero dt values @@ -274,8 +275,8 @@ std::vector Propagator::select_imu_readings(const std::vector< for (size_t i = 0; i < prop_data.size() - 1; i++) { if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12) { if (warn) - printf(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i, - (int)(i + 1)); + PRINT_WARNING(YELLOW "Propagator::select_imu_readings(): Zero DT between IMU reading %d and %d, removing it!\n" RESET, (int)i, + (int)(i + 1)); prop_data.erase(prop_data.begin() + i); i--; } @@ -284,7 +285,7 @@ std::vector Propagator::select_imu_readings(const std::vector< // Check that we have at least one measurement to propagate with if (prop_data.size() < 2) { if (warn) - printf( + PRINT_WARNING( YELLOW "Propagator::select_imu_readings(): No IMU measurements to propagate with (%d of 2). IMU-CAMERA are likely messed up!!!\n" RESET, (int)prop_data.size()); diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 77a85ca5b..2fac854ac 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -23,6 +23,7 @@ #define OV_MSCKF_STATE_PROPAGATOR_H #include "state/StateHelper.h" +#include "utils/print.h" #include "utils/quat_ops.h" #include "utils/sensor_data.h" @@ -71,10 +72,10 @@ class Propagator { /// Nice print function of what parameters we have loaded void print() { - printf("\t- gyroscope_noise_density: %.6f\n", sigma_w); - printf("\t- accelerometer_noise_density: %.5f\n", sigma_a); - printf("\t- gyroscope_random_walk: %.7f\n", sigma_wb); - printf("\t- accelerometer_random_walk: %.6f\n", sigma_ab); + PRINT_INFO("\t- gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_INFO("\t- accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_INFO("\t- gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_INFO("\t- accelerometer_random_walk: %.6f\n", sigma_ab); } }; @@ -170,7 +171,7 @@ class Propagator { static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) { // time-distance lambda double lambda = (timestamp - imu_1.timestamp) / (imu_2.timestamp - imu_1.timestamp); - // cout << "lambda - " << lambda << endl; + // PRINT_DEBUG("lambda - %d\n", lambda); // interpolate between the two times ov_core::ImuData data; data.timestamp = timestamp; diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp index 15570a4bc..7ad16ecfd 100644 --- a/ov_msckf/src/state/StateHelper.cpp +++ b/ov_msckf/src/state/StateHelper.cpp @@ -20,6 +20,7 @@ */ #include "StateHelper.h" +#include "utils/print.h" using namespace ov_core; using namespace ov_msckf; @@ -30,7 +31,7 @@ void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector // We need at least one old and new variable if (order_NEW.empty() || order_OLD.empty()) { - printf(RED "StateHelper::EKFPropagation() - Called with empty variable arrays!\n" RESET); + PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with empty variable arrays!\n" RESET); std::exit(EXIT_FAILURE); } @@ -38,9 +39,9 @@ void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector int size_order_NEW = order_NEW.at(0)->size(); for (size_t i = 0; i < order_NEW.size() - 1; i++) { if (order_NEW.at(i)->id() + order_NEW.at(i)->size() != order_NEW.at(i + 1)->id()) { - printf(RED "StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n" RESET); - printf(RED - "StateHelper::EKFPropagation() - This code only support a state transition which is in the same order as the state\n" RESET); + PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n" RESET); + PRINT_ERROR( + RED "StateHelper::EKFPropagation() - This code only support a state transition which is in the same order as the state\n" RESET); std::exit(EXIT_FAILURE); } size_order_NEW += order_NEW.at(i + 1)->size(); @@ -95,7 +96,7 @@ void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector bool found_neg = false; for (int i = 0; i < diags.rows(); i++) { if (diags(i) < 0.0) { - printf(RED "StateHelper::EKFPropagation() - diagonal at %d is %.2f\n" RESET, i, diags(i)); + PRINT_WARNING(RED "StateHelper::EKFPropagation() - diagonal at %d is %.2f\n" RESET, i, diags(i)); found_neg = true; } } @@ -162,7 +163,7 @@ void StateHelper::EKFUpdate(std::shared_ptr state, const std::vector state, std::shared_ptr_variables.begin(), state->_variables.end(), marg) == state->_variables.end()) { - printf(RED "StateHelper::marginalize() - Called on variable that is not in the state\n" RESET); - printf(RED "StateHelper::marginalize() - Marginalization, does NOT work on sub-variables yet...\n" RESET); + PRINT_ERROR(RED "StateHelper::marginalize() - Called on variable that is not in the state\n" RESET); + PRINT_ERROR(RED "StateHelper::marginalize() - Marginalization, does NOT work on sub-variables yet...\n" RESET); std::exit(EXIT_FAILURE); } @@ -353,8 +354,8 @@ std::shared_ptr StateHelper::clone(std::shared_ptr state, std::shar // Check if the current state has this variable if (new_clone == nullptr) { - printf(RED "StateHelper::clone() - Called on variable is not in the state\n" RESET); - printf(RED "StateHelper::clone() - Ensure that the variable specified is a variable, or sub-variable..\n" RESET); + PRINT_ERROR(RED "StateHelper::clone() - Called on variable is not in the state\n" RESET); + PRINT_ERROR(RED "StateHelper::clone() - Ensure that the variable specified is a variable, or sub-variable..\n" RESET); std::exit(EXIT_FAILURE); } @@ -369,8 +370,8 @@ bool StateHelper::initialize(std::shared_ptr state, std::shared_ptr // Check that this new variable is not already initialized if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) { - std::cerr << "StateHelper::initialize() - Called on variable that is already in the state" << std::endl; - std::cerr << "StateHelper::initialize() - Found this variable at " << new_variable->id() << " in covariance" << std::endl; + PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n"); + PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id()); std::exit(EXIT_FAILURE); } @@ -381,12 +382,12 @@ bool StateHelper::initialize(std::shared_ptr state, std::shared_ptr for (int r = 0; r < R.rows(); r++) { for (int c = 0; c < R.cols(); c++) { if (r == c && R(0, 0) != R(r, c)) { - printf(RED "StateHelper::initialize() - Your noise is not isotropic!\n" RESET); - printf(RED "StateHelper::initialize() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0)); + PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not isotropic!\n" RESET); + PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0)); std::exit(EXIT_FAILURE); } else if (r != c && R(r, c) != 0.0) { - printf(RED "StateHelper::initialize() - Your noise is not diagonal!\n" RESET); - printf(RED "StateHelper::initialize() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c); + PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not diagonal!\n" RESET); + PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c); std::exit(EXIT_FAILURE); } } @@ -460,8 +461,8 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share // Check that this new variable is not already initialized if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) { - std::cerr << "StateHelper::initialize_invertible() - Called on variable that is already in the state" << std::endl; - std::cerr << "StateHelper::initialize_invertible() - Found this variable at " << new_variable->id() << " in covariance" << std::endl; + PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n"); + PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id()); std::exit(EXIT_FAILURE); } @@ -472,12 +473,12 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share for (int r = 0; r < R.rows(); r++) { for (int c = 0; c < R.cols(); c++) { if (r == c && R(0, 0) != R(r, c)) { - printf(RED "StateHelper::initialize_invertible() - Your noise is not isotropic!\n" RESET); - printf(RED "StateHelper::initialize_invertible() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0)); + PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not isotropic!\n" RESET); + PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0)); std::exit(EXIT_FAILURE); } else if (r != c && R(r, c) != 0.0) { - printf(RED "StateHelper::initialize_invertible() - Your noise is not diagonal!\n" RESET); - printf(RED "StateHelper::initialize_invertible() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c); + PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not diagonal!\n" RESET); + PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c); std::exit(EXIT_FAILURE); } } @@ -543,14 +544,17 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share // Now collect results, and add it to the state variables new_variable->set_local_id(oldSize); state->_variables.push_back(new_variable); - // std::cout << new_variable->id() << " init dx = " << (H_Linv * res).transpose() << std::endl; + + // std::stringstream ss; + // ss << new_variable->id() << " init dx = " << (H_Linv * res).transpose() << std::endl; + // PRINT_DEBUG(ss.str().c_str()); } void StateHelper::augment_clone(std::shared_ptr state, Eigen::Matrix last_w) { // We can't insert a clone that occured at the same timestamp! if (state->_clones_IMU.find(state->_timestamp) != state->_clones_IMU.end()) { - printf(RED "TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!#!@#!@#\n" RESET); + PRINT_ERROR(RED "TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!#!@#!@#\n" RESET); std::exit(EXIT_FAILURE); } @@ -561,7 +565,7 @@ void StateHelper::augment_clone(std::shared_ptr state, Eigen::Matrix pose = std::dynamic_pointer_cast(posetemp); if (pose == nullptr) { - printf(RED "INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!#!@#!@#\n" RESET); + PRINT_ERROR(RED "INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!#!@#!@#\n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 248691c39..c22dd7ca0 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -23,6 +23,8 @@ #define OV_MSCKF_STATE_OPTIONS_H #include "types/LandmarkRepresentation.h" +#include "utils/print.h" + #include using namespace ov_type; @@ -81,21 +83,21 @@ struct StateOptions { /// Nice print function of what parameters we have loaded void print() { - printf("\t- use_fej: %d\n", do_fej); - printf("\t- use_imuavg: %d\n", imu_avg); - printf("\t- use_rk4int: %d\n", use_rk4_integration); - printf("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose); - printf("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); - printf("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); - printf("\t- max_clones: %d\n", max_clone_size); - printf("\t- max_slam: %d\n", max_slam_features); - printf("\t- max_slam_in_update: %d\n", max_slam_in_update); - printf("\t- max_msckf_in_update: %d\n", max_msckf_in_update); - printf("\t- max_aruco: %d\n", max_aruco_features); - printf("\t- max_cameras: %d\n", num_cameras); - printf("\t- feat_rep_msckf: %s\n", LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); - printf("\t- feat_rep_slam: %s\n", LandmarkRepresentation::as_string(feat_rep_slam).c_str()); - printf("\t- feat_rep_aruco: %s\n", LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); + PRINT_INFO("\t- use_fej: %d\n", do_fej); + PRINT_INFO("\t- use_imuavg: %d\n", imu_avg); + PRINT_INFO("\t- use_rk4int: %d\n", use_rk4_integration); + PRINT_INFO("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose); + PRINT_INFO("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); + PRINT_INFO("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); + PRINT_INFO("\t- max_clones: %d\n", max_clone_size); + PRINT_INFO("\t- max_slam: %d\n", max_slam_features); + PRINT_INFO("\t- max_slam_in_update: %d\n", max_slam_in_update); + PRINT_INFO("\t- max_msckf_in_update: %d\n", max_msckf_in_update); + PRINT_INFO("\t- max_aruco: %d\n", max_aruco_features); + PRINT_INFO("\t- max_cameras: %d\n", num_cameras); + PRINT_INFO("\t- feat_rep_msckf: %s\n", LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); + PRINT_INFO("\t- feat_rep_slam: %s\n", LandmarkRepresentation::as_string(feat_rep_slam).c_str()); + PRINT_INFO("\t- feat_rep_aruco: %s\n", LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); } }; diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp index f5c1fbea3..415cda43b 100644 --- a/ov_msckf/src/test_sim_meas.cpp +++ b/ov_msckf/src/test_sim_meas.cpp @@ -69,8 +69,7 @@ int main(int argc, char **argv) { Eigen::Vector3d wm, am; bool hasimu = sim.get_next_imu(time_imu, wm, am); if (hasimu) { - cout << "new imu measurement = " << std::setprecision(15) << time_imu << std::setprecision(3) << " | w = " << wm.norm() - << " | a = " << am.norm() << endl; + PRINT_DEBUG("new imu measurement = %0.15g | w = %0.3g | a = %0.3g\n", time_imu, wm.norm(), am.norm()); } // CAM: get the next simulated camera uv measurements if we have them @@ -79,11 +78,10 @@ int main(int argc, char **argv) { std::vector>> feats; bool hascam = sim.get_next_cam(time_cam, camids, feats); if (hascam) { - cout << "new cam measurement = " << std::setprecision(15) << time_cam; - cout << std::setprecision(3) << " | " << camids.size() << " cameras | uvs(0) = " << feats.at(0).size() << std::endl; + PRINT_DEBUG("new cam measurement = %0.15g | %u cameras | uvs(0) = %u \n", time_cam, camids.size(), feats.at(0).size()); } } // Done! return EXIT_SUCCESS; -} +}; \ No newline at end of file diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp index d2b35bd48..8084058f5 100644 --- a/ov_msckf/src/test_sim_repeat.cpp +++ b/ov_msckf/src/test_sim_repeat.cpp @@ -37,6 +37,7 @@ #include "sim/Simulator.h" #include "utils/parse_cmd.h" #include "utils/parse_ros.h" +#include "utils/print.h" using namespace ov_msckf; @@ -146,6 +147,6 @@ int main(int argc, char **argv) { } // Done! - printf("success! they all are the same!\n"); + PRINT_INFO("success! they all are the same!\n"); return EXIT_SUCCESS; } diff --git a/ov_msckf/src/update/UpdaterMSCKF.cpp b/ov_msckf/src/update/UpdaterMSCKF.cpp index 4449e1051..d4e53c1fc 100644 --- a/ov_msckf/src/update/UpdaterMSCKF.cpp +++ b/ov_msckf/src/update/UpdaterMSCKF.cpp @@ -20,6 +20,10 @@ */ #include "UpdaterMSCKF.h" +#include "utils/print.h" + +#include +#include using namespace ov_core; using namespace ov_msckf; @@ -187,16 +191,18 @@ void UpdaterMSCKF::update(std::shared_ptr state, std::vector _options.chi2_multipler * chi2_check) { (*it2)->to_delete = true; it2 = feature_vec.erase(it2); - // cout << "featid = " << feat.featid << endl; - // cout << "chi2 = " << chi2 << " > " << _options.chi2_multipler*chi2_check << endl; - // cout << "res = " << endl << res.transpose() << endl; + // PRINT_DEBUG("featid = %d\n", feat.featid); + // PRINT_DEBUG("chi2 = %f > %f\n", chi2, _options.chi2_multipler*chi2_check); + // std::stringstream ss; + // ss << "res = " << std::endl << res.transpose() << std::endl; + // PRINT_DEBUG(ss.str().c_str()); continue; } @@ -253,10 +259,10 @@ void UpdaterMSCKF::update(std::shared_ptr state, std::vector @@ -211,10 +212,10 @@ void UpdaterSLAM::delayed_init(std::shared_ptr state, std::vector state, std::vector state, std::vector_options.max_aruco_features) ? _options_aruco.chi2_multipler : _options_slam.chi2_multipler; if (chi2 > chi2_multipler * chi2_check) { if ((int)feat.featid < state->_options.max_aruco_features) - printf(YELLOW "[SLAM-UP]: rejecting aruco tag %d for chi2 thresh (%.3f > %.3f)\n" RESET, (int)feat.featid, chi2, - chi2_multipler * chi2_check); + PRINT_WARNING(YELLOW "[SLAM-UP]: rejecting aruco tag %d for chi2 thresh (%.3f > %.3f)\n" RESET, (int)feat.featid, chi2, + chi2_multipler * chi2_check); (*it2)->to_delete = true; it2 = feature_vec.erase(it2); continue; @@ -386,7 +387,7 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector_options.max_aruco_features) - printf("[SLAM-UP]: accepted aruco tag %d for chi2 thresh (%.3f < %.3f)\n", (int)feat.featid, chi2, chi2_multipler * chi2_check); + PRINT_DEBUG("[SLAM-UP]: accepted aruco tag %d for chi2 thresh (%.3f < %.3f)\n", (int)feat.featid, chi2, chi2_multipler * chi2_check); // We are good!!! Append to our large H vector size_t ct_hx = 0; @@ -435,10 +436,10 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector state) { diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 704ba7989..3b4a0bdbc 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -20,6 +20,7 @@ */ #include "UpdaterZeroVelocity.h" +#include "utils/print.h" using namespace ov_msckf; @@ -62,7 +63,7 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // Check that we have at least one measurement to propagate with if (imu_recent.size() < 2) { - printf(RED "[ZUPT]: There are no IMU data to check for zero velocity with!!\n" RESET); + PRINT_WARNING(RED "[ZUPT]: There are no IMU data to check for zero velocity with!!\n" RESET); last_zupt_state_timestamp = 0.0; return false; } @@ -159,7 +160,7 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest } else { boost::math::chi_squared chi_squared_dist(res.rows()); chi2_check = boost::math::quantile(chi_squared_dist, 0.95); - printf(YELLOW "[ZUPT]: chi2_check over the residual limit - %d\n" RESET, (int)res.rows()); + PRINT_WARNING(YELLOW "[ZUPT]: chi2_check over the residual limit - %d\n" RESET, (int)res.rows()); } // Check if the image disparity @@ -202,10 +203,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest } disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20); if (disparity_passed) { - printf(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, (int)num_features); + PRINT_WARNING(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, + (int)num_features); } else { - printf(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, - (int)num_features); + PRINT_INFO(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, + (int)num_features); } } @@ -213,12 +215,12 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // We need to pass the chi2 and not be above our velocity threshold if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) { last_zupt_state_timestamp = 0.0; - printf(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, - _options.chi2_multipler * chi2_check); + PRINT_WARNING(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, + _options.chi2_multipler * chi2_check); return false; } - printf(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, - _options.chi2_multipler * chi2_check); + PRINT_DEBUG(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, + _options.chi2_multipler * chi2_check); // Do our update, only do this update if we have previously detected // If we have succeeded, then we should remove the current timestamp feature tracks diff --git a/ov_msckf/src/utils/parse_cmd.h b/ov_msckf/src/utils/parse_cmd.h index 8699fb84e..0f72b8f9e 100644 --- a/ov_msckf/src/utils/parse_cmd.h +++ b/ov_msckf/src/utils/parse_cmd.h @@ -24,6 +24,7 @@ #include "core/VioManagerOptions.h" #include "utils/CLI11.hpp" +#include "utils/print.h" namespace ov_msckf { @@ -180,20 +181,20 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) { - printf(RED "VioManager(): invalid feature representation specified:\n" RESET); - printf(RED "\t- GLOBAL_3D\n" RESET); - printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_3D\n" RESET); - printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); + PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); + PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); + PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); std::exit(EXIT_FAILURE); } // Enforce that we have enough cameras to run if (params.state_options.num_cameras < 1) { - printf(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); - printf(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); + PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); + PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); std::exit(EXIT_FAILURE); } @@ -205,10 +206,10 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { } else if (histogram_method_str == "CLAHE") { params.histogram_method = TrackBase::CLAHE; } else { - printf(RED "VioManager(): invalid feature histogram specified:\n" RESET); - printf(RED "\t- NONE\n" RESET); - printf(RED "\t- HISTOGRAM\n" RESET); - printf(RED "\t- CLAHE\n" RESET); + PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); + PRINT_ERROR(RED "\t- NONE\n" RESET); + PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET); + PRINT_ERROR(RED "\t- CLAHE\n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_msckf/src/utils/parse_ros.h b/ov_msckf/src/utils/parse_ros.h index 1bfcac890..8913f8955 100644 --- a/ov_msckf/src/utils/parse_ros.h +++ b/ov_msckf/src/utils/parse_ros.h @@ -26,6 +26,7 @@ #include #include "core/VioManagerOptions.h" +#include "utils/print.h" namespace ov_msckf { @@ -59,8 +60,8 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { // Enforce that we have enough cameras to run if (params.state_options.num_cameras < 1) { - printf(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); - printf(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); + PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); + PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); std::exit(EXIT_FAILURE); } @@ -82,13 +83,13 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) { - printf(RED "VioManager(): invalid feature representation specified:\n" RESET); - printf(RED "\t- GLOBAL_3D\n" RESET); - printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_3D\n" RESET); - printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); + PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); + PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); + PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); + PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); std::exit(EXIT_FAILURE); } @@ -160,10 +161,10 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { } else if (histogram_method_str == "CLAHE") { params.histogram_method = TrackBase::CLAHE; } else { - printf(RED "VioManager(): invalid feature histogram specified:\n" RESET); - printf(RED "\t- NONE\n" RESET); - printf(RED "\t- HISTOGRAM\n" RESET); - printf(RED "\t- CLAHE\n" RESET); + PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); + PRINT_ERROR(RED "\t- NONE\n" RESET); + PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET); + PRINT_ERROR(RED "\t- CLAHE\n" RESET); std::exit(EXIT_FAILURE); } @@ -174,9 +175,9 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { nh.param("mask" + std::to_string(i), mask_path, ""); if (params.use_mask) { if (!boost::filesystem::exists(mask_path)) { - printf(RED "VioManager(): invalid mask path:\n" RESET); - printf(RED "\t- mask%d\n" RESET, i); - printf(RED "\t- %s\n" RESET, mask_path.c_str()); + PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET); + PRINT_ERROR(RED "\t- mask%d\n" RESET, i); + PRINT_ERROR(RED "\t- %s\n" RESET, mask_path.c_str()); std::exit(EXIT_FAILURE); } params.masks.insert({i, cv::imread(mask_path, cv::IMREAD_GRAYSCALE)}); From 7a8136ca2e06d00b3808c2c284b59f5bd2baf035 Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Sat, 21 Aug 2021 13:18:23 -0700 Subject: [PATCH 03/55] Added docs --- ov_core/src/utils/print.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h index eef3d5ba5..c0bfa267c 100644 --- a/ov_core/src/utils/print.h +++ b/ov_core/src/utils/print.h @@ -26,6 +26,10 @@ namespace ov_core { +/** + * @brief Printer for open_vins that allows for various levels of printing to be done + * + */ class Printer { public: /** The different print levels possible From e402b3028d61142c8b5c6c108aeede76bf5cd131 Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Sat, 21 Aug 2021 13:53:20 -0700 Subject: [PATCH 04/55] Updated required cmake version from 2.8.8 to 3.3 (basically just needed a 3.x version of some kind), added ability to optionally build ov_eval --- CMakeLists.txt | 9 +++++++-- ov_core/CMakeLists.txt | 2 +- ov_data/CMakeLists.txt | 2 +- ov_eval/CMakeLists.txt | 2 +- ov_msckf/CMakeLists.txt | 2 +- 5 files changed, 11 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 11509d6ad..113e0f368 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,11 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.3) add_subdirectory(ov_core) -add_subdirectory(ov_eval) add_subdirectory(ov_msckf) +# Optionally build the eval module, might not needed if building for a drone/robotic system +option(BUILD_OV_EVAL "Enable or disable building of ov_eval" ON) +if (BUILD_OV_EVAL) + add_subdirectory(ov_eval) +endif() + diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 7fa4e6735..17110f833 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.3) # Project name project(ov_core) diff --git a/ov_data/CMakeLists.txt b/ov_data/CMakeLists.txt index ebbd41393..b131724a6 100644 --- a/ov_data/CMakeLists.txt +++ b/ov_data/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.3) # Project name project(ov_data) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 3ceae0906..45bee30d5 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.3) # Project name project(ov_eval) diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 169e77573..a70e9e4dc 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.3) # Project name project(ov_msckf) From 4daba2ec9dc69e7f461ef5d6963b24414ce070e5 Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Sat, 21 Aug 2021 14:36:29 -0700 Subject: [PATCH 05/55] Made ov_eval a shared lib, added install cmake calls so that the libraries and apps for open_vins can be installed --- CMakeLists.txt | 3 +++ ov_core/CMakeLists.txt | 11 ++++++++- ov_eval/CMakeLists.txt | 54 ++++++++++++++++++++++++++++++++++++++--- ov_msckf/CMakeLists.txt | 16 ++++++++++++ 4 files changed, 80 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 113e0f368..b439f0f30 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,8 @@ cmake_minimum_required(VERSION 3.3) +# Project name +project(open_vins) + add_subdirectory(ov_core) add_subdirectory(ov_msckf) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 17110f833..d3267c5d8 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -94,7 +94,11 @@ add_library(ov_core_lib SHARED ) target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC src) - +install(TARGETS ov_core_lib + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) ################################################## # Make binary files! @@ -106,5 +110,10 @@ endif() add_executable(test_webcam src/test_webcam.cpp) target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) +install(TARGETS test_webcam + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 45bee30d5..d6e4a815f 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -78,7 +78,7 @@ list(APPEND thirdparty_libraries ################################################## # Make the core library ################################################## -add_library(ov_eval_lib +add_library(ov_eval_lib SHARED src/dummy.cpp src/alignment/AlignTrajectory.cpp src/alignment/AlignUtils.cpp @@ -87,6 +87,11 @@ add_library(ov_eval_lib src/utils/Loader.cpp ) target_link_libraries(ov_eval_lib ${thirdparty_libraries}) +install(TARGETS ov_eval_lib + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) ################################################## @@ -95,42 +100,85 @@ target_link_libraries(ov_eval_lib ${thirdparty_libraries}) if (catkin_FOUND AND ENABLE_CATKIN_ROS) - add_executable(pose_to_file src/pose_to_file.cpp) target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) add_executable(live_align_trajectory src/live_align_trajectory.cpp) target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) - endif() add_executable(format_converter src/format_converter.cpp) target_link_libraries(format_converter ov_eval_lib ${thirdparty_libraries}) +install(TARGETS format_converter + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(error_comparison src/error_comparison.cpp) target_link_libraries(error_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_comparison + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(error_dataset src/error_dataset.cpp) target_link_libraries(error_dataset ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_dataset + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(error_singlerun src/error_singlerun.cpp) target_link_libraries(error_singlerun ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_singlerun + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(error_simulation src/error_simulation.cpp) target_link_libraries(error_simulation ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_simulation + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(timing_flamegraph src/timing_flamegraph.cpp) target_link_libraries(timing_flamegraph ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_flamegraph + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(timing_comparison src/timing_comparison.cpp) target_link_libraries(timing_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_comparison + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(timing_percentages src/timing_percentages.cpp) target_link_libraries(timing_percentages ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_percentages + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(plot_trajectories src/plot_trajectories.cpp) target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) +install(TARGETS plot_trajectories + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) ################################################## diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index a70e9e4dc..b9a818112 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -109,6 +109,12 @@ endif() add_library(ov_msckf_lib SHARED ${library_source_files}) target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) target_include_directories(ov_msckf_lib PUBLIC src) +install(TARGETS ov_msckf_lib + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) + ################################################## @@ -137,8 +143,18 @@ endif() add_executable(test_sim_meas src/test_sim_meas.cpp) target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_meas + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) add_executable(test_sim_repeat src/test_sim_repeat.cpp) target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_repeat + LIBRARY DESTINATION /usr/lib + RUNTIME DESTINATION /usr/bin + PUBLIC_HEADER DESTINATION /usr/include +) From 8c5b5329a751e3030dd00b58512be0aee5e8874f Mon Sep 17 00:00:00 2001 From: Ali Younis Date: Sat, 21 Aug 2021 23:51:31 -0700 Subject: [PATCH 06/55] Made the installs GNU variables --- ov_core/CMakeLists.txt | 14 ++++++---- ov_eval/CMakeLists.txt | 62 +++++++++++++++++++++-------------------- ov_msckf/CMakeLists.txt | 20 +++++++------ 3 files changed, 51 insertions(+), 45 deletions(-) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index d3267c5d8..88d4e00e5 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 3.3) +include(GNUInstallDirs) + # Project name project(ov_core) @@ -95,9 +97,9 @@ add_library(ov_core_lib SHARED target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC src) install(TARGETS ov_core_lib - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) ################################################## @@ -111,9 +113,9 @@ endif() add_executable(test_webcam src/test_webcam.cpp) target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) install(TARGETS test_webcam - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index d6e4a815f..d56332771 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 3.3) +include(GNUInstallDirs) + # Project name project(ov_eval) @@ -88,9 +90,9 @@ add_library(ov_eval_lib SHARED ) target_link_libraries(ov_eval_lib ${thirdparty_libraries}) install(TARGETS ov_eval_lib - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) @@ -111,73 +113,73 @@ endif() add_executable(format_converter src/format_converter.cpp) target_link_libraries(format_converter ov_eval_lib ${thirdparty_libraries}) install(TARGETS format_converter - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(error_comparison src/error_comparison.cpp) target_link_libraries(error_comparison ov_eval_lib ${thirdparty_libraries}) install(TARGETS error_comparison - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(error_dataset src/error_dataset.cpp) target_link_libraries(error_dataset ov_eval_lib ${thirdparty_libraries}) install(TARGETS error_dataset - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(error_singlerun src/error_singlerun.cpp) target_link_libraries(error_singlerun ov_eval_lib ${thirdparty_libraries}) install(TARGETS error_singlerun - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(error_simulation src/error_simulation.cpp) target_link_libraries(error_simulation ov_eval_lib ${thirdparty_libraries}) install(TARGETS error_simulation - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(timing_flamegraph src/timing_flamegraph.cpp) target_link_libraries(timing_flamegraph ov_eval_lib ${thirdparty_libraries}) install(TARGETS timing_flamegraph - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(timing_comparison src/timing_comparison.cpp) target_link_libraries(timing_comparison ov_eval_lib ${thirdparty_libraries}) install(TARGETS timing_comparison - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(timing_percentages src/timing_percentages.cpp) target_link_libraries(timing_percentages ov_eval_lib ${thirdparty_libraries}) install(TARGETS timing_percentages - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(plot_trajectories src/plot_trajectories.cpp) target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) install(TARGETS plot_trajectories - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index b9a818112..d6e60a593 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 3.3) +include(GNUInstallDirs) + # Project name project(ov_msckf) @@ -110,9 +112,9 @@ add_library(ov_msckf_lib SHARED ${library_source_files}) target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) target_include_directories(ov_msckf_lib PUBLIC src) install(TARGETS ov_msckf_lib - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) @@ -144,17 +146,17 @@ endif() add_executable(test_sim_meas src/test_sim_meas.cpp) target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) install(TARGETS test_sim_meas - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) add_executable(test_sim_repeat src/test_sim_repeat.cpp) target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) install(TARGETS test_sim_repeat - LIBRARY DESTINATION /usr/lib - RUNTIME DESTINATION /usr/bin - PUBLIC_HEADER DESTINATION /usr/include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) From 2ab5db9f18ba2bac65f8085618580189f0bbf08e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 1 Nov 2021 20:47:06 -0400 Subject: [PATCH 07/55] cleanup print statements, and ov_eval uses ov_core math logic --- .github/workflows/build_catkin.yml | 86 +- ReadMe.md | 111 +- docs/css/custom.css | 3 +- docs/css/m-udel+documentation.compiled.css | 6300 +++++++++++------- docs/dev-coding-style.dox | 18 + docs/img/splineframes.svg | 352 +- docs/mcss_theme/m-theme-udel.css | 268 +- docs/mcss_theme/pygments-tango.css | 496 +- ov_core/CMakeLists.txt | 24 +- ov_core/src/feat/FeatureDatabase.h | 2 +- ov_core/src/feat/FeatureInitializer.cpp | 3 - ov_core/src/feat/FeatureInitializer.h | 1 + ov_core/src/feat/FeatureInitializerOptions.h | 24 +- ov_core/src/init/InertialInitializer.cpp | 1 - ov_core/src/init/InertialInitializer.h | 1 + ov_core/src/sim/BsplineSE3.cpp | 3 - ov_core/src/sim/BsplineSE3.h | 1 + ov_core/src/test_tracking.cpp | 5 + ov_core/src/test_webcam.cpp | 5 + ov_core/src/types/Landmark.cpp | 24 +- ov_core/src/types/PoseJPL.h | 5 +- ov_core/src/utils/CLI11.hpp | 18 +- ov_core/src/utils/dataset_reader.h | 2 +- ov_core/src/utils/lambda_body.h | 2 + ov_core/src/utils/print.cpp | 54 +- ov_core/src/utils/print.h | 71 +- ov_core/src/utils/quat_ops.h | 66 +- ov_eval/CMakeLists.txt | 33 +- ov_eval/python/pid_ros.py | 47 +- ov_eval/python/pid_sys.py | 14 +- ov_eval/src/alignment/AlignTrajectory.cpp | 11 +- ov_eval/src/alignment/AlignTrajectory.h | 4 +- ov_eval/src/alignment/AlignUtils.cpp | 5 +- ov_eval/src/alignment/AlignUtils.h | 5 +- ov_eval/src/calc/ResultSimulation.cpp | 25 +- ov_eval/src/calc/ResultSimulation.h | 6 +- ov_eval/src/calc/ResultTrajectory.cpp | 58 +- ov_eval/src/calc/ResultTrajectory.h | 6 +- ov_eval/src/error_comparison.cpp | 43 +- ov_eval/src/error_dataset.cpp | 33 +- ov_eval/src/error_simulation.cpp | 5 +- ov_eval/src/error_singlerun.cpp | 47 +- ov_eval/src/format_converter.cpp | 5 +- ov_eval/src/live_align_trajectory.cpp | 13 +- ov_eval/src/plot_trajectories.cpp | 23 +- ov_eval/src/pose_to_file.cpp | 20 +- ov_eval/src/timing_comparison.cpp | 17 +- ov_eval/src/timing_flamegraph.cpp | 11 +- ov_eval/src/timing_percentages.cpp | 5 +- ov_eval/src/utils/Colors.h | 43 - ov_eval/src/utils/Loader.cpp | 1 - ov_eval/src/utils/Loader.h | 3 +- ov_eval/src/utils/Math.h | 518 -- ov_eval/src/utils/Statistics.h | 3 +- ov_msckf/CMakeLists.txt | 34 +- ov_msckf/src/core/RosVisualizer.cpp | 25 +- ov_msckf/src/core/RosVisualizer.h | 1 + ov_msckf/src/core/VioManager.cpp | 13 +- ov_msckf/src/core/VioManager.h | 14 +- ov_msckf/src/core/VioManagerOptions.h | 134 +- ov_msckf/src/ros_serial_msckf.cpp | 44 +- ov_msckf/src/ros_subscribe_msckf.cpp | 14 +- ov_msckf/src/sim/Simulator.cpp | 3 +- ov_msckf/src/sim/Simulator.h | 4 +- ov_msckf/src/state/Propagator.cpp | 2 +- ov_msckf/src/state/Propagator.h | 10 +- ov_msckf/src/state/State.cpp | 3 +- ov_msckf/src/state/State.h | 20 +- ov_msckf/src/state/StateHelper.cpp | 2 +- ov_msckf/src/state/StateHelper.h | 23 +- ov_msckf/src/state/StateOptions.h | 38 +- ov_msckf/src/update/UpdaterHelper.cpp | 1 + ov_msckf/src/update/UpdaterHelper.h | 7 +- ov_msckf/src/update/UpdaterMSCKF.cpp | 17 +- ov_msckf/src/update/UpdaterMSCKF.h | 14 +- ov_msckf/src/update/UpdaterOptions.h | 4 +- ov_msckf/src/update/UpdaterSLAM.cpp | 30 +- ov_msckf/src/update/UpdaterSLAM.h | 17 +- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 11 +- ov_msckf/src/update/UpdaterZeroVelocity.h | 7 +- ov_msckf/src/utils/parse_cmd.h | 25 +- ov_msckf/src/utils/parse_ros.h | 25 +- 82 files changed, 5546 insertions(+), 3946 deletions(-) delete mode 100644 ov_eval/src/utils/Colors.h delete mode 100644 ov_eval/src/utils/Math.h diff --git a/.github/workflows/build_catkin.yml b/.github/workflows/build_catkin.yml index eee6557bb..a1d8b83b0 100644 --- a/.github/workflows/build_catkin.yml +++ b/.github/workflows/build_catkin.yml @@ -12,52 +12,52 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-16.04, ubuntu-18.04, ubuntu-20.04] - compiler: ["/usr/bin/g++"] + os: [ ubuntu-16.04, ubuntu-18.04, ubuntu-20.04 ] + compiler: [ "/usr/bin/g++" ] steps: - - uses: actions/checkout@v2 - - name: Checkout submodules - uses: textbook/git-checkout-submodule-action@master + - uses: actions/checkout@v2 + - name: Checkout submodules + uses: textbook/git-checkout-submodule-action@master - - name: Install apt dependencies (ubuntu-16.04) - if: matrix.os == 'ubuntu-16.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc + - name: Install apt dependencies (ubuntu-16.04) + if: matrix.os == 'ubuntu-16.04' + run: | + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && + curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && + sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && + touch ~/.bashrc && echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc - - name: Install apt dependencies (ubuntu-18.04) - if: matrix.os == 'ubuntu-18.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc + - name: Install apt dependencies (ubuntu-18.04) + if: matrix.os == 'ubuntu-18.04' + run: | + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && + curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && + sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && + touch ~/.bashrc && echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc - - name: Install apt dependencies (ubuntu-20.04) - if: matrix.os == 'ubuntu-20.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc + - name: Install apt dependencies (ubuntu-20.04) + if: matrix.os == 'ubuntu-20.04' + run: | + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && + curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && + sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && + touch ~/.bashrc && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc - - name: Create catkin workspace and compile - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - cd $GITHUB_WORKSPACE/.. && mkdir src/ && - mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && - source /opt/ros/$(ls -1 /opt/ros/ | head -n1)/setup.bash && echo "ros version: $ROS_DISTRO" && - catkin build -j2 --no-status + - name: Create catkin workspace and compile + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + source /opt/ros/$(ls -1 /opt/ros/ | head -n1)/setup.bash && echo "ros version: $ROS_DISTRO" && + catkin build -j2 --no-status - - name: Run simulation dataset - run: | - source $GITHUB_WORKSPACE/devel/setup.bash && - roscore & - export REPO=$(basename $GITHUB_REPOSITORY) && - source $GITHUB_WORKSPACE/devel/setup.bash && - rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt \ No newline at end of file + - name: Run simulation dataset + run: | + source $GITHUB_WORKSPACE/devel/setup.bash && + roscore & + export REPO=$(basename $GITHUB_REPOSITORY) && + source $GITHUB_WORKSPACE/devel/setup.bash && + rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt \ No newline at end of file diff --git a/ReadMe.md b/ReadMe.md index c21e40a94..19c878c0e 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,41 +1,56 @@ # OpenVINS -![C/C++ CI](https://github.com/rpng/open_vins/workflows/C/C++%20CI/badge.svg) +![C/C++ CI](https://github.com/rpng/open_vins/workflows/C/C++%20CI/badge.svg) Welcome to the OpenVINS project! -The OpenVINS project houses some core computer vision code along with a state-of-the art filter-based visual-inertial estimator. -The core filter is an [Extended Kalman filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter) which fuses inertial information with sparse visual feature tracks. -These visual feature tracks are fused leveraging the [Multi-State Constraint Kalman Filter (MSCKF)](https://ieeexplore.ieee.org/document/4209642) sliding window formulation which allows for 3D features to update the state estimate without directly estimating the feature states in the filter. -Inspired by graph-based optimization systems, the included filter has modularity allowing for convenient covariance management with a proper type-based state system. -Please take a look at the feature list below for full details on what the system supports. - +The OpenVINS project houses some core computer vision code along with a state-of-the art filter-based visual-inertial +estimator. The core filter is an [Extended Kalman filter](https://en.wikipedia.org/wiki/Extended_Kalman_filter) which +fuses inertial information with sparse visual feature tracks. These visual feature tracks are fused leveraging +the [Multi-State Constraint Kalman Filter (MSCKF)](https://ieeexplore.ieee.org/document/4209642) sliding window +formulation which allows for 3D features to update the state estimate without directly estimating the feature states in +the filter. Inspired by graph-based optimization systems, the included filter has modularity allowing for convenient +covariance management with a proper type-based state system. Please take a look at the feature list below for full +details on what the system supports. * Github project page - https://github.com/rpng/open_vins * Documentation - https://docs.openvins.com/ * Getting started guide - https://docs.openvins.com/getting-started.html * Publication reference - http://udel.edu/~pgeneva/downloads/papers/c10.pdf - ## News / Events -* **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See v2.4 [PR#117](https://github.com/rpng/open_vins/pull/186) for details. -* **December 1, 2020** - Released improved memory management, active feature pointcloud publishing, limiting number of features in update to bound compute, and other small fixes. See v2.3 [PR#117](https://github.com/rpng/open_vins/pull/117) for details. -* **November 18, 2020** - Released groundtruth generation utility package, [vicon2gt](https://github.com/rpng/vicon2gt) to enable creation of groundtruth trajectories in a motion capture room for evaulating VIO methods. -* **July 7, 2020** - Released zero velocity update for vehicle applications and direct initialization when standing still. See [PR#79](https://github.com/rpng/open_vins/pull/79) for details. -* **May 18, 2020** - Released secondary pose graph example repository [ov_secondary](https://github.com/rpng/ov_secondary) based on [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion). OpenVINS now publishes marginalized feature track, feature 3d position, and first camera intrinsics and extrinsics. See [PR#66](https://github.com/rpng/open_vins/pull/66) for details and discussion. -* **April 3, 2020** - Released [v2.0](https://github.com/rpng/open_vins/releases/tag/v2.0) update to the codebase with some key refactoring, ros-free building, improved dataset support, and single inverse depth feature representation. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.0) for details. -* **January 21, 2020** - Our paper has been accepted for presentation in [ICRA 2020](https://www.icra2020.org/). We look forward to seeing everybody there! We have also added links to a few videos of the system running on different datasets. +* **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See + v2.4 [PR#117](https://github.com/rpng/open_vins/pull/186) for details. +* **December 1, 2020** - Released improved memory management, active feature pointcloud publishing, limiting number of + features in update to bound compute, and other small fixes. See + v2.3 [PR#117](https://github.com/rpng/open_vins/pull/117) for details. +* **November 18, 2020** - Released groundtruth generation utility package, [vicon2gt](https://github.com/rpng/vicon2gt) + to enable creation of groundtruth trajectories in a motion capture room for evaulating VIO methods. +* **July 7, 2020** - Released zero velocity update for vehicle applications and direct initialization when standing + still. See [PR#79](https://github.com/rpng/open_vins/pull/79) for details. +* **May 18, 2020** - Released secondary pose graph example + repository [ov_secondary](https://github.com/rpng/ov_secondary) based + on [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion). OpenVINS now publishes marginalized feature + track, feature 3d position, and first camera intrinsics and extrinsics. + See [PR#66](https://github.com/rpng/open_vins/pull/66) for details and discussion. +* **April 3, 2020** - Released [v2.0](https://github.com/rpng/open_vins/releases/tag/v2.0) update to the codebase with + some key refactoring, ros-free building, improved dataset support, and single inverse depth feature representation. + Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.0) for details. +* **January 21, 2020** - Our paper has been accepted for presentation in [ICRA 2020](https://www.icra2020.org/). We look + forward to seeing everybody there! We have also added links to a few videos of the system running on different + datasets. * **October 23, 2019** - OpenVINS placed first in the [IROS 2019 FPV Drone Racing VIO Competition -](http://rpg.ifi.uzh.ch/uzh-fpv.html). We will be giving a short presentation at the [workshop](https://wp.nyu.edu/workshopiros2019mav/) at 12:45pm in Macau on November 8th. + ](http://rpg.ifi.uzh.ch/uzh-fpv.html). We will be giving a short presentation at + the [workshop](https://wp.nyu.edu/workshopiros2019mav/) at 12:45pm in Macau on November 8th. * **October 1, 2019** - We will be presenting at the [Visual-Inertial Navigation: Challenges and Applications -](http://udel.edu/~ghuang/iros19-vins-workshop/index.html) workshop at [IROS 2019](https://www.iros2019.org/). The submitted workshop paper can be found at [this](http://udel.edu/~ghuang/iros19-vins-workshop/papers/06.pdf) link. -* **August 21, 2019** - Open sourced [ov_maplab](https://github.com/rpng/ov_maplab) for interfacing OpenVINS with the [maplab](https://github.com/ethz-asl/maplab) library. -* **August 15, 2019** - Initial release of OpenVINS repository and documentation website! - + ](http://udel.edu/~ghuang/iros19-vins-workshop/index.html) workshop at [IROS 2019](https://www.iros2019.org/). The + submitted workshop paper can be found at [this](http://udel.edu/~ghuang/iros19-vins-workshop/papers/06.pdf) link. +* **August 21, 2019** - Open sourced [ov_maplab](https://github.com/rpng/ov_maplab) for interfacing OpenVINS with + the [maplab](https://github.com/ethz-asl/maplab) library. +* **August 15, 2019** - Initial release of OpenVINS repository and documentation website! ## Project Features - * Sliding window visual-inertial MSCKF * Modular covariance type system * Comprehensive documentation and derivations @@ -52,7 +67,7 @@ Please take a look at the feature list below for full details on what the system 5. Anchored MSCKF inverse depth 6. Anchored single inverse depth * Calibration of sensor intrinsics and extrinsics - * Camera to IMU transform + * Camera to IMU transform * Camera to IMU time offset * Camera intrinsics * Environmental SLAM feature @@ -69,28 +84,30 @@ Please take a look at the feature list below for full details on what the system * Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..) - - ## Codebase Extensions -* **[ov_secondary](https://github.com/rpng/ov_secondary)** - -This is an example secondary thread which provides loop closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). -This is a modification of the code originally developed by the HKUST aerial robotics group and can be found in their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. -Here we stress that this is a loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry. -This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop closure detection to improve frequency. - -* **[ov_maplab](https://github.com/rpng/ov_maplab)** - -This codebase contains the interface wrapper for exporting visual-inertial runs from [OpenVINS](https://github.com/rpng/open_vins) into the ViMap structure taken by [maplab](https://github.com/ethz-asl/maplab). -The state estimates and raw images are appended to the ViMap as OpenVINS runs through a dataset. -After completion of the dataset, features are re-extract and triangulate with maplab's feature system. -This can be used to merge multi-session maps, or to perform a batch optimization after first running the data through OpenVINS. -Some example have been provided along with a helper script to export trajectories into the standard groundtruth format. - -* **[vicon2gt](https://github.com/rpng/vicon2gt)** - -This utility was created to generate groundtruth trajectories using a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems. -Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth trajectory similar to those provided by the EurocMav datasets. -Performs fusion of inertial and motion capture information and estimates all unknown spacial-temporal calibrations between the two sensors. - +* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop + closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the + code originally developed by the HKUST aerial robotics group and can be found in + their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a + loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry. + This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to + camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop + closure detection to improve frequency. + +* **[ov_maplab](https://github.com/rpng/ov_maplab)** - This codebase contains the interface wrapper for exporting + visual-inertial runs from [OpenVINS](https://github.com/rpng/open_vins) into the ViMap structure taken + by [maplab](https://github.com/ethz-asl/maplab). The state estimates and raw images are appended to the ViMap as + OpenVINS runs through a dataset. After completion of the dataset, features are re-extract and triangulate with + maplab's feature system. This can be used to merge multi-session maps, or to perform a batch optimization after first + running the data through OpenVINS. Some example have been provided along with a helper script to export trajectories + into the standard groundtruth format. + +* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using + a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems. + Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth + trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture + information and estimates all unknown spacial-temporal calibrations between the two sensors. ## Demo Videos @@ -103,13 +120,12 @@ Performs fusion of inertial and motion capture information and estimates all unk [![](docs/youtube/oUoLlrFryk0.jpg)](http://www.youtube.com/watch?v=oUoLlrFryk0 "OpenVINS - TUM VI Datasets Demostration") [![](docs/youtube/ExPIGwORm4E.jpg)](http://www.youtube.com/watch?v=ExPIGwORm4E "OpenVINS - UZH-FPV Drone Racing Dataset Demonstration") - - ## Credit / Licensing -This code was written by the [Robot Perception and Navigation Group (RPNG)](https://sites.udel.edu/robot/) at the University of Delaware. -If you have any issues with the code please open an issue on our github page with relevant implementation details and references. -For researchers that have leveraged or compared to this work, please cite the following: +This code was written by the [Robot Perception and Navigation Group (RPNG)](https://sites.udel.edu/robot/) at the +University of Delaware. If you have any issues with the code please open an issue on our github page with relevant +implementation details and references. For researchers that have leveraged or compared to this work, please cite the +following: ```txt @Conference{Geneva2020ICRA, @@ -122,7 +138,6 @@ For researchers that have leveraged or compared to this work, please cite the fo } ``` - The codebase is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt). diff --git a/docs/css/custom.css b/docs/css/custom.css index b6bd9942d..7b8b33bc0 100644 --- a/docs/css/custom.css +++ b/docs/css/custom.css @@ -1,4 +1,3 @@ - /* don't indent each paragraph since we frequently use equations */ main p { text-indent: 0rem !important; @@ -36,9 +35,11 @@ body > header > nav a#m-navbar-brand, body > header > nav #m-navbar-brand a { .m-doc-template, dl.m-doc dd, ul.m-doc li > span.m-doc { color: #1a1a1a; } + .m-doc-template a, dl.m-doc dd a, ul.m-doc li > span.m-doc a { color: #1a1a1a; } + .m-block.m-dim, .m-text.m-dim, .m-label.m-flat.m-dim { color: #1a1a1a; } \ No newline at end of file diff --git a/docs/css/m-udel+documentation.compiled.css b/docs/css/m-udel+documentation.compiled.css index 81054871f..0a9a854db 100644 --- a/docs/css/m-udel+documentation.compiled.css +++ b/docs/css/m-udel+documentation.compiled.css @@ -24,1500 +24,2417 @@ DEALINGS IN THE SOFTWARE. */ -*, ::before, ::after { box-sizing: border-box; } -body { margin: 0; } +*, ::before, ::after { + box-sizing: border-box; +} + +body { + margin: 0; +} + .m-container { - width: 100%; - margin: auto; - padding-left: 1rem; - padding-right: 1rem; + width: 100%; + margin: auto; + padding-left: 1rem; + padding-right: 1rem; } + .m-row { - margin-left: -1rem; - margin-right: -1rem; + margin-left: -1rem; + margin-right: -1rem; } + .m-row:after { - content: ' '; - clear: both; - display: table; + content: ' '; + clear: both; + display: table; } + .m-row > [class*='m-col-'] { - position: relative; - padding: 1rem; + position: relative; + padding: 1rem; } + [class*='m-clearfix-']::after { - display: block; - content: ' '; - clear: both; + display: block; + content: ' '; + clear: both; } + [class*='m-show-'] { - display: none; + display: none; } + .m-container-inflate, :not(.m-row) > [class*='m-col-'] { - margin-bottom: 1rem; + margin-bottom: 1rem; } + .m-container-inflate:last-child, :not(.m-row) > [class*='m-col-']:last-child { - margin-bottom: 0; + margin-bottom: 0; } + .m-container.m-nopad, [class*='m-col-'].m-nopad, .m-container.m-nopadx, [class*='m-col-'].m-nopadx, .m-container.m-nopadl, [class*='m-col-'].m-nopadl { - padding-left: 0; + padding-left: 0; } + .m-container.m-nopad, [class*='m-col-'].m-nopad, .m-container.m-nopadx, [class*='m-col-'].m-nopadx, .m-container.m-nopadr, [class*='m-col-'].m-nopadr { - padding-right: 0; + padding-right: 0; } + [class*='m-col-'].m-nopad, [class*='m-col-'].m-nopady, [class*='m-col-'].m-nopadt { - padding-top: 0; + padding-top: 0; } + [class*='m-col-'].m-nopad, [class*='m-col-'].m-nopady, [class*='m-col-'].m-nopadb, .m-container-inflate.m-nopadb { - padding-bottom: 0; -} -[class*='m-col-t-'] { float: left; } -.m-left-t { - padding-right: 1rem; - float: left; -} -.m-right-t, [class*='m-col-t-'].m-right-t { - padding-left: 1rem; - float: right; -} -.m-center-t, [class*='m-col-t-'].m-center-t { - float: none; + padding-bottom: 0; } -.m-center-t, [class*='m-col-t-'].m-center-t { - margin-left: auto; - margin-right: auto; - float: none; -} -.m-col-t-1 { width: calc(1 * 100% / 12); } -.m-col-t-2 { width: calc(2 * 100% / 12); } -.m-col-t-3 { width: calc(3 * 100% / 12); } -.m-col-t-4 { width: calc(4 * 100% / 12); } -.m-col-t-5 { width: calc(5 * 100% / 12); } -.m-col-t-6 { width: calc(6 * 100% / 12); } -.m-col-t-7 { width: calc(7 * 100% / 12); } -.m-col-t-8 { width: calc(8 * 100% / 12); } -.m-col-t-9 { width: calc(9 * 100% / 12); } -.m-col-t-10 { width: calc(10 * 100% / 12); } -.m-col-t-11 { width: calc(11 * 100% / 12); } -.m-col-t-12 { width: calc(12 * 100% / 12); } -.m-push-t-1 { left: calc(1 * 100% / 12); } -.m-push-t-2 { left: calc(2 * 100% / 12); } -.m-push-t-3 { left: calc(3 * 100% / 12); } -.m-push-t-4 { left: calc(4 * 100% / 12); } -.m-push-t-5 { left: calc(5 * 100% / 12); } -.m-push-t-6 { left: calc(6 * 100% / 12); } -.m-push-t-7 { left: calc(7 * 100% / 12); } -.m-push-t-8 { left: calc(8 * 100% / 12); } -.m-push-t-9 { left: calc(9 * 100% / 12); } -.m-push-t-10 { left: calc(10 * 100% / 12); } -.m-push-t-11 { left: calc(11 * 100% / 12); } -.m-pull-t-1 { right: calc(1 * 100% / 12); } -.m-pull-t-2 { right: calc(2 * 100% / 12); } -.m-pull-t-3 { right: calc(3 * 100% / 12); } -.m-pull-t-4 { right: calc(4 * 100% / 12); } -.m-pull-t-5 { right: calc(5 * 100% / 12); } -.m-pull-t-6 { right: calc(6 * 100% / 12); } -.m-pull-t-7 { right: calc(7 * 100% / 12); } -.m-pull-t-8 { right: calc(8 * 100% / 12); } -.m-pull-t-9 { right: calc(9 * 100% / 12); } -.m-pull-t-10 { right: calc(10 * 100% / 12); } -.m-pull-t-11 { right: calc(11 * 100% / 12); } -@media screen and (min-width: 576px) { - .m-container { width: 560px; } - .m-container-inflatable .m-col-s-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { - margin-left: -10%; - margin-right: -10%; - } - .m-container-inflatable .m-col-s-10 .m-container-inflate.m-left-s { - margin-left: -10%; - } - .m-container-inflatable .m-col-s-10 .m-container-inflate.m-right-s { - margin-right: -10%; - } - [class*='m-col-s-'] { float: left; } - .m-left-s { - padding-right: 1rem; + +[class*='m-col-t-'] { float: left; - } - .m-right-s, [class*='m-col-s-'].m-right-s { - padding-left: 1rem; - float: right; - } - .m-center-s, [class*='m-col-s-'].m-center-s { - margin-left: auto; - margin-right: auto; - float: none; - } - .m-col-s-1 { width: calc(1 * 100% / 12); } - .m-col-s-2 { width: calc(2 * 100% / 12); } - .m-col-s-3 { width: calc(3 * 100% / 12); } - .m-col-s-4 { width: calc(4 * 100% / 12); } - .m-col-s-5 { width: calc(5 * 100% / 12); } - .m-col-s-6 { width: calc(6 * 100% / 12); } - .m-col-s-7 { width: calc(7 * 100% / 12); } - .m-col-s-8 { width: calc(8 * 100% / 12); } - .m-col-s-9 { width: calc(9 * 100% / 12); } - .m-col-s-10 { width: calc(10 * 100% / 12); } - .m-col-s-11 { width: calc(11 * 100% / 12); } - .m-col-s-12 { width: calc(12 * 100% / 12); } - .m-push-s-0 { left: calc(0 * 100% / 12); } - .m-push-s-1 { left: calc(1 * 100% / 12); } - .m-push-s-2 { left: calc(2 * 100% / 12); } - .m-push-s-3 { left: calc(3 * 100% / 12); } - .m-push-s-4 { left: calc(4 * 100% / 12); } - .m-push-s-5 { left: calc(5 * 100% / 12); } - .m-push-s-6 { left: calc(6 * 100% / 12); } - .m-push-s-7 { left: calc(7 * 100% / 12); } - .m-push-s-8 { left: calc(8 * 100% / 12); } - .m-push-s-9 { left: calc(9 * 100% / 12); } - .m-push-s-10 { left: calc(10 * 100% / 12); } - .m-push-s-11 { left: calc(11 * 100% / 12); } - .m-pull-s-0 { right: calc(0 * 100% / 12); } - .m-pull-s-1 { right: calc(1 * 100% / 12); } - .m-pull-s-2 { right: calc(2 * 100% / 12); } - .m-pull-s-3 { right: calc(3 * 100% / 12); } - .m-pull-s-4 { right: calc(4 * 100% / 12); } - .m-pull-s-5 { right: calc(5 * 100% / 12); } - .m-pull-s-6 { right: calc(6 * 100% / 12); } - .m-pull-s-7 { right: calc(7 * 100% / 12); } - .m-pull-s-8 { right: calc(8 * 100% / 12); } - .m-pull-s-9 { right: calc(9 * 100% / 12); } - .m-pull-s-10 { right: calc(10 * 100% / 12); } - .m-pull-s-11 { right: calc(11 * 100% / 12); } - .m-clearfix-t::after { display: none; } - .m-hide-s { display: none; } - .m-show-s { display: block; } - .m-col-s-none { - width: auto; - float: none; - } } -@media screen and (min-width: 768px) { - .m-container { width: 750px; } - .m-container-inflatable .m-col-m-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { - margin-left: -10%; - margin-right: -10%; - } - .m-container-inflatable .m-col-m-10 .m-container-inflate.m-left-m { - margin-left: -10%; - } - .m-container-inflatable .m-col-m-10 .m-container-inflate.m-right-m { - margin-right: -10%; - } - [class*='m-col-m-'] { float: left; } - .m-left-m { + +.m-left-t { padding-right: 1rem; float: left; - } - .m-right-m, [class*='m-col-m-'].m-right-m { +} + +.m-right-t, [class*='m-col-t-'].m-right-t { padding-left: 1rem; float: right; - } - .m-center-m, [class*='m-col-m-'].m-center-m { - margin-left: auto; - margin-right: auto; - float: none; - } - .m-col-m-1 { width: calc(1 * 100% / 12); } - .m-col-m-2 { width: calc(2 * 100% / 12); } - .m-col-m-3 { width: calc(3 * 100% / 12); } - .m-col-m-4 { width: calc(4 * 100% / 12); } - .m-col-m-5 { width: calc(5 * 100% / 12); } - .m-col-m-6 { width: calc(6 * 100% / 12); } - .m-col-m-7 { width: calc(7 * 100% / 12); } - .m-col-m-8 { width: calc(8 * 100% / 12); } - .m-col-m-9 { width: calc(9 * 100% / 12); } - .m-col-m-10 { width: calc(10 * 100% / 12); } - .m-col-m-11 { width: calc(11 * 100% / 12); } - .m-col-m-12 { width: calc(12 * 100% / 12); } - .m-push-m-0 { left: calc(0 * 100% / 12); } - .m-push-m-1 { left: calc(1 * 100% / 12); } - .m-push-m-2 { left: calc(2 * 100% / 12); } - .m-push-m-3 { left: calc(3 * 100% / 12); } - .m-push-m-4 { left: calc(4 * 100% / 12); } - .m-push-m-5 { left: calc(5 * 100% / 12); } - .m-push-m-6 { left: calc(6 * 100% / 12); } - .m-push-m-7 { left: calc(7 * 100% / 12); } - .m-push-m-8 { left: calc(8 * 100% / 12); } - .m-push-m-9 { left: calc(9 * 100% / 12); } - .m-push-m-10 { left: calc(10 * 100% / 12); } - .m-push-m-11 { left: calc(11 * 100% / 12); } - .m-pull-m-0 { right: calc(0 * 100% / 12); } - .m-pull-m-1 { right: calc(1 * 100% / 12); } - .m-pull-m-2 { right: calc(2 * 100% / 12); } - .m-pull-m-3 { right: calc(3 * 100% / 12); } - .m-pull-m-4 { right: calc(4 * 100% / 12); } - .m-pull-m-5 { right: calc(5 * 100% / 12); } - .m-pull-m-6 { right: calc(6 * 100% / 12); } - .m-pull-m-7 { right: calc(7 * 100% / 12); } - .m-pull-m-8 { right: calc(8 * 100% / 12); } - .m-pull-m-9 { right: calc(9 * 100% / 12); } - .m-pull-m-10 { right: calc(10 * 100% / 12); } - .m-pull-m-11 { right: calc(11 * 100% / 12); } - .m-clearfix-s::after { display: none; } - .m-hide-m { display: none; } - .m-show-m { display: block; } - .m-col-m-none { - width: auto; +} + +.m-center-t, [class*='m-col-t-'].m-center-t { float: none; - } } -@media screen and (min-width: 992px) { - .m-container { width: 960px; } - .m-container-inflatable .m-col-l-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { - margin-left: -10%; - margin-right: -10%; - } - .m-container-inflatable .m-col-l-10 .m-container-inflate.m-left-l { - margin-left: -10%; - } - .m-container-inflatable .m-col-l-10 .m-container-inflate.m-right-l { - margin-right: -10%; - } - [class*='m-col-l-'] { float: left; } - .m-left-l { - padding-right: 1rem; - float: left; - } - .m-right-l, [class*='m-col-l-'].m-right-l { - padding-left: 1rem; - float: right; - } - .m-center-l, [class*='m-col-l-'].m-center-l { + +.m-center-t, [class*='m-col-t-'].m-center-t { margin-left: auto; margin-right: auto; float: none; - } - .m-col-l-1 { width: calc(1 * 100% / 12); } - .m-col-l-2 { width: calc(2 * 100% / 12); } - .m-col-l-3 { width: calc(3 * 100% / 12); } - .m-col-l-4 { width: calc(4 * 100% / 12); } - .m-col-l-5 { width: calc(5 * 100% / 12); } - .m-col-l-6 { width: calc(6 * 100% / 12); } - .m-col-l-7 { width: calc(7 * 100% / 12); } - .m-col-l-8 { width: calc(8 * 100% / 12); } - .m-col-l-9 { width: calc(9 * 100% / 12); } - .m-col-l-10 { width: calc(10 * 100% / 12); } - .m-col-l-11 { width: calc(11 * 100% / 12); } - .m-col-l-12 { width: calc(12 * 100% / 12); } - .m-push-l-0 { left: calc(0 * 100% / 12); } - .m-push-l-1 { left: calc(1 * 100% / 12); } - .m-push-l-2 { left: calc(2 * 100% / 12); } - .m-push-l-3 { left: calc(3 * 100% / 12); } - .m-push-l-4 { left: calc(4 * 100% / 12); } - .m-push-l-5 { left: calc(5 * 100% / 12); } - .m-push-l-6 { left: calc(6 * 100% / 12); } - .m-push-l-7 { left: calc(7 * 100% / 12); } - .m-push-l-8 { left: calc(8 * 100% / 12); } - .m-push-l-9 { left: calc(9 * 100% / 12); } - .m-push-l-10 { left: calc(10 * 100% / 12); } - .m-push-l-11 { left: calc(11 * 100% / 12); } - .m-pull-l-0 { right: calc(0 * 100% / 12); } - .m-pull-l-1 { right: calc(1 * 100% / 12); } - .m-pull-l-2 { right: calc(2 * 100% / 12); } - .m-pull-l-3 { right: calc(3 * 100% / 12); } - .m-pull-l-4 { right: calc(4 * 100% / 12); } - .m-pull-l-5 { right: calc(5 * 100% / 12); } - .m-pull-l-6 { right: calc(6 * 100% / 12); } - .m-pull-l-7 { right: calc(7 * 100% / 12); } - .m-pull-l-8 { right: calc(8 * 100% / 12); } - .m-pull-l-9 { right: calc(9 * 100% / 12); } - .m-pull-l-10 { right: calc(10 * 100% / 12); } - .m-pull-l-11 { right: calc(11 * 100% / 12); } - .m-clearfix-m::after { display: none; } - .m-hide-l { display: none; } - .m-show-l { display: block; } - .m-col-l-none { - width: auto; - float: none; - } } -html { - font-size: 1em; - background-color: #ffffff; -} -body { - font-family: 'Source Sans Pro', sans-serif; - font-size: 1rem; - line-height: normal; - color: #000000; -} -h1, h2, h3, h4, h5, h6 { - margin-top: 0; - font-weight: normal; -} -h1 { - margin-bottom: 1rem; -} -h2, h3, h4, h5, h6 { - margin-bottom: 0.5rem; -} -p, ul, ol, dl { - margin-top: 0; -} -ul, ol { - padding-left: 2rem; +.m-col-t-1 { + width: calc(1 * 100% / 12); } -ul ol, ul ul, ol ol, ol ul { - margin-bottom: 0; + +.m-col-t-2 { + width: calc(2 * 100% / 12); } -main p { - text-indent: 1.5rem; - text-align: justify; + +.m-col-t-3 { + width: calc(3 * 100% / 12); } -main p.m-noindent, li > p, dd > p, table.m-table td > p { - text-indent: 0; - text-align: left; + +.m-col-t-4 { + width: calc(4 * 100% / 12); } -blockquote { - margin-top: 0; - margin-left: 1rem; - margin-right: 1rem; - padding: 1rem; - border-left-style: solid; - border-left-width: 0.25rem; + +.m-col-t-5 { + width: calc(5 * 100% / 12); } -hr { - width: 75%; - border-width: 0.0625rem; - border-style: solid; + +.m-col-t-6 { + width: calc(6 * 100% / 12); } -blockquote, hr { - border-color: #c7cfd9; + +.m-col-t-7 { + width: calc(7 * 100% / 12); } -strong, .m-text.m-strong { font-weight: bold; } -em, .m-text.m-em { font-style: italic; } -s, .m-text.m-s { text-decoration: line-through; } -sub, sup, .m-text.m-sub, .m-text.m-sup { - font-size: 0.75rem; - line-height: 0; - position: relative; - vertical-align: baseline; + +.m-col-t-8 { + width: calc(8 * 100% / 12); } -sup, .m-text.m-sup { top: -0.35rem; } -sub, .m-text.m-sub { bottom: -0.2rem; } -abbr { - cursor: help; - text-decoration: underline dotted; + +.m-col-t-9 { + width: calc(9 * 100% / 12); } -a { - color: #205275; + +.m-col-t-10 { + width: calc(10 * 100% / 12); } -a.m-flat { - text-decoration: none; + +.m-col-t-11 { + width: calc(11 * 100% / 12); } -a:hover, a:focus, a:active { - color: #2f73a3; + +.m-col-t-12 { + width: calc(12 * 100% / 12); } -a img { border: 0; } -svg a { cursor: pointer; } -mark { - padding: 0.0625rem; - background-color: #f0c63e; - color: #4c93d3; + +.m-push-t-1 { + left: calc(1 * 100% / 12); } -.m-link-wrap { - word-break: break-all; + +.m-push-t-2 { + left: calc(2 * 100% / 12); } -pre, code { - font-family: 'Source Code Pro', monospace, monospace, monospace; - font-size: 0.8em; - color: #000000; - background-color: #f2f7fa; + +.m-push-t-3 { + left: calc(3 * 100% / 12); } -pre.m-console, code.m-console { - color: #000000; - background-color: #000000; + +.m-push-t-4 { + left: calc(4 * 100% / 12); } -pre { - padding: 0.5rem 1rem; - border-radius: 0.2rem; - overflow-x: auto; - margin-top: 0; + +.m-push-t-5 { + left: calc(5 * 100% / 12); } -pre.m-console-wrap { - white-space: pre-wrap; - word-break: break-all; + +.m-push-t-6 { + left: calc(6 * 100% / 12); } -code { - padding: 0.125rem; + +.m-push-t-7 { + left: calc(7 * 100% / 12); } -*:focus { outline-color: #2f73a3; } -div.m-scroll { - max-width: 100%; - overflow-x: auto; + +.m-push-t-8 { + left: calc(8 * 100% / 12); } -.m-fullwidth { - width: 100%; + +.m-push-t-9 { + left: calc(9 * 100% / 12); } -.m-spacing-150 { - line-height: 1.5rem; + +.m-push-t-10 { + left: calc(10 * 100% / 12); } -.m-text-center, .m-text-center.m-noindent, table.m-table th.m-text-center, .m-text-center p { - text-align: center; + +.m-push-t-11 { + left: calc(11 * 100% / 12); } -.m-text-left, .m-text-left.m-noindent, table.m-table th.m-text-left, .m-text-right p { - text-align: left; + +.m-pull-t-1 { + right: calc(1 * 100% / 12); } -.m-text-right, .m-text-right.m-noindent, table.m-table th.m-text-right, .m-text-right p { - text-align: right; + +.m-pull-t-2 { + right: calc(2 * 100% / 12); } -.m-text-top, table.m-table th.m-text-top, table.m-table td.m-text-top { - vertical-align: top; + +.m-pull-t-3 { + right: calc(3 * 100% / 12); } -.m-text-middle, table.m-table th.m-text-middle, table.m-table td.m-text-middle { - vertical-align: middle; + +.m-pull-t-4 { + right: calc(4 * 100% / 12); } -.m-text-bottom, table.m-table th.m-text-bottom, table.m-table td.m-text-bottom { - vertical-align: bottom; + +.m-pull-t-5 { + right: calc(5 * 100% / 12); } -.m-text.m-tiny { font-size: 50.0%; } -.m-text.m-small { font-size: 85.4%; } -.m-text.m-big { font-size: 117%; } -h1 .m-thin, h2 .m-thin, h3 .m-thin, h4 .m-thin, h5 .m-thin, h6 .m-thin { - font-weight: normal; + +.m-pull-t-6 { + right: calc(6 * 100% / 12); } -ul.m-unstyled, ol.m-unstyled { - list-style-type: none; - padding-left: 0; + +.m-pull-t-7 { + right: calc(7 * 100% / 12); } -ul[class*='m-block-'], ol[class*='m-block-'] { - padding-left: 0; + +.m-pull-t-8 { + right: calc(8 * 100% / 12); } -ul[class*='m-block-'] li, ol[class*='m-block-'] li { - display: inline; + +.m-pull-t-9 { + right: calc(9 * 100% / 12); } -ul[class*='m-block-bar-'] li:not(:last-child)::after, ol[class*='m-block-bar-'] li:not(:last-child)::after { - content: " | "; + +.m-pull-t-10 { + right: calc(10 * 100% / 12); } -ul[class*='m-block-dot-'] li:not(:last-child)::after, ol[class*='m-block-dot-'] li:not(:last-child)::after { - content: " • "; + +.m-pull-t-11 { + right: calc(11 * 100% / 12); } + @media screen and (min-width: 576px) { - ul.m-block-bar-s, ol.m-block-bar-s, - ul.m-block-dot-s, ol.m-block-dot-s { padding-left: 2rem; } - ul.m-block-bar-s li, ol.m-block-bar-s li, - ul.m-block-dot-s li, ol.m-block-dot-s li { display: list-item; } - ul.m-block-bar-s li:not(:last-child)::after, ol.m-block-bar-s li:not(:last-child)::after, - ul.m-block-dot-s li:not(:last-child)::after, ol.m-block-dot-s li:not(:last-child)::after { content: ""; } + .m-container { + width: 560px; + } + + .m-container-inflatable .m-col-s-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { + margin-left: -10%; + margin-right: -10%; + } + + .m-container-inflatable .m-col-s-10 .m-container-inflate.m-left-s { + margin-left: -10%; + } + + .m-container-inflatable .m-col-s-10 .m-container-inflate.m-right-s { + margin-right: -10%; + } + + [class*='m-col-s-'] { + float: left; + } + + .m-left-s { + padding-right: 1rem; + float: left; + } + + .m-right-s, [class*='m-col-s-'].m-right-s { + padding-left: 1rem; + float: right; + } + + .m-center-s, [class*='m-col-s-'].m-center-s { + margin-left: auto; + margin-right: auto; + float: none; + } + + .m-col-s-1 { + width: calc(1 * 100% / 12); + } + + .m-col-s-2 { + width: calc(2 * 100% / 12); + } + + .m-col-s-3 { + width: calc(3 * 100% / 12); + } + + .m-col-s-4 { + width: calc(4 * 100% / 12); + } + + .m-col-s-5 { + width: calc(5 * 100% / 12); + } + + .m-col-s-6 { + width: calc(6 * 100% / 12); + } + + .m-col-s-7 { + width: calc(7 * 100% / 12); + } + + .m-col-s-8 { + width: calc(8 * 100% / 12); + } + + .m-col-s-9 { + width: calc(9 * 100% / 12); + } + + .m-col-s-10 { + width: calc(10 * 100% / 12); + } + + .m-col-s-11 { + width: calc(11 * 100% / 12); + } + + .m-col-s-12 { + width: calc(12 * 100% / 12); + } + + .m-push-s-0 { + left: calc(0 * 100% / 12); + } + + .m-push-s-1 { + left: calc(1 * 100% / 12); + } + + .m-push-s-2 { + left: calc(2 * 100% / 12); + } + + .m-push-s-3 { + left: calc(3 * 100% / 12); + } + + .m-push-s-4 { + left: calc(4 * 100% / 12); + } + + .m-push-s-5 { + left: calc(5 * 100% / 12); + } + + .m-push-s-6 { + left: calc(6 * 100% / 12); + } + + .m-push-s-7 { + left: calc(7 * 100% / 12); + } + + .m-push-s-8 { + left: calc(8 * 100% / 12); + } + + .m-push-s-9 { + left: calc(9 * 100% / 12); + } + + .m-push-s-10 { + left: calc(10 * 100% / 12); + } + + .m-push-s-11 { + left: calc(11 * 100% / 12); + } + + .m-pull-s-0 { + right: calc(0 * 100% / 12); + } + + .m-pull-s-1 { + right: calc(1 * 100% / 12); + } + + .m-pull-s-2 { + right: calc(2 * 100% / 12); + } + + .m-pull-s-3 { + right: calc(3 * 100% / 12); + } + + .m-pull-s-4 { + right: calc(4 * 100% / 12); + } + + .m-pull-s-5 { + right: calc(5 * 100% / 12); + } + + .m-pull-s-6 { + right: calc(6 * 100% / 12); + } + + .m-pull-s-7 { + right: calc(7 * 100% / 12); + } + + .m-pull-s-8 { + right: calc(8 * 100% / 12); + } + + .m-pull-s-9 { + right: calc(9 * 100% / 12); + } + + .m-pull-s-10 { + right: calc(10 * 100% / 12); + } + + .m-pull-s-11 { + right: calc(11 * 100% / 12); + } + + .m-clearfix-t::after { + display: none; + } + + .m-hide-s { + display: none; + } + + .m-show-s { + display: block; + } + + .m-col-s-none { + width: auto; + float: none; + } } + @media screen and (min-width: 768px) { - ul.m-block-bar-m, ol.m-block-bar-m, - ul.m-block-dot-m, ol.m-block-dot-m { padding-left: 2rem; } - ul.m-block-bar-m li, ol.m-block-bar-m li, - ul.m-block-dot-m li, ol.m-block-dot-m li { display: list-item; } - ul.m-block-bar-m li:not(:last-child)::after, ol.m-block-bar-m li:not(:last-child)::after, - ul.m-block-dot-m li:not(:last-child)::after, ol.m-block-dot-m li:not(:last-child)::after { content: ""; } + .m-container { + width: 750px; + } + + .m-container-inflatable .m-col-m-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { + margin-left: -10%; + margin-right: -10%; + } + + .m-container-inflatable .m-col-m-10 .m-container-inflate.m-left-m { + margin-left: -10%; + } + + .m-container-inflatable .m-col-m-10 .m-container-inflate.m-right-m { + margin-right: -10%; + } + + [class*='m-col-m-'] { + float: left; + } + + .m-left-m { + padding-right: 1rem; + float: left; + } + + .m-right-m, [class*='m-col-m-'].m-right-m { + padding-left: 1rem; + float: right; + } + + .m-center-m, [class*='m-col-m-'].m-center-m { + margin-left: auto; + margin-right: auto; + float: none; + } + + .m-col-m-1 { + width: calc(1 * 100% / 12); + } + + .m-col-m-2 { + width: calc(2 * 100% / 12); + } + + .m-col-m-3 { + width: calc(3 * 100% / 12); + } + + .m-col-m-4 { + width: calc(4 * 100% / 12); + } + + .m-col-m-5 { + width: calc(5 * 100% / 12); + } + + .m-col-m-6 { + width: calc(6 * 100% / 12); + } + + .m-col-m-7 { + width: calc(7 * 100% / 12); + } + + .m-col-m-8 { + width: calc(8 * 100% / 12); + } + + .m-col-m-9 { + width: calc(9 * 100% / 12); + } + + .m-col-m-10 { + width: calc(10 * 100% / 12); + } + + .m-col-m-11 { + width: calc(11 * 100% / 12); + } + + .m-col-m-12 { + width: calc(12 * 100% / 12); + } + + .m-push-m-0 { + left: calc(0 * 100% / 12); + } + + .m-push-m-1 { + left: calc(1 * 100% / 12); + } + + .m-push-m-2 { + left: calc(2 * 100% / 12); + } + + .m-push-m-3 { + left: calc(3 * 100% / 12); + } + + .m-push-m-4 { + left: calc(4 * 100% / 12); + } + + .m-push-m-5 { + left: calc(5 * 100% / 12); + } + + .m-push-m-6 { + left: calc(6 * 100% / 12); + } + + .m-push-m-7 { + left: calc(7 * 100% / 12); + } + + .m-push-m-8 { + left: calc(8 * 100% / 12); + } + + .m-push-m-9 { + left: calc(9 * 100% / 12); + } + + .m-push-m-10 { + left: calc(10 * 100% / 12); + } + + .m-push-m-11 { + left: calc(11 * 100% / 12); + } + + .m-pull-m-0 { + right: calc(0 * 100% / 12); + } + + .m-pull-m-1 { + right: calc(1 * 100% / 12); + } + + .m-pull-m-2 { + right: calc(2 * 100% / 12); + } + + .m-pull-m-3 { + right: calc(3 * 100% / 12); + } + + .m-pull-m-4 { + right: calc(4 * 100% / 12); + } + + .m-pull-m-5 { + right: calc(5 * 100% / 12); + } + + .m-pull-m-6 { + right: calc(6 * 100% / 12); + } + + .m-pull-m-7 { + right: calc(7 * 100% / 12); + } + + .m-pull-m-8 { + right: calc(8 * 100% / 12); + } + + .m-pull-m-9 { + right: calc(9 * 100% / 12); + } + + .m-pull-m-10 { + right: calc(10 * 100% / 12); + } + + .m-pull-m-11 { + right: calc(11 * 100% / 12); + } + + .m-clearfix-s::after { + display: none; + } + + .m-hide-m { + display: none; + } + + .m-show-m { + display: block; + } + + .m-col-m-none { + width: auto; + float: none; + } } + @media screen and (min-width: 992px) { - ul.m-block-bar-l, ol.m-block-bar-l, - ul.m-block-dot-l, ol.m-block-dot-l { padding-left: 2rem; } - ul.m-block-bar-l li, ol.m-block-bar-l li, - ul.m-block-dot-l li, ol.m-block-dot-l li { display: list-item; } - ul.m-block-bar-l li:not(:last-child)::after, ol.m-block-bar-l li:not(:last-child)::after, - ul.m-block-dot-l li:not(:last-child)::after, ol.m-block-dot-l li:not(:last-child)::after { content: ""; } -} -p.m-poem { - text-indent: 0; - text-align: left; - margin-left: 1.5rem; -} -p.m-transition { - color: #c7cfd9; - text-indent: 0; - text-align: center; - font-size: 2rem; -} -dl.m-diary { - margin-bottom: 1.25rem; -} -dl.m-diary:last-child { - margin-bottom: 0.25rem; -} -dl.m-diary dt { - font-weight: bold; - width: 6rem; - float: left; - clear: both; - padding-top: 0.25rem; -} -dl.m-diary dd { - padding-top: 0.25rem; - padding-left: 6rem; - margin-left: 0; -} -a.m-footnote, dl.m-footnote dd span.m-footnote { - top: -0.35rem; - font-size: 0.75rem; - line-height: 0; - position: relative; - vertical-align: baseline; -} -a.m-footnote, dl.m-footnote dd span.m-footnote a { - text-decoration: none; -} -a.m-footnote::before { content: '['; } -a.m-footnote::after { content: ']'; } -dl.m-footnote dt { - width: 1.5rem; - float: left; - clear: both; -} -dl.m-footnote dd { - margin-left: 1.5rem; -} -dl.m-footnote { - font-size: 85.4%; + .m-container { + width: 960px; + } + + .m-container-inflatable .m-col-l-10 .m-container-inflate:not([class*='m-left-']):not([class*='m-right-']) { + margin-left: -10%; + margin-right: -10%; + } + + .m-container-inflatable .m-col-l-10 .m-container-inflate.m-left-l { + margin-left: -10%; + } + + .m-container-inflatable .m-col-l-10 .m-container-inflate.m-right-l { + margin-right: -10%; + } + + [class*='m-col-l-'] { + float: left; + } + + .m-left-l { + padding-right: 1rem; + float: left; + } + + .m-right-l, [class*='m-col-l-'].m-right-l { + padding-left: 1rem; + float: right; + } + + .m-center-l, [class*='m-col-l-'].m-center-l { + margin-left: auto; + margin-right: auto; + float: none; + } + + .m-col-l-1 { + width: calc(1 * 100% / 12); + } + + .m-col-l-2 { + width: calc(2 * 100% / 12); + } + + .m-col-l-3 { + width: calc(3 * 100% / 12); + } + + .m-col-l-4 { + width: calc(4 * 100% / 12); + } + + .m-col-l-5 { + width: calc(5 * 100% / 12); + } + + .m-col-l-6 { + width: calc(6 * 100% / 12); + } + + .m-col-l-7 { + width: calc(7 * 100% / 12); + } + + .m-col-l-8 { + width: calc(8 * 100% / 12); + } + + .m-col-l-9 { + width: calc(9 * 100% / 12); + } + + .m-col-l-10 { + width: calc(10 * 100% / 12); + } + + .m-col-l-11 { + width: calc(11 * 100% / 12); + } + + .m-col-l-12 { + width: calc(12 * 100% / 12); + } + + .m-push-l-0 { + left: calc(0 * 100% / 12); + } + + .m-push-l-1 { + left: calc(1 * 100% / 12); + } + + .m-push-l-2 { + left: calc(2 * 100% / 12); + } + + .m-push-l-3 { + left: calc(3 * 100% / 12); + } + + .m-push-l-4 { + left: calc(4 * 100% / 12); + } + + .m-push-l-5 { + left: calc(5 * 100% / 12); + } + + .m-push-l-6 { + left: calc(6 * 100% / 12); + } + + .m-push-l-7 { + left: calc(7 * 100% / 12); + } + + .m-push-l-8 { + left: calc(8 * 100% / 12); + } + + .m-push-l-9 { + left: calc(9 * 100% / 12); + } + + .m-push-l-10 { + left: calc(10 * 100% / 12); + } + + .m-push-l-11 { + left: calc(11 * 100% / 12); + } + + .m-pull-l-0 { + right: calc(0 * 100% / 12); + } + + .m-pull-l-1 { + right: calc(1 * 100% / 12); + } + + .m-pull-l-2 { + right: calc(2 * 100% / 12); + } + + .m-pull-l-3 { + right: calc(3 * 100% / 12); + } + + .m-pull-l-4 { + right: calc(4 * 100% / 12); + } + + .m-pull-l-5 { + right: calc(5 * 100% / 12); + } + + .m-pull-l-6 { + right: calc(6 * 100% / 12); + } + + .m-pull-l-7 { + right: calc(7 * 100% / 12); + } + + .m-pull-l-8 { + right: calc(8 * 100% / 12); + } + + .m-pull-l-9 { + right: calc(9 * 100% / 12); + } + + .m-pull-l-10 { + right: calc(10 * 100% / 12); + } + + .m-pull-l-11 { + right: calc(11 * 100% / 12); + } + + .m-clearfix-m::after { + display: none; + } + + .m-hide-l { + display: none; + } + + .m-show-l { + display: block; + } + + .m-col-l-none { + width: auto; + float: none; + } } -dl.m-footnote dd span.m-footnote a { - font-weight: bold; - font-style: italic; + +html { + font-size: 1em; + background-color: #ffffff; } -.m-note { - border-radius: 0.2rem; - padding: 1rem; + +body { + font-family: 'Source Sans Pro', sans-serif; + font-size: 1rem; + line-height: normal; + color: #000000; } -.m-frame { - background-color: #ffffff; - border-style: solid; - border-width: 0.125rem; - border-radius: 0.2rem; - border-color: #c7cfd9; - padding: 0.875rem; + +h1, h2, h3, h4, h5, h6 { + margin-top: 0; + font-weight: normal; } -.m-block { - border-style: solid; - border-width: 0.0625rem; - border-left-width: 0.25rem; - border-radius: 0.2rem; - border-color: #c7cfd9; - padding: 0.9375rem 0.9375rem 0.9375rem 0.75rem; + +h1 { + margin-bottom: 1rem; } -.m-block.m-badge::after { - content: ' '; - display: block; - clear: both; + +h2, h3, h4, h5, h6 { + margin-bottom: 0.5rem; } -.m-block.m-badge h3 { - margin-left: 5rem; + +p, ul, ol, dl { + margin-top: 0; } -.m-block.m-badge p { - margin-left: 5rem; - text-indent: 0; + +ul, ol { + padding-left: 2rem; } -.m-block.m-badge img { - width: 4rem; - height: 4rem; - border-radius: 2rem; - float: left; + +ul ol, ul ul, ol ol, ol ul { + margin-bottom: 0; } -div.m-button { - text-align: center; + +main p { + text-indent: 1.5rem; + text-align: justify; } -div.m-button a { - display: inline-block; - border-radius: 0.2rem; - padding-top: 0.75rem; - padding-bottom: 0.75rem; - padding-left: 1.5rem; - padding-right: 1.5rem; - text-decoration: none; - font-size: 1.17rem; + +main p.m-noindent, li > p, dd > p, table.m-table td > p { + text-indent: 0; + text-align: left; } -div.m-button.m-fullwidth a { - display: block; - padding-left: 0.5rem; - padding-right: 0.5rem; + +blockquote { + margin-top: 0; + margin-left: 1rem; + margin-right: 1rem; + padding: 1rem; + border-left-style: solid; + border-left-width: 0.25rem; } -div.m-button a .m-big:first-child { - font-size: 1.37rem; - font-weight: bold; + +hr { + width: 75%; + border-width: 0.0625rem; + border-style: solid; } -div.m-button a .m-small:last-child { - font-size: 0.854rem; + +blockquote, hr { + border-color: #c7cfd9; } -.m-label { - border-radius: 0.2rem; - font-size: 75%; - font-weight: normal; - padding: 0.125rem 0.25rem; - vertical-align: 7.5%; + +strong, .m-text.m-strong { + font-weight: bold; } -.m-label.m-flat { - border-width: 0.0625rem; - border-style: solid; - border-color: #bdbdbd; - padding: 0.0625rem 0.1875rem; + +em, .m-text.m-em { + font-style: italic; } -table.m-table { - border-collapse: collapse; - margin-left: auto; - margin-right: auto; + +s, .m-text.m-s { + text-decoration: line-through; } -table.m-table.m-big { - margin-top: 1.75rem; + +sub, sup, .m-text.m-sub, .m-text.m-sup { + font-size: 0.75rem; + line-height: 0; + position: relative; + vertical-align: baseline; } -div.m-scroll > table.m-table:last-child { - margin-bottom: 0.0625rem; + +sup, .m-text.m-sup { + top: -0.35rem; } -table.m-table:not(.m-flat) tbody tr:hover { - background-color: #c7cfd9; + +sub, .m-text.m-sub { + bottom: -0.2rem; } -table.m-table th, table.m-table td { - vertical-align: top; - border-style: solid; - border-top-width: 0.0625rem; - border-left-width: 0; - border-right-width: 0; - border-bottom-width: 0; - border-color: #c7cfd9; + +abbr { + cursor: help; + text-decoration: underline dotted; } -table.m-table caption { - padding-bottom: 0.5rem; + +a { + color: #205275; } -table.m-table thead tr:first-child th, table.m-table thead tr:first-child td { - border-top-width: 0.125rem; + +a.m-flat { + text-decoration: none; } -table.m-table thead th, table.m-table thead td { - border-bottom-width: 0.125rem; - vertical-align: bottom; + +a:hover, a:focus, a:active { + color: #2f73a3; } -table.m-table tfoot th, table.m-table tfoot td { - border-top-width: 0.125rem; + +a img { + border: 0; } -table.m-table th, table.m-table td { - padding: 0.5rem; + +svg a { + cursor: pointer; } -table.m-table.m-big th, table.m-table.m-big td { - padding: 0.75rem 1rem; + +mark { + padding: 0.0625rem; + background-color: #f0c63e; + color: #4c93d3; } -table.m-table th { - text-align: left; + +.m-link-wrap { + word-break: break-all; } -table.m-table th.m-thin { - font-weight: normal; + +pre, code { + font-family: 'Source Code Pro', monospace, monospace, monospace; + font-size: 0.8em; + color: #000000; + background-color: #f2f7fa; } -table.m-table td.m-default, table.m-table th.m-default, -table.m-table td.m-primary, table.m-table th.m-primary, -table.m-table td.m-success, table.m-table th.m-success, -table.m-table td.m-warning, table.m-table th.m-warning, -table.m-table td.m-danger, table.m-table th.m-danger, -table.m-table td.m-info, table.m-table th.m-info, -table.m-table td.m-dim, table.m-table th.m-dim { - padding-left: 0.4375rem; - padding-right: 0.4375rem; - border-left-width: 0.0625rem; + +pre.m-console, code.m-console { + color: #000000; + background-color: #000000; } -table.m-table.m-big td.m-default, table.m-table.m-big th.m-default, -table.m-table.m-big td.m-primary, table.m-table.m-big th.m-primary, -table.m-table.m-big td.m-success, table.m-table.m-big th.m-success, -table.m-table.m-big td.m-warning, table.m-table.m-big th.m-warning, -table.m-table.m-big td.m-danger, table.m-table.m-big th.m-danger, -table.m-table.m-big td.m-info, table.m-table.m-big th.m-info, -table.m-table.m-big td.m-dim, table.m-table.m-big th.m-dim { - padding-left: 0.9375rem; - padding-right: 0.9375rem; - border-left-width: 0.0625rem; + +pre { + padding: 0.5rem 1rem; + border-radius: 0.2rem; + overflow-x: auto; + margin-top: 0; } -table.m-table tr.m-default td, table.m-table td.m-default, -table.m-table tr.m-default th, table.m-table th.m-default, -table.m-table tr.m-primary td, table.m-table td.m-primary, -table.m-table tr.m-primary th, table.m-table th.m-primary, -table.m-table tr.m-success td, table.m-table td.m-success, -table.m-table tr.m-success th, table.m-table th.m-success, -table.m-table tr.m-warning td, table.m-table td.m-warning, -table.m-table tr.m-warning th, table.m-table th.m-warning, -table.m-table tr.m-danger td, table.m-table td.m-danger, -table.m-table tr.m-danger th, table.m-table th.m-danger, -table.m-table tr.m-info td, table.m-table td.m-info, -table.m-table tr.m-info th, table.m-table th.m-info, -table.m-table tr.m-dim td, table.m-table td.m-dim, -table.m-table tr.m-dim th, table.m-table th.m-dim { - border-color: #ffffff; + +pre.m-console-wrap { + white-space: pre-wrap; + word-break: break-all; } -.m-note pre, .m-note code, -table.m-table tr.m-default pre, table.m-table tr.m-default code, -table.m-table td.m-default pre, table.m-table td.m-default code, -table.m-table th.m-default pre, table.m-table th.m-default code, -table.m-table tr.m-primary pre, table.m-table tr.m-primary code, -table.m-table td.m-primary pre, table.m-table td.m-primary code, -table.m-table th.m-primary pre, table.m-table th.m-primary code, -table.m-table tr.m-success pre, table.m-table tr.m-success code, -table.m-table td.m-success pre, table.m-table td.m-success code, -table.m-table th.m-success pre, table.m-table th.m-success code, -table.m-table tr.m-warning pre, table.m-table tr.m-warning code, -table.m-table td.m-warning pre, table.m-table td.m-warning code, -table.m-table th.m-warning pre, table.m-table th.m-warning code, -table.m-table tr.m-danger pre, table.m-table tr.m-danger code, -table.m-table td.m-danger pre, table.m-table td.m-danger code, -table.m-table th.m-danger pre, table.m-table th.m-danger code, -table.m-table tr.m-info pre, table.m-table tr.m-info code, -table.m-table td.m-info pre, table.m-table td.m-info code, -table.m-table th.m-info pre, table.m-table th.m-info code, -table.m-table tr.m-dim pre, table.m-table tr.m-dim code, -table.m-table td.m-dim pre, table.m-table td.m-dim code, -table.m-table th.m-dim pre, table.m-table th.m-dim code { - background-color: rgba(251, 240, 236, 0.5); + +code { + padding: 0.125rem; } -img.m-image, svg.m-image { - display: block; - margin-left: auto; - margin-right: auto; + +*:focus { + outline-color: #2f73a3; } -div.m-image { - text-align: center; + +div.m-scroll { + max-width: 100%; + overflow-x: auto; } -img.m-image, svg.m-image, div.m-image img, div.m-image svg { - max-width: 100%; - border-radius: 0.2rem; + +.m-fullwidth { + width: 100%; } -div.m-image.m-fullwidth img, div.m-image.m-fullwidth svg { - width: 100%; + +.m-spacing-150 { + line-height: 1.5rem; } -img.m-image.m-badge, div.m-image.m-badge img { - border-radius: 50%; + +.m-text-center, .m-text-center.m-noindent, table.m-table th.m-text-center, .m-text-center p { + text-align: center; } -figure.m-figure { - max-width: 100%; - margin-top: 0; - margin-left: auto; - margin-right: auto; - position: relative; - display: table; + +.m-text-left, .m-text-left.m-noindent, table.m-table th.m-text-left, .m-text-right p { + text-align: left; } -figure.m-figure:before { - position: absolute; - content: ' '; - top: 0; - bottom: 0; - left: 0; - right: 0; - z-index: -1; - border-style: solid; - border-width: 0.125rem; - border-radius: 0.2rem; - border-color: #c7cfd9; + +.m-text-right, .m-text-right.m-noindent, table.m-table th.m-text-right, .m-text-right p { + text-align: right; } -figure.m-figure.m-flat:before { - border-color: transparent; + +.m-text-top, table.m-table th.m-text-top, table.m-table td.m-text-top { + vertical-align: top; } -figure.m-figure > * { - margin-left: 1rem; - margin-right: 1rem; - display: table-caption; - caption-side: bottom; + +.m-text-middle, table.m-table th.m-text-middle, table.m-table td.m-text-middle { + vertical-align: middle; } -figure.m-figure > *:first-child { - display: inline; + +.m-text-bottom, table.m-table th.m-text-bottom, table.m-table td.m-text-bottom { + vertical-align: bottom; } -figure.m-figure > *:last-child { - margin-bottom: 1rem !important; + +.m-text.m-tiny { + font-size: 50.0%; } -figure.m-figure img, figure.m-figure svg { - position: relative; - margin-left: 0; - margin-right: 0; - margin-bottom: 0; - border-top-left-radius: 0.2rem; - border-top-right-radius: 0.2rem; - max-width: 100%; + +.m-text.m-small { + font-size: 85.4%; } -figure.m-figure.m-flat img, figure.m-figure.m-flat svg { - border-bottom-left-radius: 0.2rem; - border-bottom-right-radius: 0.2rem; + +.m-text.m-big { + font-size: 117%; } -figure.m-figure a img, figure.m-figure a svg { - margin-left: -1rem; - margin-right: -1rem; + +h1 .m-thin, h2 .m-thin, h3 .m-thin, h4 .m-thin, h5 .m-thin, h6 .m-thin { + font-weight: normal; } -figure.m-figure.m-fullwidth, figure.m-figure.m-fullwidth > * { - display: block; + +ul.m-unstyled, ol.m-unstyled { + list-style-type: none; + padding-left: 0; } -figure.m-figure.m-fullwidth > *:first-child { - display: inline; + +ul[class*='m-block-'], ol[class*='m-block-'] { + padding-left: 0; } -figure.m-figure.m-fullwidth img, figure.m-figure.m-fullwidth svg { - width: 100%; + +ul[class*='m-block-'] li, ol[class*='m-block-'] li { + display: inline; } -figure.m-figure.m-fullwidth:after { - content: ' '; - display: block; - margin-top: 1rem; - height: 1px; + +ul[class*='m-block-bar-'] li:not(:last-child)::after, ol[class*='m-block-bar-'] li:not(:last-child)::after { + content: " | "; } -.m-code-figure, .m-console-figure { - margin-top: 0; - margin-left: 0; - margin-right: 0; - position: relative; - padding: 1rem; + +ul[class*='m-block-dot-'] li:not(:last-child)::after, ol[class*='m-block-dot-'] li:not(:last-child)::after { + content: " • "; } -.m-code-figure:before, .m-console-figure:before { - position: absolute; - content: ' '; - top: 0; - bottom: 0; - left: 0; - right: 0; - z-index: -1; - border-style: solid; - border-width: 0.125rem; - border-radius: 0.2rem; + +@media screen and (min-width: 576px) { + ul.m-block-bar-s, ol.m-block-bar-s, + ul.m-block-dot-s, ol.m-block-dot-s { + padding-left: 2rem; + } + + ul.m-block-bar-s li, ol.m-block-bar-s li, + ul.m-block-dot-s li, ol.m-block-dot-s li { + display: list-item; + } + + ul.m-block-bar-s li:not(:last-child)::after, ol.m-block-bar-s li:not(:last-child)::after, + ul.m-block-dot-s li:not(:last-child)::after, ol.m-block-dot-s li:not(:last-child)::after { + content: ""; + } } -.m-code-figure:before { - border-color: #f2f7fa; + +@media screen and (min-width: 768px) { + ul.m-block-bar-m, ol.m-block-bar-m, + ul.m-block-dot-m, ol.m-block-dot-m { + padding-left: 2rem; + } + + ul.m-block-bar-m li, ol.m-block-bar-m li, + ul.m-block-dot-m li, ol.m-block-dot-m li { + display: list-item; + } + + ul.m-block-bar-m li:not(:last-child)::after, ol.m-block-bar-m li:not(:last-child)::after, + ul.m-block-dot-m li:not(:last-child)::after, ol.m-block-dot-m li:not(:last-child)::after { + content: ""; + } } -.m-console-figure:before { - border-color: #000000; + +@media screen and (min-width: 992px) { + ul.m-block-bar-l, ol.m-block-bar-l, + ul.m-block-dot-l, ol.m-block-dot-l { + padding-left: 2rem; + } + + ul.m-block-bar-l li, ol.m-block-bar-l li, + ul.m-block-dot-l li, ol.m-block-dot-l li { + display: list-item; + } + + ul.m-block-bar-l li:not(:last-child)::after, ol.m-block-bar-l li:not(:last-child)::after, + ul.m-block-dot-l li:not(:last-child)::after, ol.m-block-dot-l li:not(:last-child)::after { + content: ""; + } } -.m-code-figure.m-flat:before, .m-console-figure.m-flat:before { - border-color: transparent; -} -.m-code-figure > pre:first-child, .m-console-figure > pre:first-child { - position: relative; - margin: -1rem -1rem 1rem -1rem; - border-bottom-left-radius: 0; - border-bottom-right-radius: 0; + +p.m-poem { + text-indent: 0; + text-align: left; + margin-left: 1.5rem; } -.m-code-figure > pre.m-nopad, .m-console-figure > pre.m-nopad { - margin-left: -0.875rem; - margin-right: -0.875rem; - margin-top: -1rem; - margin-bottom: -0.875rem; - padding-left: 0.875rem; + +p.m-transition { + color: #c7cfd9; + text-indent: 0; + text-align: center; + font-size: 2rem; } -figure.m-figure figcaption, .m-code-figure figcaption, .m-console-figure figcaption { - margin-top: 0.5rem; - margin-bottom: 0.5rem; - font-weight: normal; - font-size: 1.17rem; + +dl.m-diary { + margin-bottom: 1.25rem; } -.m-imagegrid > div { - background-color: #ffffff; + +dl.m-diary:last-child { + margin-bottom: 0.25rem; } -.m-imagegrid > div > figure { - display: block; - float: left; - position: relative; - margin: 0; + +dl.m-diary dt { + font-weight: bold; + width: 6rem; + float: left; + clear: both; + padding-top: 0.25rem; } -.m-imagegrid > div > figure > div, -.m-imagegrid > div > figure > figcaption, -.m-imagegrid > div > figure > a > div, -.m-imagegrid > div > figure > a > figcaption { - position: absolute; - top: 0; - left: 0; - width: 100%; - height: 100%; - border-color: #ffffff; - border-style: solid; - border-width: 0.25rem; - padding: 0.5rem; + +dl.m-diary dd { + padding-top: 0.25rem; + padding-left: 6rem; + margin-left: 0; } -.m-imagegrid > div > figure:first-child > div, -.m-imagegrid > div > figure:first-child > figcaption, -.m-imagegrid > div > figure:first-child > a > div, -.m-imagegrid > div > figure:first-child > a > figcaption { - border-left-width: 0; + +a.m-footnote, dl.m-footnote dd span.m-footnote { + top: -0.35rem; + font-size: 0.75rem; + line-height: 0; + position: relative; + vertical-align: baseline; } -.m-imagegrid > div > figure:last-child > div, -.m-imagegrid > div > figure:last-child > figcaption, -.m-imagegrid > div > figure:last-child > a > div, -.m-imagegrid > div > figure:last-child > a > figcaption { - border-right-width: 0; + +a.m-footnote, dl.m-footnote dd span.m-footnote a { + text-decoration: none; } -.m-imagegrid > div > figure > figcaption, -.m-imagegrid > div > figure > a > figcaption { - color: transparent; - overflow: hidden; - text-overflow: ellipsis; - white-space: nowrap; - font-size: 0.75rem; + +a.m-footnote::before { + content: '['; } -.m-imagegrid > div > figure > div::before, -.m-imagegrid > div > figure > figcaption::before, -.m-imagegrid > div > figure > a > div::before, -.m-imagegrid > div > figure > a > figcaption::before { - content: ''; - display: inline-block; - height: 100%; - vertical-align: bottom; - width: 0; + +a.m-footnote::after { + content: ']'; } -.m-imagegrid > div > figure:hover > figcaption, -.m-imagegrid > div > figure:hover > a > figcaption { - background: linear-gradient(transparent 0%, transparent 75%, rgba(0, 0, 0, 0.85) 100%); - color: #ffffff; + +dl.m-footnote dt { + width: 1.5rem; + float: left; + clear: both; } -.m-imagegrid > div > figure > img, -.m-imagegrid > div > figure > a > img { - width: 100%; - height: 100%; + +dl.m-footnote dd { + margin-left: 1.5rem; } -.m-imagegrid > div::after { - display: block; - content: ' '; - clear: both; + +dl.m-footnote { + font-size: 85.4%; } -@media screen and (max-width: 767px) { - .m-imagegrid > div > figure { - float: none; - width: 100% !important; - } - .m-imagegrid > div > figure > div, - .m-imagegrid > div > figure > figcaption, - .m-imagegrid > div > figure > a > div, - .m-imagegrid > div > figure > a > figcaption { - border-left-width: 0; - border-right-width: 0; - } + +dl.m-footnote dd span.m-footnote a { + font-weight: bold; + font-style: italic; } -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-note, -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-frame, -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-block, -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-imagegrid, -.m-container-inflatable > .m-row > [class*='m-col-'] > pre, -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-code-figure, -.m-container-inflatable > .m-row > [class*='m-col-'] > .m-console-figure, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-note, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-frame, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-block, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-imagegrid, -.m-container-inflatable > .m-row > [class*='m-col-'] section > pre, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-code-figure, -.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-console-figure, -.m-container-inflatable [class*='m-center-'] > .m-note, -.m-container-inflatable [class*='m-center-'] > .m-frame, -.m-container-inflatable [class*='m-center-'] > .m-block, -.m-container-inflatable [class*='m-center-'] > .m-imagegrid, -.m-container-inflatable [class*='m-center-'] > pre, -.m-container-inflatable [class*='m-center-'] > .m-code-figure, -.m-container-inflatable [class*='m-center-'] > .m-console-figure, -.m-container-inflatable [class*='m-left-'] > .m-note, -.m-container-inflatable [class*='m-left-'] > .m-frame, -.m-container-inflatable [class*='m-left-'] > .m-block, -.m-container-inflatable [class*='m-left-'] > .m-imagegrid, -.m-container-inflatable [class*='m-left-'] > pre, -.m-container-inflatable [class*='m-left-'] > .m-code-figure, -.m-container-inflatable [class*='m-left-'] > .m-console-figure, -.m-container-inflatable [class*='m-right-'] > .m-note, -.m-container-inflatable [class*='m-right-'] > .m-frame, -.m-container-inflatable [class*='m-right-'] > .m-block, -.m-container-inflatable [class*='m-right-'] > .m-imagegrid, -.m-container-inflatable [class*='m-right-'] > pre, -.m-container-inflatable [class*='m-right-'] > .m-code-figure, -.m-container-inflatable [class*='m-right-'] > .m-console-figure, -.m-container-inflatable .m-container-inflate > .m-note, -.m-container-inflatable .m-container-inflate > .m-frame, -.m-container-inflatable .m-container-inflate > .m-block, -.m-container-inflatable .m-container-inflate > .m-imagegrid, -.m-container-inflatable .m-container-inflate > pre, -.m-container-inflatable .m-container-inflate > .m-code-figure, -.m-container-inflatable .m-container-inflate > .m-console-figure -{ - margin-left: -1rem; - margin-right: -1rem; + +.m-note { + border-radius: 0.2rem; + padding: 1rem; } -@media screen and (min-width: 576px) { - .m-container-inflatable .m-center-s > .m-note, - .m-container-inflatable .m-center-s > .m-frame, - .m-container-inflatable .m-center-s > .m-block, - .m-container-inflatable .m-center-s > .m-imagegrid, - .m-container-inflatable .m-center-s > pre, - .m-container-inflatable .m-center-s > .m-code-figure, - .m-container-inflatable .m-center-s > .m-console-figure { - margin-left: -1rem; - margin-right: -1rem; - } - .m-container-inflatable .m-left-s > .m-note, - .m-container-inflatable .m-left-s > .m-frame, - .m-container-inflatable .m-left-s > .m-block, - .m-container-inflatable .m-left-s > .m-imagegrid, - .m-container-inflatable .m-left-s > pre, - .m-container-inflatable .m-left-s > .m-code-figure, - .m-container-inflatable .m-left-s > .m-console-figure { - margin-left: -1rem; - margin-right: 0; - } - .m-container-inflatable .m-right-s > .m-note, - .m-container-inflatable .m-right-s > .m-frame, - .m-container-inflatable .m-right-s > .m-block, - .m-container-inflatable .m-right-s > .m-imagegrid, - .m-container-inflatable .m-right-s > pre, - .m-container-inflatable .m-right-s > .m-code-figure, - .m-container-inflatable .m-right-s > .m-console-figure { - margin-left: 0; - margin-right: -1rem; - } - .m-container-inflatable > .m-row > .m-col-s-10 > .m-imagegrid.m-container-inflate, - .m-container-inflatable > .m-row > .m-col-s-10 section > .m-imagegrid.m-container-inflate { - margin-left: -10%; - margin-right: -10%; - } + +.m-frame { + background-color: #ffffff; + border-style: solid; + border-width: 0.125rem; + border-radius: 0.2rem; + border-color: #c7cfd9; + padding: 0.875rem; } -@media screen and (min-width: 768px) { - .m-container-inflatable .m-center-m > .m-note, - .m-container-inflatable .m-center-m > .m-frame, - .m-container-inflatable .m-center-m > .m-block, - .m-container-inflatable .m-center-m > .m-imagegrid, - .m-container-inflatable .m-center-m > pre, - .m-container-inflatable .m-center-m > .m-code-figure, - .m-container-inflatable .m-center-m > .m-console-figure { - margin-left: -1rem; - margin-right: -1rem; - } - .m-container-inflatable .m-left-m > .m-note, - .m-container-inflatable .m-left-m > .m-frame, - .m-container-inflatable .m-left-m > .m-block, - .m-container-inflatable .m-left-m > .m-imagegrid, - .m-container-inflatable .m-left-m > pre, - .m-container-inflatable .m-left-m > .m-code-figure, - .m-container-inflatable .m-left-m > .m-console-figure { - margin-left: -1rem; - margin-right: 0; - } - .m-container-inflatable .m-right-m > .m-note, - .m-container-inflatable .m-right-m > .m-frame, - .m-container-inflatable .m-right-m > .m-block, - .m-container-inflatable .m-right-m > .m-imagegrid, - .m-container-inflatable .m-right-m > pre, - .m-container-inflatable .m-right-m > .m-code-figure, - .m-container-inflatable .m-right-m > .m-console-figure { - margin-left: 0; - margin-right: -1rem; - } - .m-container-inflatable > .m-row > .m-col-m-10 > .m-imagegrid.m-container-inflate, - .m-container-inflatable > .m-row > .m-col-m-10 section > .m-imagegrid.m-container-inflate { - margin-left: -10%; - margin-right: -10%; - } + +.m-block { + border-style: solid; + border-width: 0.0625rem; + border-left-width: 0.25rem; + border-radius: 0.2rem; + border-color: #c7cfd9; + padding: 0.9375rem 0.9375rem 0.9375rem 0.75rem; } -@media screen and (min-width: 992px) { - .m-container-inflatable .m-center-l > .m-note, - .m-container-inflatable .m-center-l > .m-frame, - .m-container-inflatable .m-center-l > .m-block, - .m-container-inflatable .m-center-l > .m-imagegrid, - .m-container-inflatable .m-center-l > pre, - .m-container-inflatable .m-center-l > .m-code-figure, - .m-container-inflatable .m-center-l > .m-console-figure { - margin-left: -1rem; - margin-right: -1rem; - } - .m-container-inflatable .m-left-l > .m-note, - .m-container-inflatable .m-left-l > .m-frame, - .m-container-inflatable .m-left-l > .m-block, - .m-container-inflatable .m-left-l > .m-imagegrid, - .m-container-inflatable .m-left-l > pre, - .m-container-inflatable .m-left-l > .m-code-figure, - .m-container-inflatable .m-left-l > .m-console-figure { - margin-left: -1rem; - margin-right: 0; - } - .m-container-inflatable .m-right-l > .m-note, - .m-container-inflatable .m-right-l > .m-frame, - .m-container-inflatable .m-right-l > .m-block, - .m-container-inflatable .m-right-l > .m-imagegrid, - .m-container-inflatable .m-right-l > pre, - .m-container-inflatable .m-right-l > .m-code-figure, - .m-container-inflatable .m-right-l > .m-console-figure { - margin-left: 0; - margin-right: -1rem; - } - .m-container-inflatable > .m-row > .m-col-l-10 > .m-imagegrid.m-container-inflate, - .m-container-inflatable > .m-row > .m-col-l-10 section > .m-imagegrid.m-container-inflate { - margin-left: -10%; - margin-right: -10%; - } + +.m-block.m-badge::after { + content: ' '; + display: block; + clear: both; } -pre.m-code span.hll { - margin-left: -1.0rem; - margin-right: -1.0rem; - padding-left: 1.0rem; + +.m-block.m-badge h3 { + margin-left: 5rem; } -.m-code.m-inverted > span, .m-console.m-inverted > span { - opacity: 0.3333; + +.m-block.m-badge p { + margin-left: 5rem; + text-indent: 0; } -.m-code.m-inverted > span.hll, .m-console.m-inverted > span.hll { - opacity: 1; - background-color: transparent; - border-color: transparent; -} -.m-code.m-inverted { color: rgba(91, 91, 91, 0.33); } -.m-console.m-inverted { color: rgba(91, 91, 91, 0.33); } -.m-code.m-inverted > span.hll { color: #000000; } -.m-cosole.m-inverted > span.hll { color: #000000; } -.m-code-color { - display: inline-block; - width: 0.75rem; - height: 0.75rem; - vertical-align: -0.05rem; - margin-left: 0.2rem; - margin-right: 0.1rem; - border-radius: 0.1rem; + +.m-block.m-badge img { + width: 4rem; + height: 4rem; + border-radius: 2rem; + float: left; } -div.m-math { - overflow-x: auto; - overflow-y: hidden; + +div.m-button { + text-align: center; } -div.m-math svg { - margin-left: auto; - margin-right: auto; - display: block; + +div.m-button a { + display: inline-block; + border-radius: 0.2rem; + padding-top: 0.75rem; + padding-bottom: 0.75rem; + padding-left: 1.5rem; + padding-right: 1.5rem; + text-decoration: none; + font-size: 1.17rem; } -div.m-button a svg.m-math { fill: #ffffff; } -div.m-button.m-flat a svg.m-math { fill: #000000; } -div.m-button.m-flat a:hover svg.m-math, div.m-button.m-default a:focus svg.m-math, -div.m-button.m-default a:active svg.m-math { - fill: #2f73a3; + +div.m-button.m-fullwidth a { + display: block; + padding-left: 0.5rem; + padding-right: 0.5rem; } -.m-graph { font-size: 1em; } -div.m-plot svg, div.m-graph svg { - max-width: 100%; - margin-left: auto; - margin-right: auto; - display: block; -} -div.m-plot .m-background { fill: #f2f7fa; } -div.m-plot svg .m-label { font-size: 11px; } -div.m-plot svg .m-title { font-size: 13px; } -div.m-plot svg .m-label, div.m-plot svg .m-title { fill: #000000; } -div.m-plot svg .m-line { - stroke: #000000; - stroke-width: 0.8; + +div.m-button a .m-big:first-child { + font-size: 1.37rem; + font-weight: bold; } -div.m-plot svg .m-error { - stroke: #000000; - stroke-width: 1.5; + +div.m-button a .m-small:last-child { + font-size: 0.854rem; } -div.m-plot svg .m-label.m-dim { fill: #bdbdbd; } -.m-graph g.m-edge path, .m-graph g.m-node.m-flat ellipse, -.m-graph g.m-node.m-flat polygon { - fill: none; + +.m-label { + border-radius: 0.2rem; + font-size: 75%; + font-weight: normal; + padding: 0.125rem 0.25rem; + vertical-align: 7.5%; } -.m-graph g.m-node:not(.m-flat) text { - fill: #ffffff; + +.m-label.m-flat { + border-width: 0.0625rem; + border-style: solid; + border-color: #bdbdbd; + padding: 0.0625rem 0.1875rem; } -figure.m-figure > svg.m-math:first-child, -figure.m-figure > svg.m-graph:first-child { - padding: 1rem; - box-sizing: content-box; + +table.m-table { + border-collapse: collapse; + margin-left: auto; + margin-right: auto; } -figure.m-figure:not(.m-flat) > svg.m-math:first-child, -figure.m-figure:not(.m-flat) > svg.m-graph:first-child { - background-color: #c7cfd9; + +table.m-table.m-big { + margin-top: 1.75rem; } -.m-block.m-default { border-left-color: #c7cfd9; } -.m-block.m-default h3, .m-block.m-default h4, .m-block.m-default h5, .m-block.m-default h6, -.m-text.m-default, .m-label.m-flat.m-default { - color: #000000; + +div.m-scroll > table.m-table:last-child { + margin-bottom: 0.0625rem; } -.m-block.m-default h3 a, .m-block.m-default h4 a, .m-block.m-default h5 a, .m-block.m-default h6 a { - color: #205275; + +table.m-table:not(.m-flat) tbody tr:hover { + background-color: #c7cfd9; } -.m-block.m-primary { border-left-color: #205275; } -.m-block.m-primary h3, .m-block.m-primary h4, .m-block.m-primary h5, .m-block.m-primary h6, -.m-block.m-primary h3 a, .m-block.m-primary h4 a, .m-block.m-primary h5 a, .m-block.m-primary h6 a, -.m-text.m-primary, .m-label.m-flat.m-primary { - color: #205275; + +table.m-table th, table.m-table td { + vertical-align: top; + border-style: solid; + border-top-width: 0.0625rem; + border-left-width: 0; + border-right-width: 0; + border-bottom-width: 0; + border-color: #c7cfd9; } -.m-block.m-success { border-left-color: #31c25d; } -.m-block.m-success h3, .m-block.m-success h4, .m-block.m-success h5, .m-block.m-success h6, -.m-block.m-success h3 a, .m-block.m-success h4 a, .m-block.m-success h5 a, .m-block.m-success h6 a, -.m-text.m-success, .m-label.m-flat.m-success { - color: #31c25d; + +table.m-table caption { + padding-bottom: 0.5rem; } -.m-block.m-warning { border-left-color: #d19600; } -.m-block.m-warning h3, .m-block.m-warning h4, .m-block.m-warning h5, .m-block.m-warning h6, -.m-block.m-warning h3 a, .m-block.m-warning h4 a, .m-block.m-warning h5 a, .m-block.m-warning h6 a, -.m-text.m-warning, .m-label.m-flat.m-warning { - color: #d19600; + +table.m-table thead tr:first-child th, table.m-table thead tr:first-child td { + border-top-width: 0.125rem; } -.m-block.m-danger { border-left-color: #f60000; } -.m-block.m-danger h3, .m-block.m-danger h4, .m-block.m-danger h5, .m-block.m-danger h6, -.m-block.m-danger h3 a, .m-block.m-danger h4 a, .m-block.m-danger h5 a, .m-block.m-danger h6 a, -.m-text.m-danger, .m-label.m-flat.m-danger { - color: #f60000; + +table.m-table thead th, table.m-table thead td { + border-bottom-width: 0.125rem; + vertical-align: bottom; } -.m-block.m-info { border-left-color: #2e7dc5; } -.m-block.m-info h3, .m-block.m-info h4, .m-block.m-info h5, .m-block.m-info h6, -.m-block.m-info h3 a, .m-block.m-info h4 a, .m-block.m-info h5 a, .m-block.m-info h6 a, -.m-text.m-info, .m-label.m-flat.m-info { - color: #2e7dc5; + +table.m-table tfoot th, table.m-table tfoot td { + border-top-width: 0.125rem; } -.m-block.m-dim { border-left-color: #bdbdbd; } -.m-block.m-dim, .m-text.m-dim, .m-label.m-flat.m-dim { - color: #bdbdbd; + +table.m-table th, table.m-table td { + padding: 0.5rem; } -.m-block.m-dim a, .m-text.m-dim a { color: #c0c0c0; } -.m-block.m-dim a:hover, .m-block.m-dim a:focus, .m-block.m-dim a:active, -.m-text.m-dim a:hover, .m-text.m-dim a:focus, .m-text.m-dim a:active { - color: #949494; + +table.m-table.m-big th, table.m-table.m-big td { + padding: 0.75rem 1rem; } -.m-block.m-flat { border-color: transparent; } -.m-block.m-flat h3, .m-block.m-flat h4, .m-block.m-flat h5, .m-block.m-flat h6 { - color: #000000; + +table.m-table th { + text-align: left; } -.m-block.m-default h3 a:hover, .m-block.m-default h3 a:focus, .m-block.m-default h3 a:active, -.m-block.m-default h4 a:hover, .m-block.m-default h4 a:focus, .m-block.m-default h4 a:active, -.m-block.m-default h5 a:hover, .m-block.m-default h5 a:focus, .m-block.m-default h5 a:active, -.m-block.m-default h6 a:hover, .m-block.m-default h6 a:focus, .m-block.m-default h6 a:active { - color: #205275; + +table.m-table th.m-thin { + font-weight: normal; } -.m-block.m-primary h3 a:hover, .m-block.m-primary h3 a:focus, .m-block.m-primary h3 a:active, -.m-block.m-primary h4 a:hover, .m-block.m-primary h4 a:focus, .m-block.m-primary h4 a:active, -.m-block.m-primary h5 a:hover, .m-block.m-primary h5 a:focus, .m-block.m-primary h5 a:active, -.m-block.m-primary h6 a:hover, .m-block.m-primary h6 a:focus, .m-block.m-primary h6 a:active { - color: #000000; + +table.m-table td.m-default, table.m-table th.m-default, +table.m-table td.m-primary, table.m-table th.m-primary, +table.m-table td.m-success, table.m-table th.m-success, +table.m-table td.m-warning, table.m-table th.m-warning, +table.m-table td.m-danger, table.m-table th.m-danger, +table.m-table td.m-info, table.m-table th.m-info, +table.m-table td.m-dim, table.m-table th.m-dim { + padding-left: 0.4375rem; + padding-right: 0.4375rem; + border-left-width: 0.0625rem; } -.m-block.m-success h3 a:hover, .m-block.m-success h3 a:focus, .m-block.m-success h3 a:active, -.m-block.m-success h4 a:hover, .m-block.m-success h4 a:focus, .m-block.m-success h4 a:active, -.m-block.m-success h5 a:hover, .m-block.m-success h5 a:focus, .m-block.m-success h5 a:active, -.m-block.m-success h6 a:hover, .m-block.m-success h6 a:focus, .m-block.m-success h6 a:active { - color: #dcf6e3; + +table.m-table.m-big td.m-default, table.m-table.m-big th.m-default, +table.m-table.m-big td.m-primary, table.m-table.m-big th.m-primary, +table.m-table.m-big td.m-success, table.m-table.m-big th.m-success, +table.m-table.m-big td.m-warning, table.m-table.m-big th.m-warning, +table.m-table.m-big td.m-danger, table.m-table.m-big th.m-danger, +table.m-table.m-big td.m-info, table.m-table.m-big th.m-info, +table.m-table.m-big td.m-dim, table.m-table.m-big th.m-dim { + padding-left: 0.9375rem; + padding-right: 0.9375rem; + border-left-width: 0.0625rem; } -.m-block.m-warning h3 a:hover, .m-block.m-warning h3 a:focus, .m-block.m-warning h3 a:active, -.m-block.m-warning h4 a:hover, .m-block.m-warning h4 a:focus, .m-block.m-warning h4 a:active, -.m-block.m-warning h5 a:hover, .m-block.m-warning h5 a:focus, .m-block.m-warning h5 a:active, -.m-block.m-warning h6 a:hover, .m-block.m-warning h6 a:focus, .m-block.m-warning h6 a:active { - color: #fff1cc; + +table.m-table tr.m-default td, table.m-table td.m-default, +table.m-table tr.m-default th, table.m-table th.m-default, +table.m-table tr.m-primary td, table.m-table td.m-primary, +table.m-table tr.m-primary th, table.m-table th.m-primary, +table.m-table tr.m-success td, table.m-table td.m-success, +table.m-table tr.m-success th, table.m-table th.m-success, +table.m-table tr.m-warning td, table.m-table td.m-warning, +table.m-table tr.m-warning th, table.m-table th.m-warning, +table.m-table tr.m-danger td, table.m-table td.m-danger, +table.m-table tr.m-danger th, table.m-table th.m-danger, +table.m-table tr.m-info td, table.m-table td.m-info, +table.m-table tr.m-info th, table.m-table th.m-info, +table.m-table tr.m-dim td, table.m-table td.m-dim, +table.m-table tr.m-dim th, table.m-table th.m-dim { + border-color: #ffffff; } -.m-block.m-danger h3 a:hover, .m-block.m-danger h3 a:focus, .m-block.m-danger h3 a:active, -.m-block.m-danger h4 a:hover, .m-block.m-danger h4 a:focus, .m-block.m-danger h4 a:active, -.m-block.m-danger h5 a:hover, .m-block.m-danger h5 a:focus, .m-block.m-danger h5 a:active, -.m-block.m-danger h6 a:hover, .m-block.m-danger h6 a:focus, .m-block.m-danger h6 a:active { - color: #f6dddc; + +.m-note pre, .m-note code, +table.m-table tr.m-default pre, table.m-table tr.m-default code, +table.m-table td.m-default pre, table.m-table td.m-default code, +table.m-table th.m-default pre, table.m-table th.m-default code, +table.m-table tr.m-primary pre, table.m-table tr.m-primary code, +table.m-table td.m-primary pre, table.m-table td.m-primary code, +table.m-table th.m-primary pre, table.m-table th.m-primary code, +table.m-table tr.m-success pre, table.m-table tr.m-success code, +table.m-table td.m-success pre, table.m-table td.m-success code, +table.m-table th.m-success pre, table.m-table th.m-success code, +table.m-table tr.m-warning pre, table.m-table tr.m-warning code, +table.m-table td.m-warning pre, table.m-table td.m-warning code, +table.m-table th.m-warning pre, table.m-table th.m-warning code, +table.m-table tr.m-danger pre, table.m-table tr.m-danger code, +table.m-table td.m-danger pre, table.m-table td.m-danger code, +table.m-table th.m-danger pre, table.m-table th.m-danger code, +table.m-table tr.m-info pre, table.m-table tr.m-info code, +table.m-table td.m-info pre, table.m-table td.m-info code, +table.m-table th.m-info pre, table.m-table th.m-info code, +table.m-table tr.m-dim pre, table.m-table tr.m-dim code, +table.m-table td.m-dim pre, table.m-table td.m-dim code, +table.m-table th.m-dim pre, table.m-table th.m-dim code { + background-color: rgba(251, 240, 236, 0.5); } -.m-block.m-info h3 a:hover, .m-block.m-info h3 a:focus, .m-block.m-info h3 a:active, -.m-block.m-info h4 a:hover, .m-block.m-info h4 a:focus, .m-block.m-info h4 a:active, -.m-block.m-info h5 a:hover, .m-block.m-info h5 a:focus, .m-block.m-info h5 a:active, -.m-block.m-info h6 a:hover, .m-block.m-info h6 a:focus, .m-block.m-info h6 a:active { - color: #c6ddf2; + +img.m-image, svg.m-image { + display: block; + margin-left: auto; + margin-right: auto; } -div.m-button a, .m-label { color: #ffffff; } -div.m-button.m-flat a { color: #000000; } -div.m-button.m-flat a:hover, div.m-button.m-default a:focus, div.m-button.m-default a:active { - color: #2f73a3; -} -div.m-button.m-default a, .m-label:not(.m-flat).m-default { background-color: #000000; } -div.m-button.m-primary a, .m-label:not(.m-flat).m-primary { background-color: #205275; } -div.m-button.m-success a, .m-label:not(.m-flat).m-success { background-color: #31c25d; } -div.m-button.m-warning a, .m-label:not(.m-flat).m-warning { background-color: #d19600; } -div.m-button.m-danger a, .m-label:not(.m-flat).m-danger { background-color: #f60000; } -div.m-button.m-info a, .m-label:not(.m-flat).m-info { background-color: #2e7dc5; } -div.m-button.m-dim a, .m-label:not(.m-flat).m-dim { background-color: #bdbdbd; } -div.m-button.m-default a:hover, div.m-button.m-default a:focus, div.m-button.m-default a:active { - background-color: #205275; + +div.m-image { + text-align: center; } -div.m-button.m-primary a:hover, div.m-button.m-primary a:focus, div.m-button.m-primary a:active { - background-color: #000000; + +img.m-image, svg.m-image, div.m-image img, div.m-image svg { + max-width: 100%; + border-radius: 0.2rem; } -div.m-button.m-success a:hover, div.m-button.m-success a:focus, div.m-button.m-success a:active { - background-color: #dcf6e3; + +div.m-image.m-fullwidth img, div.m-image.m-fullwidth svg { + width: 100%; } -div.m-button.m-warning a:hover, div.m-button.m-warning a:focus, div.m-button.m-warning a:active { - background-color: #fff1cc; + +img.m-image.m-badge, div.m-image.m-badge img { + border-radius: 50%; } -div.m-button.m-danger a:hover, div.m-button.m-danger a:focus, div.m-button.m-danger a:active { - background-color: #f6dddc; + +figure.m-figure { + max-width: 100%; + margin-top: 0; + margin-left: auto; + margin-right: auto; + position: relative; + display: table; } -div.m-button.m-info a:hover, div.m-button.m-info a:focus, div.m-button.m-info a:active { - background-color: #c6ddf2; + +figure.m-figure:before { + position: absolute; + content: ' '; + top: 0; + bottom: 0; + left: 0; + right: 0; + z-index: -1; + border-style: solid; + border-width: 0.125rem; + border-radius: 0.2rem; + border-color: #c7cfd9; } -div.m-button.m-dim a:hover, div.m-button.m-dim a:focus, div.m-button.m-dim a:active { - background-color: #c0c0c0; + +figure.m-figure.m-flat:before { + border-color: transparent; } -.m-note.m-default { background-color: #f2f7fa; } -.m-note.m-default, -table.m-table tr.m-default td, table.m-table td.m-default, -table.m-table tr.m-default th, table.m-table th.m-default { - color: #000000; + +figure.m-figure > * { + margin-left: 1rem; + margin-right: 1rem; + display: table-caption; + caption-side: bottom; } -.m-note.m-default a:hover, -table.m-table tr.m-default td a:hover, table.m-table td.m-default a:hover, -table.m-table tr.m-default th a:hover, table.m-table th.m-default a:hover, -.m-note.m-default a:focus, -table.m-table tr.m-default td a:focus, table.m-table td.m-default a:focus, -table.m-table tr.m-default th a:focus, table.m-table th.m-default a:focus, -.m-note.m-default a:active, -table.m-table tr.m-default td a:active, table.m-table td.m-default a:active, -table.m-table tr.m-default th a:active, table.m-table th.m-default a:active { - color: #205275; + +figure.m-figure > *:first-child { + display: inline; } -.m-note.m-primary a, -table.m-table tr.m-primary td a, table.m-table td.m-primary a, -table.m-table tr.m-primary th a, table.m-table th.m-primary a { - color: #2f73a3; + +figure.m-figure > *:last-child { + margin-bottom: 1rem !important; } -.m-note.m-primary, -table.m-table tr.m-primary td, table.m-table td.m-primary, -table.m-table tr.m-primary th, table.m-table th.m-primary { - background-color: #ef9069; - color: #fbe4d9; + +figure.m-figure img, figure.m-figure svg { + position: relative; + margin-left: 0; + margin-right: 0; + margin-bottom: 0; + border-top-left-radius: 0.2rem; + border-top-right-radius: 0.2rem; + max-width: 100%; } -.m-note.m-primary a, -table.m-table tr.m-primary td a, table.m-table td.m-primary a, -table.m-table tr.m-primary th a, table.m-table th.m-primary a { - color: #782f0d; + +figure.m-figure.m-flat img, figure.m-figure.m-flat svg { + border-bottom-left-radius: 0.2rem; + border-bottom-right-radius: 0.2rem; } -.m-note.m-primary a:hover, -table.m-table tr.m-primary td a:hover, table.m-table td.m-primary a:hover, -table.m-table tr.m-primary th a:hover, table.m-table th.m-primary a:hover, -.m-note.m-primary a:focus, -table.m-table tr.m-primary td a:focus, table.m-table td.m-primary a:focus, -table.m-table tr.m-primary th a:focus, table.m-table th.m-primary a:focus, -.m-note.m-primary a:active, -table.m-table tr.m-primary td a:active, table.m-table td.m-primary a:active, -table.m-table tr.m-primary th a:active, table.m-table th.m-primary a:active { - color: #2f1205; + +figure.m-figure a img, figure.m-figure a svg { + margin-left: -1rem; + margin-right: -1rem; } -.m-note.m-success, -table.m-table tr.m-success td, table.m-table td.m-success, -table.m-table tr.m-success th, table.m-table th.m-success { - background-color: #4dd376; - color: #f4fcf6; + +figure.m-figure.m-fullwidth, figure.m-figure.m-fullwidth > * { + display: block; } -.m-note.m-success a, -table.m-table tr.m-success td a, table.m-table td.m-success a, -table.m-table tr.m-success th a, table.m-table th.m-success a { - color: #c5f2d1; + +figure.m-figure.m-fullwidth > *:first-child { + display: inline; } -.m-note.m-success a:hover, -table.m-table tr.m-success td a:hover, table.m-table td.m-success a:hover, -table.m-table tr.m-success th a:hover, table.m-table th.m-success a:hover, -.m-note.m-success a:focus, -table.m-table tr.m-success td a:focus, table.m-table td.m-success a:focus, -table.m-table tr.m-success th a:focus, table.m-table th.m-success a:focus, -.m-note.m-success a:active, -table.m-table tr.m-success td a:active, table.m-table td.m-success a:active, -table.m-table tr.m-success th a:active, table.m-table th.m-success a:active { - color: #dcf6e3; + +figure.m-figure.m-fullwidth img, figure.m-figure.m-fullwidth svg { + width: 100%; } -.m-note.m-warning, table.m-table tr.m-warning td, table.m-table td.m-warning, - table.m-table tr.m-warning th, table.m-table th.m-warning { - background-color: #d19600; - color: #fff7e3; + +figure.m-figure.m-fullwidth:after { + content: ' '; + display: block; + margin-top: 1rem; + height: 1px; } -.m-note.m-warning a, table.m-table tr.m-warning td a, table.m-table td.m-warning a, - table.m-table tr.m-warning th a, table.m-table th.m-warning a { - color: #fff1cc; + +.m-code-figure, .m-console-figure { + margin-top: 0; + margin-left: 0; + margin-right: 0; + position: relative; + padding: 1rem; } -.m-note.m-warning a:hover, -table.m-table tr.m-warning td a:hover, table.m-table td.m-warning a:hover, -table.m-table tr.m-warning th a:hover, table.m-table th.m-warning a:hover, -.m-note.m-warning a:focus, -table.m-table tr.m-warning td a:focus, table.m-table td.m-warning a:focus, -table.m-table tr.m-warning th a:focus, table.m-table th.m-warning a:focus, -.m-note.m-warning a:active, -table.m-table tr.m-warning td a:active, table.m-table td.m-warning a:active, -table.m-table tr.m-warning th a:active, table.m-table th.m-warning a:active { - color: #fff7e3; + +.m-code-figure:before, .m-console-figure:before { + position: absolute; + content: ' '; + top: 0; + bottom: 0; + left: 0; + right: 0; + z-index: -1; + border-style: solid; + border-width: 0.125rem; + border-radius: 0.2rem; } -.m-note.m-danger, -table.m-table tr.m-danger td, table.m-table td.m-danger, -table.m-table tr.m-danger th, table.m-table th.m-danger { - background-color: #e23e3e; - color: #fdf3f3; + +.m-code-figure:before { + border-color: #f2f7fa; } -.m-note.m-danger a, -table.m-table tr.m-danger td a, table.m-table td.m-danger a, -table.m-table tr.m-danger th a, table.m-table th.m-danger a { - color: #f2c7c6; + +.m-console-figure:before { + border-color: #000000; } -.m-note.m-danger a:hover, -table.m-table tr.m-danger td a:hover, table.m-table td.m-danger a:hover, -table.m-table tr.m-danger th a:hover, table.m-table th.m-danger a:hover, -.m-note.m-danger a:focus, -table.m-table tr.m-danger td a:focus, table.m-table td.m-danger a:focus, -table.m-table tr.m-danger th a:focus, table.m-table th.m-danger a:focus, -.m-note.m-danger a:active, -table.m-table tr.m-danger td a:active, table.m-table td.m-danger a:active, -table.m-table tr.m-danger th a:active, table.m-table th.m-danger a:active { - color: #f6dddc; + +.m-code-figure.m-flat:before, .m-console-figure.m-flat:before { + border-color: transparent; } -.m-note.m-info, -table.m-table tr.m-info td, table.m-table td.m-info, -table.m-table tr.m-info th, table.m-table th.m-info { - background-color: #4c93d3; - color: #f4f8fc; + +.m-code-figure > pre:first-child, .m-console-figure > pre:first-child { + position: relative; + margin: -1rem -1rem 1rem -1rem; + border-bottom-left-radius: 0; + border-bottom-right-radius: 0; } -.m-note.m-info a, -table.m-table tr.m-info td a, table.m-table td.m-info a, -table.m-table tr.m-info th a, table.m-table th.m-info a { - color: #c6ddf2; + +.m-code-figure > pre.m-nopad, .m-console-figure > pre.m-nopad { + margin-left: -0.875rem; + margin-right: -0.875rem; + margin-top: -1rem; + margin-bottom: -0.875rem; + padding-left: 0.875rem; } -.m-note.m-info a:hover, -table.m-table tr.m-info td a:hover, table.m-table td.m-info a:hover, -table.m-table tr.m-info th a:hover, table.m-table th.m-info a:hover, -.m-note.m-info a:focus, -table.m-table tr.m-info td a:focus, table.m-table td.m-info a:focus, -table.m-table tr.m-info th a:focus, table.m-table th.m-info a:focus, -.m-note.m-info a:active, -table.m-table tr.m-info td a:active, table.m-table td.m-info a:active, -table.m-table tr.m-info th a:active, table.m-table th.m-info a:active { - color: #dbeaf7; + +figure.m-figure figcaption, .m-code-figure figcaption, .m-console-figure figcaption { + margin-top: 0.5rem; + margin-bottom: 0.5rem; + font-weight: normal; + font-size: 1.17rem; } -.m-note.m-dim, -table.m-table tr.m-dim td, table.m-table td.m-dim, -table.m-table tr.m-dim th, table.m-table th.m-dim { - background-color: #f1f1f1; - color: #7c7c7c; + +.m-imagegrid > div { + background-color: #ffffff; } -.m-note.m-dim a, -table.m-table tr.m-dim td a, table.m-table td.m-dim a, -table.m-table tr.m-dim th a, table.m-table th.m-dim a { - color: #c0c0c0; + +.m-imagegrid > div > figure { + display: block; + float: left; + position: relative; + margin: 0; } -.m-note.m-dim a:hover, -table.m-table tr.m-dim td a:hover, table.m-table td.m-dim a:hover, -table.m-table tr.m-dim th a:hover, table.m-table th.m-dim a:hover, -.m-note.m-dim a:focus, -table.m-table tr.m-dim td a:focus, table.m-table td.m-dim a:focus, -table.m-table tr.m-dim th a:focus, table.m-table th.m-dim a:focus, -.m-note.m-dim a:active, -table.m-table tr.m-dim td a:active, table.m-table td.m-dim a:active, -table.m-table tr.m-dim th a:active, table.m-table th.m-dim a:active { - color: #949494; -} -figure.m-figure.m-default:before { border-color: #f2f7fa; } -figure.m-figure.m-default figcaption { color: #000000; } -figure.m-figure.m-primary:before { border-color: #ef9069; } -figure.m-figure.m-primary figcaption { color: #205275; } -figure.m-figure.m-success:before { border-color: #4dd376; } -figure.m-figure.m-success figcaption { color: #31c25d; } -figure.m-figure.m-warning:before { border-color: #d19600; } -figure.m-figure.m-warning figcaption { color: #d19600; } -figure.m-figure.m-danger:before { border-color: #e23e3e; } -figure.m-figure.m-danger figcaption { color: #f60000; } -figure.m-figure.m-info:before { border-color: #4c93d3; } -figure.m-figure.m-info figcaption { color: #2e7dc5; } -figure.m-figure.m-dim:before { border-color: #f1f1f1; } -figure.m-figure.m-dim { color: #bdbdbd; } -figure.m-figure.m-dim a { color: #c0c0c0; } -figure.m-figure.m-dim a:hover, figure.m-figure.m-dim a:focus, figure.m-figure.m-dim a:active { - color: #949494; + +.m-imagegrid > div > figure > div, +.m-imagegrid > div > figure > figcaption, +.m-imagegrid > div > figure > a > div, +.m-imagegrid > div > figure > a > figcaption { + position: absolute; + top: 0; + left: 0; + width: 100%; + height: 100%; + border-color: #ffffff; + border-style: solid; + border-width: 0.25rem; + padding: 0.5rem; } -.m-math { fill: #000000; } -.m-math.m-default, .m-math g.m-default, .m-math rect.m-default, -div.m-plot svg .m-bar.m-default, -.m-graph g.m-edge polygon, -.m-graph g.m-node:not(.m-flat) ellipse, -.m-graph g.m-node:not(.m-flat) polygon, -.m-graph g.m-edge text, -.m-graph g.m-node.m-flat text, -.m-graph.m-default g.m-edge polygon, -.m-graph.m-default g.m-node:not(.m-flat) ellipse, -.m-graph.m-default g.m-node:not(.m-flat) polygon, -.m-graph.m-default g.m-edge text, -.m-graph.m-default g.m-node.m-flat text { - fill: #000000; + +.m-imagegrid > div > figure:first-child > div, +.m-imagegrid > div > figure:first-child > figcaption, +.m-imagegrid > div > figure:first-child > a > div, +.m-imagegrid > div > figure:first-child > a > figcaption { + border-left-width: 0; } + +.m-imagegrid > div > figure:last-child > div, +.m-imagegrid > div > figure:last-child > figcaption, +.m-imagegrid > div > figure:last-child > a > div, +.m-imagegrid > div > figure:last-child > a > figcaption { + border-right-width: 0; +} + +.m-imagegrid > div > figure > figcaption, +.m-imagegrid > div > figure > a > figcaption { + color: transparent; + overflow: hidden; + text-overflow: ellipsis; + white-space: nowrap; + font-size: 0.75rem; +} + +.m-imagegrid > div > figure > div::before, +.m-imagegrid > div > figure > figcaption::before, +.m-imagegrid > div > figure > a > div::before, +.m-imagegrid > div > figure > a > figcaption::before { + content: ''; + display: inline-block; + height: 100%; + vertical-align: bottom; + width: 0; +} + +.m-imagegrid > div > figure:hover > figcaption, +.m-imagegrid > div > figure:hover > a > figcaption { + background: linear-gradient(transparent 0%, transparent 75%, rgba(0, 0, 0, 0.85) 100%); + color: #ffffff; +} + +.m-imagegrid > div > figure > img, +.m-imagegrid > div > figure > a > img { + width: 100%; + height: 100%; +} + +.m-imagegrid > div::after { + display: block; + content: ' '; + clear: both; +} + +@media screen and (max-width: 767px) { + .m-imagegrid > div > figure { + float: none; + width: 100% !important; + } + + .m-imagegrid > div > figure > div, + .m-imagegrid > div > figure > figcaption, + .m-imagegrid > div > figure > a > div, + .m-imagegrid > div > figure > a > figcaption { + border-left-width: 0; + border-right-width: 0; + } +} + +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-note, +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-frame, +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-block, +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-imagegrid, +.m-container-inflatable > .m-row > [class*='m-col-'] > pre, +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-code-figure, +.m-container-inflatable > .m-row > [class*='m-col-'] > .m-console-figure, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-note, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-frame, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-block, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-imagegrid, +.m-container-inflatable > .m-row > [class*='m-col-'] section > pre, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-code-figure, +.m-container-inflatable > .m-row > [class*='m-col-'] section > .m-console-figure, +.m-container-inflatable [class*='m-center-'] > .m-note, +.m-container-inflatable [class*='m-center-'] > .m-frame, +.m-container-inflatable [class*='m-center-'] > .m-block, +.m-container-inflatable [class*='m-center-'] > .m-imagegrid, +.m-container-inflatable [class*='m-center-'] > pre, +.m-container-inflatable [class*='m-center-'] > .m-code-figure, +.m-container-inflatable [class*='m-center-'] > .m-console-figure, +.m-container-inflatable [class*='m-left-'] > .m-note, +.m-container-inflatable [class*='m-left-'] > .m-frame, +.m-container-inflatable [class*='m-left-'] > .m-block, +.m-container-inflatable [class*='m-left-'] > .m-imagegrid, +.m-container-inflatable [class*='m-left-'] > pre, +.m-container-inflatable [class*='m-left-'] > .m-code-figure, +.m-container-inflatable [class*='m-left-'] > .m-console-figure, +.m-container-inflatable [class*='m-right-'] > .m-note, +.m-container-inflatable [class*='m-right-'] > .m-frame, +.m-container-inflatable [class*='m-right-'] > .m-block, +.m-container-inflatable [class*='m-right-'] > .m-imagegrid, +.m-container-inflatable [class*='m-right-'] > pre, +.m-container-inflatable [class*='m-right-'] > .m-code-figure, +.m-container-inflatable [class*='m-right-'] > .m-console-figure, +.m-container-inflatable .m-container-inflate > .m-note, +.m-container-inflatable .m-container-inflate > .m-frame, +.m-container-inflatable .m-container-inflate > .m-block, +.m-container-inflatable .m-container-inflate > .m-imagegrid, +.m-container-inflatable .m-container-inflate > pre, +.m-container-inflatable .m-container-inflate > .m-code-figure, +.m-container-inflatable .m-container-inflate > .m-console-figure { + margin-left: -1rem; + margin-right: -1rem; +} + +@media screen and (min-width: 576px) { + .m-container-inflatable .m-center-s > .m-note, + .m-container-inflatable .m-center-s > .m-frame, + .m-container-inflatable .m-center-s > .m-block, + .m-container-inflatable .m-center-s > .m-imagegrid, + .m-container-inflatable .m-center-s > pre, + .m-container-inflatable .m-center-s > .m-code-figure, + .m-container-inflatable .m-center-s > .m-console-figure { + margin-left: -1rem; + margin-right: -1rem; + } + + .m-container-inflatable .m-left-s > .m-note, + .m-container-inflatable .m-left-s > .m-frame, + .m-container-inflatable .m-left-s > .m-block, + .m-container-inflatable .m-left-s > .m-imagegrid, + .m-container-inflatable .m-left-s > pre, + .m-container-inflatable .m-left-s > .m-code-figure, + .m-container-inflatable .m-left-s > .m-console-figure { + margin-left: -1rem; + margin-right: 0; + } + + .m-container-inflatable .m-right-s > .m-note, + .m-container-inflatable .m-right-s > .m-frame, + .m-container-inflatable .m-right-s > .m-block, + .m-container-inflatable .m-right-s > .m-imagegrid, + .m-container-inflatable .m-right-s > pre, + .m-container-inflatable .m-right-s > .m-code-figure, + .m-container-inflatable .m-right-s > .m-console-figure { + margin-left: 0; + margin-right: -1rem; + } + + .m-container-inflatable > .m-row > .m-col-s-10 > .m-imagegrid.m-container-inflate, + .m-container-inflatable > .m-row > .m-col-s-10 section > .m-imagegrid.m-container-inflate { + margin-left: -10%; + margin-right: -10%; + } +} + +@media screen and (min-width: 768px) { + .m-container-inflatable .m-center-m > .m-note, + .m-container-inflatable .m-center-m > .m-frame, + .m-container-inflatable .m-center-m > .m-block, + .m-container-inflatable .m-center-m > .m-imagegrid, + .m-container-inflatable .m-center-m > pre, + .m-container-inflatable .m-center-m > .m-code-figure, + .m-container-inflatable .m-center-m > .m-console-figure { + margin-left: -1rem; + margin-right: -1rem; + } + + .m-container-inflatable .m-left-m > .m-note, + .m-container-inflatable .m-left-m > .m-frame, + .m-container-inflatable .m-left-m > .m-block, + .m-container-inflatable .m-left-m > .m-imagegrid, + .m-container-inflatable .m-left-m > pre, + .m-container-inflatable .m-left-m > .m-code-figure, + .m-container-inflatable .m-left-m > .m-console-figure { + margin-left: -1rem; + margin-right: 0; + } + + .m-container-inflatable .m-right-m > .m-note, + .m-container-inflatable .m-right-m > .m-frame, + .m-container-inflatable .m-right-m > .m-block, + .m-container-inflatable .m-right-m > .m-imagegrid, + .m-container-inflatable .m-right-m > pre, + .m-container-inflatable .m-right-m > .m-code-figure, + .m-container-inflatable .m-right-m > .m-console-figure { + margin-left: 0; + margin-right: -1rem; + } + + .m-container-inflatable > .m-row > .m-col-m-10 > .m-imagegrid.m-container-inflate, + .m-container-inflatable > .m-row > .m-col-m-10 section > .m-imagegrid.m-container-inflate { + margin-left: -10%; + margin-right: -10%; + } +} + +@media screen and (min-width: 992px) { + .m-container-inflatable .m-center-l > .m-note, + .m-container-inflatable .m-center-l > .m-frame, + .m-container-inflatable .m-center-l > .m-block, + .m-container-inflatable .m-center-l > .m-imagegrid, + .m-container-inflatable .m-center-l > pre, + .m-container-inflatable .m-center-l > .m-code-figure, + .m-container-inflatable .m-center-l > .m-console-figure { + margin-left: -1rem; + margin-right: -1rem; + } + + .m-container-inflatable .m-left-l > .m-note, + .m-container-inflatable .m-left-l > .m-frame, + .m-container-inflatable .m-left-l > .m-block, + .m-container-inflatable .m-left-l > .m-imagegrid, + .m-container-inflatable .m-left-l > pre, + .m-container-inflatable .m-left-l > .m-code-figure, + .m-container-inflatable .m-left-l > .m-console-figure { + margin-left: -1rem; + margin-right: 0; + } + + .m-container-inflatable .m-right-l > .m-note, + .m-container-inflatable .m-right-l > .m-frame, + .m-container-inflatable .m-right-l > .m-block, + .m-container-inflatable .m-right-l > .m-imagegrid, + .m-container-inflatable .m-right-l > pre, + .m-container-inflatable .m-right-l > .m-code-figure, + .m-container-inflatable .m-right-l > .m-console-figure { + margin-left: 0; + margin-right: -1rem; + } + + .m-container-inflatable > .m-row > .m-col-l-10 > .m-imagegrid.m-container-inflate, + .m-container-inflatable > .m-row > .m-col-l-10 section > .m-imagegrid.m-container-inflate { + margin-left: -10%; + margin-right: -10%; + } +} + +pre.m-code span.hll { + margin-left: -1.0rem; + margin-right: -1.0rem; + padding-left: 1.0rem; +} + +.m-code.m-inverted > span, .m-console.m-inverted > span { + opacity: 0.3333; +} + +.m-code.m-inverted > span.hll, .m-console.m-inverted > span.hll { + opacity: 1; + background-color: transparent; + border-color: transparent; +} + +.m-code.m-inverted { + color: rgba(91, 91, 91, 0.33); +} + +.m-console.m-inverted { + color: rgba(91, 91, 91, 0.33); +} + +.m-code.m-inverted > span.hll { + color: #000000; +} + +.m-cosole.m-inverted > span.hll { + color: #000000; +} + +.m-code-color { + display: inline-block; + width: 0.75rem; + height: 0.75rem; + vertical-align: -0.05rem; + margin-left: 0.2rem; + margin-right: 0.1rem; + border-radius: 0.1rem; +} + +div.m-math { + overflow-x: auto; + overflow-y: hidden; +} + +div.m-math svg { + margin-left: auto; + margin-right: auto; + display: block; +} + +div.m-button a svg.m-math { + fill: #ffffff; +} + +div.m-button.m-flat a svg.m-math { + fill: #000000; +} + +div.m-button.m-flat a:hover svg.m-math, div.m-button.m-default a:focus svg.m-math, +div.m-button.m-default a:active svg.m-math { + fill: #2f73a3; +} + +.m-graph { + font-size: 1em; +} + +div.m-plot svg, div.m-graph svg { + max-width: 100%; + margin-left: auto; + margin-right: auto; + display: block; +} + +div.m-plot .m-background { + fill: #f2f7fa; +} + +div.m-plot svg .m-label { + font-size: 11px; +} + +div.m-plot svg .m-title { + font-size: 13px; +} + +div.m-plot svg .m-label, div.m-plot svg .m-title { + fill: #000000; +} + +div.m-plot svg .m-line { + stroke: #000000; + stroke-width: 0.8; +} + +div.m-plot svg .m-error { + stroke: #000000; + stroke-width: 1.5; +} + +div.m-plot svg .m-label.m-dim { + fill: #bdbdbd; +} + +.m-graph g.m-edge path, .m-graph g.m-node.m-flat ellipse, +.m-graph g.m-node.m-flat polygon { + fill: none; +} + +.m-graph g.m-node:not(.m-flat) text { + fill: #ffffff; +} + +figure.m-figure > svg.m-math:first-child, +figure.m-figure > svg.m-graph:first-child { + padding: 1rem; + box-sizing: content-box; +} + +figure.m-figure:not(.m-flat) > svg.m-math:first-child, +figure.m-figure:not(.m-flat) > svg.m-graph:first-child { + background-color: #c7cfd9; +} + +.m-block.m-default { + border-left-color: #c7cfd9; +} + +.m-block.m-default h3, .m-block.m-default h4, .m-block.m-default h5, .m-block.m-default h6, +.m-text.m-default, .m-label.m-flat.m-default { + color: #000000; +} + +.m-block.m-default h3 a, .m-block.m-default h4 a, .m-block.m-default h5 a, .m-block.m-default h6 a { + color: #205275; +} + +.m-block.m-primary { + border-left-color: #205275; +} + +.m-block.m-primary h3, .m-block.m-primary h4, .m-block.m-primary h5, .m-block.m-primary h6, +.m-block.m-primary h3 a, .m-block.m-primary h4 a, .m-block.m-primary h5 a, .m-block.m-primary h6 a, +.m-text.m-primary, .m-label.m-flat.m-primary { + color: #205275; +} + +.m-block.m-success { + border-left-color: #31c25d; +} + +.m-block.m-success h3, .m-block.m-success h4, .m-block.m-success h5, .m-block.m-success h6, +.m-block.m-success h3 a, .m-block.m-success h4 a, .m-block.m-success h5 a, .m-block.m-success h6 a, +.m-text.m-success, .m-label.m-flat.m-success { + color: #31c25d; +} + +.m-block.m-warning { + border-left-color: #d19600; +} + +.m-block.m-warning h3, .m-block.m-warning h4, .m-block.m-warning h5, .m-block.m-warning h6, +.m-block.m-warning h3 a, .m-block.m-warning h4 a, .m-block.m-warning h5 a, .m-block.m-warning h6 a, +.m-text.m-warning, .m-label.m-flat.m-warning { + color: #d19600; +} + +.m-block.m-danger { + border-left-color: #f60000; +} + +.m-block.m-danger h3, .m-block.m-danger h4, .m-block.m-danger h5, .m-block.m-danger h6, +.m-block.m-danger h3 a, .m-block.m-danger h4 a, .m-block.m-danger h5 a, .m-block.m-danger h6 a, +.m-text.m-danger, .m-label.m-flat.m-danger { + color: #f60000; +} + +.m-block.m-info { + border-left-color: #2e7dc5; +} + +.m-block.m-info h3, .m-block.m-info h4, .m-block.m-info h5, .m-block.m-info h6, +.m-block.m-info h3 a, .m-block.m-info h4 a, .m-block.m-info h5 a, .m-block.m-info h6 a, +.m-text.m-info, .m-label.m-flat.m-info { + color: #2e7dc5; +} + +.m-block.m-dim { + border-left-color: #bdbdbd; +} + +.m-block.m-dim, .m-text.m-dim, .m-label.m-flat.m-dim { + color: #bdbdbd; +} + +.m-block.m-dim a, .m-text.m-dim a { + color: #c0c0c0; +} + +.m-block.m-dim a:hover, .m-block.m-dim a:focus, .m-block.m-dim a:active, +.m-text.m-dim a:hover, .m-text.m-dim a:focus, .m-text.m-dim a:active { + color: #949494; +} + +.m-block.m-flat { + border-color: transparent; +} + +.m-block.m-flat h3, .m-block.m-flat h4, .m-block.m-flat h5, .m-block.m-flat h6 { + color: #000000; +} + +.m-block.m-default h3 a:hover, .m-block.m-default h3 a:focus, .m-block.m-default h3 a:active, +.m-block.m-default h4 a:hover, .m-block.m-default h4 a:focus, .m-block.m-default h4 a:active, +.m-block.m-default h5 a:hover, .m-block.m-default h5 a:focus, .m-block.m-default h5 a:active, +.m-block.m-default h6 a:hover, .m-block.m-default h6 a:focus, .m-block.m-default h6 a:active { + color: #205275; +} + +.m-block.m-primary h3 a:hover, .m-block.m-primary h3 a:focus, .m-block.m-primary h3 a:active, +.m-block.m-primary h4 a:hover, .m-block.m-primary h4 a:focus, .m-block.m-primary h4 a:active, +.m-block.m-primary h5 a:hover, .m-block.m-primary h5 a:focus, .m-block.m-primary h5 a:active, +.m-block.m-primary h6 a:hover, .m-block.m-primary h6 a:focus, .m-block.m-primary h6 a:active { + color: #000000; +} + +.m-block.m-success h3 a:hover, .m-block.m-success h3 a:focus, .m-block.m-success h3 a:active, +.m-block.m-success h4 a:hover, .m-block.m-success h4 a:focus, .m-block.m-success h4 a:active, +.m-block.m-success h5 a:hover, .m-block.m-success h5 a:focus, .m-block.m-success h5 a:active, +.m-block.m-success h6 a:hover, .m-block.m-success h6 a:focus, .m-block.m-success h6 a:active { + color: #dcf6e3; +} + +.m-block.m-warning h3 a:hover, .m-block.m-warning h3 a:focus, .m-block.m-warning h3 a:active, +.m-block.m-warning h4 a:hover, .m-block.m-warning h4 a:focus, .m-block.m-warning h4 a:active, +.m-block.m-warning h5 a:hover, .m-block.m-warning h5 a:focus, .m-block.m-warning h5 a:active, +.m-block.m-warning h6 a:hover, .m-block.m-warning h6 a:focus, .m-block.m-warning h6 a:active { + color: #fff1cc; +} + +.m-block.m-danger h3 a:hover, .m-block.m-danger h3 a:focus, .m-block.m-danger h3 a:active, +.m-block.m-danger h4 a:hover, .m-block.m-danger h4 a:focus, .m-block.m-danger h4 a:active, +.m-block.m-danger h5 a:hover, .m-block.m-danger h5 a:focus, .m-block.m-danger h5 a:active, +.m-block.m-danger h6 a:hover, .m-block.m-danger h6 a:focus, .m-block.m-danger h6 a:active { + color: #f6dddc; +} + +.m-block.m-info h3 a:hover, .m-block.m-info h3 a:focus, .m-block.m-info h3 a:active, +.m-block.m-info h4 a:hover, .m-block.m-info h4 a:focus, .m-block.m-info h4 a:active, +.m-block.m-info h5 a:hover, .m-block.m-info h5 a:focus, .m-block.m-info h5 a:active, +.m-block.m-info h6 a:hover, .m-block.m-info h6 a:focus, .m-block.m-info h6 a:active { + color: #c6ddf2; +} + +div.m-button a, .m-label { + color: #ffffff; +} + +div.m-button.m-flat a { + color: #000000; +} + +div.m-button.m-flat a:hover, div.m-button.m-default a:focus, div.m-button.m-default a:active { + color: #2f73a3; +} + +div.m-button.m-default a, .m-label:not(.m-flat).m-default { + background-color: #000000; +} + +div.m-button.m-primary a, .m-label:not(.m-flat).m-primary { + background-color: #205275; +} + +div.m-button.m-success a, .m-label:not(.m-flat).m-success { + background-color: #31c25d; +} + +div.m-button.m-warning a, .m-label:not(.m-flat).m-warning { + background-color: #d19600; +} + +div.m-button.m-danger a, .m-label:not(.m-flat).m-danger { + background-color: #f60000; +} + +div.m-button.m-info a, .m-label:not(.m-flat).m-info { + background-color: #2e7dc5; +} + +div.m-button.m-dim a, .m-label:not(.m-flat).m-dim { + background-color: #bdbdbd; +} + +div.m-button.m-default a:hover, div.m-button.m-default a:focus, div.m-button.m-default a:active { + background-color: #205275; +} + +div.m-button.m-primary a:hover, div.m-button.m-primary a:focus, div.m-button.m-primary a:active { + background-color: #000000; +} + +div.m-button.m-success a:hover, div.m-button.m-success a:focus, div.m-button.m-success a:active { + background-color: #dcf6e3; +} + +div.m-button.m-warning a:hover, div.m-button.m-warning a:focus, div.m-button.m-warning a:active { + background-color: #fff1cc; +} + +div.m-button.m-danger a:hover, div.m-button.m-danger a:focus, div.m-button.m-danger a:active { + background-color: #f6dddc; +} + +div.m-button.m-info a:hover, div.m-button.m-info a:focus, div.m-button.m-info a:active { + background-color: #c6ddf2; +} + +div.m-button.m-dim a:hover, div.m-button.m-dim a:focus, div.m-button.m-dim a:active { + background-color: #c0c0c0; +} + +.m-note.m-default { + background-color: #f2f7fa; +} + +.m-note.m-default, +table.m-table tr.m-default td, table.m-table td.m-default, +table.m-table tr.m-default th, table.m-table th.m-default { + color: #000000; +} + +.m-note.m-default a:hover, +table.m-table tr.m-default td a:hover, table.m-table td.m-default a:hover, +table.m-table tr.m-default th a:hover, table.m-table th.m-default a:hover, +.m-note.m-default a:focus, +table.m-table tr.m-default td a:focus, table.m-table td.m-default a:focus, +table.m-table tr.m-default th a:focus, table.m-table th.m-default a:focus, +.m-note.m-default a:active, +table.m-table tr.m-default td a:active, table.m-table td.m-default a:active, +table.m-table tr.m-default th a:active, table.m-table th.m-default a:active { + color: #205275; +} + +.m-note.m-primary a, +table.m-table tr.m-primary td a, table.m-table td.m-primary a, +table.m-table tr.m-primary th a, table.m-table th.m-primary a { + color: #2f73a3; +} + +.m-note.m-primary, +table.m-table tr.m-primary td, table.m-table td.m-primary, +table.m-table tr.m-primary th, table.m-table th.m-primary { + background-color: #ef9069; + color: #fbe4d9; +} + +.m-note.m-primary a, +table.m-table tr.m-primary td a, table.m-table td.m-primary a, +table.m-table tr.m-primary th a, table.m-table th.m-primary a { + color: #782f0d; +} + +.m-note.m-primary a:hover, +table.m-table tr.m-primary td a:hover, table.m-table td.m-primary a:hover, +table.m-table tr.m-primary th a:hover, table.m-table th.m-primary a:hover, +.m-note.m-primary a:focus, +table.m-table tr.m-primary td a:focus, table.m-table td.m-primary a:focus, +table.m-table tr.m-primary th a:focus, table.m-table th.m-primary a:focus, +.m-note.m-primary a:active, +table.m-table tr.m-primary td a:active, table.m-table td.m-primary a:active, +table.m-table tr.m-primary th a:active, table.m-table th.m-primary a:active { + color: #2f1205; +} + +.m-note.m-success, +table.m-table tr.m-success td, table.m-table td.m-success, +table.m-table tr.m-success th, table.m-table th.m-success { + background-color: #4dd376; + color: #f4fcf6; +} + +.m-note.m-success a, +table.m-table tr.m-success td a, table.m-table td.m-success a, +table.m-table tr.m-success th a, table.m-table th.m-success a { + color: #c5f2d1; +} + +.m-note.m-success a:hover, +table.m-table tr.m-success td a:hover, table.m-table td.m-success a:hover, +table.m-table tr.m-success th a:hover, table.m-table th.m-success a:hover, +.m-note.m-success a:focus, +table.m-table tr.m-success td a:focus, table.m-table td.m-success a:focus, +table.m-table tr.m-success th a:focus, table.m-table th.m-success a:focus, +.m-note.m-success a:active, +table.m-table tr.m-success td a:active, table.m-table td.m-success a:active, +table.m-table tr.m-success th a:active, table.m-table th.m-success a:active { + color: #dcf6e3; +} + +.m-note.m-warning, table.m-table tr.m-warning td, table.m-table td.m-warning, +table.m-table tr.m-warning th, table.m-table th.m-warning { + background-color: #d19600; + color: #fff7e3; +} + +.m-note.m-warning a, table.m-table tr.m-warning td a, table.m-table td.m-warning a, +table.m-table tr.m-warning th a, table.m-table th.m-warning a { + color: #fff1cc; +} + +.m-note.m-warning a:hover, +table.m-table tr.m-warning td a:hover, table.m-table td.m-warning a:hover, +table.m-table tr.m-warning th a:hover, table.m-table th.m-warning a:hover, +.m-note.m-warning a:focus, +table.m-table tr.m-warning td a:focus, table.m-table td.m-warning a:focus, +table.m-table tr.m-warning th a:focus, table.m-table th.m-warning a:focus, +.m-note.m-warning a:active, +table.m-table tr.m-warning td a:active, table.m-table td.m-warning a:active, +table.m-table tr.m-warning th a:active, table.m-table th.m-warning a:active { + color: #fff7e3; +} + +.m-note.m-danger, +table.m-table tr.m-danger td, table.m-table td.m-danger, +table.m-table tr.m-danger th, table.m-table th.m-danger { + background-color: #e23e3e; + color: #fdf3f3; +} + +.m-note.m-danger a, +table.m-table tr.m-danger td a, table.m-table td.m-danger a, +table.m-table tr.m-danger th a, table.m-table th.m-danger a { + color: #f2c7c6; +} + +.m-note.m-danger a:hover, +table.m-table tr.m-danger td a:hover, table.m-table td.m-danger a:hover, +table.m-table tr.m-danger th a:hover, table.m-table th.m-danger a:hover, +.m-note.m-danger a:focus, +table.m-table tr.m-danger td a:focus, table.m-table td.m-danger a:focus, +table.m-table tr.m-danger th a:focus, table.m-table th.m-danger a:focus, +.m-note.m-danger a:active, +table.m-table tr.m-danger td a:active, table.m-table td.m-danger a:active, +table.m-table tr.m-danger th a:active, table.m-table th.m-danger a:active { + color: #f6dddc; +} + +.m-note.m-info, +table.m-table tr.m-info td, table.m-table td.m-info, +table.m-table tr.m-info th, table.m-table th.m-info { + background-color: #4c93d3; + color: #f4f8fc; +} + +.m-note.m-info a, +table.m-table tr.m-info td a, table.m-table td.m-info a, +table.m-table tr.m-info th a, table.m-table th.m-info a { + color: #c6ddf2; +} + +.m-note.m-info a:hover, +table.m-table tr.m-info td a:hover, table.m-table td.m-info a:hover, +table.m-table tr.m-info th a:hover, table.m-table th.m-info a:hover, +.m-note.m-info a:focus, +table.m-table tr.m-info td a:focus, table.m-table td.m-info a:focus, +table.m-table tr.m-info th a:focus, table.m-table th.m-info a:focus, +.m-note.m-info a:active, +table.m-table tr.m-info td a:active, table.m-table td.m-info a:active, +table.m-table tr.m-info th a:active, table.m-table th.m-info a:active { + color: #dbeaf7; +} + +.m-note.m-dim, +table.m-table tr.m-dim td, table.m-table td.m-dim, +table.m-table tr.m-dim th, table.m-table th.m-dim { + background-color: #f1f1f1; + color: #7c7c7c; +} + +.m-note.m-dim a, +table.m-table tr.m-dim td a, table.m-table td.m-dim a, +table.m-table tr.m-dim th a, table.m-table th.m-dim a { + color: #c0c0c0; +} + +.m-note.m-dim a:hover, +table.m-table tr.m-dim td a:hover, table.m-table td.m-dim a:hover, +table.m-table tr.m-dim th a:hover, table.m-table th.m-dim a:hover, +.m-note.m-dim a:focus, +table.m-table tr.m-dim td a:focus, table.m-table td.m-dim a:focus, +table.m-table tr.m-dim th a:focus, table.m-table th.m-dim a:focus, +.m-note.m-dim a:active, +table.m-table tr.m-dim td a:active, table.m-table td.m-dim a:active, +table.m-table tr.m-dim th a:active, table.m-table th.m-dim a:active { + color: #949494; +} + +figure.m-figure.m-default:before { + border-color: #f2f7fa; +} + +figure.m-figure.m-default figcaption { + color: #000000; +} + +figure.m-figure.m-primary:before { + border-color: #ef9069; +} + +figure.m-figure.m-primary figcaption { + color: #205275; +} + +figure.m-figure.m-success:before { + border-color: #4dd376; +} + +figure.m-figure.m-success figcaption { + color: #31c25d; +} + +figure.m-figure.m-warning:before { + border-color: #d19600; +} + +figure.m-figure.m-warning figcaption { + color: #d19600; +} + +figure.m-figure.m-danger:before { + border-color: #e23e3e; +} + +figure.m-figure.m-danger figcaption { + color: #f60000; +} + +figure.m-figure.m-info:before { + border-color: #4c93d3; +} + +figure.m-figure.m-info figcaption { + color: #2e7dc5; +} + +figure.m-figure.m-dim:before { + border-color: #f1f1f1; +} + +figure.m-figure.m-dim { + color: #bdbdbd; +} + +figure.m-figure.m-dim a { + color: #c0c0c0; +} + +figure.m-figure.m-dim a:hover, figure.m-figure.m-dim a:focus, figure.m-figure.m-dim a:active { + color: #949494; +} + +.m-math { + fill: #000000; +} + +.m-math.m-default, .m-math g.m-default, .m-math rect.m-default, +div.m-plot svg .m-bar.m-default, +.m-graph g.m-edge polygon, +.m-graph g.m-node:not(.m-flat) ellipse, +.m-graph g.m-node:not(.m-flat) polygon, +.m-graph g.m-edge text, +.m-graph g.m-node.m-flat text, +.m-graph.m-default g.m-edge polygon, +.m-graph.m-default g.m-node:not(.m-flat) ellipse, +.m-graph.m-default g.m-node:not(.m-flat) polygon, +.m-graph.m-default g.m-edge text, +.m-graph.m-default g.m-node.m-flat text { + fill: #000000; +} + .m-graph g.m-edge polygon, .m-graph g.m-edge path, .m-graph g.m-node ellipse, @@ -1528,1087 +2445,1668 @@ div.m-plot svg .m-bar.m-default, .m-graph.m-default g.m-node ellipse, .m-graph.m-default g.m-node polygon, .m-graph.m-default g.m-node polyline { - stroke: #000000; + stroke: #000000; +} + +.m-math.m-primary, .m-math g.m-primary, .m-math rect.m-primary, +div.m-plot svg .m-bar.m-primary, +.m-graph.m-primary g.m-edge polygon, +.m-graph.m-primary g.m-node:not(.m-flat) ellipse, +.m-graph.m-primary g.m-node:not(.m-flat) polygon, +.m-graph.m-primary g.m-edge text, +.m-graph.m-primary g.m-node.m-flat text { + fill: #205275; +} + +.m-graph.m-primary g.m-edge polygon, +.m-graph.m-primary g.m-edge path, +.m-graph.m-primary g.m-node ellipse, +.m-graph.m-primary g.m-node polygon, +.m-graph.m-primary g.m-node polyline { + stroke: #205275; +} + +.m-math.m-success, .m-math g.m-success, .m-math rect.m-success, +div.m-plot svg .m-bar.m-success, +.m-graph.m-success g.m-edge polygon, +.m-graph.m-success g.m-node:not(.m-flat) ellipse, +.m-graph.m-success g.m-node:not(.m-flat) polygon, +.m-graph.m-success g.m-edge text, +.m-graph.m-success g.m-node.m-flat text { + fill: #31c25d; +} + +.m-graph.m-success g.m-edge polygon, +.m-graph.m-success g.m-edge path, +.m-graph.m-success g.m-node ellipse, +.m-graph.m-success g.m-node polygon, +.m-graph.m-success g.m-node polyline { + stroke: #31c25d; +} + +.m-math.m-warning, .m-math g.m-warning, .m-math rect.m-warning, +div.m-plot svg .m-bar.m-warning, +.m-graph.m-warning g.m-edge polygon, +.m-graph.m-warning g.m-node:not(.m-flat) ellipse, +.m-graph.m-warning g.m-node:not(.m-flat) polygon, +.m-graph.m-warning g.m-edge text, +.m-graph.m-warning g.m-node.m-flat text { + fill: #d19600; +} + +.m-graph.m-warning g.m-edge polygon, +.m-graph.m-warning g.m-edge path, +.m-graph.m-warning g.m-node ellipse, +.m-graph.m-warning g.m-node polygon, +.m-graph.m-warning g.m-node polyline { + stroke: #d19600; +} + +.m-math.m-danger, .m-math g.m-danger, .m-math rect.m-danger, +div.m-plot svg .m-bar.m-danger, +.m-graph.m-danger g.m-edge polygon, +.m-graph.m-danger g.m-node:not(.m-flat) ellipse, +.m-graph.m-danger g.m-node:not(.m-flat) polygon, +.m-graph.m-danger g.m-edge text, +.m-graph.m-danger g.m-node.m-flat text { + fill: #f60000; +} + +.m-graph.m-danger g.m-edge polygon, +.m-graph.m-danger g.m-edge path, +.m-graph.m-danger g.m-node ellipse, +.m-graph.m-danger g.m-node polygon, +.m-graph.m-danger g.m-node polyline { + stroke: #f60000; +} + +.m-math.m-info, .m-math g.m-info, .m-math rect.m-info, +div.m-plot svg .m-bar.m-info, +.m-graph.m-info g.m-edge polygon, +.m-graph.m-info g.m-node:not(.m-flat) ellipse, +.m-graph.m-info g.m-node:not(.m-flat) polygon, +.m-graph.m-info g.m-edge text, +.m-graph.m-info g.m-node.m-flat text { + fill: #2e7dc5; +} + +.m-graph.m-info g.m-edge polygon, +.m-graph.m-info g.m-edge path, +.m-graph.m-info g.m-node ellipse, +.m-graph.m-info g.m-node polygon, +.m-graph.m-info g.m-node polyline { + stroke: #2e7dc5; +} + +.m-math.m-dim, .m-math g.m-dim, .m-math rect.m-dim, +div.m-plot svg .m-bar.m-dim, +.m-graph.m-dim g.m-edge polygon, +.m-graph.m-dim g.m-node:not(.m-flat) ellipse, +.m-graph.m-dim g.m-node:not(.m-flat) polygon, +.m-graph.m-dim g.m-edge text, +.m-graph.m-dim g.m-node.m-flat text { + fill: #bdbdbd; +} + +.m-graph.m-dim g.m-edge polygon, +.m-graph.m-dim g.m-edge path, +.m-graph.m-dim g.m-node ellipse, +.m-graph.m-dim g.m-node polygon, +.m-graph.m-dim g.m-node polyline { + stroke: #bdbdbd; +} + +.m-graph g.m-edge.m-default polygon, +.m-graph g.m-node.m-default:not(.m-flat) ellipse, +.m-graph g.m-node.m-default:not(.m-flat) polygon, +.m-graph g.m-edge.m-default text, +.m-graph g.m-node.m-default.m-flat text { + fill: #000000; +} + +.m-graph g.m-edge.m-default polygon, +.m-graph g.m-edge.m-default path, +.m-graph g.m-node.m-default ellipse, +.m-graph g.m-node.m-default polygon, +.m-graph g.m-node.m-default polyline { + stroke: #000000; +} + +.m-graph g.m-edge.m-primary polygon, +.m-graph g.m-node.m-primary:not(.m-flat) ellipse, +.m-graph g.m-node.m-primary:not(.m-flat) polygon, +.m-graph g.m-edge.m-primary text, +.m-graph g.m-node.m-primary.m-flat text { + fill: #205275; +} + +.m-graph g.m-edge.m-primary polygon, +.m-graph g.m-edge.m-primary path, +.m-graph g.m-node.m-primary ellipse, +.m-graph g.m-node.m-primary polygon, +.m-graph g.m-node.m-primary polyline { + stroke: #205275; +} + +.m-graph g.m-edge.m-success polygon, +.m-graph g.m-node.m-success:not(.m-flat) ellipse, +.m-graph g.m-node.m-success:not(.m-flat) polygon, +.m-graph g.m-edge.m-success text, +.m-graph g.m-node.m-success.m-flat text { + fill: #31c25d; +} + +.m-graph g.m-edge.m-success polygon, +.m-graph g.m-edge.m-success path, +.m-graph g.m-node.m-success ellipse, +.m-graph g.m-node.m-success polygon, +.m-graph g.m-node.m-success polyline { + stroke: #31c25d; +} + +.m-graph g.m-edge.m-warning polygon, +.m-graph g.m-node.m-warning:not(.m-flat) ellipse, +.m-graph g.m-node.m-warning:not(.m-flat) polygon, +.m-graph g.m-edge.m-warning text, +.m-graph g.m-node.m-warning.m-flat text { + fill: #d19600; +} + +.m-graph g.m-edge.m-warning polygon, +.m-graph g.m-edge.m-warning path, +.m-graph g.m-node.m-warning ellipse, +.m-graph g.m-node.m-warning polygon, +.m-graph g.m-node.m-warning polyline { + stroke: #d19600; +} + +.m-graph g.m-edge.m-danger polygon, +.m-graph g.m-node.m-danger:not(.m-flat) ellipse, +.m-graph g.m-node.m-danger:not(.m-flat) polygon, +.m-graph g.m-edge.m-danger text, +.m-graph g.m-node.m-danger.m-flat text { + fill: #f60000; +} + +.m-graph g.m-edge.m-danger polygon, +.m-graph g.m-edge.m-danger path, +.m-graph g.m-node.m-danger ellipse, +.m-graph g.m-node.m-danger polygon, +.m-graph g.m-node.m-danger polyline { + stroke: #f60000; +} + +.m-graph g.m-edge.m-info polygon, +.m-graph g.m-node.m-info:not(.m-flat) ellipse, +.m-graph g.m-node.m-info:not(.m-flat) polygon, +.m-graph g.m-edge.m-info text, +.m-graph g.m-node.m-info.m-flat text { + fill: #2e7dc5; +} + +.m-graph g.m-edge.m-info polygon, +.m-graph g.m-edge.m-info path, +.m-graph g.m-node.m-info ellipse, +.m-graph g.m-node.m-info polygon, +.m-graph g.m-node.m-info polyline { + stroke: #2e7dc5; +} + +.m-graph g.m-edge.m-dim polygon, +.m-graph g.m-node.m-dim:not(.m-flat) ellipse, +.m-graph g.m-node.m-dim:not(.m-flat) polygon, +.m-graph g.m-edge.m-dim text, +.m-graph g.m-node.m-dim.m-flat text { + fill: #bdbdbd; +} + +.m-graph g.m-edge.m-dim polygon, +.m-graph g.m-edge.m-dim path, +.m-graph g.m-node.m-dim ellipse, +.m-graph g.m-node.m-dim polygon, +.m-graph g.m-node.m-dim polyline { + stroke: #bdbdbd; +} + +p, ul, ol, dl, blockquote, pre, .m-code-figure, .m-console-figure, hr, .m-note, +.m-frame, .m-block, div.m-button, div.m-scroll, table.m-table, div.m-image, +img.m-image, svg.m-image, figure.m-figure, .m-imagegrid, div.m-math, +div.m-graph, div.m-plot { + margin-bottom: 1rem; +} + +p:last-child, p.m-nopadb, ul:last-child, ul.m-nopadb, +ol:last-child, ol.m-nopadb, dl:last-child, dl.m-nopadb, +blockquote:last-child, blockquote.m-nopadb, pre:last-child, pre.m-nopadb, +.m-code-figure:last-child, .m-code-figure.m-nopadb, +.m-console-figure:last-child, .m-console-figure.m-nopadb, +hr:last-child, hr.m-nopadb, .m-note:last-child, .m-note.m-nopadb, +.m-frame:last-child, .m-frame.m-nopadb, .m-block:last-child, .m-block.m-nopadb, +div.m-button:last-child, div.m-button.m-nopadb, +div.m-scroll:last-child, div.m-scroll.m-nopadb, +table.m-table:last-child, table.m-table.m-nopadb, +img.m-image:last-child, img.m-image.m-nopadb, +svg.m-image:last-child, svg.m-image.m-nopadb, +div.m-image:last-child, div.m-image.m-nopadb, +figure.m-figure:last-child, figure.m-figure.m-nopadb, +.m-imagegrid:last-child, .m-imagegrid.m-nopadb, +div.m-math:last-child, div.m-math.m-nopadb, +div.m-graph:last-child, div.m-graph.m-nopadb, +div.m-plot:last-child, div.m-plot.m-nopadb { + margin-bottom: 0; +} + +li > p:last-child, li > blockquote:last-child, li > pre:last-child, +li > .m-code-figure:last-child, li > .m-console-figure:last-child, +li > .m-note:last-child, li > .m-frame:last-child, li > .m-block:last-child, +li > div.m-button:last-child, li > div.m-scroll:last-child, li > table.m-table:last-child, +li > img.m-image:last-child, li > svg.m-image:last-child, li > div.m-image:last-child, +li > figure.m-figure:last-child, li > div.m-math:last-child, +li > div.m-graph:last-child, li > div.m-plot:last-child { + margin-bottom: 1rem; +} + +li:last-child > p:last-child, li:last-child > p.m-nopadb, +li:last-child > blockquote:last-child, li:last-child > blockquote.m-nopadb, +li:last-child > pre:last-child, li:last-child > pre.m-nopadb, +li:last-child > .m-code-figure:last-child, li:last-child > .m-code-figure.m-nopadb, +li:last-child > .m-console-figure:last-child, li:last-child > .m-console-figure.m-nopadb, +li:last-child > .m-note:last-child, li:last-child > .m-note.m-nopadb, +li:last-child > .m-frame:last-child, li:last-child > .m-frame.m-nopadb, +li:last-child > .m-block:last-child, li:last-child > .m-block.m-nopadb, +li:last-child > div.m-button:last-child, li:last-child > div.m-button.m-nopadb, +li:last-child > div.m-scroll:last-child, li:last-child > div.m-scroll.m-nopadb, +li:last-child > table.m-table:last-child, li:last-child > table.m-table.m-nopadb, +li:last-child > img.m-image:last-child, li:last-child > img.m-image.m-nopadb, +li:last-child > svg.m-image:last-child, li:last-child > svg.m-image.m-nopadb, +li:last-child > div.m-image:last-child, li:last-child > div.m-image.m-nopadb, +li:last-child > figure.m-figure:last-child, li:last-child > figure.m-figure.m-nopadb, +li:last-child > div.m-math:last-child, li:last-child > div.m-math.m-nopadb, +li:last-child > div.m-graph:last-child, li:last-child > div.m-graph.m-nopadb, +li:last-child > div.m-plot:last-child, li:last-child > div.m-plot.m-nopadb { + margin-bottom: 0; +} + +body > header > nav { + width: 100%; + background-color: #ffffff; + min-height: 3rem; +} + +body > header > nav.m-navbar-landing, +body > header > nav.m-navbar-cover { + background-color: transparent; + position: relative; +} + +body > header > nav.m-navbar-landing { + opacity: 0.8; +} + +body > header > nav.m-navbar-cover { + background-color: rgba(255, 255, 255, 0.25); + opacity: 1; +} + +body > header > nav.m-navbar-landing:hover, +body > header > nav.m-navbar-cover:hover { + background-color: rgba(255, 255, 255, 0.75); + opacity: 1; +} + +body > header > nav.m-navbar-landing:target, +body > header > nav.m-navbar-cover:target { + background-color: #ffffff; + opacity: 1; +} + +body > header > nav.m-navbar-landing #m-navbar-brand.m-navbar-brand-hidden { + visibility: hidden; +} + +body > header > nav.m-navbar-landing:target #m-navbar-brand.m-navbar-brand-hidden { + visibility: visible; +} + +body > header > nav { + margin-left: auto; + margin-right: auto; + color: #000000; +} + +body > header > nav a { + text-decoration: none; + text-transform: lowercase; + display: inline-block; + vertical-align: middle; + line-height: 2.75rem; + color: #000000; +} + +body > header > nav #m-navbar-brand, body > header > nav a#m-navbar-show, body > header > nav a#m-navbar-hide { + font-weight: normal; + font-size: 1.125rem; + padding-left: 1rem; + padding-right: 1rem; +} + +body > header > nav a#m-navbar-brand, body > header > nav #m-navbar-brand a { + text-transform: lowercase; +} + +body > header > nav a#m-navbar-brand img, body > header > nav #m-navbar-brand a img { + width: 1.75rem; + height: 1.75rem; + vertical-align: -22.5%; + margin-right: 0.5rem; +} + +body > header > nav #m-navbar-brand a { + padding-left: 0; + padding-right: 0; +} + +body > header > nav #m-navbar-brand .m-thin { + font-weight: normal; +} + +body > header > nav #m-navbar-brand .m-breadcrumb { + color: #bdbdbd; +} + +body > header > nav a#m-navbar-show:before, body > header > nav a#m-navbar-hide:before { + content: '\2630'; +} + +body > header > nav #m-navbar-collapse { + padding-bottom: 1rem; +} + +body > header > nav #m-navbar-collapse li { + border-style: solid; + border-color: transparent; + border-width: 0 0 0 0.25rem; + margin-left: -1rem; +} + +body > header > nav #m-navbar-collapse li a { + border-style: solid; + border-color: transparent; + line-height: 1.5rem; + margin-left: -0.25rem; + padding-left: 0.75rem; + border-width: 0 0 0 0.25rem; + width: 100%; +} + +body > header > nav #m-navbar-collapse li a#m-navbar-current { + color: #2f73a3; + border-color: #2f73a3; +} + +body > header > nav ol { + list-style-type: none; + margin: 0; +} + +body > header > nav ol ol { + padding-left: 1.5rem; +} + +body > header > nav .m-row > [class*='m-col-'] { + padding-top: 0; + padding-bottom: 0; +} + +body > header > nav a:hover, body > header > nav a:focus, body > header > nav a:active { + color: #205275; +} + +body > header > nav #m-navbar-collapse li:hover { + border-color: #205275; +} + +body > header > nav #m-navbar-collapse li a:hover, +body > header > nav #m-navbar-collapse li a:focus, +body > header > nav #m-navbar-collapse li a:active { + border-color: #205275; + background-color: rgba(255, 255, 255, 0.5); +} + +body > header > nav.m-navbar-landing #m-navbar-collapse li a:hover, +body > header > nav.m-navbar-cover #m-navbar-collapse li a:hover, +body > header > nav.m-navbar-landing #m-navbar-collapse li a:focus, +body > header > nav.m-navbar-cover #m-navbar-collapse li a:focus, +body > header > nav.m-navbar-landing #m-navbar-collapse li a:active, +body > header > nav.m-navbar-cover #m-navbar-collapse li a:active { + background-color: var(--header-link-active-background-color-semi); +} + +body > header > nav #m-navbar-hide { + display: none; +} + +body > header > nav:target #m-navbar-collapse { + display: block; +} + +body > header > nav:target #m-navbar-show { + display: none; +} + +body > header > nav:target #m-navbar-hide { + display: inline-block; +} + +@media screen and (min-width: 768px) { + body > header > nav #m-navbar-show, body > header > nav #m-navbar-hide, + body > header > nav:target #m-navbar-show, body > header > nav:target #m-navbar-hide { + display: none; + } + + body > header > nav #m-navbar-collapse li a { + line-height: 2.75rem; + } + + body > header > nav a, body > header > nav #m-navbar-collapse li a { + margin-left: 0; + padding-left: 1rem; + padding-right: 1rem; + white-space: nowrap; + } + + body > header > nav #m-navbar-collapse { + padding-bottom: 0; + } + + body > header > nav #m-navbar-collapse li ol { + background-color: #ffffff; + } + + body > header > nav #m-navbar-collapse ol ol li { + margin-left: 0; + padding-left: 0; + border-left-width: 0; + } + + body > header > nav #m-navbar-collapse ol ol li a { + padding-left: 0.75rem; + } + + body > header > nav #m-navbar-collapse > .m-row > ol > li { + margin-left: 0; + border-left-width: 0; + } + + body > header > nav #m-navbar-collapse > .m-row > ol > li > a { + border-width: 0.25rem 0 0 0; + } + + body > header > nav #m-navbar-collapse ol { + padding-left: 0; + padding-right: 0; + } + + body > header > nav #m-navbar-collapse > .m-row > ol, body > header > nav #m-navbar-collapse > .m-row > ol > li { + float: left; + } + + body > header > nav #m-navbar-collapse ol ol { + z-index: 99999; + position: absolute; + visibility: hidden; + } + + body > header > nav #m-navbar-collapse li:hover ol { + visibility: visible; + } +} + +body > footer { + width: 100%; +} + +body > footer > nav { + padding-top: 1rem; + padding-bottom: 1rem; + font-size: 0.85rem; + text-align: center; + color: #7c7c7c; + background-color: #eeeeee; +} + +body > footer > nav h3, body > footer > nav h3 a { + text-transform: uppercase; + font-weight: normal; +} + +body > footer > nav ul { + list-style-type: none; + padding: 0; + margin: 0; +} + +body > footer > nav a { + text-decoration: none; + text-transform: lowercase; + color: #191919; +} + +body > footer > nav a:hover, body > footer > nav a:focus, body > footer > nav a:active { + color: #494949; +} + +body > main { + padding-top: 1rem; + padding-bottom: 1rem; +} + +article h1 { + font-size: 1.75rem; +} + +article h1 .m-breadcrumb { + color: #bdbdbd; + font-weight: normal; +} + +article h1 .m-breadcrumb a { + color: #205275; +} + +article h1 .m-breadcrumb a:hover, article h1 a:focus, article h1 a:active { + color: #2e73a3; +} + +article > header h1 { + font-size: 2rem; + margin-bottom: 0.5rem; +} + +article h1 a, article > header h1, article > header h1 a, +article section > h2, article section > h2 a, +article section > h3, article section > h3 a, +article section > h4, article section > h4 a, +article section > h5, article section > h5 a, +article section > h6, article section > h6 a { + color: #205275; +} + +article h1 a:hover, article > header h1 a:hover, article > header h1 a:focus, article > header h1 a:active, +article section > h2 a:hover, article section > h2 a:focus, article section > h2 a:active, +article section > h3 a:hover, article section > h3 a:focus, article section > h3 a:active, +article section > h4 a:hover, article section > h4 a:focus, article section > h4 a:active, +article section > h5 a:hover, article section > h5 a:focus, article section > h5 a:active, +article section > h6 a:hover, article section > h6 a:focus, article section > h6 a:active { + color: #2e73a3; +} + +article > header .m-date { + display: block; + width: 2.5rem; + float: left; + text-align: center; + line-height: 95%; + font-size: 0.75rem; + font-weight: normal; + white-space: nowrap; + border-right-style: solid; + border-right-width: 0.125rem; + border-color: #205275; + padding-right: 0.75rem; + margin-top: -0.1rem; + margin-right: 0.75rem; + margin-bottom: 0.25rem; +} + +article > header .m-date-day { + display: block; + font-weight: bold; + padding-top: 0.2rem; + padding-bottom: 0.15rem; + font-size: 1.25rem; +} + +article > header p { + color: #7a7a7a; + font-size: 1.125rem; +} + +article > header h1::after { + content: " "; + clear: both; + display: table; +} + +article > footer { + color: #969696; +} + +article > footer p { + font-style: italic; + font-size: 0.85rem; + text-indent: 0; +} + +article section:target { + margin-left: -1.0rem; + border-left-style: solid; + border-left-width: 0.25rem; + padding-left: 0.75rem; + border-color: #205275; +} + +article h1 a, article > header h1 a, article section > h2 a, article section > h3 a, +article section > h4 a, article section > h5 a, article section > h6 a { + text-decoration: none; +} + +#m-landing-image, #m-cover-image, article#m-jumbo > header #m-jumbo-image { + background-size: cover; + background-color: #666666; + background-position: center center; + background-repeat: no-repeat; + margin-top: -4rem; + padding-top: 5rem; +} + +#m-landing-image { + color: #ffffff; +} + +#m-cover-image { + height: 30rem; + margin-bottom: -26rem; +} + +#m-landing-cover h1 { + font-size: 2.8rem; + margin-top: -0.5rem; + padding-left: 1.5rem; + padding-bottom: 1rem; + text-transform: lowercase; +} + +#m-landing-cover { + padding-bottom: 10rem; + margin-bottom: -6rem; +} + +article#m-jumbo { + margin-top: -1rem; +} + +#m-landing-cover, #m-cover-image > div, article#m-jumbo > header #m-jumbo-cover { + background: linear-gradient(transparent 0%, transparent 50%, #ffffff 100%); + width: 100%; + height: 100%; +} + +article#m-jumbo > header h1, article#m-jumbo > header h2 { + text-align: center; + font-weight: bold; +} + +article#m-jumbo > header a { + text-decoration: none; +} + +article#m-jumbo > header #m-jumbo-cover { + padding-bottom: 5rem; +} + +article#m-jumbo > header #m-jumbo-image { + font-size: 2.5vmin; + margin-bottom: -3rem; +} + +article#m-jumbo > header h1 { + font-size: 10vmin; +} + +article#m-jumbo > header h2 { + font-size: 3vmin; +} + +@media screen and (max-height: 640px) , screen and (max-width: 640px) { + article#m-jumbo > header h1 { + font-size: 3rem; + } + + article#m-jumbo > header #m-jumbo-image, article#m-jumbo > header h2 { + font-size: 1rem; + } +} + +article#m-jumbo > header, article#m-jumbo > header h1, article#m-jumbo > header a { + color: #ffffff; +} + +article#m-jumbo > header a:hover, article#m-jumbo > header a:focus, article#m-jumbo > header a:active { + color: #f0f0f0; +} + +article#m-jumbo.m-inverted > header, article#m-jumbo.m-inverted > header h1, article#m-jumbo.m-inverted > header a { + color: #000000; +} + +article#m-jumbo.m-inverted > header a:hover, article#m-jumbo.m-inverted > header a:focus, article#m-jumbo.m-inverted > header a:active { + color: #0f0f0f; +} + +.m-landing-news h3 a { + color: #000000; + text-decoration: none; + text-transform: uppercase; +} + +.m-landing-news h3 a:hover, .m-landing-news h3 a:hover, .m-landing-news h3 a:focus, .m-landing-news h3 a:active { + color: #205275; +} + +.m-landing-news time { + display: inline-block; + margin-left: 1rem; + float: right; +} + +.m-article-pagination { + text-align: center; + padding: 1rem; +} + +nav.m-navpanel { + text-align: center; +} + +nav.m-navpanel h3 { + text-transform: uppercase; + font-weight: normal; +} + +nav.m-navpanel ol { + text-transform: lowercase; +} + +nav.m-navpanel ol, nav.m-navpanel ul { + list-style-type: none; + padding: 0; +} + +nav.m-navpanel a { + color: #292929; + text-decoration: none; +} + +nav.m-navpanel a:hover, nav.m-navpanel a:focus, nav.m-navpanel a:active { + color: #205275; +} + +ul.m-tagcloud li { + display: inline; +} + +ul.m-tagcloud li.m-tag-1 { + font-size: 0.75rem; +} + +ul.m-tagcloud li.m-tag-2 { + font-size: 0.825rem; +} + +ul.m-tagcloud li.m-tag-3 { + font-size: 1rem; +} + +ul.m-tagcloud li.m-tag-4 { + font-size: 1.25rem; +} + +ul.m-tagcloud li.m-tag-5 { + font-size: 1.5rem; +} + +article section:target figure.m-code-figure, article section:target figure.m-console-figure { + z-index: 1; +} + +article, article > header, article section { + margin-bottom: 1rem; +} + +article:last-child, article section:last-child { + margin-bottom: 0; +} + +.m-container-inflatable section:target > .m-note, +.m-container-inflatable section:target > .m-frame, +.m-container-inflatable section:target > .m-block, +.m-container-inflatable section:target > pre, +.m-container-inflatable section:target > .m-code-figure > pre:first-child, +.m-container-inflatable section:target > .m-console-figure > pre:first-child, +.m-container-inflatable section:target section > .m-note, +.m-container-inflatable section:target section > .m-frame, +.m-container-inflatable section:target section > .m-block, +.m-container-inflatable section:target section > pre, +.m-container-inflatable section:target section > .m-code-figure > pre:first-child, +.m-container-inflatable section:target section > .m-console-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-center-'] > .m-note, +.m-container-inflatable section:target [class*='m-center-'] > .m-frame, +.m-container-inflatable section:target [class*='m-center-'] > .m-block, +.m-container-inflatable section:target [class*='m-center-'] > pre, +.m-container-inflatable section:target [class*='m-center-'] > .m-code-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-center-'] > .m-console-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-left-'] > .m-note, +.m-container-inflatable section:target [class*='m-left-'] > .m-frame, +.m-container-inflatable section:target [class*='m-left-'] > .m-block, +.m-container-inflatable section:target [class*='m-left-'] > pre, +.m-container-inflatable section:target [class*='m-left-'] > .m-code-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-left-'] > .m-console-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-right-'] > .m-note, +.m-container-inflatable section:target [class*='m-right-'] > .m-frame, +.m-container-inflatable section:target [class*='m-right-'] > .m-block, +.m-container-inflatable section:target [class*='m-right-'] > pre, +.m-container-inflatable section:target [class*='m-right-'] > .m-code-figure > pre:first-child, +.m-container-inflatable section:target [class*='m-right-'] > .m-console-figure > pre:first-child, +.m-container-inflatable section:target .m-container-inflate > .m-note, +.m-container-inflatable section:target .m-container-inflate > .m-frame, +.m-container-inflatable section:target .m-container-inflate > .m-block, +.m-container-inflatable section:target .m-container-inflate > pre, +.m-container-inflatable section:target .m-container-inflate > .m-code-figure > pre:first-child, +.m-container-inflatable section:target .m-container-inflate > .m-console-figure > pre:first-child { + margin-left: -1.0rem; + border-left-style: solid; + border-left-width: 0.25rem; + border-top-left-radius: 0; + border-bottom-left-radius: 0; + padding-left: 0.75rem; } -.m-math.m-primary, .m-math g.m-primary, .m-math rect.m-primary, -div.m-plot svg .m-bar.m-primary, -.m-graph.m-primary g.m-edge polygon, -.m-graph.m-primary g.m-node:not(.m-flat) ellipse, -.m-graph.m-primary g.m-node:not(.m-flat) polygon, -.m-graph.m-primary g.m-edge text, -.m-graph.m-primary g.m-node.m-flat text { - fill: #205275; + +.m-container-inflatable section:target > .m-code-figure::before, +.m-container-inflatable section:target > .m-console-figure::before, +.m-container-inflatable section:target section > .m-code-figure::before, +.m-container-inflatable section:target section > .m-console-figure::before, +.m-container-inflatable section:target [class*='m-center-'] > .m-code-figure::before, +.m-container-inflatable section:target [class*='m-center-'] > .m-console-figure::before, +.m-container-inflatable section:target [class*='m-left-'] > .m-code-figure::before, +.m-container-inflatable section:target [class*='m-left-'] > .m-console-figure::before, +.m-container-inflatable section:target [class*='m-right-'] > .m-code-figure::before, +.m-container-inflatable section:target [class*='m-right-'] > .m-console-figure::before, +.m-container-inflatable section:target .m-container-inflate > .m-code-figure::before, +.m-container-inflatable section:target .m-container-inflate > .m-console-figure::before { + border-top-left-radius: 0; + border-bottom-left-radius: 0; + border-left-width: 0.25rem; } -.m-graph.m-primary g.m-edge polygon, -.m-graph.m-primary g.m-edge path, -.m-graph.m-primary g.m-node ellipse, -.m-graph.m-primary g.m-node polygon, -.m-graph.m-primary g.m-node polyline { - stroke: #205275; + +.m-container-inflatable section:target > .m-code-figure > pre.m-nopad, +.m-container-inflatable section:target > .m-console-figure > pre.m-nopad { + margin-left: -0.75rem; + padding-left: -0.75rem; } -.m-math.m-success, .m-math g.m-success, .m-math rect.m-success, -div.m-plot svg .m-bar.m-success, -.m-graph.m-success g.m-edge polygon, -.m-graph.m-success g.m-node:not(.m-flat) ellipse, -.m-graph.m-success g.m-node:not(.m-flat) polygon, -.m-graph.m-success g.m-edge text, -.m-graph.m-success g.m-node.m-flat text { - fill: #31c25d; + +@media screen and (min-width: 576px) { + .m-container-inflatable section:target .m-center-s > .m-note, + .m-container-inflatable section:target .m-center-s > pre, + .m-container-inflatable section:target .m-center-s > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-center-s > figure.m-console-figure > pre:first-child, + .m-container-inflatable section:target .m-right-s > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-right-s > figure.m-console-figure > pre:first-child { + border-left-width: 0; + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-s > .m-block, + .m-container-inflatable section:target .m-right-s > .m-block { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + } + + .m-container-inflatable section:target .m-center-s > .m-frame, + .m-container-inflatable section:target .m-right-s > .m-frame { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + padding-left: 0.875rem; + } + + .m-container-inflatable section:target .m-right-s > .m-block, + .m-container-inflatable section:target .m-right-s > .m-frame { + margin-left: 0; + } + + .m-container-inflatable section:target .m-right-s > .m-note, + .m-container-inflatable section:target .m-right-s > pre { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + margin-left: 0; + border-left-width: 0; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-s > figure.m-code-figure::before, + .m-container-inflatable section:target .m-center-s > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-s > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-s > figure.m-console-figure::before { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + } } -.m-graph.m-success g.m-edge polygon, -.m-graph.m-success g.m-edge path, -.m-graph.m-success g.m-node ellipse, -.m-graph.m-success g.m-node polygon, -.m-graph.m-success g.m-node polyline { - stroke: #31c25d; + +@media screen and (min-width: 768px) { + .m-container-inflatable section:target .m-center-m > .m-note, + .m-container-inflatable section:target .m-center-m > pre, + .m-container-inflatable section:target .m-center-m > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-center-m > figure.m-console-figure > pre:first-child, + .m-container-inflatable section:target .m-right-m > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-right-m > figure.m-console-figure > pre:first-child { + border-left-width: 0; + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-m > .m-block, + .m-container-inflatable section:target .m-right-m > .m-block { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + } + + .m-container-inflatable section:target .m-center-m > .m-frame, + .m-container-inflatable section:target .m-right-m > .m-frame { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + padding-left: 0.875rem; + } + + .m-container-inflatable section:target .m-right-m > .m-block, + .m-container-inflatable section:target .m-right-m > .m-frame { + margin-left: 0; + } + + .m-container-inflatable section:target .m-right-m > .m-note, + .m-container-inflatable section:target .m-right-m > pre { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + margin-left: 0; + border-left-width: 0; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-m > figure.m-code-figure::before, + .m-container-inflatable section:target .m-center-m > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-m > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-m > figure.m-console-figure::before { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + } } -.m-math.m-warning, .m-math g.m-warning, .m-math rect.m-warning, -div.m-plot svg .m-bar.m-warning, -.m-graph.m-warning g.m-edge polygon, -.m-graph.m-warning g.m-node:not(.m-flat) ellipse, -.m-graph.m-warning g.m-node:not(.m-flat) polygon, -.m-graph.m-warning g.m-edge text, -.m-graph.m-warning g.m-node.m-flat text { - fill: #d19600; + +@media screen and (min-width: 992px) { + .m-container-inflatable section:target .m-center-l > .m-note, + .m-container-inflatable section:target .m-center-l > pre, + .m-container-inflatable section:target .m-center-l > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-center-l > figure.m-console-figure > pre:first-child, + .m-container-inflatable section:target .m-right-l > figure.m-code-figure > pre:first-child, + .m-container-inflatable section:target .m-right-l > figure.m-console-figure > pre:first-child { + border-left-width: 0; + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-l > .m-block, + .m-container-inflatable section:target .m-right-l > .m-block { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + } + + .m-container-inflatable section:target .m-center-l > .m-frame, + .m-container-inflatable section:target .m-right-l > .m-frame { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + padding-left: 0.875rem; + } + + .m-container-inflatable section:target .m-right-l > .m-block, + .m-container-inflatable section:target .m-right-l > .m-frame { + margin-left: 0; + } + + .m-container-inflatable section:target .m-right-l > .m-note, + .m-container-inflatable section:target .m-right-l > pre { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + margin-left: 0; + border-left-width: 0; + padding-left: 1rem; + } + + .m-container-inflatable section:target .m-center-l > figure.m-code-figure::before, + .m-container-inflatable section:target .m-center-l > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-l > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-l > figure.m-console-figure::before { + border-top-left-radius: 0.2rem; + border-bottom-left-radius: 0.2rem; + border-left-width: 0.125rem; + } } -.m-graph.m-warning g.m-edge polygon, -.m-graph.m-warning g.m-edge path, -.m-graph.m-warning g.m-node ellipse, -.m-graph.m-warning g.m-node polygon, -.m-graph.m-warning g.m-node polyline { - stroke: #d19600; + +.m-container-inflatable section:target > figure.m-code-figure::before, +.m-container-inflatable section:target > figure.m-console-figure::before, +.m-container-inflatable section:target section > figure.m-code-figure::before, +.m-container-inflatable section:target section > figure.m-console-figure::before, +.m-container-inflatable section:target [class*='m-center-'] > figure.m-code-figure::before, +.m-container-inflatable section:target [class*='m-center-'] > figure.m-console-figure::before, +.m-container-inflatable section:target [class*='m-left-'] > figure.m-code-figure::before, +.m-container-inflatable section:target [class*='m-left-'] > figure.m-console-figure::before, +.m-container-inflatable section:target [class*='m-right-'] > figure.m-code-figure::before, +.m-container-inflatable section:target [class*='m-right-'] > figure.m-console-figure::before, +.m-container-inflatable section:target .m-container-inflatable > figure.m-code-figure::before, +.m-container-inflatable section:target .m-container-inflatable > figure.m-console-figure::before { + border-left-color: #c7cfd9; } -.m-math.m-danger, .m-math g.m-danger, .m-math rect.m-danger, -div.m-plot svg .m-bar.m-danger, -.m-graph.m-danger g.m-edge polygon, -.m-graph.m-danger g.m-node:not(.m-flat) ellipse, -.m-graph.m-danger g.m-node:not(.m-flat) polygon, -.m-graph.m-danger g.m-edge text, -.m-graph.m-danger g.m-node.m-flat text { - fill: #f60000; + +@media screen and (min-width: 576px) { + .m-container-inflatable section:target .m-center-s > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-s > figure.m-code-figure::before { + border-color: #f2f7fa; + } + + .m-container-inflatable section:target .m-center-s > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-s > figure.m-console-figure::before { + border-color: #000000; + } } -.m-graph.m-danger g.m-edge polygon, -.m-graph.m-danger g.m-edge path, -.m-graph.m-danger g.m-node ellipse, -.m-graph.m-danger g.m-node polygon, -.m-graph.m-danger g.m-node polyline { - stroke: #f60000; + +@media screen and (min-width: 768px) { + .m-container-inflatable section:target .m-center-m > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-m > figure.m-code-figure::before { + border-color: #f2f7fa; + } + + .m-container-inflatable section:target .m-center-m > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-m > figure.m-console-figure::before { + border-color: #000000; + } } -.m-math.m-info, .m-math g.m-info, .m-math rect.m-info, -div.m-plot svg .m-bar.m-info, -.m-graph.m-info g.m-edge polygon, -.m-graph.m-info g.m-node:not(.m-flat) ellipse, -.m-graph.m-info g.m-node:not(.m-flat) polygon, -.m-graph.m-info g.m-edge text, -.m-graph.m-info g.m-node.m-flat text { - fill: #2e7dc5; + +@media screen and (min-width: 992px) { + .m-container-inflatable section:target .m-center-l > figure.m-code-figure::before, + .m-container-inflatable section:target .m-right-l > figure.m-code-figure::before { + border-color: #f2f7fa; + } + + .m-container-inflatable section:target .m-center-l > figure.m-console-figure::before, + .m-container-inflatable section:target .m-right-l > figure.m-console-figure::before { + border-color: #000000; + } } -.m-graph.m-info g.m-edge polygon, -.m-graph.m-info g.m-edge path, -.m-graph.m-info g.m-node ellipse, -.m-graph.m-info g.m-node polygon, -.m-graph.m-info g.m-node polyline { - stroke: #2e7dc5; + +.m-container-inflatable section:target pre, +.m-container-inflatable section:target figure.m-code-figure > pre:first-child, +.m-container-inflatable section:target figure.m-console-figure > pre:first-child { + border-color: #c7cfd9; } -.m-math.m-dim, .m-math g.m-dim, .m-math rect.m-dim, -div.m-plot svg .m-bar.m-dim, -.m-graph.m-dim g.m-edge polygon, -.m-graph.m-dim g.m-node:not(.m-flat) ellipse, -.m-graph.m-dim g.m-node:not(.m-flat) polygon, -.m-graph.m-dim g.m-edge text, -.m-graph.m-dim g.m-node.m-flat text { - fill: #bdbdbd; + +.m-container-inflatable section:target .m-note.m-default { + border-color: #c7cfd9; } -.m-graph.m-dim g.m-edge polygon, -.m-graph.m-dim g.m-edge path, -.m-graph.m-dim g.m-node ellipse, -.m-graph.m-dim g.m-node polygon, -.m-graph.m-dim g.m-node polyline { - stroke: #bdbdbd; + +.m-container-inflatable section:target .m-note.m-primary { + border-color: #205275; } -.m-graph g.m-edge.m-default polygon, -.m-graph g.m-node.m-default:not(.m-flat) ellipse, -.m-graph g.m-node.m-default:not(.m-flat) polygon, -.m-graph g.m-edge.m-default text, -.m-graph g.m-node.m-default.m-flat text { - fill: #000000; + +.m-container-inflatable section:target .m-note.m-success { + border-color: #31c25d; } -.m-graph g.m-edge.m-default polygon, -.m-graph g.m-edge.m-default path, -.m-graph g.m-node.m-default ellipse, -.m-graph g.m-node.m-default polygon, -.m-graph g.m-node.m-default polyline { - stroke: #000000; + +.m-container-inflatable section:target .m-note.m-warning { + border-color: #d19600; } -.m-graph g.m-edge.m-primary polygon, -.m-graph g.m-node.m-primary:not(.m-flat) ellipse, -.m-graph g.m-node.m-primary:not(.m-flat) polygon, -.m-graph g.m-edge.m-primary text, -.m-graph g.m-node.m-primary.m-flat text { - fill: #205275; + +.m-container-inflatable section:target .m-note.m-danger { + border-color: #f60000; } -.m-graph g.m-edge.m-primary polygon, -.m-graph g.m-edge.m-primary path, -.m-graph g.m-node.m-primary ellipse, -.m-graph g.m-node.m-primary polygon, -.m-graph g.m-node.m-primary polyline { - stroke: #205275; + +.m-container-inflatable section:target .m-note.m-info { + border-color: #2e7dc5; } -.m-graph g.m-edge.m-success polygon, -.m-graph g.m-node.m-success:not(.m-flat) ellipse, -.m-graph g.m-node.m-success:not(.m-flat) polygon, -.m-graph g.m-edge.m-success text, -.m-graph g.m-node.m-success.m-flat text { - fill: #31c25d; + +.m-container-inflatable section:target .m-note.m-dim { + border-color: #bdbdbd; } -.m-graph g.m-edge.m-success polygon, -.m-graph g.m-edge.m-success path, -.m-graph g.m-node.m-success ellipse, -.m-graph g.m-node.m-success polygon, -.m-graph g.m-node.m-success polyline { - stroke: #31c25d; + +.m-code .hll { + background-color: #ffffcc } -.m-graph g.m-edge.m-warning polygon, -.m-graph g.m-node.m-warning:not(.m-flat) ellipse, -.m-graph g.m-node.m-warning:not(.m-flat) polygon, -.m-graph g.m-edge.m-warning text, -.m-graph g.m-node.m-warning.m-flat text { - fill: #d19600; + +.m-code { + background: #f8f8f8; } -.m-graph g.m-edge.m-warning polygon, -.m-graph g.m-edge.m-warning path, -.m-graph g.m-node.m-warning ellipse, -.m-graph g.m-node.m-warning polygon, -.m-graph g.m-node.m-warning polyline { - stroke: #d19600; + +.m-code .c { + color: #8f5902; + font-style: italic } -.m-graph g.m-edge.m-danger polygon, -.m-graph g.m-node.m-danger:not(.m-flat) ellipse, -.m-graph g.m-node.m-danger:not(.m-flat) polygon, -.m-graph g.m-edge.m-danger text, -.m-graph g.m-node.m-danger.m-flat text { - fill: #f60000; + +.m-code .err { + color: #a40000; + border: 1px solid #ef2929 } -.m-graph g.m-edge.m-danger polygon, -.m-graph g.m-edge.m-danger path, -.m-graph g.m-node.m-danger ellipse, -.m-graph g.m-node.m-danger polygon, -.m-graph g.m-node.m-danger polyline { - stroke: #f60000; + +.m-code .g { + color: #000000 } -.m-graph g.m-edge.m-info polygon, -.m-graph g.m-node.m-info:not(.m-flat) ellipse, -.m-graph g.m-node.m-info:not(.m-flat) polygon, -.m-graph g.m-edge.m-info text, -.m-graph g.m-node.m-info.m-flat text { - fill: #2e7dc5; + +.m-code .k { + color: #204a87; + font-weight: bold } -.m-graph g.m-edge.m-info polygon, -.m-graph g.m-edge.m-info path, -.m-graph g.m-node.m-info ellipse, -.m-graph g.m-node.m-info polygon, -.m-graph g.m-node.m-info polyline { - stroke: #2e7dc5; + +.m-code .l { + color: #000000 } -.m-graph g.m-edge.m-dim polygon, -.m-graph g.m-node.m-dim:not(.m-flat) ellipse, -.m-graph g.m-node.m-dim:not(.m-flat) polygon, -.m-graph g.m-edge.m-dim text, -.m-graph g.m-node.m-dim.m-flat text { - fill: #bdbdbd; + +.m-code .n { + color: #000000 +} + +.m-code .o { + color: #ce5c00; + font-weight: bold +} + +.m-code .x { + color: #000000 +} + +.m-code .p { + color: #000000; + font-weight: bold } -.m-graph g.m-edge.m-dim polygon, -.m-graph g.m-edge.m-dim path, -.m-graph g.m-node.m-dim ellipse, -.m-graph g.m-node.m-dim polygon, -.m-graph g.m-node.m-dim polyline { - stroke: #bdbdbd; + +.m-code .ch { + color: #8f5902; + font-style: italic } -p, ul, ol, dl, blockquote, pre, .m-code-figure, .m-console-figure, hr, .m-note, -.m-frame, .m-block, div.m-button, div.m-scroll, table.m-table, div.m-image, -img.m-image, svg.m-image, figure.m-figure, .m-imagegrid, div.m-math, -div.m-graph, div.m-plot { - margin-bottom: 1rem; + +.m-code .cm { + color: #8f5902; + font-style: italic } -p:last-child, p.m-nopadb, ul:last-child, ul.m-nopadb, -ol:last-child, ol.m-nopadb, dl:last-child, dl.m-nopadb, -blockquote:last-child, blockquote.m-nopadb, pre:last-child, pre.m-nopadb, -.m-code-figure:last-child, .m-code-figure.m-nopadb, -.m-console-figure:last-child, .m-console-figure.m-nopadb, -hr:last-child, hr.m-nopadb, .m-note:last-child, .m-note.m-nopadb, -.m-frame:last-child, .m-frame.m-nopadb, .m-block:last-child, .m-block.m-nopadb, -div.m-button:last-child, div.m-button.m-nopadb, -div.m-scroll:last-child, div.m-scroll.m-nopadb, -table.m-table:last-child, table.m-table.m-nopadb, -img.m-image:last-child, img.m-image.m-nopadb, -svg.m-image:last-child, svg.m-image.m-nopadb, -div.m-image:last-child, div.m-image.m-nopadb, -figure.m-figure:last-child, figure.m-figure.m-nopadb, -.m-imagegrid:last-child, .m-imagegrid.m-nopadb, -div.m-math:last-child, div.m-math.m-nopadb, -div.m-graph:last-child, div.m-graph.m-nopadb, -div.m-plot:last-child, div.m-plot.m-nopadb { - margin-bottom: 0; + +.m-code .cp { + color: #8f5902; + font-style: italic } -li > p:last-child, li > blockquote:last-child, li > pre:last-child, -li > .m-code-figure:last-child, li > .m-console-figure:last-child, -li > .m-note:last-child, li > .m-frame:last-child, li > .m-block:last-child, -li > div.m-button:last-child, li > div.m-scroll:last-child, li > table.m-table:last-child, -li > img.m-image:last-child, li > svg.m-image:last-child, li > div.m-image:last-child, -li > figure.m-figure:last-child, li > div.m-math:last-child, -li > div.m-graph:last-child, li > div.m-plot:last-child { - margin-bottom: 1rem; + +.m-code .cpf { + color: #8f5902; + font-style: italic } -li:last-child > p:last-child, li:last-child > p.m-nopadb, -li:last-child > blockquote:last-child, li:last-child > blockquote.m-nopadb, -li:last-child > pre:last-child, li:last-child > pre.m-nopadb, -li:last-child > .m-code-figure:last-child, li:last-child > .m-code-figure.m-nopadb, -li:last-child > .m-console-figure:last-child, li:last-child > .m-console-figure.m-nopadb, -li:last-child > .m-note:last-child, li:last-child > .m-note.m-nopadb, -li:last-child > .m-frame:last-child, li:last-child > .m-frame.m-nopadb, -li:last-child > .m-block:last-child, li:last-child > .m-block.m-nopadb, -li:last-child > div.m-button:last-child, li:last-child > div.m-button.m-nopadb, -li:last-child > div.m-scroll:last-child, li:last-child > div.m-scroll.m-nopadb, -li:last-child > table.m-table:last-child, li:last-child > table.m-table.m-nopadb, -li:last-child > img.m-image:last-child, li:last-child > img.m-image.m-nopadb, -li:last-child > svg.m-image:last-child, li:last-child > svg.m-image.m-nopadb, -li:last-child > div.m-image:last-child, li:last-child > div.m-image.m-nopadb, -li:last-child > figure.m-figure:last-child, li:last-child > figure.m-figure.m-nopadb, -li:last-child > div.m-math:last-child, li:last-child > div.m-math.m-nopadb, -li:last-child > div.m-graph:last-child, li:last-child > div.m-graph.m-nopadb, -li:last-child > div.m-plot:last-child, li:last-child > div.m-plot.m-nopadb { - margin-bottom: 0; + +.m-code .c1 { + color: #8f5902; + font-style: italic } -body > header > nav { - width: 100%; - background-color: #ffffff; - min-height: 3rem; +.m-code .cs { + color: #8f5902; + font-style: italic } -body > header > nav.m-navbar-landing, -body > header > nav.m-navbar-cover { - background-color: transparent; - position: relative; + +.m-code .gd { + color: #a40000 } -body > header > nav.m-navbar-landing { - opacity: 0.8; + +.m-code .ge { + color: #000000; + font-style: italic } -body > header > nav.m-navbar-cover { - background-color: rgba(255, 255, 255, 0.25); - opacity: 1; + +.m-code .gr { + color: #ef2929 } -body > header > nav.m-navbar-landing:hover, -body > header > nav.m-navbar-cover:hover { - background-color: rgba(255, 255, 255, 0.75); - opacity: 1; + +.m-code .gh { + color: #000080; + font-weight: bold } -body> header > nav.m-navbar-landing:target, -body> header > nav.m-navbar-cover:target { - background-color: #ffffff; - opacity: 1; + +.m-code .gi { + color: #00A000 } -body > header > nav.m-navbar-landing #m-navbar-brand.m-navbar-brand-hidden { - visibility: hidden; + +.m-code .go { + color: #000000; + font-style: italic } -body > header > nav.m-navbar-landing:target #m-navbar-brand.m-navbar-brand-hidden { - visibility: visible; + +.m-code .gp { + color: #8f5902 } -body > header > nav { - margin-left: auto; - margin-right: auto; - color: #000000; + +.m-code .gs { + color: #000000; + font-weight: bold } -body > header > nav a { - text-decoration: none; - text-transform: lowercase; - display: inline-block; - vertical-align: middle; - line-height: 2.75rem; - color: #000000; + +.m-code .gu { + color: #800080; + font-weight: bold } -body > header > nav #m-navbar-brand, body > header > nav a#m-navbar-show, body > header > nav a#m-navbar-hide { - font-weight: normal; - font-size: 1.125rem; - padding-left: 1rem; - padding-right: 1rem; + +.m-code .gt { + color: #a40000; + font-weight: bold } -body > header > nav a#m-navbar-brand, body > header > nav #m-navbar-brand a { - text-transform: lowercase; + +.m-code .kc { + color: #204a87; + font-weight: bold } -body > header > nav a#m-navbar-brand img, body > header > nav #m-navbar-brand a img { - width: 1.75rem; - height: 1.75rem; - vertical-align: -22.5%; - margin-right: 0.5rem; + +.m-code .kd { + color: #204a87; + font-weight: bold } -body > header > nav #m-navbar-brand a { - padding-left: 0; - padding-right: 0; + +.m-code .kn { + color: #204a87; + font-weight: bold } -body > header > nav #m-navbar-brand .m-thin { - font-weight: normal; + +.m-code .kp { + color: #204a87; + font-weight: bold } -body > header > nav #m-navbar-brand .m-breadcrumb { - color: #bdbdbd; + +.m-code .kr { + color: #204a87; + font-weight: bold } -body > header > nav a#m-navbar-show:before, body > header > nav a#m-navbar-hide:before { - content:'\2630'; + +.m-code .kt { + color: #204a87; + font-weight: bold } -body > header > nav #m-navbar-collapse { - padding-bottom: 1rem; + +.m-code .ld { + color: #000000 } -body > header > nav #m-navbar-collapse li { - border-style: solid; - border-color: transparent; - border-width: 0 0 0 0.25rem; - margin-left: -1rem; + +.m-code .m { + color: #0000cf; + font-weight: bold } -body > header > nav #m-navbar-collapse li a { - border-style: solid; - border-color: transparent; - line-height: 1.5rem; - margin-left: -0.25rem; - padding-left: 0.75rem; - border-width: 0 0 0 0.25rem; - width: 100%; + +.m-code .s { + color: #4e9a06 } -body > header > nav #m-navbar-collapse li a#m-navbar-current { - color: #2f73a3; - border-color: #2f73a3; + +.m-code .na { + color: #c4a000 } -body > header > nav ol { - list-style-type: none; - margin: 0; + +.m-code .nb { + color: #204a87 } -body > header > nav ol ol { - padding-left: 1.5rem; + +.m-code .nc { + color: #000000 } -body > header > nav .m-row > [class*='m-col-'] { - padding-top: 0; - padding-bottom: 0; + +.m-code .no { + color: #000000 } -body > header > nav a:hover, body > header > nav a:focus, body > header > nav a:active { - color: #205275; + +.m-code .nd { + color: #5c35cc; + font-weight: bold } -body > header > nav #m-navbar-collapse li:hover { - border-color: #205275; + +.m-code .ni { + color: #ce5c00 } -body > header > nav #m-navbar-collapse li a:hover, -body > header > nav #m-navbar-collapse li a:focus, -body > header > nav #m-navbar-collapse li a:active { - border-color: #205275; - background-color: rgba(255, 255, 255, 0.5); + +.m-code .ne { + color: #cc0000; + font-weight: bold } -body > header > nav.m-navbar-landing #m-navbar-collapse li a:hover, -body > header > nav.m-navbar-cover #m-navbar-collapse li a:hover, -body > header > nav.m-navbar-landing #m-navbar-collapse li a:focus, -body > header > nav.m-navbar-cover #m-navbar-collapse li a:focus, -body > header > nav.m-navbar-landing #m-navbar-collapse li a:active, -body > header > nav.m-navbar-cover #m-navbar-collapse li a:active { - background-color: var(--header-link-active-background-color-semi); + +.m-code .nf { + color: #000000 } -body > header > nav #m-navbar-hide { - display: none; + +.m-code .nl { + color: #f57900 } -body > header > nav:target #m-navbar-collapse { - display: block; + +.m-code .nn { + color: #000000 } -body > header > nav:target #m-navbar-show { - display: none; + +.m-code .nx { + color: #000000 } -body > header > nav:target #m-navbar-hide { - display: inline-block; + +.m-code .py { + color: #000000 } -@media screen and (min-width: 768px) { - body > header > nav #m-navbar-show, body > header > nav #m-navbar-hide, - body > header > nav:target #m-navbar-show, body > header > nav:target #m-navbar-hide { - display: none; - } - body > header > nav #m-navbar-collapse li a { - line-height: 2.75rem; - } - body > header > nav a, body > header > nav #m-navbar-collapse li a { - margin-left: 0; - padding-left: 1rem; - padding-right: 1rem; - white-space: nowrap; - } - body > header > nav #m-navbar-collapse { - padding-bottom: 0; - } - body > header > nav #m-navbar-collapse li ol { - background-color: #ffffff; - } - body > header > nav #m-navbar-collapse ol ol li { - margin-left: 0; - padding-left: 0; - border-left-width: 0; - } - body > header > nav #m-navbar-collapse ol ol li a { - padding-left: 0.75rem; - } - body > header > nav #m-navbar-collapse > .m-row > ol > li { - margin-left: 0; - border-left-width: 0; - } - body > header > nav #m-navbar-collapse > .m-row > ol > li > a { - border-width: 0.25rem 0 0 0; - } - body > header > nav #m-navbar-collapse ol { - padding-left: 0; - padding-right: 0; - } - body > header > nav #m-navbar-collapse > .m-row > ol, body > header > nav #m-navbar-collapse > .m-row > ol > li { - float: left; - } - body > header > nav #m-navbar-collapse ol ol { - z-index: 99999; - position: absolute; - visibility: hidden; - } - body > header > nav #m-navbar-collapse li:hover ol { - visibility: visible; - } + +.m-code .nt { + color: #204a87; + font-weight: bold } -body > footer { - width: 100%; + +.m-code .nv { + color: #000000 } -body > footer > nav { - padding-top: 1rem; - padding-bottom: 1rem; - font-size: 0.85rem; - text-align: center; - color: #7c7c7c; - background-color: #eeeeee; + +.m-code .ow { + color: #204a87; + font-weight: bold } -body > footer > nav h3, body > footer > nav h3 a { - text-transform: uppercase; - font-weight: normal; + +.m-code .w { + color: #f8f8f8; + text-decoration: underline } -body > footer > nav ul { - list-style-type: none; - padding: 0; - margin: 0; + +.m-code .mb { + color: #0000cf; + font-weight: bold } -body > footer > nav a { - text-decoration: none; - text-transform: lowercase; - color: #191919; + +.m-code .mf { + color: #0000cf; + font-weight: bold } -body > footer > nav a:hover, body > footer > nav a:focus, body > footer > nav a:active { - color: #494949; + +.m-code .mh { + color: #0000cf; + font-weight: bold } -body > main { - padding-top: 1rem; - padding-bottom: 1rem; + +.m-code .mi { + color: #0000cf; + font-weight: bold } -article h1 { - font-size: 1.75rem; + +.m-code .mo { + color: #0000cf; + font-weight: bold } -article h1 .m-breadcrumb { - color: #bdbdbd; - font-weight: normal; + +.m-code .sa { + color: #4e9a06 } -article h1 .m-breadcrumb a { - color: #205275; + +.m-code .sb { + color: #4e9a06 } -article h1 .m-breadcrumb a:hover, article h1 a:focus, article h1 a:active { - color: #2e73a3; + +.m-code .sc { + color: #4e9a06 } -article > header h1 { - font-size: 2rem; - margin-bottom: 0.5rem; + +.m-code .dl { + color: #4e9a06 } -article h1 a, article > header h1, article > header h1 a, -article section > h2, article section > h2 a, -article section > h3, article section > h3 a, -article section > h4, article section > h4 a, -article section > h5, article section > h5 a, -article section > h6, article section > h6 a { - color: #205275; + +.m-code .sd { + color: #8f5902; + font-style: italic } -article h1 a:hover, article > header h1 a:hover, article > header h1 a:focus, article > header h1 a:active, -article section > h2 a:hover, article section > h2 a:focus, article section > h2 a:active, -article section > h3 a:hover, article section > h3 a:focus, article section > h3 a:active, -article section > h4 a:hover, article section > h4 a:focus, article section > h4 a:active, -article section > h5 a:hover, article section > h5 a:focus, article section > h5 a:active, -article section > h6 a:hover, article section > h6 a:focus, article section > h6 a:active { - color: #2e73a3; + +.m-code .s2 { + color: #4e9a06 } -article > header .m-date { - display: block; - width: 2.5rem; - float: left; - text-align: center; - line-height: 95%; - font-size: 0.75rem; - font-weight: normal; - white-space: nowrap; - border-right-style: solid; - border-right-width: 0.125rem; - border-color: #205275; - padding-right: 0.75rem; - margin-top: -0.1rem; - margin-right: 0.75rem; - margin-bottom: 0.25rem; + +.m-code .se { + color: #4e9a06 } -article > header .m-date-day { - display: block; - font-weight: bold; - padding-top: 0.2rem; - padding-bottom: 0.15rem; - font-size: 1.25rem; + +.m-code .sh { + color: #4e9a06 } -article > header p { - color: #7a7a7a; - font-size: 1.125rem; + +.m-code .si { + color: #4e9a06 } -article > header h1::after { - content: " "; - clear: both; - display: table; + +.m-code .sx { + color: #4e9a06 } -article > footer { - color: #969696; + +.m-code .sr { + color: #4e9a06 } -article > footer p { - font-style: italic; - font-size: 0.85rem; - text-indent: 0; + +.m-code .s1 { + color: #4e9a06 } -article section:target { - margin-left: -1.0rem; - border-left-style: solid; - border-left-width: 0.25rem; - padding-left: 0.75rem; - border-color: #205275; + +.m-code .ss { + color: #4e9a06 } -article h1 a, article > header h1 a, article section > h2 a, article section > h3 a, -article section > h4 a, article section > h5 a, article section > h6 a { - text-decoration: none; + +.m-code .bp { + color: #3465a4 } -#m-landing-image, #m-cover-image, article#m-jumbo > header #m-jumbo-image { - background-size: cover; - background-color: #666666; - background-position: center center; - background-repeat: no-repeat; - margin-top: -4rem; - padding-top: 5rem; + +.m-code .fm { + color: #000000 } -#m-landing-image { - color: #ffffff; + +.m-code .vc { + color: #000000 } -#m-cover-image { - height: 30rem; - margin-bottom: -26rem; + +.m-code .vg { + color: #000000 } -#m-landing-cover h1 { - font-size: 2.8rem; - margin-top: -0.5rem; - padding-left: 1.5rem; - padding-bottom: 1rem; - text-transform: lowercase; + +.m-code .vi { + color: #000000 } -#m-landing-cover { - padding-bottom: 10rem; - margin-bottom: -6rem; + +.m-code .vm { + color: #000000 } -article#m-jumbo { - margin-top: -1rem; + +.m-code .il { + color: #0000cf; + font-weight: bold } -#m-landing-cover, #m-cover-image > div, article#m-jumbo > header #m-jumbo-cover { - background: linear-gradient(transparent 0%, transparent 50%, #ffffff 100%); - width: 100%; - height: 100%; + +.m-console .hll { + background-color: #ffffcc } -article#m-jumbo > header h1, article#m-jumbo > header h2 { - text-align: center; - font-weight: bold; + +.m-console .g-AnsiBackgroundBlack { + background-color: #232627 } -article#m-jumbo > header a { - text-decoration: none; + +.m-console .g-AnsiBackgroundBlue { + background-color: #1d99f3 } -article#m-jumbo > header #m-jumbo-cover { - padding-bottom: 5rem; + +.m-console .g-AnsiBackgroundBrightBlack { + background-color: #7f8c8d } -article#m-jumbo > header #m-jumbo-image { - font-size: 2.5vmin; - margin-bottom: -3rem; + +.m-console .g-AnsiBackgroundBrightBlue { + background-color: #3daee9 } -article#m-jumbo > header h1 { - font-size: 10vmin; + +.m-console .g-AnsiBackgroundBrightCyan { + background-color: #16a085 } -article#m-jumbo > header h2 { - font-size: 3vmin; + +.m-console .g-AnsiBackgroundBrightGreen { + background-color: #1cdc9a } -@media screen and (max-height: 640px) , screen and (max-width: 640px) { - article#m-jumbo > header h1 { - font-size: 3rem; - } - article#m-jumbo > header #m-jumbo-image, article#m-jumbo > header h2 { - font-size: 1rem; - } + +.m-console .g-AnsiBackgroundBrightMagenta { + background-color: #8e44ad } -article#m-jumbo > header, article#m-jumbo > header h1, article#m-jumbo > header a { - color: #ffffff; + +.m-console .g-AnsiBackgroundBrightRed { + background-color: #c0392b } -article#m-jumbo > header a:hover, article#m-jumbo > header a:focus, article#m-jumbo > header a:active { - color: #f0f0f0; + +.m-console .g-AnsiBackgroundBrightWhite { + background-color: #ffffff } -article#m-jumbo.m-inverted > header, article#m-jumbo.m-inverted > header h1, article#m-jumbo.m-inverted > header a { - color: #000000; + +.m-console .g-AnsiBackgroundBrightYellow { + background-color: #fdbc4b } -article#m-jumbo.m-inverted > header a:hover, article#m-jumbo.m-inverted > header a:focus, article#m-jumbo.m-inverted > header a:active { - color: #0f0f0f; + +.m-console .g-AnsiBackgroundCyan { + background-color: #1abc9c } -.m-landing-news h3 a { - color: #000000; - text-decoration: none; - text-transform: uppercase; + +.m-console .g-AnsiBackgroundDefault { + background-color: #fcfcfc } -.m-landing-news h3 a:hover, .m-landing-news h3 a:hover, .m-landing-news h3 a:focus, .m-landing-news h3 a:active { - color: #205275; + +.m-console .g-AnsiBackgroundGreen { + background-color: #11d116 } -.m-landing-news time { - display: inline-block; - margin-left: 1rem; - float: right; + +.m-console .g-AnsiBackgroundMagenta { + background-color: #9b59b6 } -.m-article-pagination { - text-align: center; - padding: 1rem; + +.m-console .g-AnsiBackgroundRed { + background-color: #ed1515 } -nav.m-navpanel { - text-align: center; + +.m-console .g-AnsiBackgroundWhite { + background-color: #fcfcfc } -nav.m-navpanel h3 { - text-transform: uppercase; - font-weight: normal; + +.m-console .g-AnsiBackgroundYellow { + background-color: #f67400 } -nav.m-navpanel ol { - text-transform: lowercase; + +.m-console .g-AnsiBlack { + color: #232627 } -nav.m-navpanel ol, nav.m-navpanel ul { - list-style-type: none; - padding: 0; + +.m-console .g-AnsiBlue { + color: #1d99f3 } -nav.m-navpanel a { - color: #292929; - text-decoration: none; + +.m-console .g-AnsiBrightBlack { + color: #7f8c8d; + font-weight: bold } -nav.m-navpanel a:hover, nav.m-navpanel a:focus, nav.m-navpanel a:active { - color: #205275; -} -ul.m-tagcloud li { display: inline; } -ul.m-tagcloud li.m-tag-1 { font-size: 0.75rem; } -ul.m-tagcloud li.m-tag-2 { font-size: 0.825rem; } -ul.m-tagcloud li.m-tag-3 { font-size: 1rem; } -ul.m-tagcloud li.m-tag-4 { font-size: 1.25rem; } -ul.m-tagcloud li.m-tag-5 { font-size: 1.5rem; } -article section:target figure.m-code-figure, article section:target figure.m-console-figure { - z-index: 1; + +.m-console .g-AnsiBrightBlue { + color: #3daee9; + font-weight: bold } -article, article > header, article section { margin-bottom: 1rem; } -article:last-child, article section:last-child { margin-bottom: 0; } -.m-container-inflatable section:target > .m-note, -.m-container-inflatable section:target > .m-frame, -.m-container-inflatable section:target > .m-block, -.m-container-inflatable section:target > pre, -.m-container-inflatable section:target > .m-code-figure > pre:first-child, -.m-container-inflatable section:target > .m-console-figure > pre:first-child, -.m-container-inflatable section:target section > .m-note, -.m-container-inflatable section:target section > .m-frame, -.m-container-inflatable section:target section > .m-block, -.m-container-inflatable section:target section > pre, -.m-container-inflatable section:target section > .m-code-figure > pre:first-child, -.m-container-inflatable section:target section > .m-console-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-center-'] > .m-note, -.m-container-inflatable section:target [class*='m-center-'] > .m-frame, -.m-container-inflatable section:target [class*='m-center-'] > .m-block, -.m-container-inflatable section:target [class*='m-center-'] > pre, -.m-container-inflatable section:target [class*='m-center-'] > .m-code-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-center-'] > .m-console-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-left-'] > .m-note, -.m-container-inflatable section:target [class*='m-left-'] > .m-frame, -.m-container-inflatable section:target [class*='m-left-'] > .m-block, -.m-container-inflatable section:target [class*='m-left-'] > pre, -.m-container-inflatable section:target [class*='m-left-'] > .m-code-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-left-'] > .m-console-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-right-'] > .m-note, -.m-container-inflatable section:target [class*='m-right-'] > .m-frame, -.m-container-inflatable section:target [class*='m-right-'] > .m-block, -.m-container-inflatable section:target [class*='m-right-'] > pre, -.m-container-inflatable section:target [class*='m-right-'] > .m-code-figure > pre:first-child, -.m-container-inflatable section:target [class*='m-right-'] > .m-console-figure > pre:first-child, -.m-container-inflatable section:target .m-container-inflate > .m-note, -.m-container-inflatable section:target .m-container-inflate > .m-frame, -.m-container-inflatable section:target .m-container-inflate > .m-block, -.m-container-inflatable section:target .m-container-inflate > pre, -.m-container-inflatable section:target .m-container-inflate > .m-code-figure > pre:first-child, -.m-container-inflatable section:target .m-container-inflate > .m-console-figure > pre:first-child { - margin-left: -1.0rem; - border-left-style: solid; - border-left-width: 0.25rem; - border-top-left-radius: 0; - border-bottom-left-radius: 0; - padding-left: 0.75rem; + +.m-console .g-AnsiBrightCyan { + color: #16a085; + font-weight: bold } -.m-container-inflatable section:target > .m-code-figure::before, -.m-container-inflatable section:target > .m-console-figure::before, -.m-container-inflatable section:target section > .m-code-figure::before, -.m-container-inflatable section:target section > .m-console-figure::before, -.m-container-inflatable section:target [class*='m-center-'] > .m-code-figure::before, -.m-container-inflatable section:target [class*='m-center-'] > .m-console-figure::before, -.m-container-inflatable section:target [class*='m-left-'] > .m-code-figure::before, -.m-container-inflatable section:target [class*='m-left-'] > .m-console-figure::before, -.m-container-inflatable section:target [class*='m-right-'] > .m-code-figure::before, -.m-container-inflatable section:target [class*='m-right-'] > .m-console-figure::before, -.m-container-inflatable section:target .m-container-inflate > .m-code-figure::before, -.m-container-inflatable section:target .m-container-inflate > .m-console-figure::before { - border-top-left-radius: 0; - border-bottom-left-radius: 0; - border-left-width: 0.25rem; + +.m-console .g-AnsiBrightDefault { + color: #ffffff; + font-weight: bold } -.m-container-inflatable section:target > .m-code-figure > pre.m-nopad, -.m-container-inflatable section:target > .m-console-figure > pre.m-nopad { - margin-left: -0.75rem; - padding-left: -0.75rem; + +.m-console .g-AnsiBrightGreen { + color: #1cdc9a; + font-weight: bold } -@media screen and (min-width: 576px) { - .m-container-inflatable section:target .m-center-s > .m-note, - .m-container-inflatable section:target .m-center-s > pre, - .m-container-inflatable section:target .m-center-s > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-center-s > figure.m-console-figure > pre:first-child, - .m-container-inflatable section:target .m-right-s > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-right-s > figure.m-console-figure > pre:first-child { - border-left-width: 0; - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-s > .m-block, - .m-container-inflatable section:target .m-right-s > .m-block { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - } - .m-container-inflatable section:target .m-center-s > .m-frame, - .m-container-inflatable section:target .m-right-s > .m-frame { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - padding-left: 0.875rem; - } - .m-container-inflatable section:target .m-right-s > .m-block, - .m-container-inflatable section:target .m-right-s > .m-frame { - margin-left: 0; - } - .m-container-inflatable section:target .m-right-s > .m-note, - .m-container-inflatable section:target .m-right-s > pre { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - margin-left: 0; - border-left-width: 0; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-s > figure.m-code-figure::before, - .m-container-inflatable section:target .m-center-s > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-s > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-s > figure.m-console-figure::before { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - } + +.m-console .g-AnsiBrightMagenta { + color: #8e44ad; + font-weight: bold } -@media screen and (min-width: 768px) { - .m-container-inflatable section:target .m-center-m > .m-note, - .m-container-inflatable section:target .m-center-m > pre, - .m-container-inflatable section:target .m-center-m > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-center-m > figure.m-console-figure > pre:first-child, - .m-container-inflatable section:target .m-right-m > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-right-m > figure.m-console-figure > pre:first-child { - border-left-width: 0; - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-m > .m-block, - .m-container-inflatable section:target .m-right-m > .m-block { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - } - .m-container-inflatable section:target .m-center-m > .m-frame, - .m-container-inflatable section:target .m-right-m > .m-frame { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - padding-left: 0.875rem; - } - .m-container-inflatable section:target .m-right-m > .m-block, - .m-container-inflatable section:target .m-right-m > .m-frame { - margin-left: 0; - } - .m-container-inflatable section:target .m-right-m > .m-note, - .m-container-inflatable section:target .m-right-m > pre { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - margin-left: 0; - border-left-width: 0; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-m > figure.m-code-figure::before, - .m-container-inflatable section:target .m-center-m > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-m > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-m > figure.m-console-figure::before { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - } + +.m-console .g-AnsiBrightRed { + color: #c0392b; + font-weight: bold } -@media screen and (min-width: 992px) { - .m-container-inflatable section:target .m-center-l > .m-note, - .m-container-inflatable section:target .m-center-l > pre, - .m-container-inflatable section:target .m-center-l > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-center-l > figure.m-console-figure > pre:first-child, - .m-container-inflatable section:target .m-right-l > figure.m-code-figure > pre:first-child, - .m-container-inflatable section:target .m-right-l > figure.m-console-figure > pre:first-child { - border-left-width: 0; - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-l > .m-block, - .m-container-inflatable section:target .m-right-l > .m-block { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - } - .m-container-inflatable section:target .m-center-l > .m-frame, - .m-container-inflatable section:target .m-right-l > .m-frame { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - padding-left: 0.875rem; - } - .m-container-inflatable section:target .m-right-l > .m-block, - .m-container-inflatable section:target .m-right-l > .m-frame { - margin-left: 0; - } - .m-container-inflatable section:target .m-right-l > .m-note, - .m-container-inflatable section:target .m-right-l > pre { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - margin-left: 0; - border-left-width: 0; - padding-left: 1rem; - } - .m-container-inflatable section:target .m-center-l > figure.m-code-figure::before, - .m-container-inflatable section:target .m-center-l > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-l > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-l > figure.m-console-figure::before { - border-top-left-radius: 0.2rem; - border-bottom-left-radius: 0.2rem; - border-left-width: 0.125rem; - } + +.m-console .g-AnsiBrightWhite { + color: #ffffff; + font-weight: bold } -.m-container-inflatable section:target > figure.m-code-figure::before, -.m-container-inflatable section:target > figure.m-console-figure::before, -.m-container-inflatable section:target section > figure.m-code-figure::before, -.m-container-inflatable section:target section > figure.m-console-figure::before, -.m-container-inflatable section:target [class*='m-center-'] > figure.m-code-figure::before, -.m-container-inflatable section:target [class*='m-center-'] > figure.m-console-figure::before, -.m-container-inflatable section:target [class*='m-left-'] > figure.m-code-figure::before, -.m-container-inflatable section:target [class*='m-left-'] > figure.m-console-figure::before, -.m-container-inflatable section:target [class*='m-right-'] > figure.m-code-figure::before, -.m-container-inflatable section:target [class*='m-right-'] > figure.m-console-figure::before, -.m-container-inflatable section:target .m-container-inflatable > figure.m-code-figure::before, -.m-container-inflatable section:target .m-container-inflatable > figure.m-console-figure::before { - border-left-color: #c7cfd9; + +.m-console .g-AnsiBrightYellow { + color: #fdbc4b; + font-weight: bold } -@media screen and (min-width: 576px) { - .m-container-inflatable section:target .m-center-s > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-s > figure.m-code-figure::before { - border-color: #f2f7fa; - } - .m-container-inflatable section:target .m-center-s > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-s > figure.m-console-figure::before { - border-color: #000000; - } + +.m-console .g-AnsiCyan { + color: #1abc9c } -@media screen and (min-width: 768px) { - .m-container-inflatable section:target .m-center-m > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-m > figure.m-code-figure::before { - border-color: #f2f7fa; - } - .m-container-inflatable section:target .m-center-m > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-m > figure.m-console-figure::before { - border-color: #000000; - } + +.m-console .g-AnsiDefault { + color: #fcfcfc } -@media screen and (min-width: 992px) { - .m-container-inflatable section:target .m-center-l > figure.m-code-figure::before, - .m-container-inflatable section:target .m-right-l > figure.m-code-figure::before { - border-color: #f2f7fa; - } - .m-container-inflatable section:target .m-center-l > figure.m-console-figure::before, - .m-container-inflatable section:target .m-right-l > figure.m-console-figure::before { - border-color: #000000; - } + +.m-console .g-AnsiGreen { + color: #11d116 } -.m-container-inflatable section:target pre, -.m-container-inflatable section:target figure.m-code-figure > pre:first-child, -.m-container-inflatable section:target figure.m-console-figure > pre:first-child { - border-color: #c7cfd9; + +.m-console .g-AnsiMagenta { + color: #9b59b6 } -.m-container-inflatable section:target .m-note.m-default { - border-color: #c7cfd9; + +.m-console .g-AnsiRed { + color: #ed1515 } -.m-container-inflatable section:target .m-note.m-primary { - border-color: #205275; + +.m-console .g-AnsiWhite { + color: #fcfcfc } -.m-container-inflatable section:target .m-note.m-success { - border-color: #31c25d; + +.m-console .g-AnsiYellow { + color: #f67400 } -.m-container-inflatable section:target .m-note.m-warning { - border-color: #d19600; + +.m-console .go { + color: #fcfcfc } -.m-container-inflatable section:target .m-note.m-danger { - border-color: #f60000; + +.m-console .gp { + color: #16a085; + font-weight: bold } -.m-container-inflatable section:target .m-note.m-info { - border-color: #2e7dc5; + +.m-console .w { + color: #fcfcfc } -.m-container-inflatable section:target .m-note.m-dim { - border-color: #bdbdbd; -} - -.m-code .hll { background-color: #ffffcc } -.m-code { background: #f8f8f8; } -.m-code .c { color: #8f5902; font-style: italic } -.m-code .err { color: #a40000; border: 1px solid #ef2929 } -.m-code .g { color: #000000 } -.m-code .k { color: #204a87; font-weight: bold } -.m-code .l { color: #000000 } -.m-code .n { color: #000000 } -.m-code .o { color: #ce5c00; font-weight: bold } -.m-code .x { color: #000000 } -.m-code .p { color: #000000; font-weight: bold } -.m-code .ch { color: #8f5902; font-style: italic } -.m-code .cm { color: #8f5902; font-style: italic } -.m-code .cp { color: #8f5902; font-style: italic } -.m-code .cpf { color: #8f5902; font-style: italic } -.m-code .c1 { color: #8f5902; font-style: italic } -.m-code .cs { color: #8f5902; font-style: italic } -.m-code .gd { color: #a40000 } -.m-code .ge { color: #000000; font-style: italic } -.m-code .gr { color: #ef2929 } -.m-code .gh { color: #000080; font-weight: bold } -.m-code .gi { color: #00A000 } -.m-code .go { color: #000000; font-style: italic } -.m-code .gp { color: #8f5902 } -.m-code .gs { color: #000000; font-weight: bold } -.m-code .gu { color: #800080; font-weight: bold } -.m-code .gt { color: #a40000; font-weight: bold } -.m-code .kc { color: #204a87; font-weight: bold } -.m-code .kd { color: #204a87; font-weight: bold } -.m-code .kn { color: #204a87; font-weight: bold } -.m-code .kp { color: #204a87; font-weight: bold } -.m-code .kr { color: #204a87; font-weight: bold } -.m-code .kt { color: #204a87; font-weight: bold } -.m-code .ld { color: #000000 } -.m-code .m { color: #0000cf; font-weight: bold } -.m-code .s { color: #4e9a06 } -.m-code .na { color: #c4a000 } -.m-code .nb { color: #204a87 } -.m-code .nc { color: #000000 } -.m-code .no { color: #000000 } -.m-code .nd { color: #5c35cc; font-weight: bold } -.m-code .ni { color: #ce5c00 } -.m-code .ne { color: #cc0000; font-weight: bold } -.m-code .nf { color: #000000 } -.m-code .nl { color: #f57900 } -.m-code .nn { color: #000000 } -.m-code .nx { color: #000000 } -.m-code .py { color: #000000 } -.m-code .nt { color: #204a87; font-weight: bold } -.m-code .nv { color: #000000 } -.m-code .ow { color: #204a87; font-weight: bold } -.m-code .w { color: #f8f8f8; text-decoration: underline } -.m-code .mb { color: #0000cf; font-weight: bold } -.m-code .mf { color: #0000cf; font-weight: bold } -.m-code .mh { color: #0000cf; font-weight: bold } -.m-code .mi { color: #0000cf; font-weight: bold } -.m-code .mo { color: #0000cf; font-weight: bold } -.m-code .sa { color: #4e9a06 } -.m-code .sb { color: #4e9a06 } -.m-code .sc { color: #4e9a06 } -.m-code .dl { color: #4e9a06 } -.m-code .sd { color: #8f5902; font-style: italic } -.m-code .s2 { color: #4e9a06 } -.m-code .se { color: #4e9a06 } -.m-code .sh { color: #4e9a06 } -.m-code .si { color: #4e9a06 } -.m-code .sx { color: #4e9a06 } -.m-code .sr { color: #4e9a06 } -.m-code .s1 { color: #4e9a06 } -.m-code .ss { color: #4e9a06 } -.m-code .bp { color: #3465a4 } -.m-code .fm { color: #000000 } -.m-code .vc { color: #000000 } -.m-code .vg { color: #000000 } -.m-code .vi { color: #000000 } -.m-code .vm { color: #000000 } -.m-code .il { color: #0000cf; font-weight: bold } - -.m-console .hll { background-color: #ffffcc } -.m-console .g-AnsiBackgroundBlack { background-color: #232627 } -.m-console .g-AnsiBackgroundBlue { background-color: #1d99f3 } -.m-console .g-AnsiBackgroundBrightBlack { background-color: #7f8c8d } -.m-console .g-AnsiBackgroundBrightBlue { background-color: #3daee9 } -.m-console .g-AnsiBackgroundBrightCyan { background-color: #16a085 } -.m-console .g-AnsiBackgroundBrightGreen { background-color: #1cdc9a } -.m-console .g-AnsiBackgroundBrightMagenta { background-color: #8e44ad } -.m-console .g-AnsiBackgroundBrightRed { background-color: #c0392b } -.m-console .g-AnsiBackgroundBrightWhite { background-color: #ffffff } -.m-console .g-AnsiBackgroundBrightYellow { background-color: #fdbc4b } -.m-console .g-AnsiBackgroundCyan { background-color: #1abc9c } -.m-console .g-AnsiBackgroundDefault { background-color: #fcfcfc } -.m-console .g-AnsiBackgroundGreen { background-color: #11d116 } -.m-console .g-AnsiBackgroundMagenta { background-color: #9b59b6 } -.m-console .g-AnsiBackgroundRed { background-color: #ed1515 } -.m-console .g-AnsiBackgroundWhite { background-color: #fcfcfc } -.m-console .g-AnsiBackgroundYellow { background-color: #f67400 } -.m-console .g-AnsiBlack { color: #232627 } -.m-console .g-AnsiBlue { color: #1d99f3 } -.m-console .g-AnsiBrightBlack { color: #7f8c8d; font-weight: bold } -.m-console .g-AnsiBrightBlue { color: #3daee9; font-weight: bold } -.m-console .g-AnsiBrightCyan { color: #16a085; font-weight: bold } -.m-console .g-AnsiBrightDefault { color: #ffffff; font-weight: bold } -.m-console .g-AnsiBrightGreen { color: #1cdc9a; font-weight: bold } -.m-console .g-AnsiBrightMagenta { color: #8e44ad; font-weight: bold } -.m-console .g-AnsiBrightRed { color: #c0392b; font-weight: bold } -.m-console .g-AnsiBrightWhite { color: #ffffff; font-weight: bold } -.m-console .g-AnsiBrightYellow { color: #fdbc4b; font-weight: bold } -.m-console .g-AnsiCyan { color: #1abc9c } -.m-console .g-AnsiDefault { color: #fcfcfc } -.m-console .g-AnsiGreen { color: #11d116 } -.m-console .g-AnsiMagenta { color: #9b59b6 } -.m-console .g-AnsiRed { color: #ed1515 } -.m-console .g-AnsiWhite { color: #fcfcfc } -.m-console .g-AnsiYellow { color: #f67400 } -.m-console .go { color: #fcfcfc } -.m-console .gp { color: #16a085; font-weight: bold } -.m-console .w { color: #fcfcfc } a.m-doc, a.m-doc-self, a.m-doc-external, ul.m-doc li.m-doc-expansible > a:first-child, ul.m-doc li.m-doc-collapsible > a:first-child, .m-code.m-inverted.m-doc-include > a { - text-decoration: none; + text-decoration: none; } + a.m-doc, a.m-doc-self { - font-weight: bold; + font-weight: bold; } + .m-thin a.m-doc, .m-thin a.m-doc-self { - font-weight: normal; + font-weight: normal; } + ul.m-doc li.m-doc-expansible > a:first-child, ul.m-doc li.m-doc-collapsible > a:first-child, ul.m-doc li.m-doc-expansible > a:first-child:hover, @@ -2617,13 +4115,15 @@ ul.m-doc li.m-doc-expansible > a:first-child:active, ul.m-doc li.m-doc-collapsible > a:first-child:hover, ul.m-doc li.m-doc-collapsible > a:first-child:focus, ul.m-doc li.m-doc-collapsible > a:first-child:active { - color: #000000; + color: #000000; } + a.m-doc-self, ul.m-doc li.m-doc-expansible > a:first-child:before, ul.m-doc li.m-doc-collapsible > a:first-child:before { - color: #205275; + color: #205275; } + a.m-doc-self:hover, a.m-doc-self:focus, a.m-doc-self:active, ul.m-doc li.m-doc-expansible > a:first-child:hover::before, ul.m-doc li.m-doc-expansible > a:first-child:focus::before, @@ -2631,258 +4131,324 @@ ul.m-doc li.m-doc-expansible > a:first-child:active::before, ul.m-doc li.m-doc-collapsible > a:first-child:hover::before, ul.m-doc li.m-doc-collapsible > a:first-child:focus::before, ul.m-doc li.m-doc-collapsible > a:first-child:active::before { - color: #2e73a3; + color: #2e73a3; } + h3 a.m-doc-external { - font-weight: normal; + font-weight: normal; } + span.m-doc-wrap-bumper { - margin-right: -1rem; + margin-right: -1rem; } + span.m-doc-wrap { - padding-left: 1rem; - display: inline-block; - vertical-align: text-top; - white-space: pre-line; - max-width: 100%; + padding-left: 1rem; + display: inline-block; + vertical-align: text-top; + white-space: pre-line; + max-width: 100%; } + dl.m-doc dd { - margin-bottom: 0.5rem; + margin-bottom: 0.5rem; } + dl.m-doc dd { - margin-left: 0; - padding-left: 2.5rem; + margin-left: 0; + padding-left: 2.5rem; } + dl.m-doc dt:target, dl.m-doc dt:target + dd { - margin-left: -1.0rem; - border-left-style: solid; - border-left-width: 0.25rem; - border-color: #205275; + margin-left: -1.0rem; + border-left-style: solid; + border-left-width: 0.25rem; + border-color: #205275; +} + +dl.m-doc dt:target { + padding-left: 0.75rem; } -dl.m-doc dt:target { padding-left: 0.75rem; } -dl.m-doc dt:target + dd { padding-left: 3.25rem; } + +dl.m-doc dt:target + dd { + padding-left: 3.25rem; +} + ul.m-doc { - list-style: none; - margin-left: 1.0375rem; - padding-left: 0.9rem; - border-left-color: #c7cfd9; - border-left-width: 0.0625rem; - border-left-style: solid; + list-style: none; + margin-left: 1.0375rem; + padding-left: 0.9rem; + border-left-color: #c7cfd9; + border-left-width: 0.0625rem; + border-left-style: solid; } + ul.m-doc li { - text-indent: -1rem; - padding-left: 1rem; + text-indent: -1rem; + padding-left: 1rem; } + ul.m-doc li.m-doc-expansible > ul { - display: none; + display: none; } + ul.m-doc li.m-doc-expansible, ul.m-doc li.m-doc-collapsible { - padding-left: 0.6rem; + padding-left: 0.6rem; } + ul.m-doc li.m-doc-expansible > ul.m-doc, ul.m-doc li.m-doc-collapsible > ul.m-doc { - margin-left: 0.5rem; + margin-left: 0.5rem; } + ul.m-doc li.m-doc-expansible > a:first-child:before, ul.m-doc li.m-doc-collapsible > a:first-child:before { - background-color: #ffffff; - display: inline-block; - width: 0.4rem; - font-weight: bold; + background-color: #ffffff; + display: inline-block; + width: 0.4rem; + font-weight: bold; +} + +ul.m-doc li.m-doc-expansible > a:first-child:before { + content: '⊕'; +} + +ul.m-doc li.m-doc-collapsible > a:first-child:before { + content: '⊖'; } -ul.m-doc li.m-doc-expansible > a:first-child:before { content: '⊕'; } -ul.m-doc li.m-doc-collapsible > a:first-child:before { content: '⊖'; } + h1 .m-doc-template, h1 .m-doc-include { - font-size: 1.3rem; - font-weight: normal; + font-size: 1.3rem; + font-weight: normal; } + h1 .m-doc-include:last-child { - margin-bottom: -0.5rem; + margin-bottom: -0.5rem; } + h3 .m-doc-template, h3 .m-doc-include { - font-size: 1rem; - font-weight: normal; + font-size: 1rem; + font-weight: normal; } + .m-doc-template, dl.m-doc dd, ul.m-doc li > span.m-doc { - color: #bdbdbd; + color: #bdbdbd; } + dl.m-doc dd svg.m-math, ul.m-doc li > span.m-doc svg.m-math { - fill: #bdbdbd; + fill: #bdbdbd; } + .m-doc-template a, dl.m-doc dd a, ul.m-doc li > span.m-doc a { - color: #c0c0c0; + color: #c0c0c0; } + .m-doc-template a:hover, .m-doc-template a:focus, .m-doc-template a:active, dl.m-doc dd a:hover, dl.m-doc dd a:focus, dl.m-doc dd a:active, ul.m-doc li > span.m-doc a:hover, ul.m-doc li > span.m-doc a:focus, ul.m-doc li > span.m-doc a:active { - color: #949494; + color: #949494; } + .m-code.m-inverted.m-doc-include > a:link, .m-code.m-inverted.m-doc-include > a:visited { - opacity: 0.6666; + opacity: 0.6666; } + .m-code.m-inverted.m-doc-include > a:hover, .m-code.m-inverted.m-doc-include > a:focus, .m-code.m-inverted.m-doc-include > a:active { - opacity: 1; + opacity: 1; } + article section.m-doc-details > div { - margin-top: 0; - margin-left: 0; - margin-right: 0; - position: relative; - padding: 1rem; + margin-top: 0; + margin-left: 0; + margin-right: 0; + position: relative; + padding: 1rem; } + article section.m-doc-details > div::before { - position: absolute; - content: ' '; - top: 0; - bottom: 0; - left: 0; - right: 0; - z-index: -1; - border-style: solid; - border-width: 0.125rem; - border-radius: 0.2rem; - border-color: #f2f7fa; + position: absolute; + content: ' '; + top: 0; + bottom: 0; + left: 0; + right: 0; + z-index: -1; + border-style: solid; + border-width: 0.125rem; + border-radius: 0.2rem; + border-color: #f2f7fa; } + article section.m-doc-details > div > h3:first-child { - position: relative; - margin: -1rem -1rem 1rem -1rem; - padding: 0.5rem 1rem; - background-color: #f2f7fa; - border-top-left-radius: 0.2rem; - border-top-right-radius: 0.2rem; - border-bottom-left-radius: 0; - border-bottom-right-radius: 0; + position: relative; + margin: -1rem -1rem 1rem -1rem; + padding: 0.5rem 1rem; + background-color: #f2f7fa; + border-top-left-radius: 0.2rem; + border-top-right-radius: 0.2rem; + border-bottom-left-radius: 0; + border-bottom-right-radius: 0; } + article section.m-doc-details:target { - border-color: transparent; + border-color: transparent; } + article section.m-doc-details:target > div { - z-index: 1; + z-index: 1; } + .m-container-inflatable > .m-row > [class*='m-col-'] section.m-doc-details > div { - margin-left: -1rem; - margin-right: -1rem; + margin-left: -1rem; + margin-right: -1rem; } + .m-container-inflatable section.m-doc-details:target > div > h3:first-child, .m-container-inflatable section.m-doc-details:target section > div > h3:first-child { - margin-left: -1.0rem; - border-left-style: solid; - border-left-color: #2e73a3; - border-left-width: 0.25rem; - padding-left: 0.75rem; + margin-left: -1.0rem; + border-left-style: solid; + border-left-color: #2e73a3; + border-left-width: 0.25rem; + padding-left: 0.75rem; } + .m-container-inflatable section.m-doc-details:target > div::before, .m-container-inflatable section-dox-details:target section > div.m::before { - border-left-width: 0.25rem; - border-left-color: #205275; + border-left-width: 0.25rem; + border-left-color: #205275; } + a.m-doc-search-icon { - padding-left: 1rem; - padding-right: 1rem; + padding-left: 1rem; + padding-right: 1rem; } + a.m-doc-search-icon svg { - fill: #000000; + fill: #000000; } + body > header > nav #m-navbar-collapse a.m-doc-search-icon svg { - vertical-align: -5%; + vertical-align: -5%; } + a.m-doc-search-icon:focus svg, a.m-doc-search-icon:hover svg, a.m-doc-search-icon:active svg { - fill: #205275; + fill: #205275; } + .m-doc-search { - display: none; - z-index: 10; - position: fixed; - left: 0; - right: 0; - top: 0; - bottom: 0; - background-color: rgba(255, 255, 255, 0.75); + display: none; + z-index: 10; + position: fixed; + left: 0; + right: 0; + top: 0; + bottom: 0; + background-color: rgba(255, 255, 255, 0.75); } + .m-doc-search:target { - display: block; + display: block; } + .m-doc-search > a { - display: block; - position: absolute; - left: 0; - right: 0; - top: 0; - bottom: 0; + display: block; + position: absolute; + left: 0; + right: 0; + top: 0; + bottom: 0; } + .m-doc-search-header { - margin-top: 2.5rem; - padding: 0.5rem 1rem; - height: 2rem; + margin-top: 2.5rem; + padding: 0.5rem 1rem; + height: 2rem; } + .m-doc-search-header > div:first-child { - float: right; + float: right; } + .m-doc-search-content { - background-color: #ffffff; - border-radius: 0.2rem; - padding: 1rem; + background-color: #ffffff; + border-radius: 0.2rem; + padding: 1rem; } + .m-doc-search input { - width: 100%; - height: 3rem; - font-size: 1.2rem; - border-width: 0; - color: #000000; - background-color: #f2f7fa; - border-radius: 0.2rem; - margin-bottom: 1rem; - padding: 0 1rem; + width: 100%; + height: 3rem; + font-size: 1.2rem; + border-width: 0; + color: #000000; + background-color: #f2f7fa; + border-radius: 0.2rem; + margin-bottom: 1rem; + padding: 0 1rem; } + .m-doc-search #search-notfound { - display: none; + display: none; } + .m-doc-search ul#search-results { - list-style-type: none; - padding-left: 0; - max-height: calc(100vh - 12.5rem); - overflow-y: auto; - display: none; + list-style-type: none; + padding-left: 0; + max-height: calc(100vh - 12.5rem); + overflow-y: auto; + display: none; } + .m-doc-search ul#search-results li a { - display: block; - padding-left: 1rem; - padding-right: 1rem; - text-decoration: none; - width: 100%; - line-height: 1.5rem; - color: #000000; + display: block; + padding-left: 1rem; + padding-right: 1rem; + text-decoration: none; + width: 100%; + line-height: 1.5rem; + color: #000000; } + .m-doc-search ul#search-results li a > div { - white-space: nowrap; - overflow: hidden; + white-space: nowrap; + overflow: hidden; } + .m-doc-search ul#search-results li a > div:not(.m-doc-search-alias) { - direction: rtl; + direction: rtl; } + .m-doc-search ul#search-results li a .m-label { - float: right; - line-height: 1rem; - margin-top: 0.1rem; - margin-left: 0.25rem; + float: right; + line-height: 1rem; + margin-top: 0.1rem; + margin-left: 0.25rem; } + .m-doc-search ul#search-results li a .m-label.m-flat { - margin-right: -0.75rem; + margin-right: -0.75rem; } + .m-doc-search ul#search-results li#search-current a { - background-color: #f2f7fa; + background-color: #f2f7fa; } + .m-doc-search ul#search-results li#search-current.m-doc-search-copied a { - background-color: #4dd376; + background-color: #4dd376; } + .m-doc-search-typed { - color: #205275; + color: #205275; } -.m-doc-search input[type="search"] { -webkit-appearance: textfield; } + +.m-doc-search input[type="search"] { + -webkit-appearance: textfield; +} + .m-doc-search input[type="search"]::-webkit-search-decoration, .m-doc-search input[type="search"]::-webkit-search-cancel-button, .m-doc-search input[type="search"]::-webkit-search-results-button, .m-doc-search input[type="search"]::-webkit-search-results-decoration { - display: none; + display: none; } diff --git a/docs/dev-coding-style.dox b/docs/dev-coding-style.dox index 994a8a36f..f988e0ea2 100644 --- a/docs/dev-coding-style.dox +++ b/docs/dev-coding-style.dox @@ -39,9 +39,27 @@ Eigen::Vector3d p_IinC; //=> 3d position of the IMU frame in the camera frame Eigen::Vector3d p_FinG; //=> position of feature F in the global frame G @endcode +@section coding-style-printing Print Statements +The code uses a simple print statement level logic that allows the user to enable and disable the verbosity. +In general the user can specify the following (see [ov_core/src/utils/print.h](ov_core/src/utils/print.h) file for details): +- PrintLevel::ALL : All PRINT_XXXX will output to the console +- PrintLevel::DEBUG : "DEBUG", "INFO", "WARNING" and "ERROR" will be printed. "ALL" will be silenced +- PrintLevel::INFO : "INFO", "WARNING" and "ERROR" will be printed. "ALL" and "DEBUG" will be silenced +- PrintLevel::WARNING : "WARNING" and "ERROR" will be printed. "ALL", "DEBUG" and "INFO" will be silenced +- PrintLevel::ERROR : Only "ERROR" will be printed. All the rest are silenced +- PrintLevel::SILENT : All PRINT_XXXX will be silenced. +To use these, you can specify the following using the [printf](https://www.cplusplus.com/reference/cstdio/printf/) standard input logic. + +@code{.cpp} +PRINT_ALL("the value is %.2f", variable); +PRINT_DEBUG("the value is %.2f", variable); +PRINT_INFO("the value is %.2f", variable); +PRINT_WARNING("the value is %.2f", variable); +PRINT_ERROR("the value is %.2f", variable); +@endcode */ \ No newline at end of file diff --git a/docs/img/splineframes.svg b/docs/img/splineframes.svg index 807c852e7..42d6adc6c 100644 --- a/docs/img/splineframes.svg +++ b/docs/img/splineframes.svg @@ -1,142 +1,214 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/mcss_theme/m-theme-udel.css b/docs/mcss_theme/m-theme-udel.css index 9ad101727..c04d50dfa 100644 --- a/docs/mcss_theme/m-theme-udel.css +++ b/docs/mcss_theme/m-theme-udel.css @@ -23,138 +23,138 @@ */ :root { - /* Text properties */ - /*--font: 'Libre Baskerville', serif;*/ - /*--code-font: 'Source Code Pro', monospace;*/ - --font: 'Source Sans Pro', sans-serif; - --code-font: 'Source Code Pro', monospace; - --font-size: 1em; - --code-font-size: 0.8em; /* *not* rem, so it follows surrounding font size */ - --line-height: normal; - --paragraph-indent: 1.5rem; - --paragraph-align: justify; - --link-decoration: underline; - --link-decoration-nav: none; - --link-decoration-heading: none; - --nav-brand-case: lowercase; - --nav-menu-case: lowercase; - --nav-heading-case: uppercase; - --nav-categories-case: lowercase; - --landing-header-case: lowercase; - --heading-font-weight: normal; - - /* Shapes */ - --border-radius: 0.2rem; - - /* Basics */ - --background-color: #ffffff; - --color: #000000; - --line-color: #c7cfd9; - --link-color: #205275; - --link-active-color: #2f73a3; - --mark-color: #4c93d3; - --mark-background-color: #f0c63e; - --code-color: #000000; - --code-inverted-color: rgba(91, 91, 91, 0.33); - --console-color: var(--code-color); - --console-inverted-color: var(--code-inverted-color); - /* This is simply color-picked --code-note-background-color on top of - --background-color */ - --code-background-color: #f2f7fa; - --code-note-background-color: rgba(251, 240, 236, 0.5); - --console-background-color: #000000; - --button-background-color: #ffffff; - - /* Header */ - --header-border-width: 0.25rem 0 0 0; - --header-color: #000000; - --header-breadcrumb-color: #bdbdbd; /* same as --dim-color */ - --header-background-color: #ffffff; - --header-background-color-landing: rgba(255, 255, 255, 0.75); - --header-background-color-jumbo: rgba(255, 255, 255, 0.25); - --header-link-color: #000000; - --header-link-active-color: #205275; - --header-link-current-color: #2f73a3; - --header-link-active-background-color: #ffffff; - --header-link-active-background-color: rgba(255, 255, 255, 0.5); - - /* Footer */ - --footer-font-size: 0.85rem; - --footer-color: #7c7c7c; - --footer-background-color: #eeeeee; - --footer-link-color: #191919; - --footer-link-active-color: #494949; - - /* Cover image */ - --cover-image-background-color: #666666; - - /* Search (used only by m-documentation.css) */ - --search-overlay-color: var(--header-background-color-landing); - --search-background-color: var(--header-background-color); - - /* Article */ - --article-header-color: #7a7a7a; - --article-footer-color: #969696; - --article-heading-color: #205275; - --article-heading-active-color: #2e73a3; - - /* Right navigation panel */ - --navpanel-link-color: #292929; - --navpanel-link-active-color: #205275; - - /* Plots */ - --plot-background-color: #f2f7fa; - --plot-error-color: #000000; - - /* Colored components */ - --default-color: #000000; - --default-link-active-color: #205275; - --default-filled-color: #000000; - --default-filled-background-color: #f2f7fa; - --default-filled-link-color: #2f73a3; - --default-filled-link-active-color: #205275; - - --primary-color: #205275; - --primary-link-active-color: #000000; - --primary-filled-color: #fbe4d9; - --primary-filled-background-color: #ef9069; - --primary-filled-link-color: #782f0d; - --primary-filled-link-active-color: #2f1205; - - --success-color: #31c25d; - --success-link-active-color: #dcf6e3; - --success-filled-color: #f4fcf6; - --success-filled-background-color: #4dd376; - --success-filled-link-color: #c5f2d1; - --success-filled-link-active-color: #dcf6e3; - - --warning-color: #d19600; - --warning-link-active-color: #fff1cc; - --warning-filled-color: #fff7e3; - --warning-filled-background-color: #d19600; - --warning-filled-link-color: #fff1cc; - --warning-filled-link-active-color: #fff7e3; - - --danger-color: #f60000; - --danger-link-active-color: #f6dddc; - --danger-filled-color: #fdf3f3; - --danger-filled-background-color: #e23e3e; - --danger-filled-link-color: #f2c7c6; - --danger-filled-link-active-color: #f6dddc; - - --info-color: #2e7dc5; - --info-link-active-color: #c6ddf2; - --info-filled-color: #f4f8fc; - --info-filled-background-color: #4c93d3; - --info-filled-link-color: #c6ddf2; - --info-filled-link-active-color: #dbeaf7; - - --dim-color: #bdbdbd; - --dim-link-color: #c0c0c0; - --dim-link-active-color: #949494; - --dim-filled-color: #7c7c7c; - --dim-filled-background-color: #f1f1f1; - --dim-filled-link-color: #c0c0c0; - --dim-filled-link-active-color: #949494; - --dim-button-active-color: #c0c0c0; + /* Text properties */ + /*--font: 'Libre Baskerville', serif;*/ + /*--code-font: 'Source Code Pro', monospace;*/ + --font: 'Source Sans Pro', sans-serif; + --code-font: 'Source Code Pro', monospace; + --font-size: 1em; + --code-font-size: 0.8em; /* *not* rem, so it follows surrounding font size */ + --line-height: normal; + --paragraph-indent: 1.5rem; + --paragraph-align: justify; + --link-decoration: underline; + --link-decoration-nav: none; + --link-decoration-heading: none; + --nav-brand-case: lowercase; + --nav-menu-case: lowercase; + --nav-heading-case: uppercase; + --nav-categories-case: lowercase; + --landing-header-case: lowercase; + --heading-font-weight: normal; + + /* Shapes */ + --border-radius: 0.2rem; + + /* Basics */ + --background-color: #ffffff; + --color: #000000; + --line-color: #c7cfd9; + --link-color: #205275; + --link-active-color: #2f73a3; + --mark-color: #4c93d3; + --mark-background-color: #f0c63e; + --code-color: #000000; + --code-inverted-color: rgba(91, 91, 91, 0.33); + --console-color: var(--code-color); + --console-inverted-color: var(--code-inverted-color); + /* This is simply color-picked --code-note-background-color on top of + --background-color */ + --code-background-color: #f2f7fa; + --code-note-background-color: rgba(251, 240, 236, 0.5); + --console-background-color: #000000; + --button-background-color: #ffffff; + + /* Header */ + --header-border-width: 0.25rem 0 0 0; + --header-color: #000000; + --header-breadcrumb-color: #bdbdbd; /* same as --dim-color */ + --header-background-color: #ffffff; + --header-background-color-landing: rgba(255, 255, 255, 0.75); + --header-background-color-jumbo: rgba(255, 255, 255, 0.25); + --header-link-color: #000000; + --header-link-active-color: #205275; + --header-link-current-color: #2f73a3; + --header-link-active-background-color: #ffffff; + --header-link-active-background-color: rgba(255, 255, 255, 0.5); + + /* Footer */ + --footer-font-size: 0.85rem; + --footer-color: #7c7c7c; + --footer-background-color: #eeeeee; + --footer-link-color: #191919; + --footer-link-active-color: #494949; + + /* Cover image */ + --cover-image-background-color: #666666; + + /* Search (used only by m-documentation.css) */ + --search-overlay-color: var(--header-background-color-landing); + --search-background-color: var(--header-background-color); + + /* Article */ + --article-header-color: #7a7a7a; + --article-footer-color: #969696; + --article-heading-color: #205275; + --article-heading-active-color: #2e73a3; + + /* Right navigation panel */ + --navpanel-link-color: #292929; + --navpanel-link-active-color: #205275; + + /* Plots */ + --plot-background-color: #f2f7fa; + --plot-error-color: #000000; + + /* Colored components */ + --default-color: #000000; + --default-link-active-color: #205275; + --default-filled-color: #000000; + --default-filled-background-color: #f2f7fa; + --default-filled-link-color: #2f73a3; + --default-filled-link-active-color: #205275; + + --primary-color: #205275; + --primary-link-active-color: #000000; + --primary-filled-color: #fbe4d9; + --primary-filled-background-color: #ef9069; + --primary-filled-link-color: #782f0d; + --primary-filled-link-active-color: #2f1205; + + --success-color: #31c25d; + --success-link-active-color: #dcf6e3; + --success-filled-color: #f4fcf6; + --success-filled-background-color: #4dd376; + --success-filled-link-color: #c5f2d1; + --success-filled-link-active-color: #dcf6e3; + + --warning-color: #d19600; + --warning-link-active-color: #fff1cc; + --warning-filled-color: #fff7e3; + --warning-filled-background-color: #d19600; + --warning-filled-link-color: #fff1cc; + --warning-filled-link-active-color: #fff7e3; + + --danger-color: #f60000; + --danger-link-active-color: #f6dddc; + --danger-filled-color: #fdf3f3; + --danger-filled-background-color: #e23e3e; + --danger-filled-link-color: #f2c7c6; + --danger-filled-link-active-color: #f6dddc; + + --info-color: #2e7dc5; + --info-link-active-color: #c6ddf2; + --info-filled-color: #f4f8fc; + --info-filled-background-color: #4c93d3; + --info-filled-link-color: #c6ddf2; + --info-filled-link-active-color: #dbeaf7; + + --dim-color: #bdbdbd; + --dim-link-color: #c0c0c0; + --dim-link-active-color: #949494; + --dim-filled-color: #7c7c7c; + --dim-filled-background-color: #f1f1f1; + --dim-filled-link-color: #c0c0c0; + --dim-filled-link-active-color: #949494; + --dim-button-active-color: #c0c0c0; } diff --git a/docs/mcss_theme/pygments-tango.css b/docs/mcss_theme/pygments-tango.css index e9353654f..d67b8f300 100644 --- a/docs/mcss_theme/pygments-tango.css +++ b/docs/mcss_theme/pygments-tango.css @@ -1,77 +1,419 @@ -.m-code .hll { background-color: #ffffcc } -.m-code { background: #f8f8f8; } -.m-code .c { color: #8f5902; font-style: italic } /* Comment */ -.m-code .err { color: #a40000; border: 1px solid #ef2929 } /* Error */ -.m-code .g { color: #000000 } /* Generic */ -.m-code .k { color: #204a87; font-weight: bold } /* Keyword */ -.m-code .l { color: #000000 } /* Literal */ -.m-code .n { color: #000000 } /* Name */ -.m-code .o { color: #ce5c00; font-weight: bold } /* Operator */ -.m-code .x { color: #000000 } /* Other */ -.m-code .p { color: #000000; font-weight: bold } /* Punctuation */ -.m-code .ch { color: #8f5902; font-style: italic } /* Comment.Hashbang */ -.m-code .cm { color: #8f5902; font-style: italic } /* Comment.Multiline */ -.m-code .cp { color: #8f5902; font-style: italic } /* Comment.Preproc */ -.m-code .cpf { color: #8f5902; font-style: italic } /* Comment.PreprocFile */ -.m-code .c1 { color: #8f5902; font-style: italic } /* Comment.Single */ -.m-code .cs { color: #8f5902; font-style: italic } /* Comment.Special */ -.m-code .gd { color: #a40000 } /* Generic.Deleted */ -.m-code .ge { color: #000000; font-style: italic } /* Generic.Emph */ -.m-code .gr { color: #ef2929 } /* Generic.Error */ -.m-code .gh { color: #000080; font-weight: bold } /* Generic.Heading */ -.m-code .gi { color: #00A000 } /* Generic.Inserted */ -.m-code .go { color: #000000; font-style: italic } /* Generic.Output */ -.m-code .gp { color: #8f5902 } /* Generic.Prompt */ -.m-code .gs { color: #000000; font-weight: bold } /* Generic.Strong */ -.m-code .gu { color: #800080; font-weight: bold } /* Generic.Subheading */ -.m-code .gt { color: #a40000; font-weight: bold } /* Generic.Traceback */ -.m-code .kc { color: #204a87; font-weight: bold } /* Keyword.Constant */ -.m-code .kd { color: #204a87; font-weight: bold } /* Keyword.Declaration */ -.m-code .kn { color: #204a87; font-weight: bold } /* Keyword.Namespace */ -.m-code .kp { color: #204a87; font-weight: bold } /* Keyword.Pseudo */ -.m-code .kr { color: #204a87; font-weight: bold } /* Keyword.Reserved */ -.m-code .kt { color: #204a87; font-weight: bold } /* Keyword.Type */ -.m-code .ld { color: #000000 } /* Literal.Date */ -.m-code .m { color: #0000cf; font-weight: bold } /* Literal.Number */ -.m-code .s { color: #4e9a06 } /* Literal.String */ -.m-code .na { color: #c4a000 } /* Name.Attribute */ -.m-code .nb { color: #204a87 } /* Name.Builtin */ -.m-code .nc { color: #000000 } /* Name.Class */ -.m-code .no { color: #000000 } /* Name.Constant */ -.m-code .nd { color: #5c35cc; font-weight: bold } /* Name.Decorator */ -.m-code .ni { color: #ce5c00 } /* Name.Entity */ -.m-code .ne { color: #cc0000; font-weight: bold } /* Name.Exception */ -.m-code .nf { color: #000000 } /* Name.Function */ -.m-code .nl { color: #f57900 } /* Name.Label */ -.m-code .nn { color: #000000 } /* Name.Namespace */ -.m-code .nx { color: #000000 } /* Name.Other */ -.m-code .py { color: #000000 } /* Name.Property */ -.m-code .nt { color: #204a87; font-weight: bold } /* Name.Tag */ -.m-code .nv { color: #000000 } /* Name.Variable */ -.m-code .ow { color: #204a87; font-weight: bold } /* Operator.Word */ -.m-code .w { color: #f8f8f8; text-decoration: underline } /* Text.Whitespace */ -.m-code .mb { color: #0000cf; font-weight: bold } /* Literal.Number.Bin */ -.m-code .mf { color: #0000cf; font-weight: bold } /* Literal.Number.Float */ -.m-code .mh { color: #0000cf; font-weight: bold } /* Literal.Number.Hex */ -.m-code .mi { color: #0000cf; font-weight: bold } /* Literal.Number.Integer */ -.m-code .mo { color: #0000cf; font-weight: bold } /* Literal.Number.Oct */ -.m-code .sa { color: #4e9a06 } /* Literal.String.Affix */ -.m-code .sb { color: #4e9a06 } /* Literal.String.Backtick */ -.m-code .sc { color: #4e9a06 } /* Literal.String.Char */ -.m-code .dl { color: #4e9a06 } /* Literal.String.Delimiter */ -.m-code .sd { color: #8f5902; font-style: italic } /* Literal.String.Doc */ -.m-code .s2 { color: #4e9a06 } /* Literal.String.Double */ -.m-code .se { color: #4e9a06 } /* Literal.String.Escape */ -.m-code .sh { color: #4e9a06 } /* Literal.String.Heredoc */ -.m-code .si { color: #4e9a06 } /* Literal.String.Interpol */ -.m-code .sx { color: #4e9a06 } /* Literal.String.Other */ -.m-code .sr { color: #4e9a06 } /* Literal.String.Regex */ -.m-code .s1 { color: #4e9a06 } /* Literal.String.Single */ -.m-code .ss { color: #4e9a06 } /* Literal.String.Symbol */ -.m-code .bp { color: #3465a4 } /* Name.Builtin.Pseudo */ -.m-code .fm { color: #000000 } /* Name.Function.Magic */ -.m-code .vc { color: #000000 } /* Name.Variable.Class */ -.m-code .vg { color: #000000 } /* Name.Variable.Global */ -.m-code .vi { color: #000000 } /* Name.Variable.Instance */ -.m-code .vm { color: #000000 } /* Name.Variable.Magic */ -.m-code .il { color: #0000cf; font-weight: bold } /* Literal.Number.Integer.Long */ +.m-code .hll { + background-color: #ffffcc +} + +.m-code { + background: #f8f8f8; +} + +.m-code .c { + color: #8f5902; + font-style: italic +} + +/* Comment */ +.m-code .err { + color: #a40000; + border: 1px solid #ef2929 +} + +/* Error */ +.m-code .g { + color: #000000 +} + +/* Generic */ +.m-code .k { + color: #204a87; + font-weight: bold +} + +/* Keyword */ +.m-code .l { + color: #000000 +} + +/* Literal */ +.m-code .n { + color: #000000 +} + +/* Name */ +.m-code .o { + color: #ce5c00; + font-weight: bold +} + +/* Operator */ +.m-code .x { + color: #000000 +} + +/* Other */ +.m-code .p { + color: #000000; + font-weight: bold +} + +/* Punctuation */ +.m-code .ch { + color: #8f5902; + font-style: italic +} + +/* Comment.Hashbang */ +.m-code .cm { + color: #8f5902; + font-style: italic +} + +/* Comment.Multiline */ +.m-code .cp { + color: #8f5902; + font-style: italic +} + +/* Comment.Preproc */ +.m-code .cpf { + color: #8f5902; + font-style: italic +} + +/* Comment.PreprocFile */ +.m-code .c1 { + color: #8f5902; + font-style: italic +} + +/* Comment.Single */ +.m-code .cs { + color: #8f5902; + font-style: italic +} + +/* Comment.Special */ +.m-code .gd { + color: #a40000 +} + +/* Generic.Deleted */ +.m-code .ge { + color: #000000; + font-style: italic +} + +/* Generic.Emph */ +.m-code .gr { + color: #ef2929 +} + +/* Generic.Error */ +.m-code .gh { + color: #000080; + font-weight: bold +} + +/* Generic.Heading */ +.m-code .gi { + color: #00A000 +} + +/* Generic.Inserted */ +.m-code .go { + color: #000000; + font-style: italic +} + +/* Generic.Output */ +.m-code .gp { + color: #8f5902 +} + +/* Generic.Prompt */ +.m-code .gs { + color: #000000; + font-weight: bold +} + +/* Generic.Strong */ +.m-code .gu { + color: #800080; + font-weight: bold +} + +/* Generic.Subheading */ +.m-code .gt { + color: #a40000; + font-weight: bold +} + +/* Generic.Traceback */ +.m-code .kc { + color: #204a87; + font-weight: bold +} + +/* Keyword.Constant */ +.m-code .kd { + color: #204a87; + font-weight: bold +} + +/* Keyword.Declaration */ +.m-code .kn { + color: #204a87; + font-weight: bold +} + +/* Keyword.Namespace */ +.m-code .kp { + color: #204a87; + font-weight: bold +} + +/* Keyword.Pseudo */ +.m-code .kr { + color: #204a87; + font-weight: bold +} + +/* Keyword.Reserved */ +.m-code .kt { + color: #204a87; + font-weight: bold +} + +/* Keyword.Type */ +.m-code .ld { + color: #000000 +} + +/* Literal.Date */ +.m-code .m { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number */ +.m-code .s { + color: #4e9a06 +} + +/* Literal.String */ +.m-code .na { + color: #c4a000 +} + +/* Name.Attribute */ +.m-code .nb { + color: #204a87 +} + +/* Name.Builtin */ +.m-code .nc { + color: #000000 +} + +/* Name.Class */ +.m-code .no { + color: #000000 +} + +/* Name.Constant */ +.m-code .nd { + color: #5c35cc; + font-weight: bold +} + +/* Name.Decorator */ +.m-code .ni { + color: #ce5c00 +} + +/* Name.Entity */ +.m-code .ne { + color: #cc0000; + font-weight: bold +} + +/* Name.Exception */ +.m-code .nf { + color: #000000 +} + +/* Name.Function */ +.m-code .nl { + color: #f57900 +} + +/* Name.Label */ +.m-code .nn { + color: #000000 +} + +/* Name.Namespace */ +.m-code .nx { + color: #000000 +} + +/* Name.Other */ +.m-code .py { + color: #000000 +} + +/* Name.Property */ +.m-code .nt { + color: #204a87; + font-weight: bold +} + +/* Name.Tag */ +.m-code .nv { + color: #000000 +} + +/* Name.Variable */ +.m-code .ow { + color: #204a87; + font-weight: bold +} + +/* Operator.Word */ +.m-code .w { + color: #f8f8f8; + text-decoration: underline +} + +/* Text.Whitespace */ +.m-code .mb { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Bin */ +.m-code .mf { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Float */ +.m-code .mh { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Hex */ +.m-code .mi { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Integer */ +.m-code .mo { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Oct */ +.m-code .sa { + color: #4e9a06 +} + +/* Literal.String.Affix */ +.m-code .sb { + color: #4e9a06 +} + +/* Literal.String.Backtick */ +.m-code .sc { + color: #4e9a06 +} + +/* Literal.String.Char */ +.m-code .dl { + color: #4e9a06 +} + +/* Literal.String.Delimiter */ +.m-code .sd { + color: #8f5902; + font-style: italic +} + +/* Literal.String.Doc */ +.m-code .s2 { + color: #4e9a06 +} + +/* Literal.String.Double */ +.m-code .se { + color: #4e9a06 +} + +/* Literal.String.Escape */ +.m-code .sh { + color: #4e9a06 +} + +/* Literal.String.Heredoc */ +.m-code .si { + color: #4e9a06 +} + +/* Literal.String.Interpol */ +.m-code .sx { + color: #4e9a06 +} + +/* Literal.String.Other */ +.m-code .sr { + color: #4e9a06 +} + +/* Literal.String.Regex */ +.m-code .s1 { + color: #4e9a06 +} + +/* Literal.String.Single */ +.m-code .ss { + color: #4e9a06 +} + +/* Literal.String.Symbol */ +.m-code .bp { + color: #3465a4 +} + +/* Name.Builtin.Pseudo */ +.m-code .fm { + color: #000000 +} + +/* Name.Function.Magic */ +.m-code .vc { + color: #000000 +} + +/* Name.Variable.Class */ +.m-code .vg { + color: #000000 +} + +/* Name.Variable.Global */ +.m-code .vi { + color: #000000 +} + +/* Name.Variable.Instance */ +.m-code .vm { + color: #000000 +} + +/* Name.Variable.Magic */ +.m-code .il { + color: #0000cf; + font-weight: bold +} + +/* Literal.Number.Integer.Long */ diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index bba5c3e78..c64eaa310 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(Eigen3 REQUIRED) find_package(OpenCV 4 QUIET) if (NOT OpenCV_FOUND) find_package(OpenCV 3 REQUIRED) -endif() +endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) # Display message to user @@ -23,9 +23,9 @@ option(ENABLE_ARUCO_TAGS "Enable or disable aruco tag (disable if no contrib mod if (NOT ENABLE_ARUCO_TAGS) add_definitions(-DENABLE_ARUCO_TAGS=0) message(WARNING "DISABLING ARUCOTAG TRACKING!") -else() +else () add_definitions(-DENABLE_ARUCO_TAGS=1) -endif() +endif () # Describe catkin project option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) @@ -36,10 +36,10 @@ if (catkin_FOUND AND ENABLE_CATKIN_ROS) INCLUDE_DIRS src LIBRARIES ov_core_lib ) -else() +else () add_definitions(-DROS_AVAILABLE=0) message(WARNING "BUILDING WITHOUT ROS!") -endif() +endif () # Try to compile with c++11 @@ -47,13 +47,13 @@ endif() include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) +if (COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) +elseif (COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else() +else () message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() +endif () # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") @@ -74,7 +74,7 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} -) + ) ################################################## # Make the core library @@ -92,7 +92,7 @@ add_library(ov_core_lib SHARED src/feat/Feature.cpp src/feat/FeatureInitializer.cpp src/utils/print.cpp -) + ) target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC src) @@ -103,7 +103,7 @@ target_include_directories(ov_core_lib PUBLIC src) if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_executable(test_tracking src/test_tracking.cpp) target_link_libraries(test_tracking ov_core_lib ${thirdparty_libraries}) -endif() +endif () add_executable(test_webcam src/test_webcam.cpp) target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h index 6580f3434..c1eb1f3cc 100644 --- a/ov_core/src/feat/FeatureDatabase.h +++ b/ov_core/src/feat/FeatureDatabase.h @@ -105,7 +105,7 @@ class FeatureDatabase { } // Debug info - // ROS_INFO("featdb - adding new feature %d",(int)id); + // PRINT_DEBUG("featdb - adding new feature %d",(int)id); // Else we have not found the feature, so lets make it be a new one! std::shared_ptr feat = std::make_shared(); diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp index 5e92098bc..c4f681d5f 100644 --- a/ov_core/src/feat/FeatureInitializer.cpp +++ b/ov_core/src/feat/FeatureInitializer.cpp @@ -20,9 +20,6 @@ */ #include "FeatureInitializer.h" -#include "utils/print.h" - -#include using namespace ov_core; diff --git a/ov_core/src/feat/FeatureInitializer.h b/ov_core/src/feat/FeatureInitializer.h index 4c8312377..0ec0547bc 100644 --- a/ov_core/src/feat/FeatureInitializer.h +++ b/ov_core/src/feat/FeatureInitializer.h @@ -26,6 +26,7 @@ #include "Feature.h" #include "FeatureInitializerOptions.h" +#include "utils/print.h" #include "utils/quat_ops.h" namespace ov_core { diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h index 352292b71..335a7e112 100644 --- a/ov_core/src/feat/FeatureInitializerOptions.h +++ b/ov_core/src/feat/FeatureInitializerOptions.h @@ -69,18 +69,18 @@ struct FeatureInitializerOptions { /// Nice print function of what parameters we have loaded void print() { - PRINT_INFO("\t- triangulate_1d: %d\n", triangulate_1d); - PRINT_INFO("\t- refine_features: %d\n", refine_features); - PRINT_INFO("\t- max_runs: %d\n", max_runs); - PRINT_INFO("\t- init_lamda: %.3f\n", init_lamda); - PRINT_INFO("\t- max_lamda: %.3f\n", max_lamda); - PRINT_INFO("\t- min_dx: %.7f\n", min_dx); - PRINT_INFO("\t- min_dcost: %.7f\n", min_dcost); - PRINT_INFO("\t- lam_mult: %.3f\n", lam_mult); - PRINT_INFO("\t- min_dist: %.3f\n", min_dist); - PRINT_INFO("\t- max_dist: %.3f\n", max_dist); - PRINT_INFO("\t- max_baseline: %.3f\n", max_baseline); - PRINT_INFO("\t- max_cond_number: %.3f\n", max_cond_number); + PRINT_DEBUG("\t- triangulate_1d: %d\n", triangulate_1d); + PRINT_DEBUG("\t- refine_features: %d\n", refine_features); + PRINT_DEBUG("\t- max_runs: %d\n", max_runs); + PRINT_DEBUG("\t- init_lamda: %.3f\n", init_lamda); + PRINT_DEBUG("\t- max_lamda: %.3f\n", max_lamda); + PRINT_DEBUG("\t- min_dx: %.7f\n", min_dx); + PRINT_DEBUG("\t- min_dcost: %.7f\n", min_dcost); + PRINT_DEBUG("\t- lam_mult: %.3f\n", lam_mult); + PRINT_DEBUG("\t- min_dist: %.3f\n", min_dist); + PRINT_DEBUG("\t- max_dist: %.3f\n", max_dist); + PRINT_DEBUG("\t- max_baseline: %.3f\n", max_baseline); + PRINT_DEBUG("\t- max_cond_number: %.3f\n", max_cond_number); } }; diff --git a/ov_core/src/init/InertialInitializer.cpp b/ov_core/src/init/InertialInitializer.cpp index 7de4ed81a..2873194f5 100644 --- a/ov_core/src/init/InertialInitializer.cpp +++ b/ov_core/src/init/InertialInitializer.cpp @@ -20,7 +20,6 @@ */ #include "InertialInitializer.h" -#include "utils/print.h" using namespace ov_core; diff --git a/ov_core/src/init/InertialInitializer.h b/ov_core/src/init/InertialInitializer.h index 1ee8ccd42..1fb0d8a21 100644 --- a/ov_core/src/init/InertialInitializer.h +++ b/ov_core/src/init/InertialInitializer.h @@ -23,6 +23,7 @@ #define OV_CORE_INERTIALINITIALIZER_H #include "utils/colors.h" +#include "utils/print.h" #include "utils/quat_ops.h" #include "utils/sensor_data.h" diff --git a/ov_core/src/sim/BsplineSE3.cpp b/ov_core/src/sim/BsplineSE3.cpp index b2978cdfe..6afd829fb 100644 --- a/ov_core/src/sim/BsplineSE3.cpp +++ b/ov_core/src/sim/BsplineSE3.cpp @@ -20,9 +20,6 @@ */ #include "BsplineSE3.h" -#include "utils/print.h" - -#include using namespace ov_core; diff --git a/ov_core/src/sim/BsplineSE3.h b/ov_core/src/sim/BsplineSE3.h index 8d3e68fca..3543e0f87 100644 --- a/ov_core/src/sim/BsplineSE3.h +++ b/ov_core/src/sim/BsplineSE3.h @@ -26,6 +26,7 @@ #include #include +#include "utils/print.h" #include "utils/quat_ops.h" namespace ov_core { diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index 75ecdb83e..3d6084143 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -63,6 +63,11 @@ int main(int argc, char **argv) { ros::init(argc, argv, "test_tracking"); ros::NodeHandle nh("~"); + // Verbosity setting + std::string verbosity; + nh.param("verbosity", verbosity, "INFO"); + ov_core::Printer::setPrintLevel(verbosity); + // Our camera topics (left and right stereo) std::string topic_camera0; std::string topic_camera1; diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 00af5b0f1..5f31a5030 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -52,6 +52,7 @@ int main(int argc, char **argv) { CLI::App app{"test_webcam"}; // Defaults + std::string verbosity = "INFO"; int num_pts = 500; int num_aruco = 1024; int clone_states = 20; @@ -65,6 +66,7 @@ int main(int argc, char **argv) { ov_core::TrackBase::HistogramMethod method = ov_core::TrackBase::HistogramMethod::NONE; // Parameters for our extractor + app.add_option("--verbosity", verbosity, "Print verbosity"); app.add_option("--num_pts", num_pts, "Number of feature tracks"); app.add_option("--num_aruco", num_aruco, "Number of aruco tag ids we have"); app.add_option("--clone_states", clone_states, "Amount of clones to visualize track length with"); @@ -82,6 +84,9 @@ int main(int argc, char **argv) { return app.exit(e); } + // Verbosity setting + ov_core::Printer::setPrintLevel(verbosity); + // Debug print! PRINT_DEBUG("max features: %d\n", num_pts); PRINT_DEBUG("max aruco: %d\n", num_aruco); diff --git a/ov_core/src/types/Landmark.cpp b/ov_core/src/types/Landmark.cpp index 79613baec..a562c1a14 100644 --- a/ov_core/src/types/Landmark.cpp +++ b/ov_core/src/types/Landmark.cpp @@ -25,15 +25,15 @@ using namespace ov_type; Eigen::Matrix Landmark::get_xyz(bool getfej) const { - /// CASE: Global 3d feature representation - /// CASE: Anchored 3D feature representation + // CASE: Global 3d feature representation + // CASE: Anchored 3D feature representation if (_feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D || _feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) { return (getfej) ? fej() : value(); } - /// CASE: Global inverse depth feature representation - /// CASE: Anchored full inverse depth feature representation + // CASE: Global inverse depth feature representation + // CASE: Anchored full inverse depth feature representation if (_feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH || _feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) { Eigen::Matrix p_invFinG = (getfej) ? fej() : value(); @@ -43,7 +43,7 @@ Eigen::Matrix Landmark::get_xyz(bool getfej) const { return p_FinG; } - /// CASE: Anchored MSCKF inverse depth feature representation + // CASE: Anchored MSCKF inverse depth feature representation if (_feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) { Eigen::Matrix p_FinA; Eigen::Matrix p_invFinA = value(); @@ -51,7 +51,7 @@ Eigen::Matrix Landmark::get_xyz(bool getfej) const { return p_FinA; } - /// CASE: Estimate single depth of the feature using the initial bearing + // CASE: Estimate single depth of the feature using the initial bearing if (_feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) { // if(getfej) return 1.0/fej()(0)*uv_norm_zero_fej; return 1.0 / value()(0) * uv_norm_zero; @@ -64,8 +64,8 @@ Eigen::Matrix Landmark::get_xyz(bool getfej) const { void Landmark::set_from_xyz(Eigen::Matrix p_FinG, bool isfej) { - /// CASE: Global 3d feature representation - /// CASE: Anchored 3d feature representation + // CASE: Global 3d feature representation + // CASE: Anchored 3d feature representation if (_feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D || _feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) { if (isfej) @@ -75,8 +75,8 @@ void Landmark::set_from_xyz(Eigen::Matrix p_FinG, bool isfej) { return; } - /// CASE: Global inverse depth feature representation - /// CASE: Anchored inverse depth feature representation + // CASE: Global inverse depth feature representation + // CASE: Anchored inverse depth feature representation if (_feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH || _feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) { @@ -100,7 +100,7 @@ void Landmark::set_from_xyz(Eigen::Matrix p_FinG, bool isfej) { return; } - /// CASE: MSCKF anchored inverse depth representation + // CASE: MSCKF anchored inverse depth representation if (_feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) { // MSCKF representation @@ -117,7 +117,7 @@ void Landmark::set_from_xyz(Eigen::Matrix p_FinG, bool isfej) { return; } - /// CASE: Estimate single depth of the feature using the initial bearing + // CASE: Estimate single depth of the feature using the initial bearing if (_feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) { // Get the inverse depth diff --git a/ov_core/src/types/PoseJPL.h b/ov_core/src/types/PoseJPL.h index be8254119..6b765119d 100644 --- a/ov_core/src/types/PoseJPL.h +++ b/ov_core/src/types/PoseJPL.h @@ -122,10 +122,7 @@ class PoseJPL : public Type { Eigen::Matrix Rot() const { return _q->Rot(); } /// FEJ Rotation access - Eigen::Matrix Rot_fej() const { - return _q->Rot_fej(); - ; - } + Eigen::Matrix Rot_fej() const { return _q->Rot_fej(); } /// Rotation access as quaternion Eigen::Matrix quat() const { return _q->value(); } diff --git a/ov_core/src/utils/CLI11.hpp b/ov_core/src/utils/CLI11.hpp index caedc700f..d41c24a37 100644 --- a/ov_core/src/utils/CLI11.hpp +++ b/ov_core/src/utils/CLI11.hpp @@ -2550,7 +2550,7 @@ class IsMember : public Validator { /// This allows in-place construction using an initializer list template - IsMember(std::initializer_list values, Args &&... args) : IsMember(std::vector(values), std::forward(args)...) {} + IsMember(std::initializer_list values, Args &&...args) : IsMember(std::vector(values), std::forward(args)...) {} /// This checks to see if an item is in a set (empty function) template explicit IsMember(T &&set) : IsMember(std::forward(set), nullptr) {} @@ -2603,7 +2603,7 @@ class IsMember : public Validator { /// You can pass in as many filter functions as you like, they nest (string only currently) template - IsMember(T &&set, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&... other) + IsMember(T &&set, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&...other) : IsMember( std::forward(set), [filter_fn_1, filter_fn_2](std::string a) { return filter_fn_2(filter_fn_1(a)); }, other...) {} }; @@ -2618,7 +2618,7 @@ class Transformer : public Validator { /// This allows in-place construction template - Transformer(std::initializer_list> values, Args &&... args) + Transformer(std::initializer_list> values, Args &&...args) : Transformer(TransformPairs(values), std::forward(args)...) {} /// direct map of std::string to std::string @@ -2661,7 +2661,7 @@ class Transformer : public Validator { /// You can pass in as many filter functions as you like, they nest template - Transformer(T &&mapping, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&... other) + Transformer(T &&mapping, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&...other) : Transformer( std::forward(mapping), [filter_fn_1, filter_fn_2](std::string a) { return filter_fn_2(filter_fn_1(a)); }, other...) {} }; @@ -2673,7 +2673,7 @@ class CheckedTransformer : public Validator { /// This allows in-place construction template - CheckedTransformer(std::initializer_list> values, Args &&... args) + CheckedTransformer(std::initializer_list> values, Args &&...args) : CheckedTransformer(TransformPairs(values), std::forward(args)...) {} /// direct map of std::string to std::string @@ -2734,7 +2734,7 @@ class CheckedTransformer : public Validator { /// You can pass in as many filter functions as you like, they nest template - CheckedTransformer(T &&mapping, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&... other) + CheckedTransformer(T &&mapping, filter_fn_t filter_fn_1, filter_fn_t filter_fn_2, Args &&...other) : CheckedTransformer( std::forward(mapping), [filter_fn_1, filter_fn_2](std::string a) { return filter_fn_2(filter_fn_1(a)); }, other...) {} }; @@ -7349,13 +7349,13 @@ struct AppFriend { /// Wrap _parse_short, perfectly forward arguments and return template - static auto parse_arg(App *app, Args &&... args) -> typename std::result_of::type { + static auto parse_arg(App *app, Args &&...args) -> typename std::result_of::type { return app->_parse_arg(std::forward(args)...); } /// Wrap _parse_subcommand, perfectly forward arguments and return template - static auto parse_subcommand(App *app, Args &&... args) -> + static auto parse_subcommand(App *app, Args &&...args) -> typename std::result_of::type { return app->_parse_subcommand(std::forward(args)...); } @@ -7951,4 +7951,4 @@ inline std::string Formatter::make_option_usage(const Option *opt) const { return opt->get_required() ? out.str() : "[" + out.str() + "]"; } -} // namespace CLI +} // namespace CLI \ No newline at end of file diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h index 49b2bd749..cf1ac8cda 100644 --- a/ov_core/src/utils/dataset_reader.h +++ b/ov_core/src/utils/dataset_reader.h @@ -139,7 +139,7 @@ class DatasetReader { // Check that we have the timestamp in our GT file if (gt_states.find(timestep) == gt_states.end()) { - PRINT_INFO(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep); + PRINT_WARNING(YELLOW "Unable to find %.6f timestamp in GT file, wrong GT file loaded???\n" RESET, timestep); return false; } diff --git a/ov_core/src/utils/lambda_body.h b/ov_core/src/utils/lambda_body.h index ea3479e9c..55978dbe6 100644 --- a/ov_core/src/utils/lambda_body.h +++ b/ov_core/src/utils/lambda_body.h @@ -28,6 +28,8 @@ namespace ov_core { /** + * @brief Helper class to do OpenCV parallelization + * * This is a utility class required to build with older version of opencv * On newer versions this doesn't seem to be needed, but here we just use it to ensure we can work for more opencv version. * https://answers.opencv.org/question/65800/how-to-use-lambda-as-a-parameter-to-parallel_for_/?answer=130691#post-id-130691 diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp index 39592bbed..455a701a6 100644 --- a/ov_core/src/utils/print.cpp +++ b/ov_core/src/utils/print.cpp @@ -20,19 +20,34 @@ */ #include "print.h" -#include -#include -#include -#include using namespace ov_core; -// Need to define the variable for everything to work +// Need to define the static variable for everything to work Printer::PrintLevel Printer::current_print_level = PrintLevel::ALL; +void Printer::setPrintLevel(const std::string &level) { + if (level == "ALL") + setPrintLevel(PrintLevel::ALL); + else if (level == "DEBUG") + setPrintLevel(PrintLevel::DEBUG); + else if (level == "INFO") + setPrintLevel(PrintLevel::INFO); + else if (level == "WARNING") + setPrintLevel(PrintLevel::WARNING); + else if (level == "ERROR") + setPrintLevel(PrintLevel::ERROR); + else if (level == "SILENT") + setPrintLevel(PrintLevel::SILENT); + else { + std::cout << "Invalid print level requested: " << level << std::endl; + std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl; + std::exit(EXIT_FAILURE); + } +} + void Printer::setPrintLevel(PrintLevel level) { Printer::current_print_level = level; - std::cout << "Setting printing level to: "; switch (current_print_level) { case PrintLevel::ALL: @@ -54,26 +69,31 @@ void Printer::setPrintLevel(PrintLevel level) { std::cout << "SILENT"; break; default: - // Can never get here - break; + std::cout << std::endl; + std::cout << "Invalid print level requested: " << level << std::endl; + std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl; + std::exit(EXIT_FAILURE); } - std::cout << std::endl; } -void Printer::debugPrint(PrintLevel level, const char location[], const char *format, ...) { +void Printer::debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...) { // Only print for the current debug level if (static_cast(level) < static_cast(Printer::current_print_level)) { return; } - // Print the location info first - if (strlen(location) > MAX_FILE_PATH_LEGTH) { - // Truncate the location length to the max size for the filepath - printf("%s", &(location[strlen(location) - MAX_FILE_PATH_LEGTH])); - } else { - // Print the full location - printf("%s", location); + // Print the location info first for our debug output + // Truncate the filename to the max size for the filepath + if(static_cast(level) < static_cast(Printer::PrintLevel::DEBUG)) { + std::string path(location); + std::string base_filename = path.substr(path.find_last_of("/\\") + 1); + if (base_filename.size() > MAX_FILE_PATH_LEGTH) { + printf("%s", base_filename.substr(base_filename.size()-MAX_FILE_PATH_LEGTH,base_filename.size()).c_str()); + } else { + printf("%s", base_filename.c_str()); + } + printf(":%s ", line); } // Print the rest of the args diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h index c0bfa267c..30d5add17 100644 --- a/ov_core/src/utils/print.h +++ b/ov_core/src/utils/print.h @@ -22,60 +22,81 @@ #ifndef PRINT_H #define PRINT_H -#include +#include +#include +#include +#include +#include namespace ov_core { /** * @brief Printer for open_vins that allows for various levels of printing to be done * + * To set the global verbosity level one can do the following: + * @code{.cpp} + * ov_core::Printer::setPrintLevel("WARNING"); + * ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel::WARNING); + * @endcode */ class Printer { public: - /** The different print levels possible + /** + * @brief The different print levels possible + * + * - PrintLevel::ALL : All PRINT_XXXX will output to the console + * - PrintLevel::DEBUG : "DEBUG", "INFO", "WARNING" and "ERROR" will be printed. "ALL" will be silenced + * - PrintLevel::INFO : "INFO", "WARNING" and "ERROR" will be printed. "ALL" and "DEBUG" will be silenced + * - PrintLevel::WARNING : "WARNING" and "ERROR" will be printed. "ALL", "DEBUG" and "INFO" will be silenced + * - PrintLevel::ERROR : Only "ERROR" will be printed. All the rest are silenced + * - PrintLevel::SILENT : All PRINT_XXXX will be silenced. */ enum PrintLevel { ALL = 0, DEBUG = 1, INFO = 2, WARNING = 3, ERROR = 4, SILENT = 5 }; - /** @brief Set the print level to use for all future printing to stdout. - * - * @param level The debug level to use + /** + * @brief Set the print level to use for all future printing to stdout. + * @param level The debug level to use */ - static void setPrintLevel(PrintLevel level); + static void setPrintLevel(const std::string &level); - /** brief The print function that prints to stdout. - * - * @param level the print level for this print call - * @param location the location the print was made from - * @param format The printf format + /** + * @brief Set the print level to use for all future printing to stdout. + * @param level The debug level to use */ - static void debugPrint(PrintLevel level, const char location[], const char *format, ...); + static void setPrintLevel(PrintLevel level); - /** brief The print level + /** + * @brief The print function that prints to stdout. + * @param level the print level for this print call + * @param location the location the print was made from + * @param line the line the print was made from + * @param format The printf format */ + static void debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...); + + /// The current print level static PrintLevel current_print_level; private: /// The max length for the file path. This is to avoid very long file paths from - static constexpr uint32_t MAX_FILE_PATH_LEGTH = 50; + static constexpr uint32_t MAX_FILE_PATH_LEGTH = 30; }; } /* namespace ov_core */ -/** Converts anything to a string +/* + * Converts anything to a string */ #define STRINGIFY(x) #x #define TOSTRING(x) STRINGIFY(x) -/** Adds line numbers - */ -#define AT "(" __FILE__ ":" TOSTRING(__LINE__) "): " - -/** The different Types of print levels +/* + * The different Types of print levels */ -#define PRINT_ALL(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ALL, AT, x); -#define PRINT_DEBUG(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::DEBUG, AT, x); -#define PRINT_INFO(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::INFO, AT, x); -#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, AT, x); -#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, AT, x); +#define PRINT_ALL(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ALL, __FILE__, TOSTRING(__LINE__), x); +#define PRINT_DEBUG(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::DEBUG, __FILE__, TOSTRING(__LINE__), x); +#define PRINT_INFO(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::INFO, __FILE__, TOSTRING(__LINE__), x); +#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x); +#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x); #endif /* PRINT_H */ diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index adfadc588..58523626b 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -61,12 +61,9 @@ * */ -#include "utils/print.h" - #include -#include -#include -#include + +#include "utils/print.h" namespace ov_core { @@ -520,6 +517,65 @@ inline Eigen::Matrix Jl_so3(Eigen::Matrix w) { */ inline Eigen::Matrix Jr_so3(Eigen::Matrix w) { return Jl_so3(-w); } +/** + * @brief Gets roll, pitch, yaw of argument rotation (in that order). + * + * To recover the matrix: R_input = R_z(yaw)*R_y(pitch)*R_x(roll) + * If you are interested in how to compute Jacobians checkout this report: + * http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf + * + * @param rot Rotation matrix + * @return roll, pitch, yaw values (in that order) + */ +inline Eigen::Matrix rot2rpy(const Eigen::Matrix &rot) { + Eigen::Matrix rpy; + rpy(1, 0) = atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0))); + if (std::abs(cos(rpy(1, 0)) > 1.0e-12)) { + rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0))); + rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0))); + } else { + rpy(2, 0) = 0; + rpy(0, 0) = atan2(rot(0, 1), rot(1, 1)); + } + return rpy; +} + +/** + * @brief Construct rotation matrix from given roll + * @param t roll angle + */ +inline Eigen::Matrix rot_x(double t) { + Eigen::Matrix r; + double ct = cos(t); + double st = sin(t); + r << 1.0, 0.0, 0.0, 0.0, ct, -st, 0.0, st, ct; + return r; +} + +/** + * @brief Construct rotation matrix from given pitch + * @param t pitch angle + */ +inline Eigen::Matrix rot_y(double t) { + Eigen::Matrix r; + double ct = cos(t); + double st = sin(t); + r << ct, 0.0, st, 0.0, 1.0, 0.0, -st, 0.0, ct; + return r; +} + +/** + * @brief Construct rotation matrix from given yaw + * @param t yaw angle + */ +inline Eigen::Matrix rot_z(double t) { + Eigen::Matrix r; + double ct = cos(t); + double st = sin(t); + r << ct, -st, 0.0, st, ct, 0.0, 0.0, 0.0, 1.0; + return r; +} + } // namespace ov_core #endif /* OV_CORE_QUAT_OPS_H */ \ No newline at end of file diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index ac3a547ae..3108e7b89 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -17,12 +17,12 @@ message(STATUS "BOOST VERSION: " ${Boost_VERSION}) # check if we have our python libs files # sudo apt-get install python-matplotlib python-numpy python2.7-dev find_package(PythonLibs 2.7) -if(PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) +if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) -endif() +endif () # Describe catkin project option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) @@ -33,23 +33,23 @@ if (catkin_FOUND AND ENABLE_CATKIN_ROS) INCLUDE_DIRS src LIBRARIES ov_eval_lib ) -else() +else () add_definitions(-DROS_AVAILABLE=0) message(WARNING "BUILDING WITHOUT ROS!") -endif() +endif () # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) +if (COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) +elseif (COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else() +else () message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() +endif () # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") @@ -73,7 +73,16 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} -) + ) + +# If we are not building with ROS then we need to manually link to its headers +# This isn't that elegant of a way, but this at least allows for building without ROS +# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") + include_directories(${ov_core_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_core_lib) +endif () ################################################## # Make the core library @@ -85,7 +94,7 @@ add_library(ov_eval_lib src/calc/ResultTrajectory.cpp src/calc/ResultSimulation.cpp src/utils/Loader.cpp -) + ) target_link_libraries(ov_eval_lib ${thirdparty_libraries}) @@ -102,7 +111,7 @@ if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_executable(live_align_trajectory src/live_align_trajectory.cpp) target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) -endif() +endif () add_executable(format_converter src/format_converter.cpp) @@ -139,7 +148,7 @@ target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) if (catkin_FOUND AND ENABLE_CATKIN_ROS) catkin_install_python(PROGRAMS python/pid_ros.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) catkin_install_python(PROGRAMS python/pid_sys.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -endif() +endif () diff --git a/ov_eval/python/pid_ros.py b/ov_eval/python/pid_ros.py index 7ae8929d2..76b54ed64 100755 --- a/ov_eval/python/pid_ros.py +++ b/ov_eval/python/pid_ros.py @@ -18,11 +18,12 @@ # along with this program. If not, see . -import sys import os -import time import rosnode import rospy +import sys +import time + try: from xmlrpc.client import ServerProxy except ImportError: @@ -30,18 +31,17 @@ import psutil - def get_process_ros(node_name, doprint=False): # get the node object from ros node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True)[2] if not node_api: - rospy.logerr("could not get api of node %s (%s)"%(node_name, node_api)) + rospy.logerr("could not get api of node %s (%s)" % (node_name, node_api)) return False # now try to get the Pid of this process try: response = ServerProxy(node_api).getPid('/NODEINFO') except: - rospy.logerr("failed to get of the pid of ros node %s (%s)"%(node_name, node_api)) + rospy.logerr("failed to get of the pid of ros node %s (%s)" % (node_name, node_api)) return False # try to get the process using psutil try: @@ -50,11 +50,10 @@ def get_process_ros(node_name, doprint=False): rospy.loginfo("adding new node monitor %s (pid %d)" % (node_name, process.pid)) return process except: - rospy.logerr("unable to open psutil object for %s"%(response[2])) + rospy.logerr("unable to open psutil object for %s" % (response[2])) return False - if __name__ == '__main__': # initialize this ros node @@ -72,11 +71,11 @@ def get_process_ros(node_name, doprint=False): save_path = rospy.get_param("~output") # debug print to console - rospy.loginfo("processes: %s (%d in total)"%(node_csv,len(node_list))) - rospy.loginfo("save path: %s"%save_path) + rospy.loginfo("processes: %s (%d in total)" % (node_csv, len(node_list))) + rospy.loginfo("save path: %s" % save_path) - #=================================================================== - #=================================================================== + # =================================================================== + # =================================================================== # make sure the directory is made if not os.path.exists(os.path.dirname(save_path)): @@ -87,18 +86,18 @@ def get_process_ros(node_name, doprint=False): sys.exit(-1) # open the file we will write the stats into - file = open(save_path,"w") + file = open(save_path, "w") # write header to file header = "# timestamp(s) summed_cpu_perc summed_mem_perc summed_threads" for node in node_list: - get_process_ros(node, True) # nice debug print! - header += " "+str(node)+"_cpu_perc "+str(node)+"_mem_perc "+str(node)+"_threads" + get_process_ros(node, True) # nice debug print! + header += " " + str(node) + "_cpu_perc " + str(node) + "_mem_perc " + str(node) + "_threads" header += "\n" file.write(header) - #=================================================================== - #=================================================================== + # =================================================================== + # =================================================================== # exit if we should end if rospy.is_shutdown(): @@ -114,9 +113,9 @@ def get_process_ros(node_name, doprint=False): for node in node_list: ps_list.append(get_process_ros(node, False)) try: - perc_cpu = ps_list[len(ps_list)-1].cpu_percent(interval=None) - perc_mem = ps_list[len(ps_list)-1].memory_percent() - threads = ps_list[len(ps_list)-1].num_threads() + perc_cpu = ps_list[len(ps_list) - 1].cpu_percent(interval=None) + perc_mem = ps_list[len(ps_list) - 1].memory_percent() + threads = ps_list[len(ps_list) - 1].num_threads() except: continue @@ -127,7 +126,7 @@ def get_process_ros(node_name, doprint=False): perc_cpu = [] perc_mem = [] threads = [] - for i in range(0,len(node_list)): + for i in range(0, len(node_list)): try: # get readings p_cpu = ps_list[i].cpu_percent(interval=None) @@ -144,12 +143,12 @@ def get_process_ros(node_name, doprint=False): threads.append(0) # print what the total summed value is - rospy.loginfo("cpu%% = %.3f | mem%% = %.3f | threads = %d"%(sum(perc_cpu),sum(perc_mem),sum(threads))) + rospy.loginfo("cpu%% = %.3f | mem%% = %.3f | threads = %d" % (sum(perc_cpu), sum(perc_mem), sum(threads))) # save the current stats to file! - data = "%.8f %.3f %.3f %d"%(time.time(),sum(perc_cpu),sum(perc_mem),sum(threads)) - for i in range(0,len(node_list)): - data += " %.3f %.3f %d"%(perc_cpu[i],perc_mem[i],threads[i]) + data = "%.8f %.3f %.3f %d" % (time.time(), sum(perc_cpu), sum(perc_mem), sum(threads)) + for i in range(0, len(node_list)): + data += " %.3f %.3f %d" % (perc_cpu[i], perc_mem[i], threads[i]) data += "\n" file.write(data) file.flush() diff --git a/ov_eval/python/pid_sys.py b/ov_eval/python/pid_sys.py index c6659b395..c9e2eec93 100755 --- a/ov_eval/python/pid_sys.py +++ b/ov_eval/python/pid_sys.py @@ -18,10 +18,10 @@ # along with this program. If not, see . -import sys import os -import rospy import psutil +import rospy +import sys def get_process_name(process_name, doprint=False): @@ -46,12 +46,10 @@ def get_process_name(process_name, doprint=False): if len(processes) > 0: return processes # else we have failed! - rospy.logerr("unable to find process for %s"%(process_name)) + rospy.logerr("unable to find process for %s" % (process_name)) return False - - if __name__ == '__main__': # initialize this ros node @@ -92,9 +90,9 @@ def get_process_name(process_name, doprint=False): sum_threads += threads # print what the total summed value is - print("cpu percent = %.3f"%sum_perc_cpu) - print("mem percent = %.3f"%sum_perc_mem) - print("num threads = %d"%sum_threads) + print("cpu percent = %.3f" % sum_perc_cpu) + print("mem percent = %.3f" % sum_perc_mem) + print("num threads = %d" % sum_threads) processes = False # try to get the process again, this allows us to handle diff --git a/ov_eval/src/alignment/AlignTrajectory.cpp b/ov_eval/src/alignment/AlignTrajectory.cpp index fa4f75b48..1ca2d9442 100644 --- a/ov_eval/src/alignment/AlignTrajectory.cpp +++ b/ov_eval/src/alignment/AlignTrajectory.cpp @@ -20,7 +20,6 @@ */ #include "AlignTrajectory.h" -#include "utils/print.h" using namespace ov_eval; @@ -66,8 +65,8 @@ void AlignTrajectory::align_posyaw_single(const std::vector #include "AlignUtils.h" -#include "utils/Colors.h" + +#include "utils/colors.h" +#include "utils/print.h" namespace ov_eval { diff --git a/ov_eval/src/alignment/AlignUtils.cpp b/ov_eval/src/alignment/AlignUtils.cpp index 4341cd159..cff1c0823 100644 --- a/ov_eval/src/alignment/AlignUtils.cpp +++ b/ov_eval/src/alignment/AlignUtils.cpp @@ -20,9 +20,6 @@ */ #include "AlignUtils.h" -#include "utils/print.h" - -#include using namespace ov_eval; @@ -72,7 +69,7 @@ void AlignUtils::align_umeyama(const std::vector> &d if (yaw_only) { Eigen::Matrix rot_C = n * C.transpose(); double theta = AlignUtils::get_best_yaw(rot_C); - R = Math::rot_z(theta); + R = ov_core::rot_z(theta); } else { R.noalias() = U_svd * S * V_svd.transpose(); } diff --git a/ov_eval/src/alignment/AlignUtils.h b/ov_eval/src/alignment/AlignUtils.h index 17d8a8e2e..5663c8504 100644 --- a/ov_eval/src/alignment/AlignUtils.h +++ b/ov_eval/src/alignment/AlignUtils.h @@ -28,8 +28,9 @@ #include #include -#include "utils/Colors.h" -#include "utils/Math.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" namespace ov_eval { diff --git a/ov_eval/src/calc/ResultSimulation.cpp b/ov_eval/src/calc/ResultSimulation.cpp index 37a012f70..2d0c96234 100644 --- a/ov_eval/src/calc/ResultSimulation.cpp +++ b/ov_eval/src/calc/ResultSimulation.cpp @@ -20,7 +20,6 @@ */ #include "ResultSimulation.h" -#include "utils/print.h" using namespace ov_eval; @@ -59,8 +58,8 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { // Calculate orientation error // NOTE: we define our error as e_R = -Log(R*Rhat^T) Eigen::Matrix3d e_R = - Math::quat_2_Rot(gt_state.at(i).block(1, 0, 4, 1)) * Math::quat_2_Rot(est_state.at(i).block(1, 0, 4, 1)).transpose(); - Eigen::Vector3d ori_err = -180.0 / M_PI * Math::log_so3(e_R); + ov_core::quat_2_Rot(gt_state.at(i).block(1, 0, 4, 1)) * ov_core::quat_2_Rot(est_state.at(i).block(1, 0, 4, 1)).transpose(); + Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R); for (int j = 0; j < 3; j++) { error_ori[j].timestamps.push_back(est_state.at(i)(0)); error_ori[j].values.push_back(ori_err(j)); @@ -110,10 +109,9 @@ void ResultSimulation::plot_state(bool doplotting, double max_time) { return; #ifndef HAVE_PYTHONLIBS - printf(RED "Unable to plot the state error, just returning..\n" RESET); + PRINT_ERROR(RED "Unable to plot the state error, just returning..\n" RESET); return; -#endif -#ifdef HAVE_PYTHONLIBS +#else //===================================================== // Plot this figure @@ -232,8 +230,7 @@ void ResultSimulation::plot_timeoff(bool doplotting, double max_time) { #ifndef HAVE_PYTHONLIBS PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; -#endif -#ifdef HAVE_PYTHONLIBS +#else //===================================================== // Plot this figure @@ -333,8 +330,7 @@ void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) { #ifndef HAVE_PYTHONLIBS PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; -#endif -#ifdef HAVE_PYTHONLIBS +#else // Plot line colors std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; @@ -429,9 +425,9 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { // Loop through each camera and calculate error for (int n = 0; n < (int)est_state.at(0)(18); n++) { // NOTE: we define our error as e_R = -Log(R*Rhat^T) - Eigen::Matrix3d e_R = Math::quat_2_Rot(gt_state.at(i).block(27 + 15 * n, 0, 4, 1)) * - Math::quat_2_Rot(est_state.at(i).block(27 + 15 * n, 0, 4, 1)).transpose(); - Eigen::Vector3d ori_err = -180.0 / M_PI * Math::log_so3(e_R); + Eigen::Matrix3d e_R = ov_core::quat_2_Rot(gt_state.at(i).block(27 + 15 * n, 0, 4, 1)) * + ov_core::quat_2_Rot(est_state.at(i).block(27 + 15 * n, 0, 4, 1)).transpose(); + Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R); // Eigen::Matrix3d e_R = Math::quat_2_Rot(est_state.at(i).block(27+15*n,0,4,1)).transpose() * // Math::quat_2_Rot(gt_state.at(i).block(27+15*n,0,4,1)); Eigen::Vector3d ori_err = 180.0/M_PI*Math::log_so3(e_R); for (int j = 0; j < 3; j++) { @@ -452,8 +448,7 @@ void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) { #ifndef HAVE_PYTHONLIBS PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET); return; -#endif -#ifdef HAVE_PYTHONLIBS +#else // Plot line colors std::vector colors = {"blue", "red", "black", "green", "cyan", "magenta"}; diff --git a/ov_eval/src/calc/ResultSimulation.h b/ov_eval/src/calc/ResultSimulation.h index 0c3259a52..0d3353d5a 100644 --- a/ov_eval/src/calc/ResultSimulation.h +++ b/ov_eval/src/calc/ResultSimulation.h @@ -30,11 +30,13 @@ #include -#include "utils/Colors.h" #include "utils/Loader.h" -#include "utils/Math.h" #include "utils/Statistics.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + #ifdef HAVE_PYTHONLIBS // import the c++ wrapper for matplot lib diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp index e5d83c127..b78c3208e 100644 --- a/ov_eval/src/calc/ResultTrajectory.cpp +++ b/ov_eval/src/calc/ResultTrajectory.cpp @@ -20,7 +20,6 @@ */ #include "ResultTrajectory.h" -#include "utils/print.h" using namespace ov_eval; @@ -54,8 +53,8 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st AlignTrajectory::align_trajectory(gt_poses, est_poses, R_GTtoEST, t_GTinEST, s_GTtoEST, alignment_method); // Debug print to the user - Eigen::Vector4d q_ESTtoGT = Math::rot_2_quat(R_ESTtoGT); - Eigen::Vector4d q_GTtoEST = Math::rot_2_quat(R_GTtoEST); + Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT); + Eigen::Vector4d q_GTtoEST = ov_core::rot_2_quat(R_GTtoEST); PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); // PRINT_DEBUG("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s = @@ -65,9 +64,9 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st for (size_t i = 0; i < gt_times.size(); i++) { Eigen::Matrix pose_ESTinGT, pose_GTinEST; pose_ESTinGT.block(0, 0, 3, 1) = s_ESTtoGT * R_ESTtoGT * est_poses.at(i).block(0, 0, 3, 1) + t_ESTinGT; - pose_ESTinGT.block(3, 0, 4, 1) = Math::quat_multiply(est_poses.at(i).block(3, 0, 4, 1), Math::Inv(q_ESTtoGT)); + pose_ESTinGT.block(3, 0, 4, 1) = ov_core::quat_multiply(est_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_ESTtoGT)); pose_GTinEST.block(0, 0, 3, 1) = s_GTtoEST * R_GTtoEST * gt_poses.at(i).block(0, 0, 3, 1) + t_GTinEST; - pose_GTinEST.block(3, 0, 4, 1) = Math::quat_multiply(gt_poses.at(i).block(3, 0, 4, 1), Math::Inv(q_GTtoEST)); + pose_GTinEST.block(3, 0, 4, 1) = ov_core::quat_multiply(gt_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_GTtoEST)); est_poses_aignedtoGT.push_back(pose_ESTinGT); gt_poses_aignedtoEST.push_back(pose_GTinEST); } @@ -83,9 +82,9 @@ void ResultTrajectory::calculate_ate(Statistics &error_ori, Statistics &error_po for (size_t i = 0; i < est_poses_aignedtoGT.size(); i++) { // Calculate orientation error - Eigen::Matrix3d e_R = - Math::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() * Math::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)); - double ori_err = 180.0 / M_PI * Math::log_so3(e_R).norm(); + Eigen::Matrix3d e_R = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() * + ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)); + double ori_err = 180.0 / M_PI * ov_core::log_so3(e_R).norm(); // Calculate position error double pos_err = (gt_poses.at(i).block(0, 0, 3, 1) - est_poses_aignedtoGT.at(i).block(0, 0, 3, 1)).norm(); @@ -112,9 +111,9 @@ void ResultTrajectory::calculate_ate_2d(Statistics &error_ori, Statistics &error for (size_t i = 0; i < est_poses_aignedtoGT.size(); i++) { // Calculate orientation error - Eigen::Matrix3d e_R = - Math::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() * Math::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)); - double ori_err = 180.0 / M_PI * Math::log_so3(e_R)(2); + Eigen::Matrix3d e_R = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() * + ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)); + double ori_err = 180.0 / M_PI * ov_core::log_so3(e_R)(2); // Calculate position error double pos_err = (gt_poses.at(i).block(0, 0, 2, 1) - est_poses_aignedtoGT.at(i).block(0, 0, 2, 1)).norm(); @@ -179,34 +178,34 @@ void ResultTrajectory::calculate_rpe(const std::vector &segment_lengths, //=============================================================================== // Get T I1 to world EST at beginning of subtrajectory (at state idx) Eigen::Matrix4d T_c1 = Eigen::Matrix4d::Identity(); - T_c1.block(0, 0, 3, 3) = Math::quat_2_Rot(est_poses_aignedtoGT.at(id_start).block(3, 0, 4, 1)).transpose(); + T_c1.block(0, 0, 3, 3) = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(id_start).block(3, 0, 4, 1)).transpose(); T_c1.block(0, 3, 3, 1) = est_poses_aignedtoGT.at(id_start).block(0, 0, 3, 1); // Get T I2 to world EST at end of subtrajectory starting (at state comparisons[idx]) Eigen::Matrix4d T_c2 = Eigen::Matrix4d::Identity(); - T_c2.block(0, 0, 3, 3) = Math::quat_2_Rot(est_poses_aignedtoGT.at(id_end).block(3, 0, 4, 1)).transpose(); + T_c2.block(0, 0, 3, 3) = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(id_end).block(3, 0, 4, 1)).transpose(); T_c2.block(0, 3, 3, 1) = est_poses_aignedtoGT.at(id_end).block(0, 0, 3, 1); // Get T I2 to I1 EST - Eigen::Matrix4d T_c1_c2 = Math::Inv_se3(T_c1) * T_c2; + Eigen::Matrix4d T_c1_c2 = ov_core::Inv_se3(T_c1) * T_c2; //=============================================================================== // Get T I1 to world GT at beginning of subtrajectory (at state idx) Eigen::Matrix4d T_m1 = Eigen::Matrix4d::Identity(); - T_m1.block(0, 0, 3, 3) = Math::quat_2_Rot(gt_poses.at(id_start).block(3, 0, 4, 1)).transpose(); + T_m1.block(0, 0, 3, 3) = ov_core::quat_2_Rot(gt_poses.at(id_start).block(3, 0, 4, 1)).transpose(); T_m1.block(0, 3, 3, 1) = gt_poses.at(id_start).block(0, 0, 3, 1); // Get T I2 to world GT at end of subtrajectory starting (at state comparisons[idx]) Eigen::Matrix4d T_m2 = Eigen::Matrix4d::Identity(); - T_m2.block(0, 0, 3, 3) = Math::quat_2_Rot(gt_poses.at(id_end).block(3, 0, 4, 1)).transpose(); + T_m2.block(0, 0, 3, 3) = ov_core::quat_2_Rot(gt_poses.at(id_end).block(3, 0, 4, 1)).transpose(); T_m2.block(0, 3, 3, 1) = gt_poses.at(id_end).block(0, 0, 3, 1); // Get T I2 to I1 GT - Eigen::Matrix4d T_m1_m2 = Math::Inv_se3(T_m1) * T_m2; + Eigen::Matrix4d T_m1_m2 = ov_core::Inv_se3(T_m1) * T_m2; //=============================================================================== // Compute error transform between EST and GT start-end transform - Eigen::Matrix4d T_error_in_c2 = Math::Inv_se3(T_m1_m2) * T_c1_c2; + Eigen::Matrix4d T_error_in_c2 = ov_core::Inv_se3(T_m1_m2) * T_c1_c2; Eigen::Matrix4d T_c2_rot = Eigen::Matrix4d::Identity(); T_c2_rot.block(0, 0, 3, 3) = T_c2.block(0, 0, 3, 3); @@ -223,7 +222,7 @@ void ResultTrajectory::calculate_rpe(const std::vector &segment_lengths, error_pos.values.push_back(T_error_in_w.block(0, 3, 3, 1).norm()); // Calculate orientation error - double ori_err = 180.0 / M_PI * Math::log_so3(T_error_in_w.block(0, 0, 3, 3)).norm(); + double ori_err = 180.0 / M_PI * ov_core::log_so3(T_error_in_w.block(0, 0, 3, 3)).norm(); error_ori.timestamps.push_back(est_times.at(id_start)); error_ori.values.push_back(ori_err); } @@ -255,8 +254,8 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos // Calculate orientation error // NOTE: we define our error as e_R = -Log(R*Rhat^T) Eigen::Matrix3d e_R = - Math::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)) * Math::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose(); - Eigen::Vector3d errori = -Math::log_so3(e_R); + ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)) * ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose(); + Eigen::Vector3d errori = -ov_core::log_so3(e_R); // Eigen::Vector4d e_q = Math::quat_multiply(gt_poses_aignedtoEST.at(i).block(3,0,4,1),Math::Inv(est_poses.at(i).block(3,0,4,1))); // Eigen::Vector3d errori = 2*e_q.block(0,0,3,1); double ori_nees = errori.transpose() * est_covori.at(i).inverse() * errori; @@ -301,20 +300,21 @@ void ResultTrajectory::calculate_error(Statistics &posx, Statistics &posy, Stati for (size_t i = 0; i < est_poses.size(); i++) { // Calculate local orientation error, then propagate its covariance into the global frame of reference - Eigen::Vector4d e_q = Math::quat_multiply(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1), Math::Inv(est_poses.at(i).block(3, 0, 4, 1))); + Eigen::Vector4d e_q = + ov_core::quat_multiply(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1), ov_core::Inv(est_poses.at(i).block(3, 0, 4, 1))); Eigen::Vector3d errori_local = 2 * e_q.block(0, 0, 3, 1); - Eigen::Vector3d errori_global = Math::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * errori_local; + Eigen::Vector3d errori_global = ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * errori_local; Eigen::Matrix3d cov_global; if (est_times.size() == est_covori.size()) { - cov_global = Math::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * est_covori.at(i) * - Math::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)); + cov_global = ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * est_covori.at(i) * + ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)); } // Convert to roll pitch yaw (also need to "wrap" the error to -pi to pi) // NOTE: our rot2rpy is in the form R_input = R_z(yaw)*R_y(pitch)*R_x(roll) // NOTE: this error is in the "global" frame since we do rot2rpy on R_ItoG rotation - Eigen::Vector3d ypr_est_ItoG = Math::rot2rpy(Math::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose()); - Eigen::Vector3d ypr_gt_ItoG = Math::rot2rpy(Math::quat_2_Rot(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1)).transpose()); + Eigen::Vector3d ypr_est_ItoG = ov_core::rot2rpy(ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose()); + Eigen::Vector3d ypr_gt_ItoG = ov_core::rot2rpy(ov_core::quat_2_Rot(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1)).transpose()); Eigen::Vector3d errori_rpy = ypr_gt_ItoG - ypr_est_ItoG; for (size_t idx = 0; idx < 3; idx++) { while (errori_rpy(idx) < -M_PI) { @@ -330,8 +330,8 @@ void ResultTrajectory::calculate_error(Statistics &posx, Statistics &posy, Stati // NOTE: R*Exp(theta_erro) = Rz*Rz(error)*Ry*Ry(error)*Rx*Rx(error) // NOTE: example can be found here http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf Eigen::Matrix H_xyz2rpy = Eigen::Matrix::Identity(); - H_xyz2rpy.block(0, 1, 3, 1) = Math::rot_x(-ypr_est_ItoG(0)) * H_xyz2rpy.block(0, 1, 3, 1); - H_xyz2rpy.block(0, 2, 3, 1) = Math::rot_x(-ypr_est_ItoG(0)) * Math::rot_y(-ypr_est_ItoG(1)) * H_xyz2rpy.block(0, 2, 3, 1); + H_xyz2rpy.block(0, 1, 3, 1) = ov_core::rot_x(-ypr_est_ItoG(0)) * H_xyz2rpy.block(0, 1, 3, 1); + H_xyz2rpy.block(0, 2, 3, 1) = ov_core::rot_x(-ypr_est_ItoG(0)) * ov_core::rot_y(-ypr_est_ItoG(1)) * H_xyz2rpy.block(0, 2, 3, 1); Eigen::Matrix3d cov_rpy; if (est_times.size() == est_covori.size()) { cov_rpy = H_xyz2rpy.inverse() * est_covori.at(i) * H_xyz2rpy.inverse().transpose(); diff --git a/ov_eval/src/calc/ResultTrajectory.h b/ov_eval/src/calc/ResultTrajectory.h index e38982bc9..970eea0c1 100644 --- a/ov_eval/src/calc/ResultTrajectory.h +++ b/ov_eval/src/calc/ResultTrajectory.h @@ -33,11 +33,13 @@ #include #include "alignment/AlignTrajectory.h" -#include "utils/Colors.h" #include "utils/Loader.h" -#include "utils/Math.h" #include "utils/Statistics.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + namespace ov_eval { /** diff --git a/ov_eval/src/error_comparison.cpp b/ov_eval/src/error_comparison.cpp index cc7596fdf..4626dfb90 100644 --- a/ov_eval/src/error_comparison.cpp +++ b/ov_eval/src/error_comparison.cpp @@ -26,8 +26,8 @@ #include #include "calc/ResultTrajectory.h" -#include "utils/Colors.h" #include "utils/Loader.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -41,6 +41,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 4) { PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); @@ -236,56 +239,56 @@ int main(int argc, char **argv) { //=============================================================================== // Finally print the ATE for all the runs - PRINT_DEBUG("============================================\n"); - PRINT_DEBUG("ATE LATEX TABLE\n"); - PRINT_DEBUG("============================================\n"); + PRINT_INFO("============================================\n"); + PRINT_INFO("ATE LATEX TABLE\n"); + PRINT_INFO("============================================\n"); for (size_t i = 0; i < path_groundtruths.size(); i++) { std::string gtname = path_groundtruths.at(i).stem().string(); boost::replace_all(gtname, "_", "\\_"); - PRINT_DEBUG(" & \\textbf{%s}", gtname.c_str()); + PRINT_INFO(" & \\textbf{%s}", gtname.c_str()); } - PRINT_DEBUG(" & \\textbf{Average} \\\\\\hline\n"); + PRINT_INFO(" & \\textbf{Average} \\\\\\hline\n"); for (auto &algo : algo_ate) { std::string algoname = algo.first; boost::replace_all(algoname, "_", "\\_"); - PRINT_DEBUG(algoname.c_str()); + PRINT_INFO(algoname.c_str()); double sum_ori = 0.0; double sum_pos = 0.0; int sum_ct = 0; for (auto &seg : algo.second) { if (seg.first.values.empty() || seg.second.values.empty()) { - PRINT_DEBUG(" & - / -"); + PRINT_INFO(" & - / -"); } else { - PRINT_DEBUG(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse); + PRINT_INFO(" & %.3f / %.3f", seg.first.rmse, seg.second.rmse); sum_ori += seg.first.rmse; sum_pos += seg.second.rmse; sum_ct++; } } - PRINT_DEBUG(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct); + PRINT_INFO(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct); } - PRINT_DEBUG("============================================\n"); + PRINT_INFO("============================================\n"); // Finally print the RPE for all the runs - PRINT_DEBUG("============================================\n"); - PRINT_DEBUG("RPE LATEX TABLE\n"); - PRINT_DEBUG("============================================\n"); + PRINT_INFO("============================================\n"); + PRINT_INFO("RPE LATEX TABLE\n"); + PRINT_INFO("============================================\n"); for (const auto &len : segments) { - PRINT_DEBUG(" & \\textbf{%dm}", (int)len); + PRINT_INFO(" & \\textbf{%dm}", (int)len); } - PRINT_DEBUG(" \\\\\\hline\n"); + PRINT_INFO(" \\\\\\hline\n"); for (auto &algo : algo_rpe) { std::string algoname = algo.first; boost::replace_all(algoname, "_", "\\_"); - PRINT_DEBUG(algoname.c_str()); + PRINT_INFO(algoname.c_str()); for (auto &seg : algo.second) { seg.second.first.calculate(); seg.second.second.calculate(); - PRINT_DEBUG(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median); + PRINT_INFO(" & %.3f / %.3f", seg.second.first.median, seg.second.second.median); } - PRINT_DEBUG(" \\\\\n"); + PRINT_INFO(" \\\\\n"); } - PRINT_DEBUG("============================================\n"); + PRINT_INFO("============================================\n"); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/error_dataset.cpp b/ov_eval/src/error_dataset.cpp index 62188b143..a301c23e1 100644 --- a/ov_eval/src/error_dataset.cpp +++ b/ov_eval/src/error_dataset.cpp @@ -24,8 +24,8 @@ #include #include "calc/ResultTrajectory.h" -#include "utils/Colors.h" #include "utils/Loader.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -39,6 +39,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 4) { PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET); @@ -84,8 +87,8 @@ int main(int argc, char **argv) { for (size_t i = 0; i < path_algorithms.size(); i++) { // Debug print - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); + PRINT_INFO("======================================\n"); + PRINT_INFO("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str()); // Get the list of datasets this algorithm records std::map path_algo_datasets; @@ -97,7 +100,7 @@ int main(int argc, char **argv) { // Check if we have runs for our dataset if (path_algo_datasets.find(path_gt.stem().string()) == path_algo_datasets.end()) { - PRINT_DEBUG(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), + PRINT_ERROR(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(), path_gt.stem().c_str()); continue; } @@ -189,17 +192,17 @@ int main(int argc, char **argv) { // Print stats for this specific dataset std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : ""; - PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, - (int)ate_dataset_ori.values.size()); - PRINT_DEBUG("\tATE: std_ori = %.5f | std_pos = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std); - PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean, - (int)ate_2d_dataset_ori.values.size()); - PRINT_DEBUG("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std); + PRINT_INFO("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean, + (int)ate_dataset_ori.values.size()); + PRINT_INFO("\tATE: std_ori = %.5f | std_pos = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std); + PRINT_INFO("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean, + (int)ate_2d_dataset_ori.values.size()); + PRINT_INFO("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std); for (auto &seg : rpe_dataset) { seg.second.first.calculate(); seg.second.second.calculate(); - PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean, - seg.second.second.mean, (int)seg.second.second.values.size()); + PRINT_INFO("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean, + seg.second.second.mean, (int)seg.second.second.values.size()); // PRINT_DEBUG("RPE: seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); } @@ -217,7 +220,7 @@ int main(int argc, char **argv) { } rmse_ori.calculate(); rmse_pos.calculate(); - PRINT_DEBUG("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean); + PRINT_INFO("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean); // RMSE: Convert into the right format (only use times where all runs have an error) ov_eval::Statistics rmse_2d_ori, rmse_2d_pos; @@ -233,7 +236,7 @@ int main(int argc, char **argv) { } rmse_2d_ori.calculate(); rmse_2d_pos.calculate(); - PRINT_DEBUG("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean); + PRINT_INFO("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean); // NEES: Convert into the right format (only use times where all runs have an error) ov_eval::Statistics nees_ori, nees_pos; @@ -249,7 +252,7 @@ int main(int argc, char **argv) { } nees_ori.calculate(); nees_pos.calculate(); - PRINT_DEBUG("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); + PRINT_INFO("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/error_simulation.cpp b/ov_eval/src/error_simulation.cpp index 2f732dcd2..6a08c0fc0 100644 --- a/ov_eval/src/error_simulation.cpp +++ b/ov_eval/src/error_simulation.cpp @@ -20,7 +20,7 @@ */ #include "calc/ResultSimulation.h" -#include "utils/Colors.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -34,6 +34,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 4) { PRINT_ERROR(RED "ERROR: ./error_simulation \n" RESET); diff --git a/ov_eval/src/error_singlerun.cpp b/ov_eval/src/error_singlerun.cpp index 90699360d..a587a43c3 100644 --- a/ov_eval/src/error_singlerun.cpp +++ b/ov_eval/src/error_singlerun.cpp @@ -24,7 +24,7 @@ #include #include "calc/ResultTrajectory.h" -#include "utils/Colors.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -84,6 +84,9 @@ void plot_3errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Stati int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 4) { PRINT_ERROR(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET); @@ -114,14 +117,14 @@ int main(int argc, char **argv) { traj.calculate_ate(error_ori, error_pos); // Print it - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("Absolute Trajectory Error\n"); - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse); - PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean); - PRINT_DEBUG("min_ori = %.3f | min_pos = %.3f\n", error_ori.min, error_pos.min); - PRINT_DEBUG("max_ori = %.3f | max_pos = %.3f\n", error_ori.max, error_pos.max); - PRINT_DEBUG("std_ori = %.3f | std_pos = %.3f\n", error_ori.std, error_pos.std); + PRINT_INFO("======================================\n"); + PRINT_INFO("Absolute Trajectory Error\n"); + PRINT_INFO("======================================\n"); + PRINT_INFO("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse); + PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean); + PRINT_INFO("min_ori = %.3f | min_pos = %.3f\n", error_ori.min, error_pos.min); + PRINT_INFO("max_ori = %.3f | max_pos = %.3f\n", error_ori.max, error_pos.max); + PRINT_INFO("std_ori = %.3f | std_pos = %.3f\n", error_ori.std, error_pos.std); //=========================================================== // Relative pose error @@ -133,12 +136,12 @@ int main(int argc, char **argv) { traj.calculate_rpe(segments, error_rpe); // Print it - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("Relative Pose Error\n"); - PRINT_DEBUG("======================================\n"); + PRINT_INFO("======================================\n"); + PRINT_INFO("Relative Pose Error\n"); + PRINT_INFO("======================================\n"); for (const auto &seg : error_rpe) { - PRINT_DEBUG("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median, - seg.second.second.median, (int)seg.second.second.values.size()); + PRINT_INFO("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median, + seg.second.second.median, (int)seg.second.second.values.size()); // PRINT_DEBUG("seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std); } @@ -199,14 +202,14 @@ int main(int argc, char **argv) { traj.calculate_nees(nees_ori, nees_pos); // Print it - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("Normalized Estimation Error Squared\n"); - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); - PRINT_DEBUG("min_ori = %.3f | min_pos = %.3f\n", nees_ori.min, nees_pos.min); - PRINT_DEBUG("max_ori = %.3f | max_pos = %.3f\n", nees_ori.max, nees_pos.max); - PRINT_DEBUG("std_ori = %.3f | std_pos = %.3f\n", nees_ori.std, nees_pos.std); - PRINT_DEBUG("======================================\n"); + PRINT_INFO("======================================\n"); + PRINT_INFO("Normalized Estimation Error Squared\n"); + PRINT_INFO("======================================\n"); + PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean); + PRINT_INFO("min_ori = %.3f | min_pos = %.3f\n", nees_ori.min, nees_pos.min); + PRINT_INFO("max_ori = %.3f | max_pos = %.3f\n", nees_ori.max, nees_pos.max); + PRINT_INFO("std_ori = %.3f | std_pos = %.3f\n", nees_ori.std, nees_pos.std); + PRINT_INFO("======================================\n"); #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/format_converter.cpp b/ov_eval/src/format_converter.cpp index f4bc52186..fc3c98586 100644 --- a/ov_eval/src/format_converter.cpp +++ b/ov_eval/src/format_converter.cpp @@ -27,7 +27,7 @@ #include #include -#include "utils/Colors.h" +#include "utils/colors.h" #include "utils/print.h" /** @@ -35,6 +35,9 @@ */ void process_csv(std::string infile) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Check if file paths are good std::ifstream file1; std::string line; diff --git a/ov_eval/src/live_align_trajectory.cpp b/ov_eval/src/live_align_trajectory.cpp index 44cf45917..011514fda 100644 --- a/ov_eval/src/live_align_trajectory.cpp +++ b/ov_eval/src/live_align_trajectory.cpp @@ -25,10 +25,10 @@ #include "alignment/AlignTrajectory.h" #include "alignment/AlignUtils.h" -#include "utils/Colors.h" #include "utils/Loader.h" -#include "utils/Math.h" +#include "utils/colors.h" #include "utils/print.h" +#include "utils/quat_ops.h" ros::Publisher pub_path; void align_and_publish(const nav_msgs::Path::ConstPtr &msg); @@ -42,6 +42,11 @@ int main(int argc, char **argv) { ros::init(argc, argv, "live_align_trajectory"); ros::NodeHandle nh("~"); + // Verbosity setting + std::string verbosity; + nh.param("verbosity", verbosity, "INFO"); + ov_core::Printer::setPrintLevel(verbosity); + // Load what type of alignment we should use nh.param("alignment_type", alignment_type, "posyaw"); @@ -104,7 +109,7 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) { Eigen::Vector3d t_ESTinGT; double s_ESTtoGT; ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type); - Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT); + Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT); PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); @@ -120,7 +125,7 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) { double timestamp = gt_times_temp.at(i); Eigen::Matrix pose_inGT = gt_poses_temp.at(i); Eigen::Vector3d pos_IinEST = R_ESTtoGT.transpose() * (pose_inGT.block(0, 0, 3, 1) - t_ESTinGT) / s_ESTtoGT; - Eigen::Vector4d quat_ESTtoI = ov_eval::Math::quat_multiply(pose_inGT.block(3, 0, 4, 1), q_ESTtoGT); + Eigen::Vector4d quat_ESTtoI = ov_core::quat_multiply(pose_inGT.block(3, 0, 4, 1), q_ESTtoGT); // Finally push back geometry_msgs::PoseStamped posetemp; posetemp.header = msg->header; diff --git a/ov_eval/src/plot_trajectories.cpp b/ov_eval/src/plot_trajectories.cpp index 7e8ac673b..7e889ca51 100644 --- a/ov_eval/src/plot_trajectories.cpp +++ b/ov_eval/src/plot_trajectories.cpp @@ -29,10 +29,10 @@ #include "alignment/AlignTrajectory.h" #include "alignment/AlignUtils.h" -#include "utils/Colors.h" #include "utils/Loader.h" -#include "utils/Math.h" +#include "utils/colors.h" #include "utils/print.h" +#include "utils/quat_ops.h" #ifdef HAVE_PYTHONLIBS @@ -86,11 +86,14 @@ void plot_z_positions(const std::string &name, const std::string &color, const s int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 3) { - PRINT_DEBUG(RED "ERROR: Please specify a align mode and trajectory file\n" RESET); - PRINT_DEBUG(RED "ERROR: ./plot_trajectories ... \n" RESET); - PRINT_DEBUG(RED "ERROR: rosrun ov_eval plot_trajectories ... \n" RESET); + PRINT_ERROR(RED "ERROR: Please specify a align mode and trajectory file\n" RESET); + PRINT_ERROR(RED "ERROR: ./plot_trajectories ... \n" RESET); + PRINT_ERROR(RED "ERROR: rosrun ov_eval plot_trajectories ... \n" RESET); std::exit(EXIT_FAILURE); } @@ -116,8 +119,8 @@ int main(int argc, char **argv) { // Return failure if we didn't have any common timestamps if (poses_temp.size() < 3) { - PRINT_DEBUG(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); - PRINT_DEBUG(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); + PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET); + PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET); std::exit(EXIT_FAILURE); } @@ -128,7 +131,7 @@ int main(int argc, char **argv) { ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, argv[1]); // Debug print to the user - Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT); + Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT); PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1), q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT); @@ -137,7 +140,7 @@ int main(int argc, char **argv) { for (size_t j = 0; j < gt_times_temp.size(); j++) { Eigen::Matrix pose_ESTinGT; pose_ESTinGT.block(0, 0, 3, 1) = s_ESTtoGT * R_ESTtoGT * poses_temp.at(j).block(0, 0, 3, 1) + t_ESTinGT; - pose_ESTinGT.block(3, 0, 4, 1) = ov_eval::Math::quat_multiply(poses_temp.at(j).block(3, 0, 4, 1), ov_eval::Math::Inv(q_ESTtoGT)); + pose_ESTinGT.block(3, 0, 4, 1) = ov_core::quat_multiply(poses_temp.at(j).block(3, 0, 4, 1), ov_core::Inv(q_ESTtoGT)); est_poses_aignedtoGT.push_back(pose_ESTinGT); } @@ -149,7 +152,7 @@ int main(int argc, char **argv) { boost::filesystem::path path(argv[i]); std::string name = path.stem().string(); double length = ov_eval::Loader::get_total_length(poses_temp); - PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length); + PRINT_INFO("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length); // Save this to our arrays names.push_back(name); diff --git a/ov_eval/src/pose_to_file.cpp b/ov_eval/src/pose_to_file.cpp index a96036059..7ab6e78d5 100644 --- a/ov_eval/src/pose_to_file.cpp +++ b/ov_eval/src/pose_to_file.cpp @@ -26,6 +26,7 @@ #include #include "utils/Recorder.h" +#include "utils/print.h" int main(int argc, char **argv) { @@ -33,6 +34,11 @@ int main(int argc, char **argv) { ros::init(argc, argv, "pose_to_file"); ros::NodeHandle nh("~"); + // Verbosity setting + std::string verbosity; + nh.param("verbosity", verbosity, "INFO"); + ov_core::Printer::setPrintLevel(verbosity); + // Get parameters to subscribe std::string topic, topic_type, fileoutput; nh.getParam("topic", topic); @@ -40,10 +46,10 @@ int main(int argc, char **argv) { nh.getParam("output", fileoutput); // Debug - ROS_INFO("Done reading config values"); - ROS_INFO(" - topic = %s", topic.c_str()); - ROS_INFO(" - topic_type = %s", topic_type.c_str()); - ROS_INFO(" - file = %s", fileoutput.c_str()); + PRINT_DEBUG("Done reading config values"); + PRINT_DEBUG(" - topic = %s", topic.c_str()); + PRINT_DEBUG(" - topic_type = %s", topic_type.c_str()); + PRINT_DEBUG(" - file = %s", fileoutput.c_str()); // Create the recorder object ov_eval::Recorder recorder(fileoutput); @@ -59,9 +65,9 @@ int main(int argc, char **argv) { } else if (topic_type == std::string("Odometry")) { sub = nh.subscribe(topic, 9999, &ov_eval::Recorder::callback_odometry, &recorder); } else { - ROS_ERROR("The specified topic type is not supported"); - ROS_ERROR("topic_type = %s", topic_type.c_str()); - ROS_ERROR("please select from: PoseWithCovarianceStamped, PoseStamped, TransformStamped, Odometry"); + PRINT_ERROR("The specified topic type is not supported"); + PRINT_ERROR("topic_type = %s", topic_type.c_str()); + PRINT_ERROR("please select from: PoseWithCovarianceStamped, PoseStamped, TransformStamped, Odometry"); std::exit(EXIT_FAILURE); } diff --git a/ov_eval/src/timing_comparison.cpp b/ov_eval/src/timing_comparison.cpp index f5b161d84..f86a9c776 100644 --- a/ov_eval/src/timing_comparison.cpp +++ b/ov_eval/src/timing_comparison.cpp @@ -27,9 +27,9 @@ #include #include -#include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -43,6 +43,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 2) { PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET); @@ -54,13 +57,13 @@ int main(int argc, char **argv) { // Read in all our trajectories from file std::vector names; std::vector total_times; + PRINT_INFO("======================================\n"); for (int z = 1; z < argc; z++) { // Parse the name of this timing boost::filesystem::path path(argv[z]); std::string name = path.stem().string(); - PRINT_DEBUG("======================================\n"); - PRINT_DEBUG("[TIME]: loading data for %s\n", name.c_str()); + PRINT_INFO("[TIME]: loading data for %s\n", name.c_str()); // Load it!! std::vector names_temp; @@ -85,8 +88,8 @@ int main(int argc, char **argv) { // Now print the statistic for this run for (size_t i = 0; i < names_temp.size(); i++) { stats.at(i).calculate(); - PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, - stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str()); + PRINT_INFO("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, + stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str()); } // Append the total stats to the big vector @@ -94,9 +97,9 @@ int main(int argc, char **argv) { names.push_back(name); total_times.push_back(stats.at(stats.size() - 1)); } else { - PRINT_DEBUG(RED "[TIME]: unable to load any data.....\n" RESET); + PRINT_ERROR(RED "[TIME]: unable to load any data.....\n" RESET); } - PRINT_DEBUG("======================================\n"); + PRINT_INFO("======================================\n"); } #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/timing_flamegraph.cpp b/ov_eval/src/timing_flamegraph.cpp index f374cb7af..99821bbe1 100644 --- a/ov_eval/src/timing_flamegraph.cpp +++ b/ov_eval/src/timing_flamegraph.cpp @@ -27,9 +27,9 @@ #include #include -#include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -43,6 +43,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("INFO"); + // Ensure we have a path if (argc < 2) { PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET); @@ -56,7 +59,7 @@ int main(int argc, char **argv) { std::vector times; std::vector timing_values; ov_eval::Loader::load_timing_flamegraph(argv[1], names, times, timing_values); - PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size()); + PRINT_INFO("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size()); // Our categories std::vector stats; @@ -74,8 +77,8 @@ int main(int argc, char **argv) { // Now print the statistic for this run for (size_t i = 0; i < names.size(); i++) { stats.at(i).calculate(); - PRINT_DEBUG("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, - stats.at(i).ninetynine, stats.at(i).max, names.at(i).c_str()); + PRINT_INFO("mean_time = %.4f | std = %.4f | 99th = %.4f | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std, + stats.at(i).ninetynine, stats.at(i).max, names.at(i).c_str()); } #ifdef HAVE_PYTHONLIBS diff --git a/ov_eval/src/timing_percentages.cpp b/ov_eval/src/timing_percentages.cpp index e163350fc..3378f9906 100644 --- a/ov_eval/src/timing_percentages.cpp +++ b/ov_eval/src/timing_percentages.cpp @@ -27,9 +27,9 @@ #include #include -#include "utils/Colors.h" #include "utils/Loader.h" #include "utils/Statistics.h" +#include "utils/colors.h" #include "utils/print.h" #ifdef HAVE_PYTHONLIBS @@ -43,6 +43,9 @@ int main(int argc, char **argv) { + // Verbosity setting + ov_core::Printer::setPrintLevel("ALL"); + // Ensure we have a path if (argc < 2) { PRINT_ERROR(RED "ERROR: Please specify a timing and memory percent folder\n" RESET); diff --git a/ov_eval/src/utils/Colors.h b/ov_eval/src/utils/Colors.h deleted file mode 100644 index 033c51ad7..000000000 --- a/ov_eval/src/utils/Colors.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2021 Patrick Geneva - * Copyright (C) 2021 Guoquan Huang - * Copyright (C) 2021 OpenVINS Contributors - * Copyright (C) 2019 Kevin Eckenhoff - * - * This program 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. - * - * This program 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 this program. If not, see . - */ - -#ifndef OV_CORE_COLOR_MACROS -#define OV_CORE_COLOR_MACROS - -#define RESET "\033[0m" -#define BLACK "\033[30m" /* Black */ -#define RED "\033[31m" /* Red */ -#define GREEN "\033[32m" /* Green */ -#define YELLOW "\033[33m" /* Yellow */ -#define BLUE "\033[34m" /* Blue */ -#define MAGENTA "\033[35m" /* Magenta */ -#define CYAN "\033[36m" /* Cyan */ -#define WHITE "\033[37m" /* White */ -#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ -#define BOLDRED "\033[1m\033[31m" /* Bold Red */ -#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ -#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ -#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ -#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ -#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ -#define BOLDWHITE "\033[1m\033[37m" /* Bold White */ - -#endif /* OV_CORE_COLOR_MACROS */ diff --git a/ov_eval/src/utils/Loader.cpp b/ov_eval/src/utils/Loader.cpp index 9f7e0edf3..e1413796f 100644 --- a/ov_eval/src/utils/Loader.cpp +++ b/ov_eval/src/utils/Loader.cpp @@ -20,7 +20,6 @@ */ #include "Loader.h" -#include "utils/print.h" using namespace ov_eval; diff --git a/ov_eval/src/utils/Loader.h b/ov_eval/src/utils/Loader.h index d06a54fc8..0fb1cabd2 100644 --- a/ov_eval/src/utils/Loader.h +++ b/ov_eval/src/utils/Loader.h @@ -29,7 +29,8 @@ #include #include -#include "Colors.h" +#include "utils/colors.h" +#include "utils/print.h" namespace ov_eval { diff --git a/ov_eval/src/utils/Math.h b/ov_eval/src/utils/Math.h deleted file mode 100644 index e37439a2d..000000000 --- a/ov_eval/src/utils/Math.h +++ /dev/null @@ -1,518 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2021 Patrick Geneva - * Copyright (C) 2021 Guoquan Huang - * Copyright (C) 2021 OpenVINS Contributors - * Copyright (C) 2019 Kevin Eckenhoff - * - * This program 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. - * - * This program 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 this program. If not, see . - */ - -#ifndef OV_EVAL_MATH_H -#define OV_EVAL_MATH_H - -#include -#include -#include -#include - -#include "utils/print.h" - -using namespace std; - -namespace ov_eval { - -/** - * @brief JPL quaternion math utilities - * - * This file contains the common utility functions for operating on JPL quaternions. - * We need to take special care to handle edge cases when converting to and from other rotation formats. - * All equations are based on the following tech report: - * > Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation." - * > University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005. - * > http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf - * - */ -class Math { - -public: - /** - * @brief Returns a JPL quaternion from a rotation matrix - * - * This is based on the equation 74 in [Indirect Kalman Filter for 3D Attitude - * Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). In the implementation, we have 4 statements so that we avoid a division - * by zero and instead always divide by the largest diagonal element. This all comes from the definition of a rotation matrix, using the - * diagonal elements and an off-diagonal. \f{align*}{ \mathbf{R}(\bar{q})= \begin{bmatrix} - * q_1^2-q_2^2-q_3^2+q_4^2 & 2(q_1q_2+q_3q_4) & 2(q_1q_3-q_2q_4) \\ - * 2(q_1q_2-q_3q_4) & -q_2^2+q_2^2-q_3^2+q_4^2 & 2(q_2q_3+q_1q_4) \\ - * 2(q_1q_3+q_2q_4) & 2(q_2q_3-q_1q_4) & -q_1^2-q_2^2+q_3^2+q_4^2 - * \end{bmatrix} - * \f} - * - * @param[in] rot 3x3 rotation matrix - * @return 4x1 quaternion - */ - static inline Eigen::Matrix rot_2_quat(const Eigen::Matrix &rot) { - Eigen::Matrix q; - double T = rot.trace(); - if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) { - // PRINT_DEBUG(("case 1- \n"); - q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4); - q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0)); - q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0)); - q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1)); - - } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) { - // PRINT_DEBUG(("case 2- \n"); - q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4); - q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0)); - q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1)); - q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2)); - } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) { - // PRINT_DEBUG(("case 3- \n"); - q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4); - q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0)); - q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1)); - q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0)); - } else { - // PRINT_DEBUG(("case 4- \n"); - q(3) = sqrt((1 + T) / 4); - q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1)); - q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2)); - q(2) = (1 / (4 * q(3))) * (rot(0, 1) - rot(1, 0)); - } - if (q(3) < 0) { - q = -q; - } - // normalize and return - q = q / (q.norm()); - return q; - } - - /** - * @brief Skew-symmetric matrix from a given 3x1 vector - * - * This is based on equation 6 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf): - * \f{align*}{ - * \lfloor\mathbf{v}\times\rfloor = - * \begin{bmatrix} - * 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 - * \end{bmatrix} - * @f} - * - * @param[in] w 3x1 vector to be made a skew-symmetric - * @return 3x3 skew-symmetric matrix - */ - static inline Eigen::Matrix skew_x(const Eigen::Matrix &w) { - Eigen::Matrix w_x; - w_x << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0; - return w_x; - } - - /** - * @brief Converts JPL quaterion to SO(3) rotation matrix - * - * This is based on equation 62 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf): - * \f{align*}{ - * \mathbf{R} = (2q_4^2-1)\mathbf{I}_3-2q_4\lfloor\mathbf{q}\times\rfloor+2\mathbf{q}^\top\mathbf{q} - * @f} - * - * @param[in] q JPL quaternion - * @return 3x3 SO(3) rotation matrix - */ - static inline Eigen::Matrix quat_2_Rot(const Eigen::Matrix &q) { - Eigen::Matrix q_x = skew_x(q.block(0, 0, 3, 1)); - Eigen::MatrixXd Rot = (2 * std::pow(q(3, 0), 2) - 1) * Eigen::MatrixXd::Identity(3, 3) - 2 * q(3, 0) * q_x + - 2 * q.block(0, 0, 3, 1) * (q.block(0, 0, 3, 1).transpose()); - return Rot; - } - - /** - * @brief Multiply two JPL quaternions - * - * This is based on equation 9 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). - * We also enforce that the quaternion is unique by having q_4 be greater than zero. - * \f{align*}{ - * \bar{q}\otimes\bar{p}= - * \mathcal{L}(\bar{q})\bar{p}= - * \begin{bmatrix} - * q_4\mathbf{I}_3+\lfloor\mathbf{q}\times\rfloor & \mathbf{q} \\ - * -\mathbf{q}^\top & q_4 - * \end{bmatrix} - * \begin{bmatrix} - * \mathbf{p} \\ p_4 - * \end{bmatrix} - * @f} - * - * @param[in] q First JPL quaternion - * @param[in] p Second JPL quaternion - * @return 4x1 resulting p*q quaternion - */ - static inline Eigen::Matrix quat_multiply(const Eigen::Matrix &q, const Eigen::Matrix &p) { - Eigen::Matrix q_t; - Eigen::Matrix Qm; - // create big L matrix - Qm.block(0, 0, 3, 3) = q(3, 0) * Eigen::MatrixXd::Identity(3, 3) - skew_x(q.block(0, 0, 3, 1)); - Qm.block(0, 3, 3, 1) = q.block(0, 0, 3, 1); - Qm.block(3, 0, 1, 3) = -q.block(0, 0, 3, 1).transpose(); - Qm(3, 3) = q(3, 0); - q_t = Qm * p; - // ensure unique by forcing q_4 to be >0 - if (q_t(3, 0) < 0) { - q_t *= -1; - } - // normalize and return - return q_t / q_t.norm(); - } - - /** - * @brief Returns vector portion of skew-symmetric - * - * See skew_x() for details. - * - * @param[in] w_x skew-symmetric matrix - * @return 3x1 vector portion of skew - */ - static inline Eigen::Matrix vee(const Eigen::Matrix &w_x) { - Eigen::Matrix w; - w << w_x(2, 1), w_x(0, 2), w_x(1, 0); - return w; - } - - /** - * @brief SO(3) matrix exponential - * - * SO(3) matrix exponential mapping from the vector to SO(3) lie group. - * This formula ends up being the [Rodrigues formula](https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula). - * This definition was taken from "Lie Groups for 2D and 3D Transformations" by Ethan Eade equation 15. - * http://ethaneade.com/lie.pdf - * - * \f{align*}{ - * \exp\colon\mathfrak{so}(3)&\to SO(3) \\ - * \exp(\mathbf{v}) &= - * \mathbf{I} - * +\frac{\sin{\theta}}{\theta}\lfloor\mathbf{v}\times\rfloor - * +\frac{1-\cos{\theta}}{\theta^2}\lfloor\mathbf{v}\times\rfloor^2 \\ - * \mathrm{where}&\quad \theta^2 = \mathbf{v}^\top\mathbf{v} - * @f} - * - * @param[in] w 3x1 vector we will take the exponential of - * @return SO(3) rotation matrix - */ - static inline Eigen::Matrix exp_so3(const Eigen::Matrix &w) { - // get theta - Eigen::Matrix w_x = skew_x(w); - double theta = w.norm(); - // Handle small angle values - double A, B; - if (theta < 1e-12) { - A = 1; - B = 0.5; - } else { - A = sin(theta) / theta; - B = (1 - cos(theta)) / (theta * theta); - } - // compute so(3) rotation - Eigen::Matrix R; - if (theta == 0) { - R = Eigen::MatrixXd::Identity(3, 3); - } else { - R = Eigen::MatrixXd::Identity(3, 3) + A * w_x + B * w_x * w_x; - } - return R; - } - - /** - * @brief SO(3) matrix logarithm - * - * This definition was taken from "Lie Groups for 2D and 3D Transformations" by Ethan Eade equation 17 & 18. - * http://ethaneade.com/lie.pdf - * \f{align*}{ - * \theta &= \textrm{arccos}(0.5(\textrm{trace}(\mathbf{R})-1)) \\ - * \lfloor\mathbf{v}\times\rfloor &= \frac{\theta}{2\sin{\theta}}(\mathbf{R}-\mathbf{R}^\top) - * @f} - * - * @param[in] R 3x3 SO(3) rotation matrix - * @return 3x1 in the se(3) space [omegax, omegay, omegaz] - */ - static inline Eigen::Matrix log_so3(const Eigen::Matrix &R) { - // magnitude of the skew elements (handle edge case where we sometimes have a>1...) - double a = 0.5 * (R.trace() - 1); - double theta = (a > 1) ? acos(1) : ((a < -1) ? acos(-1) : acos(a)); - // Handle small angle values - double D; - if (theta < 1e-12) { - D = 0.5; - } else { - D = theta / (2 * sin(theta)); - } - // calculate the skew symetric matrix - Eigen::Matrix w_x = D * (R - R.transpose()); - // check if we are near the identity - if (R != Eigen::MatrixXd::Identity(3, 3)) { - Eigen::Vector3d vec; - vec << w_x(2, 1), w_x(0, 2), w_x(1, 0); - return vec; - } else { - return Eigen::Vector3d::Zero(); - } - } - - /** - * @brief SE(3) matrix exponential function - * - * Equation is from Ethan Eade's reference: http://ethaneade.com/lie.pdf - * \f{align*}{ - * \exp([\boldsymbol\omega,\mathbf u])&=\begin{bmatrix} \mathbf R & \mathbf V \mathbf u \\ \mathbf 0 & 1 \end{bmatrix} \\[1em] - * \mathbf R &= \mathbf I + A \lfloor \boldsymbol\omega \times\rfloor + B \lfloor \boldsymbol\omega \times\rfloor^2 \\ - * \mathbf V &= \mathbf I + B \lfloor \boldsymbol\omega \times\rfloor + C \lfloor \boldsymbol\omega \times\rfloor^2 - * \f} - * where we have the following definitions - * \f{align*}{ - * \theta &= \sqrt{\boldsymbol\omega^\top\boldsymbol\omega} \\ - * A &= \sin\theta/\theta \\ - * B &= (1-\cos\theta)/\theta^2 \\ - * C &= (1-A)/\theta^2 - * \f} - * - * @param vec 6x1 in the se(3) space [omega, u] - * @return 4x4 SE(3) matrix - */ - static inline Eigen::Matrix4d exp_se3(Eigen::Matrix vec) { - - // Precompute our values - Eigen::Vector3d w = vec.head(3); - Eigen::Vector3d u = vec.tail(3); - double theta = sqrt(w.dot(w)); - Eigen::Matrix3d wskew; - wskew << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0; - - // Handle small angle values - double A, B, C; - if (theta < 1e-12) { - A = 1; - B = 0.5; - C = 1.0 / 6.0; - } else { - A = sin(theta) / theta; - B = (1 - cos(theta)) / (theta * theta); - C = (1 - A) / (theta * theta); - } - - // Matrices we need V and Identity - Eigen::Matrix3d I_33 = Eigen::Matrix3d::Identity(); - Eigen::Matrix3d V = I_33 + B * wskew + C * wskew * wskew; - - // Get the final matrix to return - Eigen::Matrix4d mat = Eigen::Matrix4d::Zero(); - mat.block(0, 0, 3, 3) = I_33 + A * wskew + B * wskew * wskew; - mat.block(0, 3, 3, 1) = V * u; - mat(3, 3) = 1; - return mat; - } - - /** - * @brief SE(3) matrix logarithm - * - * Equation is from Ethan Eade's reference: http://ethaneade.com/lie.pdf - * \f{align*}{ - * \boldsymbol\omega &=\mathrm{skew\_offdiags}\Big(\frac{\theta}{2\sin\theta}(\mathbf R - \mathbf R^\top)\Big) \\ - * \mathbf u &= \mathbf V^{-1}\mathbf t - * \f} - * where we have the following definitions - * \f{align*}{ - * \theta &= \mathrm{arccos}((\mathrm{tr}(\mathbf R)-1)/2) \\ - * \mathbf V^{-1} &= \mathbf I - \frac{1}{2} \lfloor \boldsymbol\omega \times\rfloor + \frac{1}{\theta^2}\Big(1-\frac{A}{2B}\Big)\lfloor - * \boldsymbol\omega \times\rfloor^2 \f} - * - * @param mat 4x4 SE(3) matrix - * @return 6x1 in the se(3) space [omega, u] - */ - static inline Eigen::Matrix log_se3(Eigen::Matrix4d mat) { - - // Get sub-matrices - Eigen::Matrix3d R = mat.block(0, 0, 3, 3); - Eigen::Vector3d t = mat.block(0, 3, 3, 1); - - // Get theta (handle edge case where we sometimes have a>1...) - double a = 0.5 * (R.trace() - 1); - double theta = (a > 1) ? acos(1) : ((a < -1) ? acos(-1) : acos(a)); - - // Handle small angle values - double A, B, D, E; - if (theta < 1e-12) { - A = 1; - B = 0.5; - D = 0.5; - E = 1.0 / 12.0; - } else { - A = sin(theta) / theta; - B = (1 - cos(theta)) / (theta * theta); - D = theta / (2 * sin(theta)); - E = 1 / (theta * theta) * (1 - 0.5 * A / B); - } - - // Get the skew matrix and V inverse - Eigen::Matrix3d I_33 = Eigen::Matrix3d::Identity(); - Eigen::Matrix3d wskew = D * (R - R.transpose()); - Eigen::Matrix3d Vinv = I_33 - 0.5 * wskew + E * wskew * wskew; - - // Calculate vector - Eigen::Matrix vec; - vec.head(3) << wskew(2, 1), wskew(0, 2), wskew(1, 0); - vec.tail(3) = Vinv * t; - return vec; - } - - /** - * @brief Hat operator for R^6 -> Lie Algebra se(3) - * - * \f{align*}{ - * \boldsymbol\Omega^{\wedge} = \begin{bmatrix} \lfloor \boldsymbol\omega \times\rfloor & \mathbf u \\ \mathbf 0 & 0 \end{bmatrix} - * \f} - * - * @param vec 6x1 in the se(3) space [omega, u] - * @return Lie algebra se(3) 4x4 matrix - */ - static inline Eigen::Matrix4d hat_se3(const Eigen::Matrix &vec) { - Eigen::Matrix4d mat = Eigen::Matrix4d::Zero(); - mat.block(0, 0, 3, 3) = skew_x(vec.head(3)); - mat.block(0, 3, 3, 1) = vec.tail(3); - return mat; - } - - /** - * @brief SE(3) matrix analytical inverse - * - * It seems that using the .inverse() function is not a good way. - * This should be used in all cases we need the inverse instead of numerical inverse. - * https://github.com/rpng/open_vins/issues/12 - * \f{align*}{ - * \mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top\mathbf{p} \\ \mathbf{0} & 1 \end{bmatrix} - * \f} - * - * @param[in] T SE(3) matrix - * @return inversed SE(3) matrix - */ - static inline Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T) { - Eigen::Matrix4d Tinv = Eigen::Matrix4d::Identity(); - Tinv.block(0, 0, 3, 3) = T.block(0, 0, 3, 3).transpose(); - Tinv.block(0, 3, 3, 1) = -Tinv.block(0, 0, 3, 3) * T.block(0, 3, 3, 1); - return Tinv; - } - - /** - * @brief JPL Quaternion inverse - * - * See equation 21 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). - * \f{align*}{ - * \bar{q}^{-1} = \begin{bmatrix} -\mathbf{q} \\ q_4 \end{bmatrix} - * \f} - * - * @param[in] q quaternion we want to change - * @return inversed quaternion - */ - static inline Eigen::Matrix Inv(Eigen::Matrix q) { - Eigen::Matrix qinv; - qinv.block(0, 0, 3, 1) = -q.block(0, 0, 3, 1); - qinv(3, 0) = q(3, 0); - return qinv; - } - - /** - * @brief Integrated quaternion from angular velocity - * - * See equation (48) of trawny tech report [Indirect Kalman Filter for 3D Attitude - * Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). - * - */ - static inline Eigen::Matrix Omega(Eigen::Matrix w) { - Eigen::Matrix mat; - mat.block(0, 0, 3, 3) = -skew_x(w); - mat.block(3, 0, 1, 3) = -w.transpose(); - mat.block(0, 3, 3, 1) = w; - mat(3, 3) = 0; - return mat; - } - - /** - * @brief Normalizes a quaternion to make sure it is unit norm - * @param q_t Quaternion to normalized - * @return Normalized quaterion - */ - static inline Eigen::Matrix quatnorm(Eigen::Matrix q_t) { - if (q_t(3, 0) < 0) { - q_t *= -1; - } - return q_t / q_t.norm(); - } - - /** - * @brief Gets roll, pitch, yaw of argument rotation (in that order). - * To recover the matrix: R_input = R_z(yaw)*R_y(pitch)*R_x(roll) - * @param rot Rotation matrix - */ - static inline Eigen::Matrix rot2rpy(const Eigen::Matrix &rot) { - Eigen::Matrix rpy; - rpy(1, 0) = atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0))); - if (std::abs(cos(rpy(1, 0)) > 1.0e-12)) { - rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0))); - rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0))); - } else { - rpy(2, 0) = 0; - rpy(0, 0) = atan2(rot(0, 1), rot(1, 1)); - } - return rpy; - } - - /** - * @brief Construct rotation matrix from given roll - * @param t roll angle - */ - static inline Eigen::Matrix rot_x(double t) { - Eigen::Matrix r; - double ct = cos(t); - double st = sin(t); - r << 1.0, 0.0, 0.0, 0.0, ct, -st, 0.0, st, ct; - return r; - } - - /** - * @brief Construct rotation matrix from given pitch - * @param t pitch angle - */ - static inline Eigen::Matrix rot_y(double t) { - Eigen::Matrix r; - double ct = cos(t); - double st = sin(t); - r << ct, 0.0, st, 0.0, 1.0, 0.0, -st, 0.0, ct; - return r; - } - - /** - * @brief Construct rotation matrix from given yaw - * @param t yaw angle - */ - static inline Eigen::Matrix rot_z(double t) { - Eigen::Matrix r; - double ct = cos(t); - double st = sin(t); - r << ct, -st, 0.0, st, ct, 0.0, 0.0, 0.0, 1.0; - return r; - } -}; - -} // namespace ov_eval - -#endif /* OV_EVAL_MATH_H */ \ No newline at end of file diff --git a/ov_eval/src/utils/Statistics.h b/ov_eval/src/utils/Statistics.h index af2a1a839..ae5b46147 100644 --- a/ov_eval/src/utils/Statistics.h +++ b/ov_eval/src/utils/Statistics.h @@ -22,11 +22,12 @@ #ifndef OV_EVAL_STATISTICS_H #define OV_EVAL_STATISTICS_H -#include #include #include #include +#include + namespace ov_eval { /** diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 169e77573..7876c9c79 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(Eigen3 REQUIRED) find_package(OpenCV 4 QUIET) if (NOT OpenCV_FOUND) find_package(OpenCV 3 REQUIRED) -endif() +endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) # Display message to user @@ -26,9 +26,9 @@ option(ENABLE_ARUCO_TAGS "Enable or disable aruco tag (disable if no contrib mod if (NOT ENABLE_ARUCO_TAGS) add_definitions(-DENABLE_ARUCO_TAGS=0) message(WARNING "DISABLING ARUCOTAG TRACKING!") -else() +else () add_definitions(-DENABLE_ARUCO_TAGS=1) -endif() +endif () # Describe catkin project option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) @@ -39,23 +39,23 @@ if (catkin_FOUND AND ENABLE_CATKIN_ROS) INCLUDE_DIRS src LIBRARIES ov_msckf_lib ) -else() +else () add_definitions(-DROS_AVAILABLE=0) message(WARNING "BUILDING WITHOUT ROS!") -endif() +endif () # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) +if (COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) +elseif (COMPILER_SUPPORTS_CXX0X) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else() +else () message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() +endif () # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") @@ -76,7 +76,7 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} -) + ) # If we are not building with ROS then we need to manually link to its headers # This isn't that elegant of a way, but this at least allows for building without ROS @@ -85,7 +85,7 @@ if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") include_directories(${ov_core_SOURCE_DIR}/src/) list(APPEND thirdparty_libraries ov_core_lib) -endif() +endif () ################################################## # Make the core library @@ -100,12 +100,12 @@ list(APPEND library_source_files src/update/UpdaterMSCKF.cpp src/update/UpdaterSLAM.cpp src/update/UpdaterZeroVelocity.cpp -) + ) if (catkin_FOUND AND ENABLE_CATKIN_ROS) list(APPEND library_source_files - src/core/RosVisualizer.cpp - ) -endif() + src/core/RosVisualizer.cpp + ) +endif () add_library(ov_msckf_lib SHARED ${library_source_files}) target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) target_include_directories(ov_msckf_lib PUBLIC src) @@ -127,12 +127,12 @@ if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_executable(run_simulation src/run_simulation.cpp) target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) -else() +else () add_executable(run_simulation src/run_simulation.cpp) target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) -endif() +endif () add_executable(test_sim_meas src/test_sim_meas.cpp) diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index 000988c6f..f82d25a83 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -20,8 +20,9 @@ */ #include "RosVisualizer.h" -#include "utils/print.h" +using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim) : _app(app), _sim(sim) { @@ -31,31 +32,31 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap // Setup pose and path publisher pub_poseimu = nh.advertise("/ov_msckf/poseimu", 2); - ROS_INFO("Publishing: %s", pub_poseimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_poseimu.getTopic().c_str()); pub_odomimu = nh.advertise("/ov_msckf/odomimu", 2); - ROS_INFO("Publishing: %s", pub_odomimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_odomimu.getTopic().c_str()); pub_pathimu = nh.advertise("/ov_msckf/pathimu", 2); - ROS_INFO("Publishing: %s", pub_pathimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_pathimu.getTopic().c_str()); // 3D points publishing pub_points_msckf = nh.advertise("/ov_msckf/points_msckf", 2); - ROS_INFO("Publishing: %s", pub_points_msckf.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_points_msckf.getTopic().c_str()); pub_points_slam = nh.advertise("/ov_msckf/points_slam", 2); - ROS_INFO("Publishing: %s", pub_points_msckf.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_points_msckf.getTopic().c_str()); pub_points_aruco = nh.advertise("/ov_msckf/points_aruco", 2); - ROS_INFO("Publishing: %s", pub_points_aruco.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_points_aruco.getTopic().c_str()); pub_points_sim = nh.advertise("/ov_msckf/points_sim", 2); - ROS_INFO("Publishing: %s", pub_points_sim.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_points_sim.getTopic().c_str()); // Our tracking image pub_tracks = nh.advertise("/ov_msckf/trackhist", 2); - ROS_INFO("Publishing: %s", pub_tracks.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_tracks.getTopic().c_str()); // Groundtruth publishers pub_posegt = nh.advertise("/ov_msckf/posegt", 2); - ROS_INFO("Publishing: %s", pub_posegt.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_posegt.getTopic().c_str()); pub_pathgt = nh.advertise("/ov_msckf/pathgt", 2); - ROS_INFO("Publishing: %s", pub_pathgt.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s", pub_pathgt.getTopic().c_str()); // Loop closure publishers pub_loop_pose = nh.advertise("/ov_msckf/loop_pose", 2); @@ -75,7 +76,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap nh.param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { DatasetReader::load_gt_file(path_to_gt, gt_states); - ROS_INFO("gt file path is: %s", path_to_gt.c_str()); + PRINT_DEBUG("gt file path is: %s", path_to_gt.c_str()); } } diff --git a/ov_msckf/src/core/RosVisualizer.h b/ov_msckf/src/core/RosVisualizer.h index 706122655..4e17cddf5 100644 --- a/ov_msckf/src/core/RosVisualizer.h +++ b/ov_msckf/src/core/RosVisualizer.h @@ -43,6 +43,7 @@ #include "VioManager.h" #include "sim/Simulator.h" #include "utils/dataset_reader.h" +#include "utils/print.h" namespace ov_msckf { diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index b75721879..a8fd939ea 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -20,11 +20,6 @@ */ #include "VioManager.h" -#include "types/Landmark.h" -#include "utils/print.h" - -#include -#include using namespace ov_core; using namespace ov_type; @@ -193,7 +188,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector // Check if we actually have a simulated tracker // If not, recreate and re-cast the tracker to our simulation tracker - std::shared_ptr trackSIM = dynamic_pointer_cast(trackFEATS); + std::shared_ptr trackSIM = std::dynamic_pointer_cast(trackFEATS); if (trackSIM == nullptr) { // Replace with the simulated tracker trackSIM = std::make_shared(state->_cam_intrinsics_cameras, state->_options.max_aruco_features); @@ -378,8 +373,8 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // This isn't super ideal, but it keeps the logic after this easier... // We can start processing things when we have at least 5 clones since we can start triangulating things... if ((int)state->_clones_IMU.size() < std::min(state->_options.max_clone_size, 5)) { - PRINT_INFO("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(), - std::min(state->_options.max_clone_size, 5)); + PRINT_DEBUG("waiting for enough clone states (%d of %d)....\n", (int)state->_clones_IMU.size(), + std::min(state->_options.max_clone_size, 5)); return; } @@ -652,7 +647,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) ss << " " << id; } ss << ")" << std::endl; - PRINT_DEBUG(BLUE "%s" RESET, ss.str().c_str()); + PRINT_INFO(BLUE "%s" RESET, ss.str().c_str()); // Finally if we are saving stats to file, lets save it to file if (params.record_timing_information && of_statistics.is_open()) { diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index d8e33bb2b..4a01b5b69 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -179,7 +179,7 @@ class VioManager { for (auto &f : state->_features_SLAM) { if ((int)f.first <= state->_options.max_aruco_features) continue; - if (LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { + if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { // Assert that we have an anchor pose for this feature assert(f.second->_anchor_cam_id != -1); // Get calibration for our anchor camera @@ -203,7 +203,7 @@ class VioManager { for (auto &f : state->_features_SLAM) { if ((int)f.first > state->_options.max_aruco_features) continue; - if (LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { + if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { // Assert that we have an anchor pose for this feature assert(f.second->_anchor_cam_id != -1); // Get calibration for our anchor camera @@ -284,16 +284,16 @@ class VioManager { std::shared_ptr propagator; /// Complete history of our feature tracks - std::shared_ptr trackDATABASE; + std::shared_ptr trackDATABASE; /// Our sparse feature tracker (klt or descriptor) - std::shared_ptr trackFEATS; + std::shared_ptr trackFEATS; /// Our aruoc tracker - std::shared_ptr trackARUCO; + std::shared_ptr trackARUCO; /// State initializer - std::shared_ptr initializer; + std::shared_ptr initializer; /// Boolean if we are initialized or not bool is_initialized_vio = false; @@ -334,7 +334,7 @@ class VioManager { std::vector good_features_MSCKF; /// Feature initializer used to triangulate all active tracks - std::shared_ptr active_tracks_initializer; + std::shared_ptr active_tracks_initializer; // Re-triangulated features 3d positions seen from the current frame (used in visualization) double active_tracks_time = -1; diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index e009d5ff3..97752bf32 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -37,9 +37,6 @@ #include "utils/print.h" #include "utils/quat_ops.h" -using namespace std; -using namespace ov_core; - namespace ov_msckf { /** @@ -91,18 +88,18 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_estimator() { - PRINT_INFO("ESTIMATOR PARAMETERS:\n"); + PRINT_DEBUG("ESTIMATOR PARAMETERS:\n"); state_options.print(); - PRINT_INFO("\t- dt_slam_delay: %.1f\n", dt_slam_delay); - PRINT_INFO("\t- init_window_time: %.2f\n", init_window_time); - PRINT_INFO("\t- init_imu_thresh: %.2f\n", init_imu_thresh); - PRINT_INFO("\t- zero_velocity_update: %d\n", try_zupt); - PRINT_INFO("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity); - PRINT_INFO("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); - PRINT_INFO("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity); - PRINT_INFO("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); - PRINT_INFO("\t- record timing?: %d\n", (int)record_timing_information); - PRINT_INFO("\t- record timing filepath: %s\n", record_timing_filepath.c_str()); + PRINT_DEBUG("\t- dt_slam_delay: %.1f\n", dt_slam_delay); + PRINT_DEBUG("\t- init_window_time: %.2f\n", init_window_time); + PRINT_DEBUG("\t- init_imu_thresh: %.2f\n", init_imu_thresh); + PRINT_DEBUG("\t- zero_velocity_update: %d\n", try_zupt); + PRINT_DEBUG("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity); + PRINT_DEBUG("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); + PRINT_DEBUG("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity); + PRINT_DEBUG("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); + PRINT_DEBUG("\t- record timing?: %d\n", (int)record_timing_information); + PRINT_DEBUG("\t- record timing filepath: %s\n", record_timing_filepath.c_str()); } // NOISE / CHI2 ============================ @@ -127,15 +124,15 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_noise() { - PRINT_INFO("NOISE PARAMETERS:\n"); + PRINT_DEBUG("NOISE PARAMETERS:\n"); imu_noises.print(); - PRINT_INFO("\tUpdater MSCKF Feats:\n"); + PRINT_DEBUG("\tUpdater MSCKF Feats:\n"); msckf_options.print(); - PRINT_INFO("\tUpdater SLAM Feats:\n"); + PRINT_DEBUG("\tUpdater SLAM Feats:\n"); slam_options.print(); - PRINT_INFO("\tUpdater ARUCO Tags:\n"); + PRINT_DEBUG("\tUpdater ARUCO Tags:\n"); aruco_options.print(); - PRINT_INFO("\tUpdater ZUPT:\n"); + PRINT_DEBUG("\tUpdater ZUPT:\n"); zupt_options.print(); } @@ -164,24 +161,24 @@ struct VioManagerOptions { * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_state() { - PRINT_INFO("STATE PARAMETERS:\n"); - PRINT_INFO("\t- gravity_mag: %.4f\n", gravity_mag); - PRINT_INFO("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); - PRINT_INFO("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt); + PRINT_DEBUG("STATE PARAMETERS:\n"); + PRINT_DEBUG("\t- gravity_mag: %.4f\n", gravity_mag); + PRINT_DEBUG("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); + PRINT_DEBUG("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt); assert(state_options.num_cameras == (int)camera_fisheye.size()); for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; - ss << "cam_" << n << "_wh:" << endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; - ss << "cam_" << n << "_intrinsic(0:3):" << endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; - ss << "cam_" << n << "_intrinsic(4:7):" << endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; - ss << "cam_" << n << "_extrinsic(0:3):" << endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; - ss << "cam_" << n << "_extrinsic(4:6):" << endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; + ss << "cam_" << n << "_wh:" << std::endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; + ss << "cam_" << n << "_intrinsic(0:3):" << std::endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_intrinsic(4:7):" << std::endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_extrinsic(0:3):" << std::endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_extrinsic(4:6):" << std::endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); - T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose(); + T_CtoI.block(0, 0, 3, 3) = ov_core::quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose(); T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1); - ss << "T_C" << n << "toI:" << endl << T_CtoI << std::endl << std::endl; - PRINT_INFO(ss.str().c_str()); + ss << "T_C" << n << "toI:" << std::endl << T_CtoI << std::endl << std::endl; + PRINT_DEBUG(ss.str().c_str()); } } @@ -221,7 +218,7 @@ struct VioManagerOptions { int min_px_dist = 10; /// What type of pre-processing histogram method should be applied to images - TrackBase::HistogramMethod histogram_method = TrackBase::HistogramMethod::HISTOGRAM; + ov_core::TrackBase::HistogramMethod histogram_method = ov_core::TrackBase::HistogramMethod::HISTOGRAM; /// KNN ration between top two descriptor matcher which is required to be a good match double knn_ratio = 0.85; @@ -233,44 +230,31 @@ struct VioManagerOptions { std::map masks; /// Parameters used by our feature initialize / triangulator - FeatureInitializerOptions featinit_options; + ov_core::FeatureInitializerOptions featinit_options; /** * @brief This function will print out all parameters releated to our visual trackers. */ void print_trackers() { - PRINT_INFO("FEATURE TRACKING PARAMETERS:\n"); - PRINT_INFO("\t- use_klt: %d\n", use_klt); - PRINT_INFO("\t- use_stereo: %d\n", use_stereo); - PRINT_INFO("\t- use_aruco: %d\n", use_aruco); - PRINT_INFO("\t- downsize aruco: %d\n", downsize_aruco); - PRINT_INFO("\t- downsize cameras: %d\n", downsample_cameras); - PRINT_INFO("\t- use multi-threading: %d\n", use_multi_threading); - PRINT_INFO("\t- num_pts: %d\n", num_pts); - PRINT_INFO("\t- fast threshold: %d\n", fast_threshold); - PRINT_INFO("\t- grid X by Y: %d by %d\n", grid_x, grid_y); - PRINT_INFO("\t- min px dist: %d\n", min_px_dist); - PRINT_INFO("\t- hist method: %d\n", (int)histogram_method); - PRINT_INFO("\t- knn ratio: %.3f\n", knn_ratio); - PRINT_INFO("\t- use mask?: %d\n", use_mask); + PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n"); + PRINT_DEBUG("\t- use_stereo: %d\n", use_stereo); + PRINT_DEBUG("\t- use_klt: %d\n", use_klt); + PRINT_DEBUG("\t- use_aruco: %d\n", use_aruco); + PRINT_DEBUG("\t- downsize aruco: %d\n", downsize_aruco); + PRINT_DEBUG("\t- downsize cameras: %d\n", downsample_cameras); + PRINT_DEBUG("\t- use multi-threading: %d\n", use_multi_threading); + PRINT_DEBUG("\t- num_pts: %d\n", num_pts); + PRINT_DEBUG("\t- fast threshold: %d\n", fast_threshold); + PRINT_DEBUG("\t- grid X by Y: %d by %d\n", grid_x, grid_y); + PRINT_DEBUG("\t- min px dist: %d\n", min_px_dist); + PRINT_DEBUG("\t- hist method: %d\n", (int)histogram_method); + PRINT_DEBUG("\t- knn ratio: %.3f\n", knn_ratio); + PRINT_DEBUG("\t- use mask?: %d\n", use_mask); featinit_options.print(); } // SIMULATOR =============================== - /// Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),ori(xyzw) format. - string sim_traj_path = "../ov_data/sim/udel_gore.txt"; - - /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in - /// simulation. - double sim_distance_threshold = 1.0; - - /// Frequency (Hz) that we will simulate our cameras - double sim_freq_cam = 10.0; - - /// Frequency (Hz) that we will simulate our inertial measurement unit - double sim_freq_imu = 400.0; - /// Seed for initial states (i.e. random feature 3d positions in the generated map) int sim_seed_state_init = 0; @@ -284,19 +268,33 @@ struct VioManagerOptions { /// If we should perturb the calibration that the estimator starts with bool sim_do_perturbation = false; + /// Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),ori(xyzw) format. + std::string sim_traj_path = "../ov_data/sim/udel_gore.txt"; + + /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in + /// simulation. + double sim_distance_threshold = 1.0; + + /// Frequency (Hz) that we will simulate our cameras + double sim_freq_cam = 10.0; + + /// Frequency (Hz) that we will simulate our inertial measurement unit + double sim_freq_imu = 400.0; + /** * @brief This function will print out all simulated parameters loaded. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. */ void print_simulation() { - PRINT_INFO("SIMULATION PARAMETERS:\n"); - PRINT_INFO(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init); - PRINT_INFO(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb); - PRINT_INFO(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements); - PRINT_INFO("\t- traj path: %s\n", sim_traj_path.c_str()); - PRINT_INFO("\t- dist thresh: %.2f\n", sim_distance_threshold); - PRINT_INFO("\t- cam feq: %.2f\n", sim_freq_cam); - PRINT_INFO("\t- imu feq: %.2f\n", sim_freq_imu); + PRINT_DEBUG("SIMULATION PARAMETERS:\n"); + PRINT_WARNING(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init); + PRINT_WARNING(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb); + PRINT_WARNING(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements); + PRINT_WARNING(BOLDRED "\t- do perturb?: %d\n" RESET, sim_do_perturbation); + PRINT_DEBUG("\t- traj path: %s\n", sim_traj_path.c_str()); + PRINT_DEBUG("\t- dist thresh: %.2f\n", sim_distance_threshold); + PRINT_DEBUG("\t- cam feq: %.2f\n", sim_freq_cam); + PRINT_DEBUG("\t- imu feq: %.2f\n", sim_freq_imu); } }; diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index 952be7fee..f9a646bba 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -69,22 +69,22 @@ int main(int argc, char **argv) { nh.param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); topic_cameras.emplace_back(0, cam_topic0); topic_cameras.emplace_back(1, cam_topic1); - ROS_INFO("serial cam (stereo): %s", cam_topic0.c_str()); - ROS_INFO("serial cam (stereo): %s", cam_topic1.c_str()); + PRINT_DEBUG("serial cam (stereo): %s\n", cam_topic0.c_str()); + PRINT_DEBUG("serial cam (stereo): %s\n", cam_topic1.c_str()); } else { for (int i = 0; i < params.state_options.num_cameras; i++) { // read in the topic std::string cam_topic; nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); topic_cameras.emplace_back(i, cam_topic); - ROS_INFO("serial cam (mono): %s", cam_topic.c_str()); + PRINT_DEBUG("serial cam (mono): %s\n", cam_topic.c_str()); } } // Location of the ROS bag we want to read in std::string path_to_bag; nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); - ROS_INFO("ros bag path is: %s", path_to_bag.c_str()); + PRINT_DEBUG("ros bag path is: %s\n", path_to_bag.c_str()); // Load groundtruth if we have it std::map> gt_states; @@ -92,8 +92,8 @@ int main(int argc, char **argv) { std::string path_to_gt; nh.param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { - DatasetReader::load_gt_file(path_to_gt, gt_states); - ROS_INFO("gt file path is: %s", path_to_gt.c_str()); + ov_core::DatasetReader::load_gt_file(path_to_gt, gt_states); + PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); } } @@ -102,8 +102,8 @@ int main(int argc, char **argv) { double bag_start, bag_durr; nh.param("bag_start", bag_start, 0); nh.param("bag_durr", bag_durr, -1); - ROS_INFO("bag start: %.1f", bag_start); - ROS_INFO("bag duration: %.1f", bag_durr); + PRINT_DEBUG("bag start: %.1f\n", bag_start); + PRINT_DEBUG("bag duration: %.1f\n", bag_durr); //=================================================================================== //=================================================================================== @@ -124,13 +124,13 @@ int main(int argc, char **argv) { ros::Time time_init = view_full.getBeginTime(); time_init += ros::Duration(bag_start); ros::Time time_finish = (bag_durr < 0) ? view_full.getEndTime() : time_init + ros::Duration(bag_durr); - ROS_INFO("time start = %.6f", time_init.toSec()); - ROS_INFO("time end = %.6f", time_finish.toSec()); + PRINT_DEBUG("time start = %.6f\n", time_init.toSec()); + PRINT_DEBUG("time end = %.6f\n", time_finish.toSec()); view.addQuery(bag, time_init, time_finish); // Check to make sure we have data to play if (view.size() == 0) { - ROS_ERROR("No messages to play on specified topics. Exiting."); + PRINT_ERROR(RED "No messages to play on specified topics. Exiting.\n" RESET); ros::shutdown(); return EXIT_FAILURE; } @@ -196,7 +196,7 @@ int main(int argc, char **argv) { message.wm << msg_imu_current->angular_velocity.x, msg_imu_current->angular_velocity.y, msg_imu_current->angular_velocity.z; message.am << msg_imu_current->linear_acceleration.x, msg_imu_current->linear_acceleration.y, msg_imu_current->linear_acceleration.z; // send it to our VIO system - // ROS_ERROR("%.15f = imu time",msg_imu_current->header.stamp.toSec()-time_init.toSec()); + // PRINT_DEBUG("%.15f = imu time",msg_imu_current->header.stamp.toSec()-time_init.toSec()); sys->feed_measurement_imu(message); viz->visualize(); viz->visualize_odometry(message.timestamp); @@ -237,12 +237,12 @@ int main(int argc, char **argv) { if (std::abs(time1 - time0) < std::abs(time1_next - time0) && std::abs(time0 - time1) < std::abs(time0_next - time1)) { have_found_pair = true; } else if (std::abs(time1 - time0) >= std::abs(time1_next - time0)) { - // ROS_WARN("skipping cam1 (%.4f >= %.4f)",std::abs(time1-time0), std::abs(time1_next-time0)); + // PRINT_WARNING("skipping cam1 (%.4f >= %.4f)",std::abs(time1-time0), std::abs(time1_next-time0)); msg_images_current.at(1) = msg_images_next.at(1); view_cameras_iterators.at(1)++; msg_images_next.at(1) = view_cameras_iterators.at(1)->instantiate(); } else { - // ROS_WARN("skipping cam0 (%.4f >= %.4f)",std::abs(time0-time1), std::abs(time0_next-time1)); + // PRINT_WARNING("skipping cam0 (%.4f >= %.4f)",std::abs(time0-time1), std::abs(time0_next-time1)); msg_images_current.at(0) = msg_images_next.at(0); view_cameras_iterators.at(0)++; msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); @@ -257,7 +257,7 @@ int main(int argc, char **argv) { // Check if we should initialize using the groundtruth (always use left) Eigen::Matrix imustate; if (!gt_states.empty() && !sys->initialized() && - DatasetReader::get_gt_state(msg_images_current.at(0)->header.stamp.toSec(), imustate, gt_states)) { + ov_core::DatasetReader::get_gt_state(msg_images_current.at(0)->header.stamp.toSec(), imustate, gt_states)) { // biases are pretty bad normally, so zero them // imustate.block(11,0,6,1).setZero(); sys->initialize_with_gt(imustate); @@ -269,7 +269,7 @@ int main(int argc, char **argv) { cv_ptr0 = cv_bridge::toCvShare(msg_images_current.at(0), sensor_msgs::image_encodings::MONO8); cv_ptr1 = cv_bridge::toCvShare(msg_images_current.at(1), sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s\n", e.what()); msg_images_current.at(0) = msg_images_next.at(0); view_cameras_iterators.at(0)++; msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); @@ -294,9 +294,9 @@ int main(int argc, char **argv) { message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); } - // ROS_ERROR("%.15f = cam %d time",msg_images_current.at(0)->header.stamp.toSec()-time_init.toSec(), 0); - // ROS_ERROR("%.15f = cam %d time",msg_images_current.at(1)->header.stamp.toSec()-time_init.toSec(), 1); - // ROS_ERROR("(difference is %.15f)",msg_images_current.at(1)->header.stamp.toSec()-msg_images_current.at(0)->header.stamp.toSec()); + // PRINT_DEBUG("%.15f = cam %d time",msg_images_current.at(0)->header.stamp.toSec()-time_init.toSec(), 0); + // PRINT_DEBUG("%.15f = cam %d time",msg_images_current.at(1)->header.stamp.toSec()-time_init.toSec(), 1); + // PRINT_DEBUG("(difference is %.15f)",msg_images_current.at(1)->header.stamp.toSec()-msg_images_current.at(0)->header.stamp.toSec()); sys->feed_measurement_camera(message); // move forward in time @@ -322,7 +322,7 @@ int main(int argc, char **argv) { // Check if we should initialize using the groundtruth auto msg_camera = msg_images_current.at(smallest_cam); Eigen::Matrix imustate; - if (!gt_states.empty() && !sys->initialized() && DatasetReader::get_gt_state(msg_camera->header.stamp.toSec(), imustate, gt_states)) { + if (!gt_states.empty() && !sys->initialized() && ov_core::DatasetReader::get_gt_state(msg_camera->header.stamp.toSec(), imustate, gt_states)) { // biases are pretty bad normally, so zero them // imustate.block(11,0,6,1).setZero(); sys->initialize_with_gt(imustate); @@ -333,7 +333,7 @@ int main(int argc, char **argv) { try { cv_ptr = cv_bridge::toCvShare(msg_camera, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s\n", e.what()); msg_images_current.at(smallest_cam) = msg_images_next.at(smallest_cam); view_cameras_iterators.at(smallest_cam)++; msg_images_next.at(smallest_cam) = view_cameras_iterators.at(smallest_cam)->instantiate(); @@ -350,7 +350,7 @@ int main(int argc, char **argv) { } else { message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); } - // ROS_ERROR("%.15f = cam %d time",msg_camera->header.stamp.toSec()-time_init.toSec(), smallest_cam); + // PRINT_DEBUG("%.15f = cam %d time",msg_camera->header.stamp.toSec()-time_init.toSec(), smallest_cam); sys->feed_measurement_camera(message); // move forward diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp index b28d9e553..112b5dfee 100644 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ b/ov_msckf/src/ros_subscribe_msckf.cpp @@ -94,8 +94,8 @@ int main(int argc, char **argv) { sync_cam.push_back(std::move(sync)); sync_subs_cam.push_back(std::move(image_sub0)); sync_subs_cam.push_back(std::move(image_sub1)); - ROS_INFO("subscribing to cam (stereo): %s", cam_topic0.c_str()); - ROS_INFO("subscribing to cam (stereo): %s", cam_topic1.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); } else { // Now we should add any non-stereo callbacks here for (int i = 0; i < params.state_options.num_cameras; i++) { @@ -104,7 +104,7 @@ int main(int argc, char **argv) { nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); // create subscriber subs_cam.push_back(nh.subscribe(cam_topic, 5, boost::bind(callback_monocular, _1, i))); - ROS_INFO("subscribing to cam (mono): %s", cam_topic.c_str()); + PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); } } @@ -115,7 +115,7 @@ int main(int argc, char **argv) { // Spin off to ROS // TODO: maybe should use multi-thread spinner // TODO: but need to support multi-threaded calls to viomanager - ROS_INFO("done...spinning to ros"); + PRINT_DEBUG("done...spinning to ros"); ros::spin(); // Final visualization @@ -146,7 +146,7 @@ void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { try { cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s", e.what()); return; } @@ -175,7 +175,7 @@ void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs:: try { cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s", e.what()); return; } @@ -184,7 +184,7 @@ void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs:: try { cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s", e.what()); return; } diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 01d7d04f7..5308c55c7 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -21,8 +21,7 @@ #include "Simulator.h" -#include - +using namespace ov_core; using namespace ov_msckf; Simulator::Simulator(VioManagerOptions ¶ms_) { diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index 070c18b50..5378928be 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -34,8 +34,6 @@ #include "sim/BsplineSE3.h" #include "utils/colors.h" -using namespace ov_core; - namespace ov_msckf { /** @@ -146,7 +144,7 @@ class Simulator { std::vector traj_data; /// Our b-spline trajectory - BsplineSE3 spline; + ov_core::BsplineSE3 spline; /// Our map of 3d features size_t id_map = 0; diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 863e076dd..720a03387 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -20,9 +20,9 @@ */ #include "Propagator.h" -#include "utils/print.h" using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; void Propagator::propagate_and_clone(std::shared_ptr state, double timestamp) { diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 2fac854ac..7e4529bd8 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -27,8 +27,6 @@ #include "utils/quat_ops.h" #include "utils/sensor_data.h" -using namespace ov_core; - namespace ov_msckf { /** @@ -72,10 +70,10 @@ class Propagator { /// Nice print function of what parameters we have loaded void print() { - PRINT_INFO("\t- gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_INFO("\t- accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_INFO("\t- gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_INFO("\t- accelerometer_random_walk: %.6f\n", sigma_ab); + PRINT_DEBUG("\t- gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG("\t- accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG("\t- gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG("\t- accelerometer_random_walk: %.6f\n", sigma_ab); } }; diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index abb45afcf..f7163865b 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -21,9 +21,8 @@ #include "State.h" -#include - using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; State::State(StateOptions &options) { diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index bc43812e9..1a2ffb737 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -23,6 +23,7 @@ #define OV_MSCKF_STATE_H #include +#include #include #include @@ -34,9 +35,6 @@ #include "types/Type.h" #include "types/Vec.h" -using namespace ov_core; -using namespace ov_type; - namespace ov_msckf { /** @@ -87,25 +85,25 @@ class State { StateOptions _options; /// Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba) - std::shared_ptr _imu; + std::shared_ptr _imu; /// Map between imaging times and clone poses (q_GtoIi, p_IiinG) - std::map> _clones_IMU; + std::map> _clones_IMU; /// Our current set of SLAM features (3d positions) - std::unordered_map> _features_SLAM; + std::unordered_map> _features_SLAM; /// Time offset base IMU to camera (t_imu = t_cam + t_off) - std::shared_ptr _calib_dt_CAMtoIMU; + std::shared_ptr _calib_dt_CAMtoIMU; /// Calibration poses for each camera (R_ItoC, p_IinC) - std::unordered_map> _calib_IMUtoCAM; + std::unordered_map> _calib_IMUtoCAM; /// Camera intrinsics - std::unordered_map> _cam_intrinsics; + std::unordered_map> _cam_intrinsics; /// Camera intrinsics camera objects - std::unordered_map> _cam_intrinsics_cameras; + std::unordered_map> _cam_intrinsics_cameras; private: // Define that the state helper is a friend class of this class @@ -117,7 +115,7 @@ class State { Eigen::MatrixXd _Cov; /// Vector of variables - std::vector> _variables; + std::vector> _variables; }; } // namespace ov_msckf diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp index 7ad16ecfd..94556cf0b 100644 --- a/ov_msckf/src/state/StateHelper.cpp +++ b/ov_msckf/src/state/StateHelper.cpp @@ -20,9 +20,9 @@ */ #include "StateHelper.h" -#include "utils/print.h" using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector> &order_NEW, diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index 69d250aae..4b9b2e376 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -25,11 +25,10 @@ #include "State.h" #include "types/Landmark.h" #include "utils/colors.h" +#include "utils/print.h" #include -using namespace ov_core; - namespace ov_msckf { /** @@ -72,8 +71,8 @@ class StateHelper { * @param Phi State transition matrix (size order_NEW by size order_OLD) * @param Q Additive state propagation noise matrix (size order_NEW by size order_NEW) */ - static void EKFPropagation(std::shared_ptr state, const std::vector> &order_NEW, - const std::vector> &order_OLD, const Eigen::MatrixXd &Phi, const Eigen::MatrixXd &Q); + static void EKFPropagation(std::shared_ptr state, const std::vector> &order_NEW, + const std::vector> &order_OLD, const Eigen::MatrixXd &Phi, const Eigen::MatrixXd &Q); /** * @brief Performs EKF update of the state (see @ref linear-meas page) @@ -83,7 +82,7 @@ class StateHelper { * @param res residual of updating measurement * @param R updating measurement covariance */ - static void EKFUpdate(std::shared_ptr state, const std::vector> &H_order, const Eigen::MatrixXd &H, + static void EKFUpdate(std::shared_ptr state, const std::vector> &H_order, const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R); /** @@ -110,7 +109,7 @@ class StateHelper { * @param small_variables Vector of variables whose marginal covariance is desired * @return marginal covariance of the passed variables */ - static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr state, const std::vector> &small_variables); + static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr state, const std::vector> &small_variables); /** * @brief This gets the full covariance matrix. @@ -136,14 +135,14 @@ class StateHelper { * @param state Pointer to state * @param marg Pointer to variable to marginalize */ - static void marginalize(std::shared_ptr state, std::shared_ptr marg); + static void marginalize(std::shared_ptr state, std::shared_ptr marg); /** * @brief Clones "variable to clone" and places it at end of covariance * @param state Pointer to state * @param variable_to_clone Pointer to variable that will be cloned */ - static std::shared_ptr clone(std::shared_ptr state, std::shared_ptr variable_to_clone); + static std::shared_ptr clone(std::shared_ptr state, std::shared_ptr variable_to_clone); /** * @brief Initializes new variable into covariance. @@ -162,8 +161,8 @@ class StateHelper { * @param res Residual of initializing measurements * @param chi_2_mult Value we should multiply the chi2 threshold by (larger means it will be accepted more measurements) */ - static bool initialize(std::shared_ptr state, std::shared_ptr new_variable, - const std::vector> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L, Eigen::MatrixXd &R, + static bool initialize(std::shared_ptr state, std::shared_ptr new_variable, + const std::vector> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L, Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult); /** @@ -180,8 +179,8 @@ class StateHelper { * @param R Covariance of initializing measurements * @param res Residual of initializing measurements */ - static void initialize_invertible(std::shared_ptr state, std::shared_ptr new_variable, - const std::vector> &H_order, const Eigen::MatrixXd &H_R, + static void initialize_invertible(std::shared_ptr state, std::shared_ptr new_variable, + const std::vector> &H_order, const Eigen::MatrixXd &H_R, const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res); /** diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index c22dd7ca0..f15866c78 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -27,8 +27,6 @@ #include -using namespace ov_type; - namespace ov_msckf { /** @@ -73,31 +71,31 @@ struct StateOptions { int num_cameras = 1; /// What representation our features are in (msckf features) - LandmarkRepresentation::Representation feat_rep_msckf = LandmarkRepresentation::Representation::GLOBAL_3D; + ov_type::LandmarkRepresentation::Representation feat_rep_msckf = ov_type::LandmarkRepresentation::Representation::GLOBAL_3D; /// What representation our features are in (slam features) - LandmarkRepresentation::Representation feat_rep_slam = LandmarkRepresentation::Representation::GLOBAL_3D; + ov_type::LandmarkRepresentation::Representation feat_rep_slam = ov_type::LandmarkRepresentation::Representation::GLOBAL_3D; /// What representation our features are in (aruco tag features) - LandmarkRepresentation::Representation feat_rep_aruco = LandmarkRepresentation::Representation::GLOBAL_3D; + ov_type::LandmarkRepresentation::Representation feat_rep_aruco = ov_type::LandmarkRepresentation::Representation::GLOBAL_3D; /// Nice print function of what parameters we have loaded void print() { - PRINT_INFO("\t- use_fej: %d\n", do_fej); - PRINT_INFO("\t- use_imuavg: %d\n", imu_avg); - PRINT_INFO("\t- use_rk4int: %d\n", use_rk4_integration); - PRINT_INFO("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose); - PRINT_INFO("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); - PRINT_INFO("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); - PRINT_INFO("\t- max_clones: %d\n", max_clone_size); - PRINT_INFO("\t- max_slam: %d\n", max_slam_features); - PRINT_INFO("\t- max_slam_in_update: %d\n", max_slam_in_update); - PRINT_INFO("\t- max_msckf_in_update: %d\n", max_msckf_in_update); - PRINT_INFO("\t- max_aruco: %d\n", max_aruco_features); - PRINT_INFO("\t- max_cameras: %d\n", num_cameras); - PRINT_INFO("\t- feat_rep_msckf: %s\n", LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); - PRINT_INFO("\t- feat_rep_slam: %s\n", LandmarkRepresentation::as_string(feat_rep_slam).c_str()); - PRINT_INFO("\t- feat_rep_aruco: %s\n", LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); + PRINT_DEBUG("\t- use_fej: %d\n", do_fej); + PRINT_DEBUG("\t- use_imuavg: %d\n", imu_avg); + PRINT_DEBUG("\t- use_rk4int: %d\n", use_rk4_integration); + PRINT_DEBUG("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose); + PRINT_DEBUG("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); + PRINT_DEBUG("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); + PRINT_DEBUG("\t- max_clones: %d\n", max_clone_size); + PRINT_DEBUG("\t- max_slam: %d\n", max_slam_features); + PRINT_DEBUG("\t- max_slam_in_update: %d\n", max_slam_in_update); + PRINT_DEBUG("\t- max_msckf_in_update: %d\n", max_msckf_in_update); + PRINT_DEBUG("\t- max_aruco: %d\n", max_aruco_features); + PRINT_DEBUG("\t- max_cameras: %d\n", num_cameras); + PRINT_DEBUG("\t- feat_rep_msckf: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); + PRINT_DEBUG("\t- feat_rep_slam: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_slam).c_str()); + PRINT_DEBUG("\t- feat_rep_aruco: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); } }; diff --git a/ov_msckf/src/update/UpdaterHelper.cpp b/ov_msckf/src/update/UpdaterHelper.cpp index b4d903e8e..6b0972951 100644 --- a/ov_msckf/src/update/UpdaterHelper.cpp +++ b/ov_msckf/src/update/UpdaterHelper.cpp @@ -22,6 +22,7 @@ #include "UpdaterHelper.h" using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; void UpdaterHelper::get_feature_jacobian_representation(std::shared_ptr state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f, diff --git a/ov_msckf/src/update/UpdaterHelper.h b/ov_msckf/src/update/UpdaterHelper.h index f01890a60..3655fb88f 100644 --- a/ov_msckf/src/update/UpdaterHelper.h +++ b/ov_msckf/src/update/UpdaterHelper.h @@ -44,7 +44,6 @@ namespace ov_msckf { * */ class UpdaterHelper { - public: /** * @brief Feature object that our UpdaterHelper leverages, has all measurements and means @@ -64,7 +63,7 @@ class UpdaterHelper { std::unordered_map> timestamps; /// What representation our feature is in - LandmarkRepresentation::Representation feat_representation; + ov_type::LandmarkRepresentation::Representation feat_representation; /// What camera ID our pose is anchored in!! By default the first measurement is the anchor. int anchor_cam_id = -1; @@ -95,7 +94,7 @@ class UpdaterHelper { * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose) */ static void get_feature_jacobian_representation(std::shared_ptr state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f, - std::vector &H_x, std::vector> &x_order); + std::vector &H_x, std::vector> &x_order); /** * @brief Will construct the "stacked" Jacobians for a single feature from all its measurements @@ -108,7 +107,7 @@ class UpdaterHelper { * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose) */ static void get_feature_jacobian_full(std::shared_ptr state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f, - Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector> &x_order); + Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector> &x_order); /** * @brief This will project the left nullspace of H_f onto the linear system. diff --git a/ov_msckf/src/update/UpdaterMSCKF.cpp b/ov_msckf/src/update/UpdaterMSCKF.cpp index d4e53c1fc..d1c2f2c3b 100644 --- a/ov_msckf/src/update/UpdaterMSCKF.cpp +++ b/ov_msckf/src/update/UpdaterMSCKF.cpp @@ -20,12 +20,9 @@ */ #include "UpdaterMSCKF.h" -#include "utils/print.h" - -#include -#include using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; void UpdaterMSCKF::update(std::shared_ptr state, std::vector> &feature_vec) { @@ -259,10 +256,10 @@ void UpdaterMSCKF::update(std::shared_ptr state, std::vector +#include +#include + #include "feat/Feature.h" #include "feat/FeatureInitializer.h" #include "feat/FeatureInitializerOptions.h" @@ -29,8 +33,8 @@ #include "state/StateHelper.h" #include "types/LandmarkRepresentation.h" #include "utils/colors.h" +#include "utils/print.h" #include "utils/quat_ops.h" -#include #include "UpdaterHelper.h" #include "UpdaterOptions.h" @@ -59,13 +63,13 @@ class UpdaterMSCKF { * @param options Updater options (include measurement noise value) * @param feat_init_options Feature initializer options */ - UpdaterMSCKF(UpdaterOptions &options, FeatureInitializerOptions &feat_init_options) : _options(options) { + UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options) : _options(options) { // Save our raw pixel noise squared _options.sigma_pix_sq = std::pow(_options.sigma_pix, 2); // Save our feature initializer - initializer_feat = std::unique_ptr(new FeatureInitializer(feat_init_options)); + initializer_feat = std::unique_ptr(new ov_core::FeatureInitializer(feat_init_options)); // Initialize the chi squared test table with confidence level 0.95 // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 @@ -81,14 +85,14 @@ class UpdaterMSCKF { * @param state State of the filter * @param feature_vec Features that can be used for update */ - void update(std::shared_ptr state, std::vector> &feature_vec); + void update(std::shared_ptr state, std::vector> &feature_vec); protected: /// Options used during update UpdaterOptions _options; /// Feature initializer class object - std::unique_ptr initializer_feat; + std::unique_ptr initializer_feat; /// Chi squared 95th percentile table (lookup would be size of residual) std::map chi_squared_table; diff --git a/ov_msckf/src/update/UpdaterOptions.h b/ov_msckf/src/update/UpdaterOptions.h index 8a8c87278..293ad9480 100644 --- a/ov_msckf/src/update/UpdaterOptions.h +++ b/ov_msckf/src/update/UpdaterOptions.h @@ -42,8 +42,8 @@ struct UpdaterOptions { /// Nice print function of what parameters we have loaded void print() { - PRINT_INFO("\t- chi2_multipler: %.1f\n", chi2_multipler); - PRINT_INFO("\t- sigma_pix: %.2f\n", sigma_pix); + PRINT_DEBUG("\t- chi2_multipler: %.1f\n", chi2_multipler); + PRINT_DEBUG("\t- sigma_pix: %.2f\n", sigma_pix); } }; diff --git a/ov_msckf/src/update/UpdaterSLAM.cpp b/ov_msckf/src/update/UpdaterSLAM.cpp index aae9178bd..401f8e821 100644 --- a/ov_msckf/src/update/UpdaterSLAM.cpp +++ b/ov_msckf/src/update/UpdaterSLAM.cpp @@ -20,11 +20,9 @@ */ #include "UpdaterSLAM.h" -#include "utils/print.h" - -#include using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; void UpdaterSLAM::delayed_init(std::shared_ptr state, std::vector> &feature_vec) { @@ -211,12 +209,12 @@ void UpdaterSLAM::delayed_init(std::shared_ptr state, std::vector state, std::vector> &feature_vec) { @@ -355,7 +353,7 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector> Hxf_order = Hx_order; Hxf_order.push_back(landmark); - /// Chi2 distance check + // Chi2 distance check Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hxf_order); Eigen::MatrixXd S = H_xf * P_marg * H_xf.transpose(); double sigma_pix_sq = @@ -386,8 +384,9 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector_options.max_aruco_features) + if ((int)feat.featid < state->_options.max_aruco_features) { PRINT_DEBUG("[SLAM-UP]: accepted aruco tag %d for chi2 thresh (%.3f < %.3f)\n", (int)feat.featid, chi2, chi2_multipler * chi2_check); + } // We are good!!! Append to our large H vector size_t ct_hx = 0; @@ -436,10 +435,11 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector state) { diff --git a/ov_msckf/src/update/UpdaterSLAM.h b/ov_msckf/src/update/UpdaterSLAM.h index 35dfb50ec..f2720d98f 100644 --- a/ov_msckf/src/update/UpdaterSLAM.h +++ b/ov_msckf/src/update/UpdaterSLAM.h @@ -22,6 +22,9 @@ #ifndef OV_MSCKF_UPDATER_SLAM_H #define OV_MSCKF_UPDATER_SLAM_H +#include +#include + #include "feat/Feature.h" #include "feat/FeatureInitializer.h" #include "feat/FeatureInitializerOptions.h" @@ -30,8 +33,8 @@ #include "types/Landmark.h" #include "types/LandmarkRepresentation.h" #include "utils/colors.h" +#include "utils/print.h" #include "utils/quat_ops.h" -#include #include "UpdaterHelper.h" #include "UpdaterOptions.h" @@ -60,7 +63,7 @@ class UpdaterSLAM { * @param options_aruco Updater options (include measurement noise value) for ARUCO features * @param feat_init_options Feature initializer options */ - UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, FeatureInitializerOptions &feat_init_options) + UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options) : _options_slam(options_slam), _options_aruco(options_aruco) { // Save our raw pixel noise squared @@ -68,7 +71,7 @@ class UpdaterSLAM { _options_aruco.sigma_pix_sq = std::pow(_options_aruco.sigma_pix, 2); // Save our feature initializer - initializer_feat = std::unique_ptr(new FeatureInitializer(feat_init_options)); + initializer_feat = std::unique_ptr(new ov_core::FeatureInitializer(feat_init_options)); // Initialize the chi squared test table with confidence level 0.95 // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 @@ -83,14 +86,14 @@ class UpdaterSLAM { * @param state State of the filter * @param feature_vec Features that can be used for update */ - void update(std::shared_ptr state, std::vector> &feature_vec); + void update(std::shared_ptr state, std::vector> &feature_vec); /** * @brief Given max track features, this will try to use them to initialize them in the state. * @param state State of the filter * @param feature_vec Features that can be used for update */ - void delayed_init(std::shared_ptr state, std::vector> &feature_vec); + void delayed_init(std::shared_ptr state, std::vector> &feature_vec); /** * @brief Will change SLAM feature anchors if it will be marginalized @@ -110,7 +113,7 @@ class UpdaterSLAM { * @param new_anchor_timestamp Clone timestamp we want to move to * @param new_cam_id Which camera frame we want to move to */ - void perform_anchor_change(std::shared_ptr state, std::shared_ptr landmark, double new_anchor_timestamp, + void perform_anchor_change(std::shared_ptr state, std::shared_ptr landmark, double new_anchor_timestamp, size_t new_cam_id); /// Options used during update for slam features @@ -120,7 +123,7 @@ class UpdaterSLAM { UpdaterOptions _options_aruco; /// Feature initializer class object - std::unique_ptr initializer_feat; + std::unique_ptr initializer_feat; /// Chi squared 95th percentile table (lookup would be size of residual) std::map chi_squared_table; diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 3b4a0bdbc..e632efbe9 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -20,8 +20,9 @@ */ #include "UpdaterZeroVelocity.h" -#include "utils/print.h" +using namespace ov_core; +using namespace ov_type; using namespace ov_msckf; bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timestamp) { @@ -203,10 +204,10 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest } disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20); if (disparity_passed) { - PRINT_WARNING(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, + PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, (int)num_features); } else { - PRINT_INFO(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, + PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, (int)num_features); } } @@ -215,11 +216,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest // We need to pass the chi2 and not be above our velocity threshold if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) { last_zupt_state_timestamp = 0.0; - PRINT_WARNING(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, + PRINT_DEBUG(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, _options.chi2_multipler * chi2_check); return false; } - PRINT_DEBUG(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, + PRINT_INFO(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, _options.chi2_multipler * chi2_check); // Do our update, only do this update if we have previously detected diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index af1756129..c9eec9d30 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -29,6 +29,7 @@ #include "utils/colors.h" #include "utils/quat_ops.h" #include "utils/sensor_data.h" +#include "utils/print.h" #include "UpdaterHelper.h" #include "UpdaterOptions.h" @@ -61,7 +62,7 @@ class UpdaterZeroVelocity { * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) * @param zupt_max_disparity Max disparity we should consider to do a update with */ - UpdaterZeroVelocity(UpdaterOptions &options, Propagator::NoiseManager &noises, std::shared_ptr db, + UpdaterZeroVelocity(UpdaterOptions &options, Propagator::NoiseManager &noises, std::shared_ptr db, std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, double zupt_max_disparity) : _options(options), _noises(noises), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), @@ -127,7 +128,7 @@ class UpdaterZeroVelocity { Propagator::NoiseManager _noises; /// Feature tracker database with all features in it - std::shared_ptr _db; + std::shared_ptr _db; /// Our propagator! std::shared_ptr _prop; @@ -154,7 +155,7 @@ class UpdaterZeroVelocity { double last_prop_time_offset = 0.0; bool have_last_prop_time_offset = false; - // Last timestamp we did zero velocity update with + /// Last timestamp we did zero velocity update with double last_zupt_state_timestamp = 0.0; }; diff --git a/ov_msckf/src/utils/parse_cmd.h b/ov_msckf/src/utils/parse_cmd.h index 0f72b8f9e..9d64287fb 100644 --- a/ov_msckf/src/utils/parse_cmd.h +++ b/ov_msckf/src/utils/parse_cmd.h @@ -47,6 +47,10 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { // ESTIMATOR ====================================================================== + // Verbosity + std::string verbosity; + app1.add_option("--verbosity", verbosity, ""); + // Main EKF parameters app1.add_option("--use_fej", params.state_options.do_fej, ""); app1.add_option("--use_imuavg", params.state_options.imu_avg, ""); @@ -171,16 +175,19 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { std::exit(app1.exit(e)); } + // Verbosity setting + ov_core::Printer::setPrintLevel(verbosity); + // Set what representation we should be using std::transform(feat_rep_msckf_str.begin(), feat_rep_msckf_str.end(), feat_rep_msckf_str.begin(), ::toupper); std::transform(feat_rep_slam_str.begin(), feat_rep_slam_str.end(), feat_rep_slam_str.begin(), ::toupper); std::transform(feat_rep_aruco_str.begin(), feat_rep_aruco_str.end(), feat_rep_aruco_str.begin(), ::toupper); - params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string(feat_rep_msckf_str); - params.state_options.feat_rep_slam = LandmarkRepresentation::from_string(feat_rep_slam_str); - params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string(feat_rep_aruco_str); - if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) { + params.state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(feat_rep_msckf_str); + params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string(feat_rep_slam_str); + params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(feat_rep_aruco_str); + if (params.state_options.feat_rep_msckf == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_slam == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_aruco == ov_type::LandmarkRepresentation::Representation::UNKNOWN) { PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); @@ -200,11 +207,11 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { // Preprocessing histogram method if (histogram_method_str == "NONE") { - params.histogram_method = TrackBase::NONE; + params.histogram_method = ov_core::TrackBase::NONE; } else if (histogram_method_str == "HISTOGRAM") { - params.histogram_method = TrackBase::HISTOGRAM; + params.histogram_method = ov_core::TrackBase::HISTOGRAM; } else if (histogram_method_str == "CLAHE") { - params.histogram_method = TrackBase::CLAHE; + params.histogram_method = ov_core::TrackBase::CLAHE; } else { PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); PRINT_ERROR(RED "\t- NONE\n" RESET); diff --git a/ov_msckf/src/utils/parse_ros.h b/ov_msckf/src/utils/parse_ros.h index 8913f8955..ef215a373 100644 --- a/ov_msckf/src/utils/parse_ros.h +++ b/ov_msckf/src/utils/parse_ros.h @@ -43,6 +43,11 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { // ESTIMATOR ====================================================================== + // Verbosity setting + std::string verbosity; + nh.param("verbosity", verbosity, "INFO"); + ov_core::Printer::setPrintLevel(verbosity); + // Main EKF parameters nh.param("use_fej", params.state_options.do_fej, params.state_options.do_fej); nh.param("use_imuavg", params.state_options.imu_avg, params.state_options.imu_avg); @@ -77,12 +82,12 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { std::transform(feat_rep_msckf_str.begin(), feat_rep_msckf_str.end(), feat_rep_msckf_str.begin(), ::toupper); std::transform(feat_rep_slam_str.begin(), feat_rep_slam_str.end(), feat_rep_slam_str.begin(), ::toupper); std::transform(feat_rep_aruco_str.begin(), feat_rep_aruco_str.end(), feat_rep_aruco_str.begin(), ::toupper); - params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string(feat_rep_msckf_str); - params.state_options.feat_rep_slam = LandmarkRepresentation::from_string(feat_rep_slam_str); - params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string(feat_rep_aruco_str); - if (params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) { + params.state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(feat_rep_msckf_str); + params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string(feat_rep_slam_str); + params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(feat_rep_aruco_str); + if (params.state_options.feat_rep_msckf == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_slam == ov_type::LandmarkRepresentation::Representation::UNKNOWN || + params.state_options.feat_rep_aruco == ov_type::LandmarkRepresentation::Representation::UNKNOWN) { PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); @@ -155,11 +160,11 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { std::string histogram_method_str = "HISTOGRAM"; nh.param("histogram_method", histogram_method_str, histogram_method_str); if (histogram_method_str == "NONE") { - params.histogram_method = TrackBase::NONE; + params.histogram_method = ov_core::TrackBase::NONE; } else if (histogram_method_str == "HISTOGRAM") { - params.histogram_method = TrackBase::HISTOGRAM; + params.histogram_method = ov_core::TrackBase::HISTOGRAM; } else if (histogram_method_str == "CLAHE") { - params.histogram_method = TrackBase::CLAHE; + params.histogram_method = ov_core::TrackBase::CLAHE; } else { PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); PRINT_ERROR(RED "\t- NONE\n" RESET); @@ -260,7 +265,7 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { // Load these into our state Eigen::Matrix cam_eigen; - cam_eigen.block(0, 0, 4, 1) = rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); + cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); // Insert From b88cd54b2222252eedf8932a2a1d715844cab0fd Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 1 Nov 2021 21:27:56 -0400 Subject: [PATCH 08/55] cleanup simulator to use camera objects --- ov_core/src/utils/dataset_reader.h | 65 ++++++++++ ov_msckf/src/core/VioManager.cpp | 2 +- ov_msckf/src/core/VioManagerOptions.h | 8 ++ ov_msckf/src/sim/Simulator.cpp | 169 +++++--------------------- ov_msckf/src/sim/Simulator.h | 13 +- ov_msckf/src/utils/parse_cmd.h | 4 + ov_msckf/src/utils/parse_ros.h | 4 + 7 files changed, 121 insertions(+), 144 deletions(-) diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h index cf1ac8cda..8edd6c707 100644 --- a/ov_core/src/utils/dataset_reader.h +++ b/ov_core/src/utils/dataset_reader.h @@ -169,6 +169,71 @@ class DatasetReader { return true; } + /** + * @brief This will load the trajectory into memory (space separated) + * @param path Path to the trajectory file that we want to read in. + * @param path_traj Will be filled with groundtruth states (timestamp(s), q_GtoI, p_IinG) + */ + static void load_simulated_trajectory(std::string path, std::vector &traj_data) { + + // Try to open our groundtruth file + std::ifstream file; + file.open(path); + if (!file) { + PRINT_ERROR(RED "ERROR: Unable to open simulation trajectory file...\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, path.c_str()); + std::exit(EXIT_FAILURE); + } + + // Debug print + std::string base_filename = path.substr(path.find_last_of("/\\") + 1); + PRINT_DEBUG("loaded trajectory %s\n", base_filename.c_str()); + + // Loop through each line of this file + std::string current_line; + while (std::getline(file, current_line)) { + + // Skip if we start with a comment + if (!current_line.find("#")) + continue; + + // Loop variables + int i = 0; + std::istringstream s(current_line); + std::string field; + Eigen::Matrix data; + + // Loop through this line (timestamp(s) tx ty tz qx qy qz qw) + while (std::getline(s, field, ' ')) { + // Skip if empty + if (field.empty() || i >= data.rows()) + continue; + // save the data to our vector + data(i) = std::atof(field.c_str()); + i++; + } + + // Only a valid line if we have all the parameters + if (i > 7) { + traj_data.push_back(data); + + // std::stringstream ss; + // ss << std::setprecision(15) << data.transpose() << std::endl; + // PRINT_DEBUG(ss.str().c_str()); + } + } + + // Finally close the file + file.close(); + + // Error if we don't have any data + if (traj_data.empty()) { + PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET); + PRINT_ERROR(RED "ERROR: %s\n" RESET, path.c_str()); + std::exit(EXIT_FAILURE); + } + } + private: /** * All function in this class should be static. diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index a8fd939ea..a5d5d1f11 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -54,7 +54,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { state->_calib_dt_CAMtoIMU->set_value(temp_camimu_dt); state->_calib_dt_CAMtoIMU->set_fej(temp_camimu_dt); - // Loop through through, and load each of the cameras + // Loop through and load each of the cameras for (int i = 0; i < state->_options.num_cameras; i++) { // Create the actual camera object and set the values diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 97752bf32..847ad74ce 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -281,6 +281,12 @@ struct VioManagerOptions { /// Frequency (Hz) that we will simulate our inertial measurement unit double sim_freq_imu = 400.0; + /// Feature distance we generate features from (minimum) + double sim_min_feature_gen_distance = 5; + + /// Feature distance we generate features from (maximum) + double sim_max_feature_gen_distance = 10; + /** * @brief This function will print out all simulated parameters loaded. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -295,6 +301,8 @@ struct VioManagerOptions { PRINT_DEBUG("\t- dist thresh: %.2f\n", sim_distance_threshold); PRINT_DEBUG("\t- cam feq: %.2f\n", sim_freq_cam); PRINT_DEBUG("\t- imu feq: %.2f\n", sim_freq_imu); + PRINT_DEBUG("\t- min feat dist: %.2f\n", sim_min_feature_gen_distance); + PRINT_DEBUG("\t- max feat dist: %.2f\n", sim_max_feature_gen_distance); } }; diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 5308c55c7..0ef8e0e97 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -50,8 +50,19 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { std::exit(EXIT_FAILURE); } + // Loop through and load each of the cameras + for (int i = 0; i < params.state_options.num_cameras; i++) { + if (params.camera_fisheye.at(i)) { + _cam_intrinsics_cameras.insert({i, std::make_shared()}); + _cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); + } else { + _cam_intrinsics_cameras.insert({i, std::make_shared()}); + _cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); + } + } + // Load the groundtruth trajectory and its spline - load_data(params.sim_traj_path); + DatasetReader::load_simulated_trajectory(params.sim_traj_path, traj_data); spline.feed_trajectory(traj_data); // Set all our timestamps as starting from the minimum spline time @@ -377,71 +388,16 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, feats.push_back(uvs); camids.push_back(i); } + /** + * @brief This will load the trajectory into memory. + * @param path_traj Path to the trajectory file that we want to read in. + */ + void load_data(std::string path_traj); // Return success return true; } -void Simulator::load_data(std::string path_traj) { - - // Try to open our groundtruth file - std::ifstream file; - file.open(path_traj); - if (!file) { - PRINT_ERROR(RED "ERROR: Unable to open simulation trajectory file...\n" RESET); - PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str()); - std::exit(EXIT_FAILURE); - } - - // Debug print - std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1); - PRINT_DEBUG("[SIM]: loaded trajectory %s\n", base_filename.c_str()); - - // Loop through each line of this file - std::string current_line; - while (std::getline(file, current_line)) { - - // Skip if we start with a comment - if (!current_line.find("#")) - continue; - - // Loop variables - int i = 0; - std::istringstream s(current_line); - std::string field; - Eigen::Matrix data; - - // Loop through this line (timestamp(s) tx ty tz qx qy qz qw) - while (std::getline(s, field, ' ')) { - // Skip if empty - if (field.empty() || i >= data.rows()) - continue; - // save the data to our vector - data(i) = std::atof(field.c_str()); - i++; - } - - // Only a valid line if we have all the parameters - if (i > 7) { - traj_data.push_back(data); - - // std::stringstream ss; - // ss << std::setprecision(15) << data.transpose() << std::endl; - // PRINT_DEBUG(ss.str().c_str()); - } - } - - // Finally close the file - file.close(); - - // Error if we don't have any data - if (traj_data.empty()) { - PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET); - PRINT_ERROR(RED "ERROR: %s\n" RESET, path_traj.c_str()); - std::exit(EXIT_FAILURE); - } -} - std::vector> Simulator::project_pointcloud(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, int camid, const std::unordered_map &feats) { @@ -450,13 +406,13 @@ std::vector> Simulator::project_pointcloud(co assert(camid < params.state_options.num_cameras); assert((int)params.camera_fisheye.size() == params.state_options.num_cameras); assert((int)params.camera_wh.size() == params.state_options.num_cameras); - assert((int)params.camera_intrinsics.size() == params.state_options.num_cameras); + assert((int)_cam_intrinsics_cameras.size() == params.state_options.num_cameras); assert((int)params.camera_extrinsics.size() == params.state_options.num_cameras); // Grab our extrinsic and intrinsic values Eigen::Matrix R_ItoC = quat_2_Rot(params.camera_extrinsics.at(camid).block(0, 0, 4, 1)); Eigen::Matrix p_IinC = params.camera_extrinsics.at(camid).block(4, 0, 3, 1); - Eigen::Matrix cam_d = params.camera_intrinsics.at(camid); + std::shared_ptr camera = _cam_intrinsics_cameras.at(camid); // Our projected uv true measurements std::vector> uvs; @@ -474,43 +430,11 @@ std::vector> Simulator::project_pointcloud(co // Project to normalized coordinates Eigen::Vector2f uv_norm; - uv_norm << p_FinC(0) / p_FinC(2), p_FinC(1) / p_FinC(2); + uv_norm << (float)(p_FinC(0) / p_FinC(2)), (float)(p_FinC(1) / p_FinC(2)); - // Distort the normalized coordinates (false=radtan, true=fisheye) + // Distort the normalized coordinates Eigen::Vector2f uv_dist; - - // Calculate distortion uv and jacobian - if (params.camera_fisheye.at(camid)) { - - // Calculate distorted coordinates for fisheye - double r = sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1)); - double theta = std::atan(r); - double theta_d = theta + cam_d(4) * std::pow(theta, 3) + cam_d(5) * std::pow(theta, 5) + cam_d(6) * std::pow(theta, 7) + - cam_d(7) * std::pow(theta, 9); - - // Handle when r is small (meaning our xy is near the camera center) - double inv_r = r > 1e-8 ? 1.0 / r : 1; - double cdist = r > 1e-8 ? theta_d * inv_r : 1; - - // Calculate distorted coordinates for fisheye - double x1 = uv_norm(0) * cdist; - double y1 = uv_norm(1) * cdist; - uv_dist(0) = cam_d(0) * x1 + cam_d(2); - uv_dist(1) = cam_d(1) * y1 + cam_d(3); - - } else { - - // Calculate distorted coordinates for radial - double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1)); - double r_2 = r * r; - double r_4 = r_2 * r_2; - double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) + - cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0)); - double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) + - 2 * cam_d(7) * uv_norm(0) * uv_norm(1); - uv_dist(0) = cam_d(0) * x1 + cam_d(2); - uv_dist(1) = cam_d(1) * y1 + cam_d(3); - } + uv_dist = camera->distort_f(uv_norm); // Check that it is inside our bounds if (uv_dist(0) < 0 || uv_dist(0) > params.camera_wh.at(camid).first || uv_dist(1) < 0 || @@ -533,30 +457,13 @@ void Simulator::generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vect assert(camid < params.state_options.num_cameras); assert((int)params.camera_fisheye.size() == params.state_options.num_cameras); assert((int)params.camera_wh.size() == params.state_options.num_cameras); - assert((int)params.camera_intrinsics.size() == params.state_options.num_cameras); + assert((int)_cam_intrinsics_cameras.size() == params.state_options.num_cameras); assert((int)params.camera_extrinsics.size() == params.state_options.num_cameras); // Grab our extrinsic and intrinsic values Eigen::Matrix R_ItoC = quat_2_Rot(params.camera_extrinsics.at(camid).block(0, 0, 4, 1)); Eigen::Matrix p_IinC = params.camera_extrinsics.at(camid).block(4, 0, 3, 1); - Eigen::Matrix cam_d = params.camera_intrinsics.at(camid); - - // Convert to opencv format since we will use their undistort functions - cv::Matx33d camK; - camK(0, 0) = cam_d(0); - camK(0, 1) = 0; - camK(0, 2) = cam_d(2); - camK(1, 0) = 0; - camK(1, 1) = cam_d(1); - camK(1, 2) = cam_d(3); - camK(2, 0) = 0; - camK(2, 1) = 0; - camK(2, 2) = 1; - cv::Vec4d camD; - camD(0) = cam_d(4); - camD(1) = cam_d(5); - camD(2) = cam_d(6); - camD(3) = cam_d(7); + std::shared_ptr camera = _cam_intrinsics_cameras.at(camid); // Generate the desired number of features for (int i = 0; i < numpts; i++) { @@ -568,33 +475,21 @@ void Simulator::generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vect double v_dist = gen_v(gen_state_init); // Convert to opencv format - cv::Mat mat(1, 2, CV_32F); - mat.at(0, 0) = u_dist; - mat.at(0, 1) = v_dist; - mat = mat.reshape(2); // Nx1, 2-channel - - // Undistort this point to our normalized coordinates (false=radtan, true=fisheye) - if (params.camera_fisheye.at(camid)) { - cv::fisheye::undistortPoints(mat, mat, camK, camD); - } else { - cv::undistortPoints(mat, mat, camK, camD); - } + cv::Point2f uv_dist((float)u_dist, (float)v_dist); - // Construct our return vector - Eigen::Vector3d uv_norm; - mat = mat.reshape(1); // Nx2, 1-channel - uv_norm(0) = mat.at(0, 0); - uv_norm(1) = mat.at(0, 1); - uv_norm(2) = 1; + // Undistort this point to our normalized coordinates + cv::Point2f uv_norm; + uv_norm = camera->undistort_cv(uv_dist); // Generate a random depth - // TODO: we should probably have this as a simulation parameter - std::uniform_real_distribution gen_depth(5, 10); + std::uniform_real_distribution gen_depth(params.sim_min_feature_gen_distance, params.sim_max_feature_gen_distance); double depth = gen_depth(gen_state_init); // Get the 3d point + Eigen::Vector3d bearing; + bearing << uv_norm.x, uv_norm.y, 1; Eigen::Vector3d p_FinC; - p_FinC = depth * uv_norm; + p_FinC = depth * bearing; // Move to the global frame of reference Eigen::Vector3d p_FinI = R_ItoC.transpose() * (p_FinC - p_IinC); diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index 5378928be..77a9cbb98 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -30,9 +30,13 @@ #include #include +#include "cam/CamBase.h" +#include "cam/CamEqui.h" +#include "cam/CamRadtan.h" #include "core/VioManagerOptions.h" #include "sim/BsplineSE3.h" #include "utils/colors.h" +#include "utils/dataset_reader.h" namespace ov_msckf { @@ -101,12 +105,6 @@ class Simulator { VioManagerOptions get_true_paramters() { return params; } protected: - /** - * @brief This will load the trajectory into memory. - * @param path_traj Path to the trajectory file that we want to read in. - */ - void load_data(std::string path_traj); - /** * @brief Projects the passed map features into the desired camera frame. * @param R_GtoI Orientation of the IMU pose @@ -136,6 +134,9 @@ class Simulator { /// True vio manager params (a copy of the parsed ones) VioManagerOptions params; + /// Camera intrinsics camera objects + std::unordered_map> _cam_intrinsics_cameras; + //=================================================================== // State related variables //=================================================================== diff --git a/ov_msckf/src/utils/parse_cmd.h b/ov_msckf/src/utils/parse_cmd.h index 9d64287fb..79f3f10fa 100644 --- a/ov_msckf/src/utils/parse_cmd.h +++ b/ov_msckf/src/utils/parse_cmd.h @@ -166,6 +166,10 @@ VioManagerOptions parse_command_line_arguments(int argc, char **argv) { app1.add_option("--sim_seed_preturb", params.sim_seed_preturb, ""); app1.add_option("--sim_seed_measurements", params.sim_seed_measurements, ""); + // Other simulation parameters + app1.add_option("--sim_min_feat_distance", params.sim_min_feature_gen_distance, ""); + app1.add_option("--sim_max_feat_distance", params.sim_max_feature_gen_distance, ""); + // CMD PARSE ============================================================================== // Finally actually parse the command line and load it diff --git a/ov_msckf/src/utils/parse_ros.h b/ov_msckf/src/utils/parse_ros.h index ef215a373..631f2b12c 100644 --- a/ov_msckf/src/utils/parse_ros.h +++ b/ov_msckf/src/utils/parse_ros.h @@ -219,6 +219,10 @@ VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { nh.param("sim_seed_preturb", params.sim_seed_preturb, params.sim_seed_preturb); nh.param("sim_seed_measurements", params.sim_seed_measurements, params.sim_seed_measurements); + // Other simulation parameters + nh.param("sim_min_feat_distance", params.sim_min_feature_gen_distance, params.sim_min_feature_gen_distance); + nh.param("sim_max_feat_distance", params.sim_max_feature_gen_distance, params.sim_max_feature_gen_distance); + //==================================================================================== //==================================================================================== //==================================================================================== From ddb52d9c4bc097dd78e73bfb6b61457c6793a46c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 2 Nov 2021 19:51:18 -0400 Subject: [PATCH 09/55] move to new config system with YAML files --- docs/gs-datasets.dox | 8 +- ov_core/CMakeLists.txt | 5 +- ov_core/src/feat/FeatureInitializerOptions.h | 17 +- ov_core/src/test_tracking.cpp | 80 +++- ov_core/src/test_webcam.cpp | 81 +++- ov_core/src/track/Grider_FAST.h | 2 +- ov_core/src/track/TrackAruco.h | 2 +- ov_core/src/track/TrackBase.h | 2 +- .../{lambda_body.h => opencv_lambda_body.h} | 6 +- ov_core/src/utils/opencv_yaml_parse.h | 449 ++++++++++++++++++ ov_core/src/utils/print.cpp | 2 +- ov_msckf/CMakeLists.txt | 5 +- .../config/euroc_mav/estimator_config.yaml | 93 ++++ .../config/euroc_mav/kalibr_imu_chain.yaml | 16 + .../config/euroc_mav/kalibr_imucam_chain.yaml | 28 ++ ov_msckf/config/kaist/kalibr_imu_chain.yaml | 19 + .../config/kaist/kalibr_imucam_chain.yaml | 28 ++ .../kaist}/pgeneva_ros_kaist.launch | 0 .../config/kaist_vio/kalibr_imu_chain.yaml | 16 + .../config/kaist_vio/kalibr_imucam_chain.yaml | 93 ++++ .../kaist_vio}/pgeneva_ros_kaistvio.launch | 0 .../config/rpng_aruco/estimator_config.yaml | 92 ++++ .../config/rpng_aruco/kalibr_imu_chain.yaml | 16 + .../rpng_aruco/kalibr_imucam_chain.yaml | 28 ++ .../rpng_ironsides/estimator_config.yaml | 92 ++++ .../rpng_ironsides/kalibr_imu_chain.yaml | 16 + .../rpng_ironsides/kalibr_imucam_chain.yaml | 31 ++ ov_msckf/config/tum_vi/estimator_config.yaml | 92 ++++ ov_msckf/config/tum_vi/kalibr_imu_chain.yaml | 16 + .../config/tum_vi/kalibr_imucam_chain.yaml | 33 ++ .../uzhfpv_indoor/kalibr_imu_chain.yaml | 16 + .../uzhfpv_indoor/kalibr_imucam_chain.yaml | 37 ++ .../uzhfpv_indoor_45/kalibr_imu_chain.yaml | 16 + .../uzhfpv_indoor_45/kalibr_imucam_chain.yaml | 37 ++ .../uzhfpv_outdoor/kalibr_imu_chain.yaml | 16 + .../uzhfpv_outdoor/kalibr_imucam_chain.yaml | 37 ++ .../uzhfpv_outdoor_45/kalibr_imu_chain.yaml | 16 + .../kalibr_imucam_chain.yaml | 37 ++ ov_msckf/launch/pgeneva_ros_eth.launch | 146 ------ ov_msckf/launch/pgeneva_ros_tum.launch | 139 ------ ov_msckf/launch/pgeneva_serial_aruco.launch | 108 ----- ov_msckf/launch/pgeneva_serial_eth.launch | 129 ----- ov_msckf/launch/pgeneva_serial_tum.launch | 122 ----- ov_msckf/launch/serial.launch | 75 +++ ov_msckf/launch/subscribe.launch | 71 +++ ov_msckf/src/core/RosVisualizer.cpp | 22 +- ov_msckf/src/core/VioManager.cpp | 8 +- ov_msckf/src/core/VioManager.h | 2 +- ov_msckf/src/core/VioManagerOptions.h | 253 +++++++--- ov_msckf/src/ros_serial_msckf.cpp | 33 +- ov_msckf/src/ros_subscribe_msckf.cpp | 31 +- ov_msckf/src/run_simulation.cpp | 37 +- ov_msckf/src/sim/Simulator.cpp | 10 +- ov_msckf/src/state/Propagator.h | 8 +- ov_msckf/src/state/StateOptions.h | 60 ++- ov_msckf/src/test_sim_meas.cpp | 9 +- ov_msckf/src/test_sim_repeat.cpp | 21 +- ov_msckf/src/update/UpdaterOptions.h | 4 +- ov_msckf/src/utils/parse_cmd.h | 294 ------------ ov_msckf/src/utils/parse_ros.h | 289 ----------- 60 files changed, 2028 insertions(+), 1423 deletions(-) rename ov_core/src/utils/{lambda_body.h => opencv_lambda_body.h} (94%) create mode 100644 ov_core/src/utils/opencv_yaml_parse.h create mode 100644 ov_msckf/config/euroc_mav/estimator_config.yaml create mode 100644 ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/kaist/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/kaist/kalibr_imucam_chain.yaml rename ov_msckf/{launch => config/kaist}/pgeneva_ros_kaist.launch (100%) create mode 100644 ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml rename ov_msckf/{launch => config/kaist_vio}/pgeneva_ros_kaistvio.launch (100%) create mode 100644 ov_msckf/config/rpng_aruco/estimator_config.yaml create mode 100644 ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/rpng_ironsides/estimator_config.yaml create mode 100644 ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/tum_vi/estimator_config.yaml create mode 100644 ov_msckf/config/tum_vi/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml delete mode 100644 ov_msckf/launch/pgeneva_ros_eth.launch delete mode 100644 ov_msckf/launch/pgeneva_ros_tum.launch delete mode 100644 ov_msckf/launch/pgeneva_serial_aruco.launch delete mode 100644 ov_msckf/launch/pgeneva_serial_eth.launch delete mode 100644 ov_msckf/launch/pgeneva_serial_tum.launch create mode 100644 ov_msckf/launch/serial.launch create mode 100644 ov_msckf/launch/subscribe.launch delete mode 100644 ov_msckf/src/utils/parse_cmd.h delete mode 100644 ov_msckf/src/utils/parse_ros.h diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox index 951d21586..2f564e469 100644 --- a/docs/gs-datasets.dox +++ b/docs/gs-datasets.dox @@ -152,7 +152,7 @@ Please take a look at the [run_ros_uzhfpv.sh](https://github.com/rpng/open_vins/ @section gs-data-kaist KAIST Urban Dataset -The [KAIST urban dataset](https://irap.kaist.ac.kr/dataset/index.html) @cite Jeong2019IJRR is a dataset focus on autonomous driving and localization in challenging complex urban environments. +The [KAIST urban dataset](https://sites.google.com/view/complex-urban-dataset) @cite Jeong2019IJRR is a dataset focus on autonomous driving and localization in challenging complex urban environments. The dataset was collected in Korea with a vehicle equipped with stereo camera pair, 2d SICK LiDARs, 3d Velodyne LiDAR, Xsens IMU, fiber optic gyro (FoG), wheel encoders, and RKT GPS. The camera is 10 Hz, while the Xsens IMU is 100 Hz sensing rate. A groundtruth "baseline" trajectory is also provided which is the resulting output from fusion of the FoG, RKT GPS, and wheel encoders. @@ -181,9 +181,9 @@ Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and @m_div{m-text-center} | Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch | |-------------:|--------|--------------|------------------|------------------| -| Urban 28 | 11.47 | [download](https://irap.kaist.ac.kr/dataset/download_2.html) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | -| Urban 38 | 11.42 | [download](https://irap.kaist.ac.kr/dataset/download_2.html) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | -| Urban 39 | 11.06 | [download](https://irap.kaist.ac.kr/dataset/download_2.html) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | +| Urban 28 | 11.47 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | +| Urban 38 | 11.42 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | +| Urban 39 | 11.06 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | @m_enddiv diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 3634e724e..63b9e87fb 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -11,10 +11,11 @@ project(ov_core) find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge) # Include libraries (if we don't have opencv 4, then fallback to opencv 3) +# The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! find_package(Eigen3 REQUIRED) -find_package(OpenCV 4 QUIET) +find_package(OpenCV 3 QUIET) if (NOT OpenCV_FOUND) - find_package(OpenCV 3 REQUIRED) + find_package(OpenCV 4 REQUIRED) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION}) diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h index 335a7e112..c5252fec0 100644 --- a/ov_core/src/feat/FeatureInitializerOptions.h +++ b/ov_core/src/feat/FeatureInitializerOptions.h @@ -23,6 +23,7 @@ #define OV_CORE_INITIALIZEROPTIONS_H #include "utils/print.h" +#include "utils/opencv_yaml_parse.h" namespace ov_core { @@ -68,7 +69,21 @@ struct FeatureInitializerOptions { double max_cond_number = 10000; /// Nice print function of what parameters we have loaded - void print() { + void print(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("fi_triangulate_1d", triangulate_1d, false); + parser->parse_config("fi_refine_features", refine_features, false); + parser->parse_config("fi_max_runs", max_runs, false); + parser->parse_config("fi_init_lamda", init_lamda, false); + parser->parse_config("fi_max_lamda", max_lamda, false); + parser->parse_config("fi_min_dx", min_dx, false); + parser->parse_config("fi_min_dcost", min_dcost, false); + parser->parse_config("fi_lam_mult", lam_mult, false); + parser->parse_config("fi_min_dist", min_dist, false); + parser->parse_config("fi_max_dist", max_dist, false); + parser->parse_config("fi_max_baseline", max_baseline, false); + parser->parse_config("fi_max_cond_number", max_cond_number, false); + } PRINT_DEBUG("\t- triangulate_1d: %d\n", triangulate_1d); PRINT_DEBUG("\t- refine_features: %d\n", refine_features); PRINT_DEBUG("\t- max_runs: %d\n", max_runs); diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index 3d6084143..ef600eccc 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -38,6 +38,7 @@ #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" +#include "utils/opencv_yaml_parse.h" #include "utils/print.h" using namespace ov_core; @@ -60,19 +61,33 @@ void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1); // Main function int main(int argc, char **argv) { + + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path.txt"; + if (argc > 1) { + config_path = argv[1]; + } + + // Initialize this as a ROS node ros::init(argc, argv, "test_tracking"); ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); - // Verbosity setting - std::string verbosity; - nh.param("verbosity", verbosity, "INFO"); + // Load parameters + auto parser = std::make_shared(config_path, false); + parser->set_node_handler(nh); + + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); ov_core::Printer::setPrintLevel(verbosity); // Our camera topics (left and right stereo) - std::string topic_camera0; - std::string topic_camera1; + std::string topic_camera0, topic_camera1; nh.param("topic_camera0", topic_camera0, "/cam0/image_raw"); nh.param("topic_camera1", topic_camera1, "/cam1/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", topic_camera0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", topic_camera1); // Location of the ROS bag we want to read in std::string path_to_bag; @@ -91,20 +106,43 @@ int main(int argc, char **argv) { //=================================================================================== // Parameters for our extractor - ov_core::TrackBase::HistogramMethod method = ov_core::TrackBase::HistogramMethod::NONE; - int num_pts, num_aruco, fast_threshold, grid_x, grid_y, min_px_dist; - double knn_ratio; - bool do_downsizing, use_stereo; - nh.param("num_pts", num_pts, 400); - nh.param("num_aruco", num_aruco, 1024); - nh.param("clone_states", clone_states, 11); - nh.param("fast_threshold", fast_threshold, 10); - nh.param("grid_x", grid_x, 9); - nh.param("grid_y", grid_y, 7); - nh.param("min_px_dist", min_px_dist, 10); - nh.param("knn_ratio", knn_ratio, 0.70); - nh.param("downsize_aruco", do_downsizing, false); - nh.param("use_stereo", use_stereo, false); + int num_pts = 400; + int num_aruco = 1024; + int fast_threshold = 15; + int grid_x = 9; + int grid_y = 7; + int min_px_dist = 10; + double knn_ratio = 0.70; + bool do_downsizing = false; + bool use_stereo = false; + parser->parse_config("num_pts", num_pts, false); + parser->parse_config("num_aruco", num_aruco, false); + parser->parse_config("clone_states", clone_states, false); + parser->parse_config("fast_threshold", fast_threshold, false); + parser->parse_config("grid_x", grid_x, false); + parser->parse_config("grid_y", grid_y, false); + parser->parse_config("min_px_dist", min_px_dist, false); + parser->parse_config("knn_ratio", knn_ratio, false); + parser->parse_config("do_downsizing", do_downsizing, false); + parser->parse_config("use_stereo", use_stereo, false); + + // Histogram method + ov_core::TrackBase::HistogramMethod method; + std::string histogram_method_str = "HISTOGRAM"; + parser->parse_config("histogram_method", histogram_method_str, false); + if (histogram_method_str == "NONE") { + method = ov_core::TrackBase::NONE; + } else if (histogram_method_str == "HISTOGRAM") { + method = ov_core::TrackBase::HISTOGRAM; + } else if (histogram_method_str == "CLAHE") { + method = ov_core::TrackBase::CLAHE; + } else { + printf(RED "invalid feature histogram specified:\n" RESET); + printf(RED "\t- NONE\n" RESET); + printf(RED "\t- HISTOGRAM\n" RESET); + printf(RED "\t- CLAHE\n" RESET); + std::exit(EXIT_FAILURE); + } // Debug print! PRINT_DEBUG("max features: %d\n", num_pts); @@ -126,10 +164,10 @@ int main(int argc, char **argv) { } // Lets make a feature extractor - // extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); + extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist, // knn_ratio); - extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing); + // extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing); //=================================================================================== //=================================================================================== diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 5f31a5030..569cc091d 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -37,8 +37,12 @@ #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" -#include "utils/CLI11.hpp" #include "utils/print.h" +#include "utils/opencv_yaml_parse.h" + +#if ROS_AVAILABLE +#include +#endif using namespace ov_core; @@ -48,11 +52,31 @@ TrackBase *extractor; // Main function int main(int argc, char **argv) { - // Create our command line parser - CLI::App app{"test_webcam"}; + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path.txt"; + if (argc > 1) { + config_path = argv[1]; + } - // Defaults +#if ROS_AVAILABLE + // Initialize this as a ROS node + ros::init(argc, argv, "test_webcam"); + ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); +#endif + + // Load parameters + auto parser = std::make_shared(config_path, false); +#if ROS_AVAILABLE + parser->set_node_handler(nh); +#endif + + // Verbosity std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + + // Defaults int num_pts = 500; int num_aruco = 1024; int clone_states = 20; @@ -63,30 +87,35 @@ int main(int argc, char **argv) { double knn_ratio = 0.85; bool do_downsizing = false; bool use_stereo = false; - ov_core::TrackBase::HistogramMethod method = ov_core::TrackBase::HistogramMethod::NONE; - - // Parameters for our extractor - app.add_option("--verbosity", verbosity, "Print verbosity"); - app.add_option("--num_pts", num_pts, "Number of feature tracks"); - app.add_option("--num_aruco", num_aruco, "Number of aruco tag ids we have"); - app.add_option("--clone_states", clone_states, "Amount of clones to visualize track length with"); - app.add_option("--fast_threshold", fast_threshold, "Fast extraction threshold"); - app.add_option("--grid_x", grid_x, "Grid x size"); - app.add_option("--grid_y", grid_y, "Grid y size"); - app.add_option("--min_px_dist", min_px_dist, "Minimum number of pixels between different tracks"); - app.add_option("--knn_ratio", knn_ratio, "Knn descriptor ratio threshold"); - app.add_option("--do_downsizing", do_downsizing, "If we should downsize our arucotag images"); - - // Finally actually parse the command line and load it - try { - app.parse(argc, argv); - } catch (const CLI::ParseError &e) { - return app.exit(e); + parser->parse_config("num_pts", num_pts, false); + parser->parse_config("num_aruco", num_aruco, false); + parser->parse_config("clone_states", clone_states, false); + parser->parse_config("fast_threshold", fast_threshold, false); + parser->parse_config("grid_x", grid_x, false); + parser->parse_config("grid_y", grid_y, false); + parser->parse_config("min_px_dist", min_px_dist, false); + parser->parse_config("knn_ratio", knn_ratio, false); + parser->parse_config("do_downsizing", do_downsizing, false); + parser->parse_config("use_stereo", use_stereo, false); + + // Histogram method + ov_core::TrackBase::HistogramMethod method; + std::string histogram_method_str = "HISTOGRAM"; + parser->parse_config("histogram_method", histogram_method_str, false); + if (histogram_method_str == "NONE") { + method = ov_core::TrackBase::NONE; + } else if (histogram_method_str == "HISTOGRAM") { + method = ov_core::TrackBase::HISTOGRAM; + } else if (histogram_method_str == "CLAHE") { + method = ov_core::TrackBase::CLAHE; + } else { + printf(RED "invalid feature histogram specified:\n" RESET); + printf(RED "\t- NONE\n" RESET); + printf(RED "\t- HISTOGRAM\n" RESET); + printf(RED "\t- CLAHE\n" RESET); + std::exit(EXIT_FAILURE); } - // Verbosity setting - ov_core::Printer::setPrintLevel(verbosity); - // Debug print! PRINT_DEBUG("max features: %d\n", num_pts); PRINT_DEBUG("max aruco: %d\n", num_aruco); diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h index 9abd222e1..7f585e76c 100644 --- a/ov_core/src/track/Grider_FAST.h +++ b/ov_core/src/track/Grider_FAST.h @@ -31,7 +31,7 @@ #include #include -#include "utils/lambda_body.h" +#include "utils/opencv_lambda_body.h" namespace ov_core { diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index 431d4c9e0..33be419d0 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -53,7 +53,7 @@ class TrackAruco : public TrackBase { HistogramMethod histmethod, bool downsize) : TrackBase(cameras, 0, numaruco, binocular, histmethod), max_tag_id(numaruco), do_downsizing(downsize) { #if ENABLE_ARUCO_TAGS - aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); + aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000); aruco_params = cv::aruco::DetectorParameters::create(); // NOTE: people with newer opencv might fail here // aruco_params->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_SUBPIX; diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h index 92c16dbb5..5bcaa4d47 100644 --- a/ov_core/src/track/TrackBase.h +++ b/ov_core/src/track/TrackBase.h @@ -37,7 +37,7 @@ #include "cam/CamBase.h" #include "feat/FeatureDatabase.h" #include "utils/colors.h" -#include "utils/lambda_body.h" +#include "utils/opencv_lambda_body.h" #include "utils/sensor_data.h" namespace ov_core { diff --git a/ov_core/src/utils/lambda_body.h b/ov_core/src/utils/opencv_lambda_body.h similarity index 94% rename from ov_core/src/utils/lambda_body.h rename to ov_core/src/utils/opencv_lambda_body.h index 55978dbe6..32b0cdcb7 100644 --- a/ov_core/src/utils/lambda_body.h +++ b/ov_core/src/utils/opencv_lambda_body.h @@ -19,8 +19,8 @@ * along with this program. If not, see . */ -#ifndef LAMBDA_BODY_H -#define LAMBDA_BODY_H +#ifndef OPENCV_LAMBDA_BODY_H +#define OPENCV_LAMBDA_BODY_H #include #include @@ -45,4 +45,4 @@ class LambdaBody : public cv::ParallelLoopBody { } /* namespace ov_core */ -#endif /* LAMBDA_BODY_H */ +#endif /* OPENCV_LAMBDA_BODY_H */ diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h new file mode 100644 index 000000000..a5ff3f6a4 --- /dev/null +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -0,0 +1,449 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OPENCV_YAML_PARSER_H +#define OPENCV_YAML_PARSER_H + +#include +#include +#include + +#if ROS_AVAILABLE +#include +#endif + +#include "colors.h" +#include "print.h" +#include "quat_ops.h" + +namespace ov_core { + +/** + * @brief Helper class to do OpenCV yaml parsing from both file and ROS. + * + * The logic is as follows: + * - Given a path to the main config file we will load it into our cv::FileStorage object. + * - From there the user can request for different parameters of different types from the config. + * - If we have ROS, then we will also check to see if the user has overridden any config files via ROS. + * - The ROS parameters always take priority over the ones in our config. + * + * NOTE: There are no "nested" yaml parameters. They are all under the "root" of the yaml file!!! + * NOTE: The camera and imu have nested, but those are handled externally.... + */ +class YamlParser { +public: + /** + * @brief Constructor that loads all three configuration files + * @param config_path Path to the YAML file we will parse + * @param fail_if_not_found If we should terminate the program if we can't open the config file + */ + explicit YamlParser(const std::string &config_path, bool fail_if_not_found = true) : config_path_(config_path) { + + // Check if file exists + if (!fail_if_not_found && !boost::filesystem::exists(config_path)) { + config = nullptr; + return; + } + if (!boost::filesystem::exists(config_path)) { + PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, config_path.c_str()); + std::exit(EXIT_FAILURE); + } + + // Open the file, error if we can't + config = std::make_shared(config_path, cv::FileStorage::READ); + if (!fail_if_not_found && !config->isOpened()) { + config = nullptr; + return; + } + if (!config->isOpened()) { + PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, config_path.c_str()); + std::exit(EXIT_FAILURE); + } + } + +#if ROS_AVAILABLE + /// Allows setting of the node handler if we have ROS to override parameters + void set_node_handler(ros::NodeHandle &nh_) { this->nh = nh_; } +#endif + + /** + * @brief Check to see if all parameters were read succesfully + * @return True if we found all parameters + */ + bool successful() const { return all_params_found_successfully; } + + /** + * @brief Custom parser for the ESTIMATOR parameters. + * + * This will load the data from the main config file. + * If it is unable it will give a warning to the user it couldn't be found. + * + * @tparam T Type of parameter we are looking for. + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + template void parse_config(const std::string &node_name, T &node_result, bool required = true) { + +#if ROS_AVAILABLE + // If we have the ROS parameter, we should just get that one + if (nh.getParam(node_name, node_result)) { + nh.param(node_name, node_result); + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); + return; + } +#endif + + // Directly return if the config hasn't been opened + if (config == nullptr) + return; + + // Else lets get the one from the config + try { + parse(config->root(), node_name, node_result, required); + } catch (...) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } + } + + /** + * @brief Custom parser for the external parameter files with levels. + * + * This will first load the external file requested. + * From there it will try to find the first level requested (e.g. imu0, cam0, cam1). + * Then the requested node can be found under this sensor name. + * ROS can override the config with `_` (e.g., cam0_distortion). + * + * @tparam T Type of parameter we are looking for. + * @param external_node_name Name of the node we will get our relative path from + * @param sensor_name The first level node we will try to get the requested node under + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + template + void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, T &node_result, + bool required = true) { + +#if ROS_AVAILABLE + // If we have the ROS parameter, we should just get that one + std::string rosnode = sensor_name + "_" + node_name; + if (nh.getParam(rosnode, node_result)) { + nh.param(rosnode, node_result); + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + return; + } +#endif + + // Directly return if the config hasn't been opened + if (config == nullptr) + return; + + // Create the path the external yaml file + std::string path; + if (!node_found(config->root(), external_node_name)) { + PRINT_ERROR(RED "the external node %s could not be found!\n" RESET, external_node_name.c_str()); + std::exit(EXIT_FAILURE); + } + (*config)[external_node_name] >> path; + std::string relative_folder = config_path_.substr(0, config_path_.find_last_of('/')) + "/"; + + // Now actually try to load them from file! + auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); + if (!config_external->isOpened()) { + PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str()); + std::exit(EXIT_FAILURE); + } + + // Check that we have the requested node + if (!node_found(config_external->root(), sensor_name)) { + PRINT_WARNING(YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + return; + } + + // Else lets get it! + try { + parse((*config_external)[sensor_name], node_name, node_result, required); + } catch (...) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(), + typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str()); + all_params_found_successfully = false; + } + } + + /** + * @brief Custom parser for Matrix4d in the external parameter files with levels. + * + * This will first load the external file requested. + * From there it will try to find the first level requested (e.g. imu0, cam0, cam1). + * Then the requested node can be found under this sensor name. + * ROS can override the config with `_` (e.g., cam0_distortion). + * + * @param external_node_name Name of the node we will get our relative path from + * @param sensor_name The first level node we will try to get the requested node under + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, + Eigen::Matrix4d &node_result, bool required = true) { + +#if ROS_AVAILABLE + // If we have the ROS parameter, we should just get that one + // NOTE: for our 4x4 matrix we should read it as an array from ROS then covert it back into the 4x4 + std::string rosnode = sensor_name + "_" + node_name; + std::vector matrix_TCtoI; + if (nh.getParam(rosnode, matrix_TCtoI)) { + nh.param>(rosnode, matrix_TCtoI); + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5), + matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11), + matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15); + return; + } +#endif + + // Directly return if the config hasn't been opened + if (config == nullptr) + return; + + // Create the path the external yaml file + std::string path; + if (!node_found(config->root(), external_node_name)) { + PRINT_ERROR(RED "the external node %s could not be found!\n" RESET, external_node_name.c_str()); + std::exit(EXIT_FAILURE); + } + (*config)[external_node_name] >> path; + std::string relative_folder = config_path_.substr(0, config_path_.find_last_of('/')) + "/"; + + // Now actually try to load them from file! + auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); + if (!config_external->isOpened()) { + PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str()); + std::exit(EXIT_FAILURE); + } + + // Check that we have the requested node + if (!node_found(config_external->root(), sensor_name)) { + PRINT_WARNING(YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + return; + } + + // Else lets get it! + try { + parse((*config_external)[sensor_name], node_name, node_result, required); + } catch (...) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(), + typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str()); + all_params_found_successfully = false; + } + } + +private: + /// Path to the config file + std::string config_path_; + + /// Our config file with the data in it + std::shared_ptr config; + + /// Record if all parameters were found + bool all_params_found_successfully = true; + +#if ROS_AVAILABLE + /// Ros node handler that will override values + ros::NodeHandle nh; +#endif + + /** + * @brief Given a YAML node object, this check to see if we have a valid key + * @param file_node OpenCV file node we will get the data from + * @param node_name Name of the node + * @return True if we can get the data + */ + static bool node_found(const cv::FileNode &file_node, const std::string &node_name) { + bool found_node = false; + for (const auto &item : file_node) { + if (item.name() == node_name) { + found_node = true; + } + } + return found_node; + } + + /** + * @brief This function will try to get the requested parameter from our config. + * + * If it is unable to find it, it will give a warning to the user it couldn't be found. + * + * @tparam T Type of parameter we are looking for. + * @param file_node OpenCV file node we will get the data from + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + template void parse(const cv::FileNode &file_node, const std::string &node_name, T &node_result, bool required = true) { + + // Check that we have the requested node + if (!node_found(file_node, node_name)) { + if (required) { + PRINT_WARNING(YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("the node %s of type [%s] was not found (not required)...\n", node_name.c_str(), typeid(node_result).name()); + } + return; + } + + // Now try to get it from the config + try { + file_node[node_name] >> node_result; + } catch (...) { + if (required) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(), + typeid(node_result).name()); + } + } + } + + /** + * @brief Custom parser for booleans (0,false,False,FALSE=>false and 1,true,True,TRUE=>false) + * @param file_node OpenCV file node we will get the data from + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + void parse(const cv::FileNode &file_node, const std::string &node_name, bool &node_result, bool required = true) { + + // Check that we have the requested node + if (!node_found(file_node, node_name)) { + if (required) { + PRINT_WARNING(YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("the node %s of type [%s] was not found (not required)...\n", node_name.c_str(), typeid(node_result).name()); + } + return; + } + + // Now try to get it from the config + try { + if (file_node[node_name].isInt() && (int)file_node[node_name] == 1) { + node_result = true; + return; + } + if (file_node[node_name].isInt() && (int)file_node[node_name] == 0) { + node_result = false; + return; + } + // NOTE: we select the first bit of text as there can be a comment afterwards + // NOTE: for example we could have "key: true #comment here..." where we only want "true" + std::string value; + file_node[node_name] >> value; + value = value.substr(0, value.find_first_of('#')); + value = value.substr(0, value.find_first_of(' ')); + if (value == "1" || value == "true" || value == "True" || value == "TRUE") { + node_result = true; + } else if (value == "0" || value == "false" || value == "False" || value == "FALSE") { + node_result = false; + } else { + PRINT_WARNING(YELLOW "the node %s has an invalid boolean type of [%s]\n" RESET, node_name.c_str(), value.c_str()); + all_params_found_successfully = false; + } + } catch (...) { + if (required) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(), + typeid(node_result).name()); + } + } + } + + /** + * @brief Custom parser for camera extrinsic 4x4 transformations + * @param file_node OpenCV file node we will get the data from + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + void parse(const cv::FileNode &file_node, const std::string &node_name, Eigen::Matrix4d &node_result, bool required = true) { + + // See if we need to flip the node name + std::string node_name_local = node_name; + if (node_name == "T_cam_imu" && !node_found(file_node, node_name)) { + PRINT_INFO("parameter T_cam_imu not found, trying T_imu_cam instead (will return T_cam_imu still)!\n"); + node_name_local = "T_imu_cam"; + } else if (node_name == "T_imu_cam" && !node_found(file_node, node_name)) { + PRINT_INFO("parameter T_imu_cam not found, trying T_cam_imu instead (will return T_imu_cam still)!\n"); + node_name_local = "T_cam_imu"; + } + + // Check that we have the requested node + if (!node_found(file_node, node_name_local)) { + if (required) { + PRINT_WARNING(YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name_local.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("the node %s of type [%s] was not found (not required)...\n", node_name_local.c_str(), typeid(node_result).name()); + } + return; + } + + // Now try to get it from the config + node_result = Eigen::Matrix4d::Identity(); + try { + for (int r = 0; r < (int)file_node[node_name_local].size() && r < 4; r++) { + for (int c = 0; c < (int)file_node[node_name_local][r].size() && c < 4; c++) { + node_result(r, c) = (double)file_node[node_name_local][r][c]; + } + } + } catch (...) { + if (required) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(), + typeid(node_result).name()); + } + } + + // Finally if we flipped the transform, get the correct value + if (node_name_local != node_name) { + Eigen::Matrix4d tmp(node_result); + node_result = ov_core::Inv_se3(tmp); + } + } +}; + +} /* namespace ov_core */ + +#endif /* OPENCV_YAML_PARSER_H */ diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp index 455a701a6..372a5c6a7 100644 --- a/ov_core/src/utils/print.cpp +++ b/ov_core/src/utils/print.cpp @@ -85,7 +85,7 @@ void Printer::debugPrint(PrintLevel level, const char location[], const char lin // Print the location info first for our debug output // Truncate the filename to the max size for the filepath - if(static_cast(level) < static_cast(Printer::PrintLevel::DEBUG)) { + if(static_cast(Printer::current_print_level) <= static_cast(Printer::PrintLevel::DEBUG)) { std::string path(location); std::string base_filename = path.substr(path.find_last_of("/\\") + 1); if (base_filename.size() > MAX_FILE_PATH_LEGTH) { diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index dd9e1f64a..89158018e 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -14,10 +14,11 @@ project(ov_msckf) find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core) # Include libraries (if we don't have opencv 4, then fallback to opencv 3) +# The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! find_package(Eigen3 REQUIRED) -find_package(OpenCV 4 QUIET) +find_package(OpenCV 3 QUIET) if (NOT OpenCV_FOUND) - find_package(OpenCV 3 REQUIRED) + find_package(OpenCV 4 REQUIRED) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION}) diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml new file mode 100644 index 000000000..681574b67 --- /dev/null +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -0,0 +1,93 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: true +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 75 +max_slam_in_update: 25 +max_msckf_in_update: 40 +dt_slam_delay: 3 + +init_window_time: 0.75 +init_imu_thresh: 1.5 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 250 +fast_threshold: 15 +grid_x: 5 +grid_y: 3 +min_px_dist: 8 +knn_ratio: 0.70 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml b/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml new file mode 100644 index 000000000..394cdca13 --- /dev/null +++ b/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml b/ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..6e14a9084 --- /dev/null +++ b/ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] + - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] + - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] + distortion_model: radtan + intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam0/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] + - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] + - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] + distortion_model: radtan + intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam1/image_raw \ No newline at end of file diff --git a/ov_msckf/config/kaist/kalibr_imu_chain.yaml b/ov_msckf/config/kaist/kalibr_imu_chain.yaml new file mode 100644 index 000000000..6fdd0ef0a --- /dev/null +++ b/ov_msckf/config/kaist/kalibr_imu_chain.yaml @@ -0,0 +1,19 @@ +%YAML:1.0 # need to specify the file type at the top! + + +# MTI-100 series converted from data sheet, guess on bias random walk +# https://www.xsens.com/hubfs/Downloads/usermanual/MTi_usermanual.pdf +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 5.8860e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 1.0000e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.7453e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.0000e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu/data_raw + time_offset: 0.0 + update_rate: 500.0 \ No newline at end of file diff --git a/ov_msckf/config/kaist/kalibr_imucam_chain.yaml b/ov_msckf/config/kaist/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..bda3a5d55 --- /dev/null +++ b/ov_msckf/config/kaist/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [-0.00413,-0.01966,0.99980,1.73944] + - [-0.99993,-0.01095,-0.00435,0.27803] + - [0.01103,-0.99975,-0.01962,-0.08785] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-5.6143027800000002e-02,1.3952563200000001e-01,-1.2155906999999999e-03,-9.7281389999999998e-04] + distortion_model: radtan + intrinsics: [8.1690378992770002e+02,8.1156803828490001e+02,6.0850726281690004e+02,2.6347599764440002e+02] + resolution: [1280, 560] + rostopic: /stereo/left/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [-0.00768,-0.01509,0.99986,1.73376] + - [-0.99988,-0.01305,-0.00788,-0.19706] + - [0.01317,-0.99980,-0.01499,-0.08271] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] + distortion_model: radtan + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [1280, 560] + rostopic: /stereo/right/image_raw \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_ros_kaist.launch b/ov_msckf/config/kaist/pgeneva_ros_kaist.launch similarity index 100% rename from ov_msckf/launch/pgeneva_ros_kaist.launch rename to ov_msckf/config/kaist/pgeneva_ros_kaist.launch diff --git a/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml b/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8bec50983 --- /dev/null +++ b/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /mavros/imu/data + time_offset: 0.0 + update_rate: 100.0 \ No newline at end of file diff --git a/ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml b/ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..80e7e4acc --- /dev/null +++ b/ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml @@ -0,0 +1,93 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: + - - -0.04030123999740945 + - -0.9989998755524683 + - 0.01936643232049068 + - 0.02103955032447366 + - - 0.026311325355146964 + - -0.020436499663524704 + - -0.9994448777394171 + - -0.038224929976612206 + - - 0.9988410905708309 + - -0.0397693113802049 + - 0.027108627033059024 + - -0.1363488241088845 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: + - 1 + camera_model: pinhole + distortion_coeffs: + - 0.006896928127777268 + - -0.009144207062654397 + - 0.000254113977103925 + - 0.0021434982252719545 + distortion_model: radtan + intrinsics: + - 380.9229090195708 + - 380.29264802262736 + - 324.68121181846755 + - 224.6741321466431 + resolution: + - 640 + - 480 + rostopic: /camera/infra1/image_rect_raw + timeshift_cam_imu: -0.029958533056650416 +cam1: + T_cam_imu: + - - -0.03905752472566068 + - -0.9990498568899562 + - 0.019336318430946575 + - -0.02909273113160158 + - - 0.025035478432625047 + - -0.020323396666370924 + - -0.9994799569614147 + - -0.03811090793611019 + - - 0.99892328763622 + - -0.03855311914877835 + - 0.02580547271309183 + - -0.13656684822705098 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + T_cn_cnm1: + - - 0.9999992248836708 + - 6.384241340452582e-05 + - 0.0012434452955667624 + - -0.049960282472300055 + - - -6.225102643531651e-05 + - 0.9999991790958949 + - -0.0012798173093508036 + - -5.920119010064575e-05 + - - -0.001243525981443161 + - 0.0012797389115975439 + - 0.9999984079544582 + - -0.00014316003395349448 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + cam_overlaps: + - 0 + camera_model: pinhole + distortion_coeffs: + - 0.007044055287844759 + - -0.010251485722185347 + - 0.0006674304399871926 + - 0.001678899816379666 + distortion_model: radtan + intrinsics: + - 380.95187095303424 + - 380.3065956074995 + - 324.0678433553536 + - 225.9586983198407 + resolution: + - 640 + - 480 + rostopic: /camera/infra2/image_rect_raw + timeshift_cam_imu: -0.030340187355085417 \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_ros_kaistvio.launch b/ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch similarity index 100% rename from ov_msckf/launch/pgeneva_ros_kaistvio.launch rename to ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml new file mode 100644 index 000000000..fe45f2b64 --- /dev/null +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -0,0 +1,92 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: true +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 15 +max_slam: 75 +max_slam_in_update: 25 +max_msckf_in_update: 40 +dt_slam_delay: 2 + +init_window_time: 0.5 +init_imu_thresh: 1.5 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 150 +fast_threshold: 15 +grid_x: 5 +grid_y: 3 +min_px_dist: 10 +knn_ratio: 0.85 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: true +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 8 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + diff --git a/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml new file mode 100644 index 000000000..394cdca13 --- /dev/null +++ b/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml b/ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..74ebdbe18 --- /dev/null +++ b/ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [-0.99997806, -0.00600501, 0.0027968, 0.03847796] + - [0.00601449, -0.99997615, 0.00339343, -0.0045601] + - [0.00277635, 0.00341018, 0.99999033, 0.00418038] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.292031518680717,0.08753155838496009,0.0009568457669165753,2.3463489813256424e-05] + distortion_model: radtan + intrinsics: [470.0502737897896,468.7574814232544,405.80799445368035,245.2879780490104] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam0/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [-0.99984421, -0.00389232, 0.01721638, -0.07075565] + - [0.00394176, -0.9999882, 0.0028389, -0.00418534] + - [0.01720512, 0.00290632, 0.99984776, 0.00388861] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.2847596229068525,0.07908861097045865,0.0011071433505703875,0.0005094909873658998] + distortion_model: radtan + intrinsics: [472.98384780424163,471.9917417027018,382.1928744696835,268.2536666120421] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam1/image_raw \ No newline at end of file diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml new file mode 100644 index 000000000..334e036e3 --- /dev/null +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -0,0 +1,92 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: true +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 75 +max_slam_in_update: 25 +max_msckf_in_update: 40 +dt_slam_delay: 3 + +init_window_time: 0.75 +init_imu_thresh: 1.5 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 250 +fast_threshold: 15 +grid_x: 5 +grid_y: 3 +min_px_dist: 8 +knn_ratio: 0.70 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml new file mode 100644 index 000000000..bdc2e32e3 --- /dev/null +++ b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 1.1186830841306218e-04 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 8.997530210630026e-07 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.0027052931930236323 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.3054568211204843e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml b/ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..8af656255 --- /dev/null +++ b/ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml @@ -0,0 +1,31 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.99992127, -0.0078594, 0.0097819, -0.05845078] + - [0.00784873, 0.99996856, 0.00112822, -0.00728728] + - [-0.00979046, -0.00105136, 0.99995152, 0.0623674] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.03149689493503132, 0.07696336480701078, -0.06608854732019281, 0.019667561645120218] + distortion_model: equidistant + intrinsics: [276.4850207717928, 278.0310503180516, 314.5836189313042, 240.16980920673427] #fu, fv, cu, cv + resolution: [640, 480] + rostopic: /cam0/image_raw + timeshift_cam_imu: 0.00621 + +cam1: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.99995933, 0.00327998, 0.00840069, 0.00793529] + - [-0.00328309, 0.99999455, 0.000356, -0.00716413] + - [-0.00839948, -0.00038357, 0.99996465, 0.06245421] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.02998039058251529, 0.07202819722706337, -0.06178718820631651, 0.017655045017816777] + distortion_model: equidistant + intrinsics: [277.960323846132, 279.4348778432714, 322.404194404853, 236.72685252691352] #fu, fv, cu, cv + resolution: [640, 480] + rostopic: /cam1/image_raw + timeshift_cam_imu: 0.00621 \ No newline at end of file diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml new file mode 100644 index 000000000..aef6fba08 --- /dev/null +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -0,0 +1,92 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: true +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 50 +max_slam_in_update: 25 +max_msckf_in_update: 40 +dt_slam_delay: 5 + +init_window_time: 1.0 +init_imu_thresh: 0.60 # room1-5:0.60, room6:0.45 +gravity_mag: 9.80766 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 2.0 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 300 +fast_threshold: 15 +grid_x: 5 +grid_y: 5 +min_px_dist: 5 +knn_ratio: 0.65 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + diff --git a/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml b/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8571eeb9f --- /dev/null +++ b/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.0028 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.00086 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.00016 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 2.2e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml b/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..00413462a --- /dev/null +++ b/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml @@ -0,0 +1,33 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: #rotation from IMU to camera, position of camera in IMU + - [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392] + - [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084] + - [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] + distortion_model: equidistant + intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] + resolution: [512, 512] + rostopic: /cam0/image_raw +cam1: + T_cam_imu: #rotation from IMU to camera, position of camera in IMU + - [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734] + - [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924] + - [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9999994457734953, -0.0008233639921576076, -0.0006561436136444361, -0.10106110275180535] + - [0.0007916877528168117, 0.9988994619156757, -0.04689603624058988, -0.0019764575873431013] + - [0.0006940340102242531, 0.04689549078870055, 0.9988995601463054, -0.0011756424802046581] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] + distortion_model: equidistant + intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] + resolution: [512, 512] + rostopic: /cam1/image_raw \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8fb43e3dc --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..aedd2f589 --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml @@ -0,0 +1,37 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: + - [-0.028228787368606456, -0.999601488301944, 1.2175294828553618e-05, 0.02172388268966517] + - [0.014401251861751119, -0.00041887083271471837, -0.9998962088597202, -6.605455433829172e-05] + - [0.999497743623523, -0.028225682131089447, 0.014407337010089172, -0.00048817563004522853] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.013721808247486035, 0.020727425669427896, -0.012786476702685545, + 0.0025242267320687625] + distortion_model: equidistant + intrinsics: [278.66723066149086, 278.48991409740296, 319.75221200593535, 241.96858910358173] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_l + timeshift_cam_imu: -0.016684572091862235 +cam1: + T_cam_imu: + - [-0.011823057800830705, -0.9998701444077991, -0.010950325390841398, -0.057904961033265645] + - [0.011552991631909482, 0.01081376681432078, -0.9998747875767439, 0.00043766687615362694] + - [0.9998633625093938, -0.011948086424720228, 0.011423639621249038, -0.00039944945687402214] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9998053017199788, 0.011197738450911484, 0.01624713224548414, -0.07961594300469246] + - [-0.011147758116324, 0.9999328574031386, -0.0031635699090552883, 0.0007443452072558462] + - [-0.016281466199246444, 0.00298183486707869, 0.9998630018753686, 0.0004425529195268342] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.008456929295619607, 0.011407590938612062, -0.006951788325762078, + 0.0015368127092821786] + distortion_model: equidistant + intrinsics: [277.61640629770613, 277.63749695723294, 314.8944703346039, 236.04310050462587] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_r + timeshift_cam_imu: -0.016591431247074982 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8fb43e3dc --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..37838c854 --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml @@ -0,0 +1,37 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: + - [-0.027256691772188965, -0.9996260641688061, 0.0021919370477445077, 0.02422852666805565] + - [-0.7139206120417471, 0.017931469899155242, -0.6999970157716363, 0.008974432843748055] + - [0.6996959571525168, -0.020644471939022302, -0.714142404092339, -0.000638971731537894] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-6.545154718304953e-06, -0.010379525898159981, 0.014935312423953146, + -0.005639061406567785] + distortion_model: equidistant + intrinsics: [275.46015578667294, 274.9948095922592, 315.958384100568, 242.7123497822731] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_l + timeshift_cam_imu: -0.01484888826656275 +cam1: + T_cam_imu: + - [-0.01749277298389329, -0.9997914625864506, -0.010537278233961556, -0.05569997768397372] + - [-0.7090991957246053, 0.019835234209851005, -0.7048296915614142, 0.00884826894411553] + - [0.7048917175822481, -0.004857450265962848, -0.7092982952614942, -0.0019997713120269607] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9998713028057842, 0.0019367839962957476, 0.015925661468614183, -0.07993259169905036] + - [-0.0020450612045710957, 0.9999748874354272, 0.0067854420026733295, -7.205400304978925e-05] + - [-0.01591211959893364, -0.006817137687752675, 0.9998501543149417, -0.0009141881692325364] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.012138050918285051, 0.02244029339184358, -0.013753165428754275, + 0.002725090438517269] + distortion_model: equidistant + intrinsics: [274.4628309070672, 273.9261674470783, 315.93654481793794, 235.779167375461] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_r + timeshift_cam_imu: -0.014950736007814259 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8fb43e3dc --- /dev/null +++ b/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..0a5584d7c --- /dev/null +++ b/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml @@ -0,0 +1,37 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: + - [-0.03179778293757218, -0.9994933985910031, -0.001359107523862424, 0.021115239798621798] + - [0.012827844120885779, 0.0009515801497960164, -0.9999172670328424, -0.0008992998316121829] + - [0.9994120008362244, -0.03181258663210035, 0.012791087377928778, -0.009491094814035777] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.005719912631104124, 0.004742449009601135, 0.0012060658036136048, + -0.001580292679344826] + distortion_model: equidistant + intrinsics: [277.4786896484645, 277.42548548840034, 320.1052053576385, 242.10083077857894] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_l + timeshift_cam_imu: -0.007999243205055177 +cam1: + T_cam_imu: + - [-0.011450159873389598, -0.9998746482793399, -0.010935335712288774, -0.05828448770624624] + - [0.009171247533644289, 0.010830579777447058, -0.9998992883087583, -0.0002362068202437068] + - [0.999892385238307, -0.01154929737910465, 0.009046086032012068, -0.00947464531803495] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9997470623689986, 0.009836089265916417, 0.020225296846065624, -0.07919358086270675] + - [-0.00975774768296796, 0.9999445171722606, -0.0039684930755682956, 0.000831414953842084] + - [-0.020263209141547188, 0.0037701359508940783, 0.9997875716521978, 0.00044568632114983057] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.009025009906076716, 0.009967427035376123, -0.0029538969814842117, + -0.0003503551771748748] + distortion_model: equidistant + intrinsics: [276.78679780974477, 276.79332134030807, 314.2862327340746, 236.51313088043128] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_r + timeshift_cam_imu: -0.007983859928063504 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml new file mode 100644 index 000000000..8fb43e3dc --- /dev/null +++ b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 # need to specify the file type at the top! + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..c29412052 --- /dev/null +++ b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml @@ -0,0 +1,37 @@ +%YAML:1.0 # need to specify the file type at the top! + +cam0: + T_cam_imu: + - [-0.024041523213909927, -0.9996640790624955, 0.009681642096550924, 0.02023430742078562] + - [-0.7184527320882621, 0.010542697330412382, -0.6954958830129113, 0.008311861463499775] + - [0.6951601807615744, -0.023676582632001453, -0.7184648512755534, -0.026628438421085154] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.017811595366268803, 0.04897078939103475, -0.041363300782847834, + 0.011440891936886532] + distortion_model: equidistant + intrinsics: [275.3385453506587, 275.0852058534152, 315.7697752181792, 233.72625444124952] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_l + timeshift_cam_imu: -0.008637511810764048 +cam1: + T_cam_imu: + - [-0.004527750456351745, -0.9999560749011355, -0.008206567133703047, -0.05986676424716047] + - [-0.7208238256076104, 0.008951751262681593, -0.6930605158178762, 0.008989928313050033] + - [0.6931035362139012, 0.0027774840496477826, -0.7208326946456712, -0.026595921269512067] + - [0.0, 0.0, 0.0, 1.0] + T_cn_cnm1: + - [0.9996495696908625, -0.0015816259006504233, 0.026424160845286468, -0.07937720055807065] + - [0.0016709946890799124, 0.9999929578963755, -0.0033603473630593734, 0.0005548331594719445] + - [-0.026418659951183095, 0.0034033244279300045, 0.9996451729434878, 0.0005293439870858398] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.027860492621377443, -0.027723581962855317, 0.0375199775145906, + -0.018152613898714216] + distortion_model: equidistant + intrinsics: [273.2895238376505, 273.35830490745764, 314.60557378520133, 251.0359907029701] + resolution: [640, 480] + rostopic: /snappy_cam/stereo_r + timeshift_cam_imu: -0.008613446015312496 \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_ros_eth.launch b/ov_msckf/launch/pgeneva_ros_eth.launch deleted file mode 100644 index 394ef9fe0..000000000 --- a/ov_msckf/launch/pgeneva_ros_eth.launch +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [752, 480] - [752, 480] - - - [458.654,457.296,367.215,248.375] - [-0.28340811,0.07395907,0.00019359,1.76187114e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - - - - [ - 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, - 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, - -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - 0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, - 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, - -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_ros_tum.launch b/ov_msckf/launch/pgeneva_ros_tum.launch deleted file mode 100644 index 31b6a55c6..000000000 --- a/ov_msckf/launch/pgeneva_ros_tum.launch +++ /dev/null @@ -1,139 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [512, 512] - [512, 512] - - - [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] - [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] - [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - - - - [ - -0.99952504, 0.00750192, -0.02989013, 0.04557484, - 0.02961534, -0.03439736, -0.99896935, -0.07116180, - -0.00852233, -0.99938008, 0.03415885, -0.04468125, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.99951105, 0.00810408, -0.03019914, -0.05545634, - 0.03029912, 0.01251164, -0.99946257, -0.06925002, - -0.00772188, -0.99988889, -0.01275107, -0.04745286, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_serial_aruco.launch b/ov_msckf/launch/pgeneva_serial_aruco.launch deleted file mode 100644 index ddc5907bb..000000000 --- a/ov_msckf/launch/pgeneva_serial_aruco.launch +++ /dev/null @@ -1,108 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [470.0502737897896,468.7574814232544,405.80799445368035,245.2879780490104] - [-0.292031518680717,0.08753155838496009,0.0009568457669165753,2.3463489813256424e-05] - [472.98384780424163,471.9917417027018,382.1928744696835,268.2536666120421] - [-0.2847596229068525,0.07908861097045865,0.0011071433505703875,0.0005094909873658998] - - - - - [ - -0.99997806, -0.00600501, 0.0027968, 0.03847796, - 0.00601449, -0.99997615, 0.00339343, -0.0045601, - 0.00277635, 0.00341018, 0.99999033, 0.00418038, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.99984421, -0.00389232, 0.01721638, -0.07075565, - 0.00394176, -0.9999882, 0.0028389, -0.00418534, - 0.01720512, 0.00290632, 0.99984776, 0.00388861, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_serial_eth.launch b/ov_msckf/launch/pgeneva_serial_eth.launch deleted file mode 100644 index eb34b980c..000000000 --- a/ov_msckf/launch/pgeneva_serial_eth.launch +++ /dev/null @@ -1,129 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [752, 480] - [752, 480] - - - [458.654,457.296,367.215,248.375] - [-0.28340811,0.07395907,0.00019359,1.76187114e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - - - - [ - 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, - 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, - -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - 0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, - 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, - -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_serial_tum.launch b/ov_msckf/launch/pgeneva_serial_tum.launch deleted file mode 100644 index 0c980ecae..000000000 --- a/ov_msckf/launch/pgeneva_serial_tum.launch +++ /dev/null @@ -1,122 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [512, 512] - [512, 512] - - - [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] - [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] - [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983] - [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] - - - - [ - -0.99952504, 0.00750192, -0.02989013, 0.04557484, - 0.02961534, -0.03439736, -0.99896935, -0.07116180, - -0.00852233, -0.99938008, 0.03415885, -0.04468125, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.99951105, 0.00810408, -0.03019914, -0.05545634, - 0.03029912, 0.01251164, -0.99946257, -0.06925002, - -0.00772188, -0.99988889, -0.01275107, -0.04745286, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch new file mode 100644 index 000000000..97ad3d82b --- /dev/null +++ b/ov_msckf/launch/serial.launch @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch new file mode 100644 index 000000000..121c796ee --- /dev/null +++ b/ov_msckf/launch/subscribe.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index f82d25a83..585e49f5d 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -32,31 +32,31 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap // Setup pose and path publisher pub_poseimu = nh.advertise("/ov_msckf/poseimu", 2); - PRINT_DEBUG("Publishing: %s", pub_poseimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str()); pub_odomimu = nh.advertise("/ov_msckf/odomimu", 2); - PRINT_DEBUG("Publishing: %s", pub_odomimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_odomimu.getTopic().c_str()); pub_pathimu = nh.advertise("/ov_msckf/pathimu", 2); - PRINT_DEBUG("Publishing: %s", pub_pathimu.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str()); // 3D points publishing pub_points_msckf = nh.advertise("/ov_msckf/points_msckf", 2); - PRINT_DEBUG("Publishing: %s", pub_points_msckf.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_points_msckf.getTopic().c_str()); pub_points_slam = nh.advertise("/ov_msckf/points_slam", 2); - PRINT_DEBUG("Publishing: %s", pub_points_msckf.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_points_msckf.getTopic().c_str()); pub_points_aruco = nh.advertise("/ov_msckf/points_aruco", 2); - PRINT_DEBUG("Publishing: %s", pub_points_aruco.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_points_aruco.getTopic().c_str()); pub_points_sim = nh.advertise("/ov_msckf/points_sim", 2); - PRINT_DEBUG("Publishing: %s", pub_points_sim.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str()); // Our tracking image pub_tracks = nh.advertise("/ov_msckf/trackhist", 2); - PRINT_DEBUG("Publishing: %s", pub_tracks.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_tracks.getTopic().c_str()); // Groundtruth publishers pub_posegt = nh.advertise("/ov_msckf/posegt", 2); - PRINT_DEBUG("Publishing: %s", pub_posegt.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_posegt.getTopic().c_str()); pub_pathgt = nh.advertise("/ov_msckf/pathgt", 2); - PRINT_DEBUG("Publishing: %s", pub_pathgt.getTopic().c_str()); + PRINT_DEBUG("Publishing: %s\n", pub_pathgt.getTopic().c_str()); // Loop closure publishers pub_loop_pose = nh.advertise("/ov_msckf/loop_pose", 2); @@ -76,7 +76,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap nh.param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { DatasetReader::load_gt_file(path_to_gt, gt_states); - PRINT_DEBUG("gt file path is: %s", path_to_gt.c_str()); + PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); } } diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index a5d5d1f11..ce7826eb8 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -34,10 +34,10 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { // Nice debug this->params = params_; - params.print_estimator(); - params.print_noise(); - params.print_state(); - params.print_trackers(); + params.print_and_load_estimator(); + params.print_and_load_noise(); + params.print_and_load_state(); + params.print_and_load_trackers(); // This will globally set the thread count we will use // -1 will reset to the system default threading (usually the num of cores) diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 4a01b5b69..ba0119706 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -39,7 +39,7 @@ #include "track/TrackSIM.h" #include "types/Landmark.h" #include "types/LandmarkRepresentation.h" -#include "utils/lambda_body.h" +#include "utils/opencv_lambda_body.h" #include "utils/print.h" #include "utils/sensor_data.h" diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 847ad74ce..f5553d259 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -34,6 +34,7 @@ #include "track/TrackBase.h" #include "update/UpdaterOptions.h" #include "utils/colors.h" +#include "utils/opencv_yaml_parse.h" #include "utils/print.h" #include "utils/quat_ops.h" @@ -48,6 +49,17 @@ namespace ov_msckf { */ struct VioManagerOptions { + /** + * @brief This function will load the non-simulation parameters of the system and print. + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load(const std::shared_ptr &parser = nullptr) { + print_and_load_estimator(parser); + print_and_load_noise(parser); + print_and_load_state(parser); + print_and_load_trackers(parser); + } + // ESTIMATOR =============================== /// Core state options (e.g. number of cameras, use fej, stereo, what calibration to enable etc) @@ -84,22 +96,36 @@ struct VioManagerOptions { std::string record_timing_filepath = "ov_msckf_timing.txt"; /** - * @brief This function will print out all estimator settings loaded. + * @brief This function will load print out all estimator settings loaded. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters */ - void print_estimator() { + void print_and_load_estimator(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("ESTIMATOR PARAMETERS:\n"); - state_options.print(); - PRINT_DEBUG("\t- dt_slam_delay: %.1f\n", dt_slam_delay); - PRINT_DEBUG("\t- init_window_time: %.2f\n", init_window_time); - PRINT_DEBUG("\t- init_imu_thresh: %.2f\n", init_imu_thresh); - PRINT_DEBUG("\t- zero_velocity_update: %d\n", try_zupt); - PRINT_DEBUG("\t- zupt_max_velocity: %.2f\n", zupt_max_velocity); - PRINT_DEBUG("\t- zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); - PRINT_DEBUG("\t- zupt_max_disparity: %.4f\n", zupt_max_disparity); - PRINT_DEBUG("\t- zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); - PRINT_DEBUG("\t- record timing?: %d\n", (int)record_timing_information); - PRINT_DEBUG("\t- record timing filepath: %s\n", record_timing_filepath.c_str()); + state_options.print(parser); + if (parser != nullptr) { + parser->parse_config("dt_slam_delay", dt_slam_delay); + parser->parse_config("init_window_time", init_window_time); + parser->parse_config("init_imu_thresh", init_imu_thresh); + parser->parse_config("try_zupt", try_zupt); + parser->parse_config("zupt_max_velocity", zupt_max_velocity); + parser->parse_config("zupt_noise_multiplier", zupt_noise_multiplier); + parser->parse_config("zupt_max_disparity", zupt_max_disparity); + parser->parse_config("zupt_only_at_beginning", zupt_only_at_beginning); + parser->parse_config("record_timing_information", record_timing_information); + parser->parse_config("record_timing_filepath", record_timing_filepath); + } + PRINT_DEBUG(" - dt_slam_delay: %.1f\n", dt_slam_delay); + PRINT_DEBUG(" - init_window_time: %.2f\n", init_window_time); + PRINT_DEBUG(" - init_imu_thresh: %.2f\n", init_imu_thresh); + PRINT_DEBUG(" - zero_velocity_update: %d\n", try_zupt); + PRINT_DEBUG(" - zupt_max_velocity: %.2f\n", zupt_max_velocity); + PRINT_DEBUG(" - zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); + PRINT_DEBUG(" - zupt_max_disparity: %.4f\n", zupt_max_disparity); + PRINT_DEBUG(" - zupt_only_at_beginning?: %d\n", zupt_only_at_beginning); + PRINT_DEBUG(" - record timing?: %d\n", (int)record_timing_information); + PRINT_DEBUG(" - record timing filepath: %s\n", record_timing_filepath.c_str()); } // NOISE / CHI2 ============================ @@ -120,19 +146,43 @@ struct VioManagerOptions { UpdaterOptions zupt_options; /** - * @brief This function will print out all noise parameters loaded. + * @brief This function will load print out all noise parameters loaded. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters */ - void print_noise() { + void print_and_load_noise(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("NOISE PARAMETERS:\n"); + if (parser != nullptr) { + parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_noises.sigma_w); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_noises.sigma_wb); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_noises.sigma_a); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_noises.sigma_ab); + imu_noises.sigma_w_2 = std::pow(imu_noises.sigma_w, 2); + imu_noises.sigma_wb_2 = std::pow(imu_noises.sigma_wb, 2); + imu_noises.sigma_a_2 = std::pow(imu_noises.sigma_a, 2); + imu_noises.sigma_ab_2 = std::pow(imu_noises.sigma_ab, 2); + } imu_noises.print(); - PRINT_DEBUG("\tUpdater MSCKF Feats:\n"); + if (parser != nullptr) { + parser->parse_config("up_msckf_sigma_px", msckf_options.sigma_pix); + parser->parse_config("up_msckf_chi2_multipler", msckf_options.chi2_multipler); + parser->parse_config("up_slam_sigma_px", slam_options.sigma_pix); + parser->parse_config("up_slam_chi2_multipler", slam_options.chi2_multipler); + parser->parse_config("up_aruco_sigma_px", aruco_options.sigma_pix); + parser->parse_config("up_aruco_chi2_multipler", aruco_options.chi2_multipler); + msckf_options.sigma_pix_sq = std::pow(msckf_options.sigma_pix, 2); + slam_options.sigma_pix_sq = std::pow(slam_options.sigma_pix, 2); + aruco_options.sigma_pix_sq = std::pow(aruco_options.sigma_pix, 2); + parser->parse_config("zupt_chi2_multipler", aruco_options.chi2_multipler); + } + PRINT_DEBUG(" Updater MSCKF Feats:\n"); msckf_options.print(); - PRINT_DEBUG("\tUpdater SLAM Feats:\n"); + PRINT_DEBUG(" Updater SLAM Feats:\n"); slam_options.print(); - PRINT_DEBUG("\tUpdater ARUCO Tags:\n"); + PRINT_DEBUG(" Updater ARUCO Tags:\n"); aruco_options.print(); - PRINT_DEBUG("\tUpdater ZUPT:\n"); + PRINT_DEBUG(" Updater ZUPT:\n"); zupt_options.print(); } @@ -157,14 +207,65 @@ struct VioManagerOptions { std::map> camera_wh; /** - * @brief This function will print out all simulated parameters loaded. + * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters */ - void print_state() { + void print_and_load_state(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("gravity_mag", gravity_mag); + parser->parse_config("max_cameras", state_options.num_cameras); // might be redundant + for (int i = 0; i < state_options.num_cameras; i++) { + + // Time offset (use the first one, todo: support multiple!) + if (i == 0) { + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "timeshift_cam_imu", calib_camimu_dt, false); + } + + // Distortion model + std::string dist_model = "radtan"; + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_model", dist_model); + + // Distortion parameters + std::vector cam_calib1 = {1, 1, 0, 0}; + std::vector cam_calib2 = {0, 0, 0, 0}; + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "intrinsics", cam_calib1); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_coeffs", cam_calib2); + Eigen::VectorXd cam_calib = Eigen::VectorXd::Zero(8); + cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1), cam_calib2.at(2), cam_calib2.at(3); + cam_calib(0) /= (downsample_cameras) ? 2.0 : 1.0; + cam_calib(1) /= (downsample_cameras) ? 2.0 : 1.0; + cam_calib(2) /= (downsample_cameras) ? 2.0 : 1.0; + cam_calib(3) /= (downsample_cameras) ? 2.0 : 1.0; + + // FOV / resolution + std::vector matrix_wh = {1, 1}; + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "resolution", matrix_wh); + matrix_wh.at(0) /= (downsample_cameras) ? 2.0 : 1.0; + matrix_wh.at(1) /= (downsample_cameras) ? 2.0 : 1.0; + std::pair wh(matrix_wh.at(0), matrix_wh.at(1)); + + // Extrinsics + Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "T_imu_cam", T_CtoI); + + // Load these into our state + Eigen::Matrix cam_eigen; + cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); + cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); + + // Insert + camera_fisheye.insert({i, dist_model == "equidistant"}); + camera_intrinsics.insert({i, cam_calib}); + camera_extrinsics.insert({i, cam_eigen}); + camera_wh.insert({i, wh}); + } + } PRINT_DEBUG("STATE PARAMETERS:\n"); - PRINT_DEBUG("\t- gravity_mag: %.4f\n", gravity_mag); - PRINT_DEBUG("\t- gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); - PRINT_DEBUG("\t- calib_camimu_dt: %.4f\n", calib_camimu_dt); + PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); + PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); + PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); assert(state_options.num_cameras == (int)camera_fisheye.size()); for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; @@ -233,24 +334,56 @@ struct VioManagerOptions { ov_core::FeatureInitializerOptions featinit_options; /** - * @brief This function will print out all parameters releated to our visual trackers. + * @brief This function will load print out all parameters related to visual tracking + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters */ - void print_trackers() { + void print_and_load_trackers(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("use_stereo", use_stereo); + parser->parse_config("use_klt", use_klt); + parser->parse_config("use_aruco", use_aruco); + parser->parse_config("downsize_aruco", downsize_aruco); + parser->parse_config("multi_threading", use_multi_threading); + parser->parse_config("num_pts", num_pts); + parser->parse_config("fast_threshold", fast_threshold); + parser->parse_config("grid_x", grid_x); + parser->parse_config("grid_y", grid_y); + parser->parse_config("min_px_dist", min_px_dist); + std::string histogram_method_str = "HISTOGRAM"; + parser->parse_config("histogram_method", histogram_method_str); + if (histogram_method_str == "NONE") { + histogram_method = ov_core::TrackBase::NONE; + } else if (histogram_method_str == "HISTOGRAM") { + histogram_method = ov_core::TrackBase::HISTOGRAM; + } else if (histogram_method_str == "CLAHE") { + histogram_method = ov_core::TrackBase::CLAHE; + } else { + printf(RED "VioManager(): invalid feature histogram specified:\n" RESET); + printf(RED "\t- NONE\n" RESET); + printf(RED "\t- HISTOGRAM\n" RESET); + printf(RED "\t- CLAHE\n" RESET); + std::exit(EXIT_FAILURE); + } + parser->parse_config("knn_ratio", knn_ratio); + parser->parse_config("use_mask", use_mask); + } PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n"); - PRINT_DEBUG("\t- use_stereo: %d\n", use_stereo); - PRINT_DEBUG("\t- use_klt: %d\n", use_klt); - PRINT_DEBUG("\t- use_aruco: %d\n", use_aruco); - PRINT_DEBUG("\t- downsize aruco: %d\n", downsize_aruco); - PRINT_DEBUG("\t- downsize cameras: %d\n", downsample_cameras); - PRINT_DEBUG("\t- use multi-threading: %d\n", use_multi_threading); - PRINT_DEBUG("\t- num_pts: %d\n", num_pts); - PRINT_DEBUG("\t- fast threshold: %d\n", fast_threshold); - PRINT_DEBUG("\t- grid X by Y: %d by %d\n", grid_x, grid_y); - PRINT_DEBUG("\t- min px dist: %d\n", min_px_dist); - PRINT_DEBUG("\t- hist method: %d\n", (int)histogram_method); - PRINT_DEBUG("\t- knn ratio: %.3f\n", knn_ratio); - PRINT_DEBUG("\t- use mask?: %d\n", use_mask); - featinit_options.print(); + PRINT_DEBUG(" - use_stereo: %d\n", use_stereo); + PRINT_DEBUG(" - use_klt: %d\n", use_klt); + PRINT_DEBUG(" - use_aruco: %d\n", use_aruco); + PRINT_DEBUG(" - downsize aruco: %d\n", downsize_aruco); + PRINT_DEBUG(" - downsize cameras: %d\n", downsample_cameras); + PRINT_DEBUG(" - use multi-threading: %d\n", use_multi_threading); + PRINT_DEBUG(" - num_pts: %d\n", num_pts); + PRINT_DEBUG(" - fast threshold: %d\n", fast_threshold); + PRINT_DEBUG(" - grid X by Y: %d by %d\n", grid_x, grid_y); + PRINT_DEBUG(" - min px dist: %d\n", min_px_dist); + PRINT_DEBUG(" - hist method: %d\n", (int)histogram_method); + PRINT_DEBUG(" - knn ratio: %.3f\n", knn_ratio); + PRINT_DEBUG(" - use mask?: %d\n", use_mask); + featinit_options.print(parser); } // SIMULATOR =============================== @@ -273,7 +406,7 @@ struct VioManagerOptions { /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in /// simulation. - double sim_distance_threshold = 1.0; + double sim_distance_threshold = 1.2; /// Frequency (Hz) that we will simulate our cameras double sim_freq_cam = 10.0; @@ -288,21 +421,35 @@ struct VioManagerOptions { double sim_max_feature_gen_distance = 10; /** - * @brief This function will print out all simulated parameters loaded. + * @brief This function will load print out all simulated parameters. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters */ - void print_simulation() { + void print_and_load_simulation(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("sim_seed_state_init", sim_seed_state_init); + parser->parse_config("sim_seed_preturb", sim_seed_preturb); + parser->parse_config("sim_seed_measurements", sim_seed_measurements); + parser->parse_config("sim_do_perturbation", sim_do_perturbation); + parser->parse_config("sim_traj_path", sim_traj_path); + parser->parse_config("sim_distance_threshold", sim_distance_threshold); + parser->parse_config("sim_freq_cam", sim_freq_cam); + parser->parse_config("sim_freq_imu", sim_freq_imu); + parser->parse_config("sim_min_feature_gen_dist", sim_min_feature_gen_distance); + parser->parse_config("sim_max_feature_gen_dist", sim_max_feature_gen_distance); + } PRINT_DEBUG("SIMULATION PARAMETERS:\n"); - PRINT_WARNING(BOLDRED "\t- state init seed: %d \n" RESET, sim_seed_state_init); - PRINT_WARNING(BOLDRED "\t- perturb seed: %d \n" RESET, sim_seed_preturb); - PRINT_WARNING(BOLDRED "\t- measurement seed: %d \n" RESET, sim_seed_measurements); - PRINT_WARNING(BOLDRED "\t- do perturb?: %d\n" RESET, sim_do_perturbation); - PRINT_DEBUG("\t- traj path: %s\n", sim_traj_path.c_str()); - PRINT_DEBUG("\t- dist thresh: %.2f\n", sim_distance_threshold); - PRINT_DEBUG("\t- cam feq: %.2f\n", sim_freq_cam); - PRINT_DEBUG("\t- imu feq: %.2f\n", sim_freq_imu); - PRINT_DEBUG("\t- min feat dist: %.2f\n", sim_min_feature_gen_distance); - PRINT_DEBUG("\t- max feat dist: %.2f\n", sim_max_feature_gen_distance); + PRINT_WARNING(BOLDRED " - state init seed: %d \n" RESET, sim_seed_state_init); + PRINT_WARNING(BOLDRED " - perturb seed: %d \n" RESET, sim_seed_preturb); + PRINT_WARNING(BOLDRED " - measurement seed: %d \n" RESET, sim_seed_measurements); + PRINT_WARNING(BOLDRED " - do perturb?: %d\n" RESET, sim_do_perturbation); + PRINT_DEBUG(" - traj path: %s\n", sim_traj_path.c_str()); + PRINT_DEBUG(" - dist thresh: %.2f\n", sim_distance_threshold); + PRINT_DEBUG(" - cam feq: %.2f\n", sim_freq_cam); + PRINT_DEBUG(" - imu feq: %.2f\n", sim_freq_imu); + PRINT_DEBUG(" - min feat dist: %.2f\n", sim_min_feature_gen_distance); + PRINT_DEBUG(" - max feat dist: %.2f\n", sim_max_feature_gen_distance); } }; diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index f9a646bba..091ed041c 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -32,7 +32,6 @@ #include "core/VioManager.h" #include "core/VioManagerOptions.h" #include "utils/dataset_reader.h" -#include "utils/parse_ros.h" #include "utils/sensor_data.h" using namespace ov_msckf; @@ -43,15 +42,38 @@ std::shared_ptr viz; // Main function int main(int argc, char **argv) { + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path.txt"; + if (argc > 1) { + config_path = argv[1]; + } + // Launch our ros node ros::init(argc, argv, "run_serial_msckf"); ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); + + // Load the config + auto parser = std::make_shared(config_path); + parser->set_node_handler(nh); + + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); // Create our VIO system - VioManagerOptions params = parse_ros_nodehandler(nh); + VioManagerOptions params; + params.print_and_load(parser); sys = std::make_shared(params); viz = std::make_shared(nh, sys); + // Ensure we read in all parameters required + if (!parser->successful()) { + PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + std::exit(EXIT_FAILURE); + } + //=================================================================================== //=================================================================================== //=================================================================================== @@ -59,6 +81,7 @@ int main(int argc, char **argv) { // Our imu topic std::string topic_imu; nh.param("topic_imu", topic_imu, "/imu0"); + parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); // Our camera topics (stereo pairs and non-stereo mono) std::vector> topic_cameras; @@ -67,6 +90,8 @@ int main(int argc, char **argv) { std::string cam_topic0, cam_topic1; nh.param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); nh.param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); topic_cameras.emplace_back(0, cam_topic0); topic_cameras.emplace_back(1, cam_topic1); PRINT_DEBUG("serial cam (stereo): %s\n", cam_topic0.c_str()); @@ -76,6 +101,7 @@ int main(int argc, char **argv) { // read in the topic std::string cam_topic; nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); topic_cameras.emplace_back(i, cam_topic); PRINT_DEBUG("serial cam (mono): %s\n", cam_topic.c_str()); } @@ -322,7 +348,8 @@ int main(int argc, char **argv) { // Check if we should initialize using the groundtruth auto msg_camera = msg_images_current.at(smallest_cam); Eigen::Matrix imustate; - if (!gt_states.empty() && !sys->initialized() && ov_core::DatasetReader::get_gt_state(msg_camera->header.stamp.toSec(), imustate, gt_states)) { + if (!gt_states.empty() && !sys->initialized() && + ov_core::DatasetReader::get_gt_state(msg_camera->header.stamp.toSec(), imustate, gt_states)) { // biases are pretty bad normally, so zero them // imustate.block(11,0,6,1).setZero(); sys->initialize_with_gt(imustate); diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp index 112b5dfee..6bcc5429e 100644 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ b/ov_msckf/src/ros_subscribe_msckf.cpp @@ -35,7 +35,6 @@ #include "core/VioManager.h" #include "core/VioManagerOptions.h" #include "utils/dataset_reader.h" -#include "utils/parse_ros.h" #include "utils/sensor_data.h" using namespace ov_msckf; @@ -51,15 +50,39 @@ void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs:: // Main function int main(int argc, char **argv) { + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path.txt"; + if (argc > 1) { + config_path = argv[1]; + } + // Launch our ros node ros::init(argc, argv, "run_subscribe_msckf"); ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); + + // Load the config + std::cout << config_path << std::endl; + auto parser = std::make_shared(config_path); + parser->set_node_handler(nh); + + // Verbosity + std::string verbosity = "DEBUG"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); // Create our VIO system - VioManagerOptions params = parse_ros_nodehandler(nh); + VioManagerOptions params; + params.print_and_load(parser); sys = std::make_shared(params); viz = std::make_shared(nh, sys); + // Ensure we read in all parameters required + if (!parser->successful()) { + PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + std::exit(EXIT_FAILURE); + } + //=================================================================================== //=================================================================================== //=================================================================================== @@ -67,6 +90,7 @@ int main(int argc, char **argv) { // Create imu subscriber std::string topic_imu; nh.param("topic_imu", topic_imu, "/imu0"); + parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); ros::Subscriber subimu = nh.subscribe(topic_imu, 9999999, callback_inertial); // Create camera subscriber data vectors @@ -82,6 +106,8 @@ int main(int argc, char **argv) { std::string cam_topic0, cam_topic1; nh.param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); nh.param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); // Create sync filter (they have unique pointers internally, so we have to use move logic here...) auto image_sub0 = std::unique_ptr>( new message_filters::Subscriber(nh, cam_topic0, 5)); @@ -102,6 +128,7 @@ int main(int argc, char **argv) { // read in the topic std::string cam_topic; nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); // create subscriber subs_cam.push_back(nh.subscribe(cam_topic, 5, boost::bind(callback_monocular, _1, i))); PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 6c481475a..f35d3c97f 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -26,13 +26,11 @@ #include "sim/Simulator.h" #include "utils/colors.h" #include "utils/dataset_reader.h" -#include "utils/parse_cmd.h" #include "utils/print.h" #include "utils/sensor_data.h" #if ROS_AVAILABLE #include "core/RosVisualizer.h" -#include "utils/parse_ros.h" #include #endif @@ -50,23 +48,46 @@ void signal_callback_handler(int signum) { std::exit(signum); } // Main function int main(int argc, char **argv) { - // Read in our parameters - VioManagerOptions params; + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path.txt"; + if (argc > 1) { + config_path = argv[1]; + } + #if ROS_AVAILABLE - ros::init(argc, argv, "test_simulation"); + // Launch our ros node + ros::init(argc, argv, "run_simulation"); ros::NodeHandle nh("~"); - params = parse_ros_nodehandler(nh); -#else - params = parse_command_line_arguments(argc, argv); + nh.param("config_path", config_path, config_path); +#endif + + // Load the config + auto parser = std::make_shared(config_path); +#if ROS_AVAILABLE + parser->set_node_handler(nh); #endif + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + // Create our VIO system + VioManagerOptions params; + params.print_and_load(parser); + params.print_and_load_simulation(parser); sim = std::make_shared(params); sys = std::make_shared(params); #if ROS_AVAILABLE viz = std::make_shared(nh, sys, sim); #endif + // Ensure we read in all parameters required + if(!parser->successful()) { + PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + std::exit(EXIT_FAILURE); + } + //=================================================================================== //=================================================================================== //=================================================================================== diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 0ef8e0e97..3a0c4a4dc 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -36,11 +36,11 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Store a copy of our params this->params = params_; - params.print_estimator(); - params.print_noise(); - params.print_state(); - params.print_trackers(); - params.print_simulation(); + params.print_and_load_estimator(); + params.print_and_load_noise(); + params.print_and_load_state(); + params.print_and_load_trackers(); + params.print_and_load_simulation(); // Check that the max cameras matches the size of our cam matrices if (params.state_options.num_cameras != (int)params.camera_fisheye.size()) { diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index 7e4529bd8..fecb01856 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -70,10 +70,10 @@ class Propagator { /// Nice print function of what parameters we have loaded void print() { - PRINT_DEBUG("\t- gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_DEBUG("\t- accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_DEBUG("\t- gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_DEBUG("\t- accelerometer_random_walk: %.6f\n", sigma_ab); + PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); } }; diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index f15866c78..530fb8ba9 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -24,6 +24,7 @@ #include "types/LandmarkRepresentation.h" #include "utils/print.h" +#include "utils/opencv_yaml_parse.h" #include @@ -59,10 +60,10 @@ struct StateOptions { int max_slam_features = 25; /// Max number of SLAM features we allow to be included in a single EKF update. - int max_slam_in_update = INT_MAX; + int max_slam_in_update = 1000; /// Max number of MSCKF features we will use at a given image timestep. - int max_msckf_in_update = INT_MAX; + int max_msckf_in_update = 1000; /// Max number of estimated ARUCO features int max_aruco_features = 1024; @@ -80,22 +81,45 @@ struct StateOptions { ov_type::LandmarkRepresentation::Representation feat_rep_aruco = ov_type::LandmarkRepresentation::Representation::GLOBAL_3D; /// Nice print function of what parameters we have loaded - void print() { - PRINT_DEBUG("\t- use_fej: %d\n", do_fej); - PRINT_DEBUG("\t- use_imuavg: %d\n", imu_avg); - PRINT_DEBUG("\t- use_rk4int: %d\n", use_rk4_integration); - PRINT_DEBUG("\t- calib_cam_extrinsics: %d\n", do_calib_camera_pose); - PRINT_DEBUG("\t- calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); - PRINT_DEBUG("\t- calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); - PRINT_DEBUG("\t- max_clones: %d\n", max_clone_size); - PRINT_DEBUG("\t- max_slam: %d\n", max_slam_features); - PRINT_DEBUG("\t- max_slam_in_update: %d\n", max_slam_in_update); - PRINT_DEBUG("\t- max_msckf_in_update: %d\n", max_msckf_in_update); - PRINT_DEBUG("\t- max_aruco: %d\n", max_aruco_features); - PRINT_DEBUG("\t- max_cameras: %d\n", num_cameras); - PRINT_DEBUG("\t- feat_rep_msckf: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); - PRINT_DEBUG("\t- feat_rep_slam: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_slam).c_str()); - PRINT_DEBUG("\t- feat_rep_aruco: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); + void print(const std::shared_ptr& parser=nullptr) { + if (parser != nullptr) { + parser->parse_config("use_fej", do_fej); + parser->parse_config("use_imuavg", imu_avg); + parser->parse_config("use_rk4int", use_rk4_integration); + parser->parse_config("calib_cam_extrinsics", do_calib_camera_pose); + parser->parse_config("calib_cam_intrinsics", do_calib_camera_intrinsics); + parser->parse_config("calib_cam_timeoffset", do_calib_camera_timeoffset); + parser->parse_config("max_clones", max_clone_size); + parser->parse_config("max_slam", max_slam_features); + parser->parse_config("max_slam_in_update", max_slam_in_update); + parser->parse_config("max_msckf_in_update", max_msckf_in_update); + parser->parse_config("num_aruco", max_aruco_features); + parser->parse_config("max_cameras", num_cameras); + std::string rep1 = ov_type::LandmarkRepresentation::as_string(feat_rep_msckf); + parser->parse_config("feat_rep_msckf", rep1); + feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(rep1); + std::string rep2 = ov_type::LandmarkRepresentation::as_string(feat_rep_slam); + parser->parse_config("feat_rep_slam", rep2); + feat_rep_slam = ov_type::LandmarkRepresentation::from_string(rep2); + std::string rep3 = ov_type::LandmarkRepresentation::as_string(feat_rep_aruco); + parser->parse_config("feat_rep_aruco", rep3); + feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(rep3); + } + PRINT_DEBUG(" - use_fej: %d\n", do_fej); + PRINT_DEBUG(" - use_imuavg: %d\n", imu_avg); + PRINT_DEBUG(" - use_rk4int: %d\n", use_rk4_integration); + PRINT_DEBUG(" - calib_cam_extrinsics: %d\n", do_calib_camera_pose); + PRINT_DEBUG(" - calib_cam_intrinsics: %d\n", do_calib_camera_intrinsics); + PRINT_DEBUG(" - calib_cam_timeoffset: %d\n", do_calib_camera_timeoffset); + PRINT_DEBUG(" - max_clones: %d\n", max_clone_size); + PRINT_DEBUG(" - max_slam: %d\n", max_slam_features); + PRINT_DEBUG(" - max_slam_in_update: %d\n", max_slam_in_update); + PRINT_DEBUG(" - max_msckf_in_update: %d\n", max_msckf_in_update); + PRINT_DEBUG(" - max_aruco: %d\n", max_aruco_features); + PRINT_DEBUG(" - max_cameras: %d\n", num_cameras); + PRINT_DEBUG(" - feat_rep_msckf: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_msckf).c_str()); + PRINT_DEBUG(" - feat_rep_slam: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_slam).c_str()); + PRINT_DEBUG(" - feat_rep_aruco: %s\n", ov_type::LandmarkRepresentation::as_string(feat_rep_aruco).c_str()); } }; diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp index 415cda43b..47e6e797b 100644 --- a/ov_msckf/src/test_sim_meas.cpp +++ b/ov_msckf/src/test_sim_meas.cpp @@ -31,13 +31,11 @@ #include #if ROS_AVAILABLE -#include "utils/parse_ros.h" #include #endif #include "core/VioManagerOptions.h" #include "sim/Simulator.h" -#include "utils/parse_cmd.h" using namespace ov_msckf; @@ -47,17 +45,14 @@ void signal_callback_handler(int signum) { std::exit(signum); } // Main function int main(int argc, char **argv) { - // Read in our parameters - VioManagerOptions params; #if ROS_AVAILABLE + // Launch our ros node ros::init(argc, argv, "test_sim_meas"); ros::NodeHandle nh("~"); - params = parse_ros_nodehandler(nh); -#else - params = parse_command_line_arguments(argc, argv); #endif // Create the simulator + VioManagerOptions params; Simulator sim(params); // Continue to simulate until we have processed all the measurements diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp index 8084058f5..5a1e5acfb 100644 --- a/ov_msckf/src/test_sim_repeat.cpp +++ b/ov_msckf/src/test_sim_repeat.cpp @@ -35,8 +35,6 @@ #include "core/VioManagerOptions.h" #include "sim/Simulator.h" -#include "utils/parse_cmd.h" -#include "utils/parse_ros.h" #include "utils/print.h" using namespace ov_msckf; @@ -50,18 +48,17 @@ int main(int argc, char **argv) { // Register failure handler signal(SIGINT, signal_callback_handler); +#if ROS_AVAILABLE + // Launch our ros node + ros::init(argc, argv, "test_sim_repeat"); + ros::NodeHandle nh1("~"); +#endif + //=================================================== //=================================================== // Create the simulator VioManagerOptions params1; -#if ROS_AVAILABLE - ros::init(argc, argv, "test_sim_repeat"); - ros::NodeHandle nh1("~"); - params1 = parse_ros_nodehandler(nh1); -#else - params1 = parse_command_line_arguments(argc, argv); -#endif Simulator sim1(params1); // Vector of stored measurements @@ -100,12 +97,6 @@ int main(int argc, char **argv) { // Create the simulator VioManagerOptions params2; -#if ROS_AVAILABLE - ros::NodeHandle nh2("~"); - params2 = parse_ros_nodehandler(nh2); -#else - params2 = parse_command_line_arguments(argc, argv); -#endif Simulator sim2(params2); size_t ct_imu = 0; size_t ct_cam = 0; diff --git a/ov_msckf/src/update/UpdaterOptions.h b/ov_msckf/src/update/UpdaterOptions.h index 293ad9480..067e3f489 100644 --- a/ov_msckf/src/update/UpdaterOptions.h +++ b/ov_msckf/src/update/UpdaterOptions.h @@ -42,8 +42,8 @@ struct UpdaterOptions { /// Nice print function of what parameters we have loaded void print() { - PRINT_DEBUG("\t- chi2_multipler: %.1f\n", chi2_multipler); - PRINT_DEBUG("\t- sigma_pix: %.2f\n", sigma_pix); + PRINT_DEBUG(" - chi2_multipler: %.1f\n", chi2_multipler); + PRINT_DEBUG(" - sigma_pix: %.2f\n", sigma_pix); } }; diff --git a/ov_msckf/src/utils/parse_cmd.h b/ov_msckf/src/utils/parse_cmd.h deleted file mode 100644 index 79f3f10fa..000000000 --- a/ov_msckf/src/utils/parse_cmd.h +++ /dev/null @@ -1,294 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2021 Patrick Geneva - * Copyright (C) 2021 Guoquan Huang - * Copyright (C) 2021 OpenVINS Contributors - * Copyright (C) 2019 Kevin Eckenhoff - * - * This program 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. - * - * This program 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 this program. If not, see . - */ - -#ifndef OV_MSCKF_PARSE_CMDLINE_H -#define OV_MSCKF_PARSE_CMDLINE_H - -#include "core/VioManagerOptions.h" -#include "utils/CLI11.hpp" -#include "utils/print.h" - -namespace ov_msckf { - -/** - * @brief This function will parse the command line arugments using [CLI11](https://github.com/CLIUtils/CLI11). - * This is only used if you are not building with ROS, and thus isn't the primary supported way to pass arguments. - * We recommend building with ROS as compared using this parser. - * @param argc Number of parameters - * @param argv Pointer to string passed as options - * @return A fully loaded VioManagerOptions object - */ -VioManagerOptions parse_command_line_arguments(int argc, char **argv) { - - // Our vio manager options with defaults - VioManagerOptions params; - - // Create our command line parser - CLI::App app1{"parser_cmd_01"}; - app1.allow_extras(); - - // ESTIMATOR ====================================================================== - - // Verbosity - std::string verbosity; - app1.add_option("--verbosity", verbosity, ""); - - // Main EKF parameters - app1.add_option("--use_fej", params.state_options.do_fej, ""); - app1.add_option("--use_imuavg", params.state_options.imu_avg, ""); - app1.add_option("--use_rk4int", params.state_options.use_rk4_integration, ""); - app1.add_option("--calib_cam_extrinsics", params.state_options.do_calib_camera_pose, ""); - app1.add_option("--calib_cam_intrinsics", params.state_options.do_calib_camera_intrinsics, ""); - app1.add_option("--calib_cam_timeoffset", params.state_options.do_calib_camera_timeoffset, ""); - app1.add_option("--max_clones", params.state_options.max_clone_size, ""); - app1.add_option("--max_slam", params.state_options.max_slam_features, ""); - app1.add_option("--max_slam_in_update", params.state_options.max_slam_in_update, ""); - app1.add_option("--max_msckf_in_update", params.state_options.max_msckf_in_update, ""); - app1.add_option("--max_aruco", params.state_options.max_aruco_features, ""); - app1.add_option("--max_cameras", params.state_options.num_cameras, ""); - app1.add_option("--dt_slam_delay", params.dt_slam_delay, ""); - - // Read in what representation our feature is - std::string feat_rep_msckf_str = "GLOBAL_3D"; - std::string feat_rep_slam_str = "GLOBAL_3D"; - std::string feat_rep_aruco_str = "GLOBAL_3D"; - app1.add_option("--feat_rep_msckf", feat_rep_msckf_str, ""); - app1.add_option("--feat_rep_slam", feat_rep_slam_str, ""); - app1.add_option("--feat_rep_aruco", feat_rep_aruco_str, ""); - - // Filter initialization - app1.add_option("--init_window_time", params.init_window_time, ""); - app1.add_option("--init_imu_thresh", params.init_imu_thresh, ""); - - // Zero velocity update - app1.add_option("--try_zupt", params.try_zupt, ""); - app1.add_option("--zupt_chi2_multipler", params.zupt_options.chi2_multipler, ""); - app1.add_option("--zupt_max_velocity", params.zupt_max_velocity, ""); - app1.add_option("--zupt_noise_multiplier", params.zupt_noise_multiplier, ""); - app1.add_option("--zupt_max_disparity", params.zupt_max_disparity, ""); - app1.add_option("--zupt_only_at_beginning", params.zupt_only_at_beginning, ""); - - // Recording of timing information to file - app1.add_option("--record_timing_information", params.record_timing_information, ""); - app1.add_option("--record_timing_filepath", params.record_timing_filepath, ""); - - // NOISE ====================================================================== - - // Our noise values for inertial sensor - app1.add_option("--gyroscope_noise_density", params.imu_noises.sigma_w, ""); - app1.add_option("--accelerometer_noise_density", params.imu_noises.sigma_a, ""); - app1.add_option("--gyroscope_random_walk", params.imu_noises.sigma_wb, ""); - app1.add_option("--accelerometer_random_walk", params.imu_noises.sigma_ab, ""); - - // Read in update parameters - app1.add_option("--up_msckf_sigma_px", params.msckf_options.sigma_pix, ""); - app1.add_option("--up_msckf_chi2_multipler", params.msckf_options.chi2_multipler, ""); - app1.add_option("--up_slam_sigma_px", params.slam_options.sigma_pix, ""); - app1.add_option("--up_slam_chi2_multipler", params.slam_options.chi2_multipler, ""); - app1.add_option("--up_aruco_sigma_px", params.aruco_options.sigma_pix, ""); - app1.add_option("--up_aruco_chi2_multipler", params.aruco_options.chi2_multipler, ""); - - // STATE ====================================================================== - - // Timeoffset from camera to IMU - app1.add_option("--calib_camimu_dt", params.calib_camimu_dt, ""); - - // Global gravity - app1.add_option("--gravity_mag", params.gravity_mag, ""); - - // TRACKERS ====================================================================== - - // Tracking flags - app1.add_option("--use_stereo", params.use_stereo, ""); - app1.add_option("--use_klt", params.use_klt, ""); - app1.add_option("--use_aruco", params.use_aruco, ""); - app1.add_option("--downsize_aruco", params.downsize_aruco, ""); - app1.add_option("--downsample_cameras", params.downsample_cameras, ""); - app1.add_option("--multi_threading", params.use_multi_threading, ""); - - // General parameters - app1.add_option("--num_pts", params.num_pts, ""); - app1.add_option("--fast_threshold", params.fast_threshold, ""); - app1.add_option("--grid_x", params.grid_x, ""); - app1.add_option("--grid_y", params.grid_y, ""); - app1.add_option("--min_px_dist", params.min_px_dist, ""); - app1.add_option("--knn_ratio", params.knn_ratio, ""); - - // Preprocessing histogram method - std::string histogram_method_str = "HISTOGRAM"; - app1.add_option("--histogram_method", histogram_method_str, ""); - - // Feature initializer parameters - app1.add_option("--fi_triangulate_1d", params.featinit_options.triangulate_1d, ""); - app1.add_option("--fi_refine_features", params.featinit_options.refine_features, ""); - app1.add_option("--fi_max_runs", params.featinit_options.max_runs, ""); - app1.add_option("--fi_init_lamda", params.featinit_options.init_lamda, ""); - app1.add_option("--fi_max_lamda", params.featinit_options.max_lamda, ""); - app1.add_option("--fi_min_dx", params.featinit_options.min_dx, ""); - app1.add_option("--fi_min_dcost", params.featinit_options.min_dcost, ""); - app1.add_option("--fi_lam_mult", params.featinit_options.lam_mult, ""); - app1.add_option("--fi_min_dist", params.featinit_options.min_dist, ""); - app1.add_option("--fi_max_dist", params.featinit_options.max_dist, ""); - app1.add_option("--fi_max_baseline", params.featinit_options.max_baseline, ""); - app1.add_option("--fi_max_cond_number", params.featinit_options.max_cond_number, ""); - - // SIMULATION ====================================================================== - - // Load the groundtruth trajectory and its spline - app1.add_option("--sim_traj_path", params.sim_traj_path, ""); - app1.add_option("--sim_distance_threshold", params.sim_distance_threshold, ""); - app1.add_option("--sim_do_perturbation", params.sim_do_perturbation, ""); - - // Read in sensor simulation frequencies - app1.add_option("--sim_freq_cam", params.sim_freq_cam, ""); - app1.add_option("--sim_freq_imu", params.sim_freq_imu, ""); - - // Load the seeds for the random number generators - app1.add_option("--sim_seed_state_init", params.sim_seed_state_init, ""); - app1.add_option("--sim_seed_preturb", params.sim_seed_preturb, ""); - app1.add_option("--sim_seed_measurements", params.sim_seed_measurements, ""); - - // Other simulation parameters - app1.add_option("--sim_min_feat_distance", params.sim_min_feature_gen_distance, ""); - app1.add_option("--sim_max_feat_distance", params.sim_max_feature_gen_distance, ""); - - // CMD PARSE ============================================================================== - - // Finally actually parse the command line and load it - try { - app1.parse(argc, argv); - } catch (const CLI::ParseError &e) { - std::exit(app1.exit(e)); - } - - // Verbosity setting - ov_core::Printer::setPrintLevel(verbosity); - - // Set what representation we should be using - std::transform(feat_rep_msckf_str.begin(), feat_rep_msckf_str.end(), feat_rep_msckf_str.begin(), ::toupper); - std::transform(feat_rep_slam_str.begin(), feat_rep_slam_str.end(), feat_rep_slam_str.begin(), ::toupper); - std::transform(feat_rep_aruco_str.begin(), feat_rep_aruco_str.end(), feat_rep_aruco_str.begin(), ::toupper); - params.state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(feat_rep_msckf_str); - params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string(feat_rep_slam_str); - params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(feat_rep_aruco_str); - if (params.state_options.feat_rep_msckf == ov_type::LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == ov_type::LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == ov_type::LandmarkRepresentation::Representation::UNKNOWN) { - PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); - PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); - PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); - std::exit(EXIT_FAILURE); - } - - // Enforce that we have enough cameras to run - if (params.state_options.num_cameras < 1) { - PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); - PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); - std::exit(EXIT_FAILURE); - } - - // Preprocessing histogram method - if (histogram_method_str == "NONE") { - params.histogram_method = ov_core::TrackBase::NONE; - } else if (histogram_method_str == "HISTOGRAM") { - params.histogram_method = ov_core::TrackBase::HISTOGRAM; - } else if (histogram_method_str == "CLAHE") { - params.histogram_method = ov_core::TrackBase::CLAHE; - } else { - PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); - PRINT_ERROR(RED "\t- NONE\n" RESET); - PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET); - PRINT_ERROR(RED "\t- CLAHE\n" RESET); - std::exit(EXIT_FAILURE); - } - - //==================================================================================== - //==================================================================================== - //==================================================================================== - - // Create our command line parser for the cameras - // NOTE: we need to first parse how many cameras we have before we can parse this - CLI::App app2{"parser_cmd_02"}; - app2.allow_extras(); - - // Set the defaults - std::vector p_fish; - std::vector> p_intrinsic; - std::vector> p_extrinsic; - std::vector> p_wh; - for (int i = 0; i < params.state_options.num_cameras; i++) { - p_fish.push_back(false); - p_intrinsic.push_back({458.654, 457.296, 367.215, 248.375, -0.28340811, 0.07395907, 0.00019359, 1.76187114e-05}); - p_extrinsic.push_back({0, 0, 0, 1, 0, 0, 0}); - p_wh.push_back({752, 480}); - app2.add_option("--cam" + std::to_string(i) + "_fisheye", p_fish.at(i)); - app2.add_option("--cam" + std::to_string(i) + "_intrinsic", p_intrinsic.at(i), ""); - app2.add_option("--cam" + std::to_string(i) + "_extrinsic", p_extrinsic.at(i), ""); - app2.add_option("--cam" + std::to_string(i) + "_wh", p_wh.at(i), ""); - } - - // Finally actually parse the command line and load it - try { - app2.parse(argc, argv); - } catch (const CLI::ParseError &e) { - std::exit(app2.exit(e)); - } - - // Finally load it into our params - for (int i = 0; i < params.state_options.num_cameras; i++) { - - // Halve if we are doing downsampling - p_wh.at(i).at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; - p_wh.at(i).at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; - p_intrinsic.at(i).at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; - p_intrinsic.at(i).at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; - p_intrinsic.at(i).at(2) /= (params.downsample_cameras) ? 2.0 : 1.0; - p_intrinsic.at(i).at(3) /= (params.downsample_cameras) ? 2.0 : 1.0; - - // Convert to Eigen - assert(p_intrinsic.at(i).size() == 8); - Eigen::Matrix intrinsics; - intrinsics << p_intrinsic.at(i).at(0), p_intrinsic.at(i).at(1), p_intrinsic.at(i).at(2), p_intrinsic.at(i).at(3), - p_intrinsic.at(i).at(4), p_intrinsic.at(i).at(5), p_intrinsic.at(i).at(6), p_intrinsic.at(i).at(7); - assert(p_extrinsic.at(i).size() == 7); - Eigen::Matrix extrinsics; - extrinsics << p_extrinsic.at(i).at(0), p_extrinsic.at(i).at(1), p_extrinsic.at(i).at(2), p_extrinsic.at(i).at(3), - p_extrinsic.at(i).at(4), p_extrinsic.at(i).at(5), p_extrinsic.at(i).at(6); - assert(p_wh.at(i).size() == 2); - - // Insert - params.camera_fisheye.insert({i, p_fish.at(i)}); - params.camera_intrinsics.insert({i, intrinsics}); - params.camera_extrinsics.insert({i, extrinsics}); - params.camera_wh.insert({i, {p_wh.at(i).at(0), p_wh.at(i).at(1)}}); - } - - // Success, lets returned the parsed options - return params; -} - -} // namespace ov_msckf - -#endif // OV_MSCKF_PARSE_CMDLINE_H \ No newline at end of file diff --git a/ov_msckf/src/utils/parse_ros.h b/ov_msckf/src/utils/parse_ros.h deleted file mode 100644 index 631f2b12c..000000000 --- a/ov_msckf/src/utils/parse_ros.h +++ /dev/null @@ -1,289 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2021 Patrick Geneva - * Copyright (C) 2021 Guoquan Huang - * Copyright (C) 2021 OpenVINS Contributors - * Copyright (C) 2019 Kevin Eckenhoff - * - * This program 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. - * - * This program 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 this program. If not, see . - */ - -#if ROS_AVAILABLE || defined(DOXYGEN) -#ifndef OV_MSCKF_PARSE_ROSHANDLER_H -#define OV_MSCKF_PARSE_ROSHANDLER_H - -#include - -#include "core/VioManagerOptions.h" -#include "utils/print.h" - -namespace ov_msckf { - -/** - * @brief This function will load paramters from the ros node handler / paramter server - * This is the recommended way of loading parameters as compared to the command line version. - * @param nh ROS node handler - * @return A fully loaded VioManagerOptions object - */ -VioManagerOptions parse_ros_nodehandler(ros::NodeHandle &nh) { - - // Our vio manager options with defaults - VioManagerOptions params; - - // ESTIMATOR ====================================================================== - - // Verbosity setting - std::string verbosity; - nh.param("verbosity", verbosity, "INFO"); - ov_core::Printer::setPrintLevel(verbosity); - - // Main EKF parameters - nh.param("use_fej", params.state_options.do_fej, params.state_options.do_fej); - nh.param("use_imuavg", params.state_options.imu_avg, params.state_options.imu_avg); - nh.param("use_rk4int", params.state_options.use_rk4_integration, params.state_options.use_rk4_integration); - nh.param("calib_cam_extrinsics", params.state_options.do_calib_camera_pose, params.state_options.do_calib_camera_pose); - nh.param("calib_cam_intrinsics", params.state_options.do_calib_camera_intrinsics, params.state_options.do_calib_camera_intrinsics); - nh.param("calib_cam_timeoffset", params.state_options.do_calib_camera_timeoffset, params.state_options.do_calib_camera_timeoffset); - nh.param("max_clones", params.state_options.max_clone_size, params.state_options.max_clone_size); - nh.param("max_slam", params.state_options.max_slam_features, params.state_options.max_slam_features); - nh.param("max_slam_in_update", params.state_options.max_slam_in_update, params.state_options.max_slam_in_update); - nh.param("max_msckf_in_update", params.state_options.max_msckf_in_update, params.state_options.max_msckf_in_update); - nh.param("max_aruco", params.state_options.max_aruco_features, params.state_options.max_aruco_features); - nh.param("max_cameras", params.state_options.num_cameras, params.state_options.num_cameras); - nh.param("dt_slam_delay", params.dt_slam_delay, params.dt_slam_delay); - - // Enforce that we have enough cameras to run - if (params.state_options.num_cameras < 1) { - PRINT_ERROR(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); - PRINT_ERROR(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); - std::exit(EXIT_FAILURE); - } - - // Read in what representation our feature is - std::string feat_rep_msckf_str = "GLOBAL_3D"; - std::string feat_rep_slam_str = "GLOBAL_3D"; - std::string feat_rep_aruco_str = "GLOBAL_3D"; - nh.param("feat_rep_msckf", feat_rep_msckf_str, feat_rep_msckf_str); - nh.param("feat_rep_slam", feat_rep_slam_str, feat_rep_slam_str); - nh.param("feat_rep_aruco", feat_rep_aruco_str, feat_rep_aruco_str); - - // Set what representation we should be using - std::transform(feat_rep_msckf_str.begin(), feat_rep_msckf_str.end(), feat_rep_msckf_str.begin(), ::toupper); - std::transform(feat_rep_slam_str.begin(), feat_rep_slam_str.end(), feat_rep_slam_str.begin(), ::toupper); - std::transform(feat_rep_aruco_str.begin(), feat_rep_aruco_str.end(), feat_rep_aruco_str.begin(), ::toupper); - params.state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::from_string(feat_rep_msckf_str); - params.state_options.feat_rep_slam = ov_type::LandmarkRepresentation::from_string(feat_rep_slam_str); - params.state_options.feat_rep_aruco = ov_type::LandmarkRepresentation::from_string(feat_rep_aruco_str); - if (params.state_options.feat_rep_msckf == ov_type::LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_slam == ov_type::LandmarkRepresentation::Representation::UNKNOWN || - params.state_options.feat_rep_aruco == ov_type::LandmarkRepresentation::Representation::UNKNOWN) { - PRINT_ERROR(RED "VioManager(): invalid feature representation specified:\n" RESET); - PRINT_ERROR(RED "\t- GLOBAL_3D\n" RESET); - PRINT_ERROR(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_3D\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); - PRINT_ERROR(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); - std::exit(EXIT_FAILURE); - } - - // Filter initialization - nh.param("init_window_time", params.init_window_time, params.init_window_time); - nh.param("init_imu_thresh", params.init_imu_thresh, params.init_imu_thresh); - - // Zero velocity update - nh.param("try_zupt", params.try_zupt, params.try_zupt); - nh.param("zupt_chi2_multipler", params.zupt_options.chi2_multipler, params.zupt_options.chi2_multipler); - nh.param("zupt_max_velocity", params.zupt_max_velocity, params.zupt_max_velocity); - nh.param("zupt_noise_multiplier", params.zupt_noise_multiplier, params.zupt_noise_multiplier); - nh.param("zupt_max_disparity", params.zupt_max_disparity, params.zupt_max_disparity); - nh.param("zupt_only_at_beginning", params.zupt_only_at_beginning, params.zupt_only_at_beginning); - - // Recording of timing information to file - nh.param("record_timing_information", params.record_timing_information, params.record_timing_information); - nh.param("record_timing_filepath", params.record_timing_filepath, params.record_timing_filepath); - - // NOISE ====================================================================== - - // Our noise values for inertial sensor - nh.param("gyroscope_noise_density", params.imu_noises.sigma_w, params.imu_noises.sigma_w); - nh.param("accelerometer_noise_density", params.imu_noises.sigma_a, params.imu_noises.sigma_a); - nh.param("gyroscope_random_walk", params.imu_noises.sigma_wb, params.imu_noises.sigma_wb); - nh.param("accelerometer_random_walk", params.imu_noises.sigma_ab, params.imu_noises.sigma_ab); - - // Read in update parameters - nh.param("up_msckf_sigma_px", params.msckf_options.sigma_pix, params.msckf_options.sigma_pix); - nh.param("up_msckf_chi2_multipler", params.msckf_options.chi2_multipler, params.msckf_options.chi2_multipler); - nh.param("up_slam_sigma_px", params.slam_options.sigma_pix, params.slam_options.sigma_pix); - nh.param("up_slam_chi2_multipler", params.slam_options.chi2_multipler, params.slam_options.chi2_multipler); - nh.param("up_aruco_sigma_px", params.aruco_options.sigma_pix, params.aruco_options.sigma_pix); - nh.param("up_aruco_chi2_multipler", params.aruco_options.chi2_multipler, params.aruco_options.chi2_multipler); - - // STATE ====================================================================== - - // Timeoffset from camera to IMU - nh.param("calib_camimu_dt", params.calib_camimu_dt, params.calib_camimu_dt); - - // Global gravity - nh.param("gravity_mag", params.gravity_mag, params.gravity_mag); - - // TRACKERS ====================================================================== - - // Tracking flags - nh.param("use_stereo", params.use_stereo, params.use_stereo); - nh.param("use_klt", params.use_klt, params.use_klt); - nh.param("use_aruco", params.use_aruco, params.use_aruco); - nh.param("downsize_aruco", params.downsize_aruco, params.downsize_aruco); - nh.param("downsample_cameras", params.downsample_cameras, params.downsample_cameras); - nh.param("multi_threading", params.use_multi_threading, params.use_multi_threading); - - // General parameters - nh.param("num_pts", params.num_pts, params.num_pts); - nh.param("fast_threshold", params.fast_threshold, params.fast_threshold); - nh.param("grid_x", params.grid_x, params.grid_x); - nh.param("grid_y", params.grid_y, params.grid_y); - nh.param("min_px_dist", params.min_px_dist, params.min_px_dist); - nh.param("knn_ratio", params.knn_ratio, params.knn_ratio); - - // Preprocessing histogram method - std::string histogram_method_str = "HISTOGRAM"; - nh.param("histogram_method", histogram_method_str, histogram_method_str); - if (histogram_method_str == "NONE") { - params.histogram_method = ov_core::TrackBase::NONE; - } else if (histogram_method_str == "HISTOGRAM") { - params.histogram_method = ov_core::TrackBase::HISTOGRAM; - } else if (histogram_method_str == "CLAHE") { - params.histogram_method = ov_core::TrackBase::CLAHE; - } else { - PRINT_ERROR(RED "VioManager(): invalid feature histogram specified:\n" RESET); - PRINT_ERROR(RED "\t- NONE\n" RESET); - PRINT_ERROR(RED "\t- HISTOGRAM\n" RESET); - PRINT_ERROR(RED "\t- CLAHE\n" RESET); - std::exit(EXIT_FAILURE); - } - - // Feature mask - nh.param("use_mask", params.use_mask, params.use_mask); - for (int i = 0; i < params.state_options.num_cameras; i++) { - std::string mask_path; - nh.param("mask" + std::to_string(i), mask_path, ""); - if (params.use_mask) { - if (!boost::filesystem::exists(mask_path)) { - PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET); - PRINT_ERROR(RED "\t- mask%d\n" RESET, i); - PRINT_ERROR(RED "\t- %s\n" RESET, mask_path.c_str()); - std::exit(EXIT_FAILURE); - } - params.masks.insert({i, cv::imread(mask_path, cv::IMREAD_GRAYSCALE)}); - } - } - - // Feature initializer parameters - nh.param("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d); - nh.param("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features); - nh.param("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs); - nh.param("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda); - nh.param("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda); - nh.param("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx); - nh.param("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost); - nh.param("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult); - nh.param("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist); - nh.param("fi_max_dist", params.featinit_options.max_dist, params.featinit_options.max_dist); - nh.param("fi_max_baseline", params.featinit_options.max_baseline, params.featinit_options.max_baseline); - nh.param("fi_max_cond_number", params.featinit_options.max_cond_number, params.featinit_options.max_cond_number); - - // SIMULATION ====================================================================== - - // Load the groundtruth trajectory and its spline - nh.param("sim_traj_path", params.sim_traj_path, params.sim_traj_path); - nh.param("sim_distance_threshold", params.sim_distance_threshold, params.sim_distance_threshold); - nh.param("sim_do_perturbation", params.sim_do_perturbation, params.sim_do_perturbation); - - // Read in sensor simulation frequencies - nh.param("sim_freq_cam", params.sim_freq_cam, params.sim_freq_cam); - nh.param("sim_freq_imu", params.sim_freq_imu, params.sim_freq_imu); - - // Load the seeds for the random number generators - nh.param("sim_seed_state_init", params.sim_seed_state_init, params.sim_seed_state_init); - nh.param("sim_seed_preturb", params.sim_seed_preturb, params.sim_seed_preturb); - nh.param("sim_seed_measurements", params.sim_seed_measurements, params.sim_seed_measurements); - - // Other simulation parameters - nh.param("sim_min_feat_distance", params.sim_min_feature_gen_distance, params.sim_min_feature_gen_distance); - nh.param("sim_max_feat_distance", params.sim_max_feature_gen_distance, params.sim_max_feature_gen_distance); - - //==================================================================================== - //==================================================================================== - //==================================================================================== - - // Loop through through, and load each of the cameras - for (int i = 0; i < params.state_options.num_cameras; i++) { - - // If our distortions are fisheye or not! - bool is_fisheye; - nh.param("cam" + std::to_string(i) + "_is_fisheye", is_fisheye, false); - - // If the desired fov we should simulate - std::vector matrix_wh; - std::vector matrix_wd_default = {752, 480}; - nh.param>("cam" + std::to_string(i) + "_wh", matrix_wh, matrix_wd_default); - matrix_wh.at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; - matrix_wh.at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; - std::pair wh(matrix_wh.at(0), matrix_wh.at(1)); - - // Camera intrinsic properties - Eigen::Matrix cam_calib; - std::vector matrix_k, matrix_d; - std::vector matrix_k_default = {458.654, 457.296, 367.215, 248.375}; - std::vector matrix_d_default = {-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05}; - nh.param>("cam" + std::to_string(i) + "_k", matrix_k, matrix_k_default); - nh.param>("cam" + std::to_string(i) + "_d", matrix_d, matrix_d_default); - matrix_k.at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; - matrix_k.at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; - matrix_k.at(2) /= (params.downsample_cameras) ? 2.0 : 1.0; - matrix_k.at(3) /= (params.downsample_cameras) ? 2.0 : 1.0; - cam_calib << matrix_k.at(0), matrix_k.at(1), matrix_k.at(2), matrix_k.at(3), matrix_d.at(0), matrix_d.at(1), matrix_d.at(2), - matrix_d.at(3); - - // Our camera extrinsics transform - Eigen::Matrix4d T_CtoI; - std::vector matrix_TCtoI; - std::vector matrix_TtoI_default = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; - - // Read in from ROS, and save into our eigen mat - nh.param>("T_C" + std::to_string(i) + "toI", matrix_TCtoI, matrix_TtoI_default); - T_CtoI << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5), - matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11), - matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15); - - // Load these into our state - Eigen::Matrix cam_eigen; - cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); - cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); - - // Insert - params.camera_fisheye.insert({i, is_fisheye}); - params.camera_intrinsics.insert({i, cam_calib}); - params.camera_extrinsics.insert({i, cam_eigen}); - params.camera_wh.insert({i, wh}); - } - - // Success, lets returned the parsed options - return params; -} - -} // namespace ov_msckf - -#endif // OV_MSCKF_PARSE_ROSHANDLER_H -#endif // ROS_AVAILABLE \ No newline at end of file From 501497353b78c1e8b535f76ca6e5a819356c5d89 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 3 Nov 2021 00:11:43 -0400 Subject: [PATCH 10/55] add mask support back in from yaml and update ironsides yaml to be correct (with the correct IMU noises too...) --- ov_core/src/utils/opencv_yaml_parse.h | 8 +- ov_core/src/utils/print.cpp | 2 +- .../rpng_ironsides/estimator_config.yaml | 49 +++--- .../rpng_ironsides/kalibr_imu_chain.yaml | 8 +- .../launch/pgeneva_serial_ironsides.launch | 144 ------------------ ov_msckf/launch/serial.launch | 2 - ov_msckf/src/core/VioManagerOptions.h | 30 +++- 7 files changed, 62 insertions(+), 181 deletions(-) delete mode 100644 ov_msckf/launch/pgeneva_serial_ironsides.launch diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index a5ff3f6a4..bf1de9c52 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -84,6 +84,12 @@ class YamlParser { void set_node_handler(ros::NodeHandle &nh_) { this->nh = nh_; } #endif + /** + * @brief Will get the folder this config file is in + * @return Config folder + */ + std::string get_config_folder() { return config_path_.substr(0, config_path_.find_last_of('/')) + "/"; } + /** * @brief Check to see if all parameters were read succesfully * @return True if we found all parameters @@ -166,7 +172,7 @@ class YamlParser { std::exit(EXIT_FAILURE); } (*config)[external_node_name] >> path; - std::string relative_folder = config_path_.substr(0, config_path_.find_last_of('/')) + "/"; + std::string relative_folder = get_config_folder(); // Now actually try to load them from file! auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp index 372a5c6a7..4e6d15ba8 100644 --- a/ov_core/src/utils/print.cpp +++ b/ov_core/src/utils/print.cpp @@ -24,7 +24,7 @@ using namespace ov_core; // Need to define the static variable for everything to work -Printer::PrintLevel Printer::current_print_level = PrintLevel::ALL; +Printer::PrintLevel Printer::current_print_level = PrintLevel::INFO; void Printer::setPrintLevel(const std::string &level) { if (level == "ALL") diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml index 334e036e3..3fb1fe42d 100644 --- a/ov_msckf/config/rpng_ironsides/estimator_config.yaml +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -9,19 +9,19 @@ use_rk4int: true # if rk4 integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 -calib_cam_extrinsics: true +calib_cam_extrinsics: false calib_cam_intrinsics: true calib_cam_timeoffset: true -max_clones: 11 -max_slam: 75 +max_clones: 12 +max_slam: 50 max_slam_in_update: 25 max_msckf_in_update: 40 -dt_slam_delay: 3 +dt_slam_delay: 1 -init_window_time: 0.75 -init_imu_thresh: 1.5 -gravity_mag: 9.81 +init_window_time: 1.0 +init_imu_thresh: 0.5 +gravity_mag: 9.80114 feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -30,11 +30,11 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. try_zupt: true -zupt_chi2_multipler: 0 # set to 0 for only disp-based -zupt_max_velocity: 0.1 -zupt_noise_multiplier: 50 -zupt_max_disparity: 0.5 # set to 0 for only imu-based -zupt_only_at_beginning: true +zupt_chi2_multipler: 1 # set to 0 for only disp-based +zupt_max_velocity: 0.5 +zupt_noise_multiplier: 10 +zupt_max_disparity: 0.4 # set to 0 for only imu-based +zupt_only_at_beginning: false # ================================================================== # ================================================================== @@ -53,16 +53,21 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 250 -fast_threshold: 15 -grid_x: 5 -grid_y: 3 -min_px_dist: 8 -knn_ratio: 0.70 +num_pts: 200 +fast_threshold: 10 +grid_x: 20 +grid_y: 20 +min_px_dist: 20 +knn_ratio: 0.65 downsample_cameras: false multi_threading: true histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE +fi_max_dist: 100 +fi_max_baseline: 100 +fi_max_cond_number: 10000 +fi_triangulate_1d: false + # aruco tag tracker for the system # DICT_6X6_1000 from https://chev.me/arucogen/ use_aruco: false @@ -73,15 +78,17 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 1 +up_msckf_sigma_px: 2 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 1 +up_slam_sigma_px: 2 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 # masks for our images -use_mask: false +use_mask: true +mask0: "../../../ov_data/masks/ironsides0.png" #relative to current file +mask1: "../../../ov_data/masks/ironsides1.png" #relative to current file # imu and camera spacial-temporal # imu config should also have the correct noise values diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml index bdc2e32e3..7e14da487 100644 --- a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml +++ b/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml @@ -6,10 +6,10 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 1.1186830841306218e-04 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 8.997530210630026e-07 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.0027052931930236323 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 1.3054568211204843e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + accelerometer_noise_density: 0.0027052931930236323 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 1.3054568211204843e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.1186830841306218e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 8.997530210630026e-07 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated rostopic: /imu0 time_offset: 0.0 diff --git a/ov_msckf/launch/pgeneva_serial_ironsides.launch b/ov_msckf/launch/pgeneva_serial_ironsides.launch deleted file mode 100644 index 6041682fb..000000000 --- a/ov_msckf/launch/pgeneva_serial_ironsides.launch +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [640, 480] - [640, 480] - - - [276.4850207717928, 278.0310503180516, 314.5836189313042, 240.16980920673427] - [-0.03149689493503132, 0.07696336480701078, -0.06608854732019281, 0.019667561645120218] - [277.960323846132, 279.4348778432714, 322.404194404853, 236.72685252691352] - [-0.02998039058251529, 0.07202819722706337, -0.06178718820631651, 0.017655045017816777] - - - - - [ - 0.99992127, -0.0078594, 0.0097819, -0.05845078, - 0.00784873, 0.99996856, 0.00112822, -0.00728728, - -0.00979046, -0.00105136, 0.99995152, 0.0623674, - 0.00000000, 0.00000000, 0.00000000, 1.00000000 - ] - - - [ - 0.99995933, 0.00327998, 0.00840069, 0.00793529, - -0.00328309, 0.99999455, 0.000356, -0.00716413, - -0.00839948, -0.00038357, 0.99996465, 0.06245421, - -0.00000000,0.00000000,0.00000000,1.00000000 - ] - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 97ad3d82b..8580c19f8 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -50,8 +50,6 @@ - - diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index f5553d259..b1fce4928 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -206,6 +206,12 @@ struct VioManagerOptions { /// Map between camid and the dimensions of incoming images (width/cols, height/rows). This is normally only used during simulation. std::map> camera_wh; + /// If we should try to load a mask and use it to reject invalid features + bool use_mask = false; + + /// Mask images for each camera + std::map masks; + /** * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -261,11 +267,27 @@ struct VioManagerOptions { camera_extrinsics.insert({i, cam_eigen}); camera_wh.insert({i, wh}); } + parser->parse_config("use_mask", use_mask); + if (use_mask) { + for (int i = 0; i < state_options.num_cameras; i++) { + std::string mask_path; + std::string mask_node = "mask" + std::to_string(i); + parser->parse_config(mask_node, mask_path); + std::string total_mask_path = parser->get_config_folder() + mask_path; + if (!boost::filesystem::exists(total_mask_path)) { + PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET); + PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); + std::exit(EXIT_FAILURE); + } + masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); + } + } } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); + PRINT_DEBUG(" - camera masks?: %d\n", use_mask); assert(state_options.num_cameras == (int)camera_fisheye.size()); for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; @@ -324,12 +346,6 @@ struct VioManagerOptions { /// KNN ration between top two descriptor matcher which is required to be a good match double knn_ratio = 0.85; - /// If we should try to load a mask and use it to reject invalid features - bool use_mask = false; - - /// Mask images for each camera - std::map masks; - /// Parameters used by our feature initialize / triangulator ov_core::FeatureInitializerOptions featinit_options; @@ -367,7 +383,6 @@ struct VioManagerOptions { std::exit(EXIT_FAILURE); } parser->parse_config("knn_ratio", knn_ratio); - parser->parse_config("use_mask", use_mask); } PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n"); PRINT_DEBUG(" - use_stereo: %d\n", use_stereo); @@ -382,7 +397,6 @@ struct VioManagerOptions { PRINT_DEBUG(" - min px dist: %d\n", min_px_dist); PRINT_DEBUG(" - hist method: %d\n", (int)histogram_method); PRINT_DEBUG(" - knn ratio: %.3f\n", knn_ratio); - PRINT_DEBUG(" - use mask?: %d\n", use_mask); featinit_options.print(parser); } From fc8792ced6243e7f6d32a337ae8af5d5764f4a1e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 3 Nov 2021 00:45:43 -0400 Subject: [PATCH 11/55] tune feature tracking to be more spread out, fixed arucotag id logic, use mask on tumvi datasets --- ov_core/src/track/TrackAruco.h | 6 +- .../config/euroc_mav/estimator_config.yaml | 10 ++-- .../config/rpng_aruco/estimator_config.yaml | 6 +- ov_msckf/config/tum_vi/estimator_config.yaml | 10 ++-- .../config/tum_vi/kalibr_imucam_chain.yaml | 7 +-- .../uzhfpv_outdoor/kalibr_imu_chain.yaml | 16 ------ .../uzhfpv_outdoor/kalibr_imucam_chain.yaml | 37 ------------ .../uzhfpv_outdoor_45/kalibr_imu_chain.yaml | 16 ------ .../kalibr_imucam_chain.yaml | 37 ------------ ov_msckf/launch/display.rviz | 56 +++++++++++++------ ov_msckf/src/core/VioManager.cpp | 2 +- ov_msckf/src/core/VioManager.h | 4 +- ov_msckf/src/state/StateHelper.h | 2 +- 13 files changed, 64 insertions(+), 145 deletions(-) delete mode 100644 ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml delete mode 100644 ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml delete mode 100644 ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml delete mode 100644 ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index 33be419d0..d924e4261 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -35,8 +35,10 @@ namespace ov_core { * @brief Tracking of OpenCV Aruoc tags. * * This class handles the tracking of [OpenCV Aruco tags](https://github.com/opencv/opencv_contrib/tree/master/modules/aruco). - * We track the top left corner of the tag as compared to the pose of the tag or any other corners. - * Right now we hardcode the dictionary to be `cv::aruco::DICT_6X6_25`, so please generate tags in this family of tags. + * We track the corners of the tag as compared to the pose of the tag or any other corners. + * Right now we hardcode the dictionary to be `cv::aruco::DICT_6X6_1000`, so please generate tags in this family of tags. + * You can generate these tags using an online utility: https://chev.me/arucogen/ + * The actual size of the tags do not matter since we do not recover the pose and instead just use this for re-detection and tracking of the four corners of the tag. */ class TrackAruco : public TrackBase { diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index 681574b67..3a33ce255 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -14,10 +14,10 @@ calib_cam_intrinsics: true calib_cam_timeoffset: true max_clones: 11 -max_slam: 75 +max_slam: 50 max_slam_in_update: 25 max_msckf_in_update: 40 -dt_slam_delay: 3 +dt_slam_delay: 2 init_window_time: 0.75 init_imu_thresh: 1.5 @@ -55,9 +55,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 250 fast_threshold: 15 -grid_x: 5 -grid_y: 3 -min_px_dist: 8 +grid_x: 10 +grid_y: 10 +min_px_dist: 15 knn_ratio: 0.70 downsample_cameras: false multi_threading: true diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml index fe45f2b64..7398d2b9b 100644 --- a/ov_msckf/config/rpng_aruco/estimator_config.yaml +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -55,9 +55,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 150 fast_threshold: 15 -grid_x: 5 -grid_y: 3 -min_px_dist: 10 +grid_x: 20 +grid_y: 20 +min_px_dist: 20 knn_ratio: 0.85 downsample_cameras: false multi_threading: true diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index aef6fba08..af9449b18 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -55,9 +55,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 300 fast_threshold: 15 -grid_x: 5 -grid_y: 5 -min_px_dist: 5 +grid_x: 10 +grid_y: 10 +min_px_dist: 15 knn_ratio: 0.65 downsample_cameras: false multi_threading: true @@ -81,7 +81,9 @@ up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 # masks for our images -use_mask: false +use_mask: true +mask0: "../../../ov_data/masks/tumvi0.png" #relative to current file +mask1: "../../../ov_data/masks/tumvi1.png" #relative to current file # imu and camera spacial-temporal # imu config should also have the correct noise values diff --git a/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml b/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml index 00413462a..9da64c5ec 100644 --- a/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml +++ b/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml @@ -13,17 +13,14 @@ cam0: intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504] resolution: [512, 512] rostopic: /cam0/image_raw + + cam1: T_cam_imu: #rotation from IMU to camera, position of camera in IMU - [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734] - [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924] - [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751] - [0.0, 0.0, 0.0, 1.0] - T_cn_cnm1: - - [0.9999994457734953, -0.0008233639921576076, -0.0006561436136444361, -0.10106110275180535] - - [0.0007916877528168117, 0.9988994619156757, -0.04689603624058988, -0.0019764575873431013] - - [0.0006940340102242531, 0.04689549078870055, 0.9988995601463054, -0.0011756424802046581] - - [0.0, 0.0, 0.0, 1.0] cam_overlaps: [0] camera_model: pinhole distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039] diff --git a/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml deleted file mode 100644 index 8fb43e3dc..000000000 --- a/ov_msckf/config/uzhfpv_outdoor/kalibr_imu_chain.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML:1.0 # need to specify the file type at the top! - -imu0: - T_i_b: - - [1.0, 0.0, 0.0, 0.0] - - [0.0, 1.0, 0.0, 0.0] - - [0.0, 0.0, 1.0, 0.0] - - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated - rostopic: /imu0 - time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml deleted file mode 100644 index 0a5584d7c..000000000 --- a/ov_msckf/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml +++ /dev/null @@ -1,37 +0,0 @@ -%YAML:1.0 # need to specify the file type at the top! - -cam0: - T_cam_imu: - - [-0.03179778293757218, -0.9994933985910031, -0.001359107523862424, 0.021115239798621798] - - [0.012827844120885779, 0.0009515801497960164, -0.9999172670328424, -0.0008992998316121829] - - [0.9994120008362244, -0.03181258663210035, 0.012791087377928778, -0.009491094814035777] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [1] - camera_model: pinhole - distortion_coeffs: [-0.005719912631104124, 0.004742449009601135, 0.0012060658036136048, - -0.001580292679344826] - distortion_model: equidistant - intrinsics: [277.4786896484645, 277.42548548840034, 320.1052053576385, 242.10083077857894] - resolution: [640, 480] - rostopic: /snappy_cam/stereo_l - timeshift_cam_imu: -0.007999243205055177 -cam1: - T_cam_imu: - - [-0.011450159873389598, -0.9998746482793399, -0.010935335712288774, -0.05828448770624624] - - [0.009171247533644289, 0.010830579777447058, -0.9998992883087583, -0.0002362068202437068] - - [0.999892385238307, -0.01154929737910465, 0.009046086032012068, -0.00947464531803495] - - [0.0, 0.0, 0.0, 1.0] - T_cn_cnm1: - - [0.9997470623689986, 0.009836089265916417, 0.020225296846065624, -0.07919358086270675] - - [-0.00975774768296796, 0.9999445171722606, -0.0039684930755682956, 0.000831414953842084] - - [-0.020263209141547188, 0.0037701359508940783, 0.9997875716521978, 0.00044568632114983057] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [0] - camera_model: pinhole - distortion_coeffs: [-0.009025009906076716, 0.009967427035376123, -0.0029538969814842117, - -0.0003503551771748748] - distortion_model: equidistant - intrinsics: [276.78679780974477, 276.79332134030807, 314.2862327340746, 236.51313088043128] - resolution: [640, 480] - rostopic: /snappy_cam/stereo_r - timeshift_cam_imu: -0.007983859928063504 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml deleted file mode 100644 index 8fb43e3dc..000000000 --- a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml +++ /dev/null @@ -1,16 +0,0 @@ -%YAML:1.0 # need to specify the file type at the top! - -imu0: - T_i_b: - - [1.0, 0.0, 0.0, 0.0] - - [0.0, 1.0, 0.0, 0.0] - - [0.0, 0.0, 1.0, 0.0] - - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated - rostopic: /imu0 - time_offset: 0.0 - update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml b/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml deleted file mode 100644 index c29412052..000000000 --- a/ov_msckf/config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml +++ /dev/null @@ -1,37 +0,0 @@ -%YAML:1.0 # need to specify the file type at the top! - -cam0: - T_cam_imu: - - [-0.024041523213909927, -0.9996640790624955, 0.009681642096550924, 0.02023430742078562] - - [-0.7184527320882621, 0.010542697330412382, -0.6954958830129113, 0.008311861463499775] - - [0.6951601807615744, -0.023676582632001453, -0.7184648512755534, -0.026628438421085154] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [1] - camera_model: pinhole - distortion_coeffs: [-0.017811595366268803, 0.04897078939103475, -0.041363300782847834, - 0.011440891936886532] - distortion_model: equidistant - intrinsics: [275.3385453506587, 275.0852058534152, 315.7697752181792, 233.72625444124952] - resolution: [640, 480] - rostopic: /snappy_cam/stereo_l - timeshift_cam_imu: -0.008637511810764048 -cam1: - T_cam_imu: - - [-0.004527750456351745, -0.9999560749011355, -0.008206567133703047, -0.05986676424716047] - - [-0.7208238256076104, 0.008951751262681593, -0.6930605158178762, 0.008989928313050033] - - [0.6931035362139012, 0.0027774840496477826, -0.7208326946456712, -0.026595921269512067] - - [0.0, 0.0, 0.0, 1.0] - T_cn_cnm1: - - [0.9996495696908625, -0.0015816259006504233, 0.026424160845286468, -0.07937720055807065] - - [0.0016709946890799124, 0.9999929578963755, -0.0033603473630593734, 0.0005548331594719445] - - [-0.026418659951183095, 0.0034033244279300045, 0.9996451729434878, 0.0005293439870858398] - - [0.0, 0.0, 0.0, 1.0] - cam_overlaps: [0] - camera_model: pinhole - distortion_coeffs: [0.027860492621377443, -0.027723581962855317, 0.0375199775145906, - -0.018152613898714216] - distortion_model: equidistant - intrinsics: [273.2895238376505, 273.35830490745764, 314.60557378520133, 251.0359907029701] - resolution: [640, 480] - rostopic: /snappy_cam/stereo_r - timeshift_cam_imu: -0.008613446015312496 \ No newline at end of file diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz index 8d449e878..399a7ecf1 100644 --- a/ov_msckf/launch/display.rviz +++ b/ov_msckf/launch/display.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.6011396050453186 - Tree Height: 169 + Tree Height: 218 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -24,7 +24,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: SLAM Points Preferences: PromptSaveOnExit: true Toolbars: @@ -55,14 +55,30 @@ Visualization Manager: Frame Timeout: 999 Frames: All Enabled: true - Marker Alpha: 1 + cam0: + Value: true + cam1: + Value: true + global: + Value: true + imu: + Value: true + truth: + Value: true Marker Scale: 1 Name: TF Show Arrows: false Show Axes: true Show Names: true Tree: - {} + global: + imu: + cam0: + {} + cam1: + {} + truth: + {} Update Interval: 0 Value: true - Class: rviz/Image @@ -94,7 +110,6 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -118,7 +133,6 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -140,7 +154,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: MSCKF Points Position Transformer: XYZ Queue Size: 10 @@ -168,7 +184,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: SLAM Points Position Transformer: XYZ Queue Size: 10 @@ -190,13 +208,15 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/PointCloud2 - Color: 255; 170; 0 + Color: 117; 80; 123 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: ARUCO Points Position Transformer: XYZ Queue Size: 10 @@ -224,7 +244,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: ACTIVE Features Position Transformer: XYZ Queue Size: 10 @@ -252,7 +274,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 + Max Intensity: 4096 Min Color: 0; 0; 0 + Min Intensity: 0 Name: SIM Points Position Transformer: XYZ Queue Size: 10 @@ -305,13 +329,12 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 19.73822784423828 + Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -321,21 +344,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.710398256778717 + Pitch: 1.035398006439209 Target Frame: - Yaw: 0.8253981471061707 + Value: Orbit (rviz) + Yaw: 0.8153981566429138 Saved: ~ Window Geometry: Current Depths: collapsed: false Displays: collapsed: false - Height: 837 + Height: 810 Hide Left Dock: false Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -344,6 +368,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1779 - X: 2299 - Y: 1232 + Width: 1431 + X: 1760 + Y: 77 diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index ce7826eb8..4614a5427 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -459,7 +459,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) int curr_aruco_tags = 0; auto it0 = state->_features_SLAM.begin(); while (it0 != state->_features_SLAM.end()) { - if ((int)(*it0).second->_featid <= state->_options.max_aruco_features) + if ((int)(*it0).second->_featid <= 4 * state->_options.max_aruco_features) curr_aruco_tags++; it0++; } diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index ba0119706..ff1885e67 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -177,7 +177,7 @@ class VioManager { std::vector get_features_SLAM() { std::vector slam_feats; for (auto &f : state->_features_SLAM) { - if ((int)f.first <= state->_options.max_aruco_features) + if ((int)f.first <= 4 * state->_options.max_aruco_features) continue; if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { // Assert that we have an anchor pose for this feature @@ -201,7 +201,7 @@ class VioManager { std::vector get_features_ARUCO() { std::vector aruco_feats; for (auto &f : state->_features_SLAM) { - if ((int)f.first > state->_options.max_aruco_features) + if ((int)f.first > 4 * state->_options.max_aruco_features) continue; if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { // Assert that we have an anchor pose for this feature diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index 4b9b2e376..7a693a5bb 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -239,7 +239,7 @@ class StateHelper { // We also check that we do not remove any aruoctag landmarks auto it0 = state->_features_SLAM.begin(); while (it0 != state->_features_SLAM.end()) { - if ((*it0).second->should_marg && (int)(*it0).first > state->_options.max_aruco_features) { + if ((*it0).second->should_marg && (int)(*it0).first > 4 * state->_options.max_aruco_features) { StateHelper::marginalize(state, (*it0).second); it0 = state->_features_SLAM.erase(it0); } else { From 93f1e4c8ac48480879b674be8f83f832aff4ca8e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 3 Nov 2021 11:09:10 -0400 Subject: [PATCH 12/55] uzhfpv update launch, tried to tune config (their imu noises don't work well) --- .../indoor_forward_10_snapdragon_with_gt.txt | 0 .../indoor_forward_3_snapdragon_with_gt.txt | 0 .../indoor_forward_5_snapdragon_with_gt.txt | 0 .../indoor_forward_6_snapdragon_with_gt.txt | 0 .../indoor_forward_7_snapdragon_with_gt.txt | 0 .../indoor_forward_9_snapdragon_with_gt.txt | 0 .../indoor_45_12_snapdragon_with_gt.txt | 0 .../indoor_45_13_snapdragon_with_gt.txt | 0 .../indoor_45_14_snapdragon_with_gt.txt | 0 .../indoor_45_2_snapdragon_with_gt.txt | 0 .../indoor_45_4_snapdragon_with_gt.txt | 0 .../indoor_45_9_snapdragon_with_gt.txt | 0 .../outdoor_45_1_snapdragon_with_gt.txt | 0 .../outdoor_forward_1_snapdragon_with_gt.txt | 0 .../outdoor_forward_3_snapdragon_with_gt.txt | 0 .../outdoor_forward_5_snapdragon_with_gt.txt | 0 .../config/euroc_mav/estimator_config.yaml | 4 +- ov_msckf/config/tum_vi/estimator_config.yaml | 6 +- .../uzhfpv_indoor/estimator_config.yaml | 93 +++++++++++++++++++ .../uzhfpv_indoor/kalibr_imu_chain.yaml | 14 ++- .../uzhfpv_indoor_45/estimator_config.yaml | 93 +++++++++++++++++++ .../uzhfpv_indoor_45/kalibr_imu_chain.yaml | 14 ++- ov_msckf/launch/serial.launch | 2 +- ov_msckf/launch/subscribe.launch | 2 +- ov_msckf/src/core/VioManagerOptions.h | 2 +- 25 files changed, 212 insertions(+), 18 deletions(-) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_10_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_3_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_5_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_6_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_7_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor}/indoor_forward_9_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_12_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_13_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_14_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_2_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_4_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_indoor_45}/indoor_45_9_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_outdoor}/outdoor_45_1_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_outdoor}/outdoor_forward_1_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_outdoor}/outdoor_forward_3_snapdragon_with_gt.txt (100%) rename ov_data/{uzh_fpv => uzhfpv_outdoor}/outdoor_forward_5_snapdragon_with_gt.txt (100%) create mode 100644 ov_msckf/config/uzhfpv_indoor/estimator_config.yaml create mode 100644 ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml diff --git a/ov_data/uzh_fpv/indoor_forward_10_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_10_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_10_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_10_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_forward_3_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_3_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_3_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_3_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_forward_5_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_5_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_5_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_5_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_forward_6_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_6_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_6_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_6_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_forward_7_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_7_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_7_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_7_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_forward_9_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor/indoor_forward_9_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_forward_9_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor/indoor_forward_9_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_12_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_12_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_12_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_12_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_13_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_13_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_13_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_13_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_14_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_14_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_14_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_14_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_2_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_2_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_2_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_2_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_4_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_4_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_4_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_4_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/indoor_45_9_snapdragon_with_gt.txt b/ov_data/uzhfpv_indoor_45/indoor_45_9_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/indoor_45_9_snapdragon_with_gt.txt rename to ov_data/uzhfpv_indoor_45/indoor_45_9_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/outdoor_45_1_snapdragon_with_gt.txt b/ov_data/uzhfpv_outdoor/outdoor_45_1_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/outdoor_45_1_snapdragon_with_gt.txt rename to ov_data/uzhfpv_outdoor/outdoor_45_1_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/outdoor_forward_1_snapdragon_with_gt.txt b/ov_data/uzhfpv_outdoor/outdoor_forward_1_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/outdoor_forward_1_snapdragon_with_gt.txt rename to ov_data/uzhfpv_outdoor/outdoor_forward_1_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/outdoor_forward_3_snapdragon_with_gt.txt b/ov_data/uzhfpv_outdoor/outdoor_forward_3_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/outdoor_forward_3_snapdragon_with_gt.txt rename to ov_data/uzhfpv_outdoor/outdoor_forward_3_snapdragon_with_gt.txt diff --git a/ov_data/uzh_fpv/outdoor_forward_5_snapdragon_with_gt.txt b/ov_data/uzhfpv_outdoor/outdoor_forward_5_snapdragon_with_gt.txt similarity index 100% rename from ov_data/uzh_fpv/outdoor_forward_5_snapdragon_with_gt.txt rename to ov_data/uzhfpv_outdoor/outdoor_forward_5_snapdragon_with_gt.txt diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index 3a33ce255..d463f557b 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -55,8 +55,8 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 250 fast_threshold: 15 -grid_x: 10 -grid_y: 10 +grid_x: 20 +grid_y: 20 min_px_dist: 15 knn_ratio: 0.70 downsample_cameras: false diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index af9449b18..0fe205ff7 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -53,10 +53,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 300 +num_pts: 250 fast_threshold: 15 -grid_x: 10 -grid_y: 10 +grid_x: 20 +grid_y: 20 min_px_dist: 15 knn_ratio: 0.65 downsample_cameras: false diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml new file mode 100644 index 000000000..4053f46ec --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml @@ -0,0 +1,93 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: false +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 50 +max_slam_in_update: 25 +max_msckf_in_update: 30 +dt_slam_delay: 2 + +init_window_time: 1.0 +init_imu_thresh: 0.5 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.5 +zupt_noise_multiplier: 20 +zupt_max_disparity: 0.4 # set to 0 for only imu-based +zupt_only_at_beginning: false + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 200 +fast_threshold: 15 +grid_x: 20 +grid_y: 20 +min_px_dist: 15 +knn_ratio: 0.70 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 2 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 2 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml index 8fb43e3dc..5e91ccd79 100644 --- a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml +++ b/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml @@ -6,11 +6,15 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) +# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) +# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) +# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) +# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated - rostopic: /imu0 + rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml new file mode 100644 index 000000000..4053f46ec --- /dev/null +++ b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml @@ -0,0 +1,93 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: false +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 50 +max_slam_in_update: 25 +max_msckf_in_update: 30 +dt_slam_delay: 2 + +init_window_time: 1.0 +init_imu_thresh: 0.5 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.5 +zupt_noise_multiplier: 20 +zupt_max_disparity: 0.4 # set to 0 for only imu-based +zupt_only_at_beginning: false + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 200 +fast_threshold: 15 +grid_x: 20 +grid_y: 20 +min_px_dist: 15 +knn_ratio: 0.70 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 2 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 2 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml index 8fb43e3dc..32c182c76 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml @@ -6,11 +6,15 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated - rostopic: /imu0 + rostopic: /snappy_imu time_offset: 0.0 update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 8580c19f8..4268d4441 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -24,7 +24,7 @@ - + diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 121c796ee..89904b861 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -24,7 +24,7 @@ - + diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index b1fce4928..70ef176ab 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -174,7 +174,7 @@ struct VioManagerOptions { msckf_options.sigma_pix_sq = std::pow(msckf_options.sigma_pix, 2); slam_options.sigma_pix_sq = std::pow(slam_options.sigma_pix, 2); aruco_options.sigma_pix_sq = std::pow(aruco_options.sigma_pix, 2); - parser->parse_config("zupt_chi2_multipler", aruco_options.chi2_multipler); + parser->parse_config("zupt_chi2_multipler", zupt_options.chi2_multipler); } PRINT_DEBUG(" Updater MSCKF Feats:\n"); msckf_options.print(); From bc2a3c58c96278dc649f35e8348e52dade59315a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 8 Nov 2021 11:14:53 -0500 Subject: [PATCH 13/55] added simulation yaml files --- ...st.launch => pgeneva_ros_kaist.launch.old} | 0 ...launch => pgeneva_ros_kaistvio.launch.old} | 0 .../config/rpng_sim/estimator_config.yaml | 105 +++++++++ .../config/rpng_sim/kalibr_imu_chain.yaml | 16 ++ .../config/rpng_sim/kalibr_imucam_chain.yaml | 28 +++ ov_msckf/launch/pgeneva_ros_uzhfpv.launch | 223 ------------------ .../{pgeneva_sim.launch => simulation.launch} | 87 ++----- 7 files changed, 163 insertions(+), 296 deletions(-) rename ov_msckf/config/kaist/{pgeneva_ros_kaist.launch => pgeneva_ros_kaist.launch.old} (100%) rename ov_msckf/config/kaist_vio/{pgeneva_ros_kaistvio.launch => pgeneva_ros_kaistvio.launch.old} (100%) create mode 100644 ov_msckf/config/rpng_sim/estimator_config.yaml create mode 100644 ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml create mode 100644 ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml delete mode 100644 ov_msckf/launch/pgeneva_ros_uzhfpv.launch rename ov_msckf/launch/{pgeneva_sim.launch => simulation.launch} (57%) diff --git a/ov_msckf/config/kaist/pgeneva_ros_kaist.launch b/ov_msckf/config/kaist/pgeneva_ros_kaist.launch.old similarity index 100% rename from ov_msckf/config/kaist/pgeneva_ros_kaist.launch rename to ov_msckf/config/kaist/pgeneva_ros_kaist.launch.old diff --git a/ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch b/ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch.old similarity index 100% rename from ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch rename to ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch.old diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml new file mode 100644 index 000000000..be1e73e9e --- /dev/null +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -0,0 +1,105 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: true +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 11 +max_slam: 50 +max_slam_in_update: 25 +max_msckf_in_update: 10 +dt_slam_delay: 2 + +init_window_time: 0.75 +init_imu_thresh: 1.0 +gravity_mag: 9.81 + +feat_rep_msckf: "GLOBAL_3D" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: false +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 250 +fast_threshold: 15 +grid_x: 20 +grid_y: 20 +min_px_dist: 15 +knn_ratio: 0.70 +downsample_cameras: false +multi_threading: true +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + +# ================================================================== +# ================================================================== + + +sim_seed_state_init: 0 +sim_seed_preturb: 0 +sim_seed_measurements: 0 +sim_do_perturbation: false +sim_traj_path: "/tmp/trajectory_not_set.txt" +sim_distance_threshold: 1.2 +sim_freq_cam: 10 +sim_freq_imu: 400 +sim_min_feature_gen_dist: 5.0 +sim_max_feature_gen_dist: 7.0 \ No newline at end of file diff --git a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml new file mode 100644 index 000000000..394cdca13 --- /dev/null +++ b/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml @@ -0,0 +1,16 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /imu0 + time_offset: 0.0 + update_rate: 200.0 \ No newline at end of file diff --git a/ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml b/ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..6e14a9084 --- /dev/null +++ b/ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] + - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] + - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] + distortion_model: radtan + intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam0/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU, position of IMU in camera + - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] + - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] + - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] + distortion_model: radtan + intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam1/image_raw \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_ros_uzhfpv.launch b/ov_msckf/launch/pgeneva_ros_uzhfpv.launch deleted file mode 100644 index cf91ca664..000000000 --- a/ov_msckf/launch/pgeneva_ros_uzhfpv.launch +++ /dev/null @@ -1,223 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [640, 480] - [640, 480] - - - - - [278.66723066149086, 278.48991409740296, 319.75221200593535, 241.96858910358173] - [-0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625] - [277.61640629770613, 277.63749695723294, 314.8944703346039, 236.04310050462587] - [-0.008456929295619607, 0.011407590938612062, -0.006951788325762078, 0.0015368127092821786] - - [ - -0.02822879, 0.01440125, 0.99949774, 0.00110212, - -0.99960149, -0.00041887, -0.02822568, 0.02170142, - 0.00001218, -0.99989621, 0.01440734, -0.00005928, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.01182306, 0.01155299, 0.99986336, -0.00029028, - -0.99987014, 0.01081377, -0.01194809, -0.05790695, - -0.01095033, -0.99987479, 0.01142364, -0.0001919, - 0.0, 0.0, 0.0, 1.0 - ] - - - - [275.46015578667294, 274.9948095922592, 315.958384100568, 242.7123497822731] - [-6.545154718304953e-06, -0.010379525898159981, 0.014935312423953146, -0.005639061406567785] - [274.4628309070672, 273.9261674470783, 315.93654481793794, 235.779167375461] - [-0.012138050918285051, 0.02244029339184358, -0.013753165428754275, 0.002725090438517269] - - [ - -0.02725669, -0.71392061, 0.69969596, 0.00751451, - -0.99962606, 0.01793147, -0.02064447, 0.02404535, - 0.00219194, -0.69999702, -0.7141424, 0.00577265, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.01749277, -0.7090992, 0.70489172, 0.00670958, - -0.99979146, 0.01983523, -0.00485745, -0.05587358, - -0.01053728, -0.70482969, -0.7092983, 0.00423116, - 0.0, 0.0, 0.0, 1.0 - ] - - - - [277.4786896484645, 277.42548548840034, 320.1052053576385, 242.10083077857894] - [-0.005719912631104124, 0.004742449009601135, 0.0012060658036136048, -0.001580292679344826] - [276.78679780974477, 276.79332134030807, 314.2862327340746, 236.51313088043128] - [-0.009025009906076716, 0.009967427035376123, -0.0029538969814842117, -0.0003503551771748748] - - [ - -0.03179778, 0.01282784, 0.999412, 0.01016847, - -0.9994934, 0.00095158, -0.03181259, 0.02080346, - -0.00135911, -0.99991727, 0.01279109, -0.00074913, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.01145016, 0.00917125, 0.99989239, 0.00880843, - -0.99987465, 0.01083058, -0.0115493, -0.05838405, - -0.01093534, -0.99989929, 0.00904609, -0.00078784, - 0.0, 0.0, 0.0, 1.0 - ] - - - - [275.3385453506587, 275.0852058534152, 315.7697752181792, 233.72625444124952] - [-0.017811595366268803, 0.04897078939103475, -0.041363300782847834, 0.011440891936886532] - [273.2895238376505, 273.35830490745764, 314.60557378520133, 251.0359907029701] - [0.027860492621377443, -0.027723581962855317, 0.0375199775145906, -0.018152613898714216] - - [ - -0.02404152, -0.71845273, 0.69516018, 0.02496917, - -0.99966408, 0.0105427, -0.02367658, 0.01950941, - 0.00968164, -0.69549588, -0.71846485, -0.01354663, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - -0.00452775, -0.72082383, 0.69310354, 0.02464282, - -0.99995607, 0.00895175, 0.00277748, -0.05987074, - -0.00820657, -0.69306052, -0.72083269, -0.01343195, - 0.0, 0.0, 0.0, 1.0 - ] - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/ov_msckf/launch/pgeneva_sim.launch b/ov_msckf/launch/simulation.launch similarity index 57% rename from ov_msckf/launch/pgeneva_sim.launch rename to ov_msckf/launch/simulation.launch index c61b13c4f..4fa0c3739 100644 --- a/ov_msckf/launch/pgeneva_sim.launch +++ b/ov_msckf/launch/simulation.launch @@ -4,7 +4,12 @@ + + + + + @@ -51,7 +56,6 @@ - @@ -61,51 +65,24 @@ + + + + - - - - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - @@ -114,46 +91,10 @@ - - - - - - - [752,480] - [752,480] - [752,480] - - - - - [458.654,457.296,367.215,248.375] - [-0.28340811,0.07395907,0.00019359,1.76187114e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - - - - - [ - 0,-1, 0, -0.05, - 1, 0, 0, 0.00, - 0, 0, 1, 0.00, - 0, 0, 0, 1.00 - ] - - - [ - 0,-1, 0, 0.05, - 1, 0, 0, 0.00, - 0, 0, 1, 0.00, - 0, 0, 0, 1.00 - ] - + + + + From 8b6c52c2f36b5e22a7a4f20e0d7154ae56eb8097 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Thu, 11 Nov 2021 20:21:58 -0500 Subject: [PATCH 14/55] add basic static initalizer to ov_init package --- CMakeLists.txt | 1 + docs/bib/extra.bib | 12 +- docs/gs-calibration.dox | 2 +- ov_core/CMakeLists.txt | 15 +- {ov_eval => ov_core}/src/plot/LICENSE | 0 {ov_eval => ov_core}/src/plot/matplotlibcpp.h | 0 ov_init/CMakeLists.txt | 116 +++++ ov_init/package.xml | 38 ++ ov_init/src/dummy.cpp | 31 ++ ov_init/src/init/InertialInitializer.cpp | 55 ++ ov_init/src/init/InertialInitializer.h | 104 ++++ ov_init/src/init/InertialInitializerOptions.h | 483 ++++++++++++++++++ .../src/static/StaticInitializer.cpp | 104 ++-- .../src/static/StaticInitializer.h | 69 +-- ov_init/src/test_orientation.cpp | 36 ++ ov_init/src/test_static.cpp | 36 ++ ov_msckf/CMakeLists.txt | 7 +- .../config/euroc_mav/estimator_config.yaml | 18 +- .../config/rpng_aruco/estimator_config.yaml | 19 +- .../rpng_ironsides/estimator_config.yaml | 28 +- .../config/rpng_sim/estimator_config.yaml | 18 +- ov_msckf/config/tum_vi/estimator_config.yaml | 18 +- .../uzhfpv_indoor/estimator_config.yaml | 18 +- .../uzhfpv_indoor_45/estimator_config.yaml | 18 +- ov_msckf/launch/serial.launch | 6 - ov_msckf/launch/subscribe.launch | 6 - ov_msckf/package.xml | 2 + ov_msckf/src/core/VioManager.cpp | 34 +- ov_msckf/src/core/VioManager.h | 20 +- ov_msckf/src/core/VioManagerOptions.h | 24 +- ov_msckf/src/state/State.h | 2 +- ov_msckf/src/state/StateHelper.cpp | 34 +- ov_msckf/src/state/StateHelper.h | 25 +- 33 files changed, 1210 insertions(+), 189 deletions(-) rename {ov_eval => ov_core}/src/plot/LICENSE (100%) rename {ov_eval => ov_core}/src/plot/matplotlibcpp.h (100%) create mode 100644 ov_init/CMakeLists.txt create mode 100644 ov_init/package.xml create mode 100644 ov_init/src/dummy.cpp create mode 100644 ov_init/src/init/InertialInitializer.cpp create mode 100644 ov_init/src/init/InertialInitializer.h create mode 100644 ov_init/src/init/InertialInitializerOptions.h rename ov_core/src/init/InertialInitializer.cpp => ov_init/src/static/StaticInitializer.cpp (57%) rename ov_core/src/init/InertialInitializer.h => ov_init/src/static/StaticInitializer.h (52%) create mode 100644 ov_init/src/test_orientation.cpp create mode 100644 ov_init/src/test_static.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 78419353d..8eada6a3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ project(open_vins) # Main required modules that are needed to build the whole system add_subdirectory(ov_core) +add_subdirectory(ov_init) add_subdirectory(ov_msckf) # Optionally build the eval module, might not needed if building for a drone/robotic system diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib index 783aa7566..26c656879 100644 --- a/docs/bib/extra.bib +++ b/docs/bib/extra.bib @@ -40,7 +40,7 @@ @inproceedings{Schubert2018IROS url = {https://arxiv.org/pdf/1804.06120.pdf}, } -@inproceedings{Furgale2013ICRA, +@inproceedings{Furgale2013IROS, title = {Unified temporal and spatial calibration for multi-sensor systems}, author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland}, booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems}, @@ -228,4 +228,14 @@ @article{Jeon2021RAL publisher={IEEE} } +@inproceedings{Dong2012IROS, + title={Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration}, + author={Dong-Si, Tue-Cuong and Mourikis, Anastasios I}, + booktitle={2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, + pages={1064--1071}, + year={2012}, + organization={IEEE} +} + + diff --git a/docs/gs-calibration.dox b/docs/gs-calibration.dox index 9a49c9f4f..364abdbd9 100644 --- a/docs/gs-calibration.dox +++ b/docs/gs-calibration.dox @@ -29,7 +29,7 @@ Within this *OpenVINS* project we address these by advocating the *online* estim The first task is to calibrate the camera intrinsic values such as the focal length, camera center, and distortion coefficients. -Our group often uses the [Kalibr](https://github.com/ethz-asl/kalibr/) @cite Furgale2013ICRA calibration toolbox to perform both intrinsic and extrinsic offline calibrations, by proceeding the following steps: +Our group often uses the [Kalibr](https://github.com/ethz-asl/kalibr/) @cite Furgale2013IROS calibration toolbox to perform both intrinsic and extrinsic offline calibrations, by proceeding the following steps: 1. Clone and build the [Kalibr](https://github.com/ethz-asl/kalibr/) toolbox 2. Print out a calibration board to use (we normally use the [Aprilgrid 6x6 0.8x0.8 m (A0 page)](https://drive.google.com/file/d/0B0T1sizOvRsUdjFJem9mQXdiMTQ/edit?usp=sharing)) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 63b9e87fb..2a8e0d3d8 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -29,6 +29,17 @@ else () add_definitions(-DENABLE_ARUCO_TAGS=1) endif () +# check if we have our python libs files +# sudo apt-get install python-matplotlib python-numpy python2.7-dev +find_package(PythonLibs 2.7) +option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) +if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) + add_definitions(-DHAVE_PYTHONLIBS=1) + message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) + message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) + message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) +endif () + # Describe catkin project option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) if (catkin_FOUND AND ENABLE_CATKIN_ROS) @@ -43,7 +54,6 @@ else () message(WARNING "BUILDING WITHOUT ROS!") endif () - # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) @@ -68,6 +78,8 @@ include_directories( src ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include ${catkin_INCLUDE_DIRS} ) @@ -84,7 +96,6 @@ list(APPEND thirdparty_libraries add_library(ov_core_lib SHARED src/dummy.cpp - src/init/InertialInitializer.cpp src/sim/BsplineSE3.cpp src/track/TrackBase.cpp src/track/TrackAruco.cpp diff --git a/ov_eval/src/plot/LICENSE b/ov_core/src/plot/LICENSE similarity index 100% rename from ov_eval/src/plot/LICENSE rename to ov_core/src/plot/LICENSE diff --git a/ov_eval/src/plot/matplotlibcpp.h b/ov_core/src/plot/matplotlibcpp.h similarity index 100% rename from ov_eval/src/plot/matplotlibcpp.h rename to ov_core/src/plot/matplotlibcpp.h diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt new file mode 100644 index 000000000..6ae813744 --- /dev/null +++ b/ov_init/CMakeLists.txt @@ -0,0 +1,116 @@ +cmake_minimum_required(VERSION 3.3) + +# Define our install directories based on GNU conventions +# https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html +include(GNUInstallDirs) + +# Project name +project(ov_init) + +# Find catkin (the ROS build system) +find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core) + +# Include libraries +find_package(Eigen3 REQUIRED) +find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) + +# check if we have our python libs files +# sudo apt-get install python-matplotlib python-numpy python2.7-dev +find_package(PythonLibs 2.7) +option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) +if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) + add_definitions(-DHAVE_PYTHONLIBS=1) + message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) + message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) + message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) +endif () + +# Describe catkin project +option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) +if (catkin_FOUND AND ENABLE_CATKIN_ROS) + add_definitions(-DROS_AVAILABLE=1) + catkin_package( + CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core + INCLUDE_DIRS src + LIBRARIES ov_init_lib + ) +else () + add_definitions(-DROS_AVAILABLE=0) + message(WARNING "BUILDING WITHOUT ROS!") +endif () + +# Try to compile with c++11 +# http://stackoverflow.com/a/25836953 +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if (COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif (COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else () + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif () + +# Enable compile optimizations +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") + +# Enable debug flags (use if you want to debug in gdb) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${PYTHON_LIBRARIES} + ${catkin_LIBRARIES} +) + +# If we are not building with ROS then we need to manually link to its headers +# This isn't that elegant of a way, but this at least allows for building without ROS +# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") + include_directories(${ov_core_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_core_lib) +endif () + +################################################## +# Make the shared library +################################################## + +add_library(ov_init_lib SHARED + src/dummy.cpp + src/init/InertialInitializer.cpp + src/static/StaticInitializer.cpp +) +target_link_libraries(ov_init_lib ${thirdparty_libraries}) +install(TARGETS ov_init_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +################################################## +# Make binary files! +################################################## + +if (catkin_FOUND AND ENABLE_CATKIN_ROS) + + add_executable(test_orientation src/test_orientation.cpp) + target_link_libraries(test_orientation ov_init_lib ${thirdparty_libraries}) + +endif () + + + diff --git a/ov_init/package.xml b/ov_init/package.xml new file mode 100644 index 000000000..91b782a8c --- /dev/null +++ b/ov_init/package.xml @@ -0,0 +1,38 @@ + + + + ov_init + 0.0.0 + + Initialization package which handles dynamic and static initialization. + + + + Patrick Geneva + Patrick Geneva + + + GNU General Public License v3.0 + + + catkin + + + cmake_modules + roscpp + rospy + geometry_msgs + nav_msgs + sensor_msgs + ov_core + + + roscpp + rospy + geometry_msgs + nav_msgs + sensor_msgs + ov_core + + + \ No newline at end of file diff --git a/ov_init/src/dummy.cpp b/ov_init/src/dummy.cpp new file mode 100644 index 000000000..302c292fa --- /dev/null +++ b/ov_init/src/dummy.cpp @@ -0,0 +1,31 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +/** + * @namespace ov_init + * + * This is an implementation and extension of the work [Estimator initialization in vision-aided inertial navigation with unknown camera-IMU + * calibration](https://ieeexplore.ieee.org/document/6386235) @cite Dong2012IROS which solves the initialization problem by first creating a + * linear system for recovering the camera to IMU rotation, then for velocity, gravity, and feature positions, and finally a full + * optimization to allow for covariance recovery. Specifically, we focus on also recovering the biases of the platform and the time offset. + * + */ +namespace ov_init {} diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp new file mode 100644 index 000000000..db766eee5 --- /dev/null +++ b/ov_init/src/init/InertialInitializer.cpp @@ -0,0 +1,55 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#include "InertialInitializer.h" + +using namespace ov_core; +using namespace ov_type; +using namespace ov_init; + +InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_) : params(params_) { + + // Vector of our IMU data + imu_data = std::make_shared>(); + + // Create static initializer + init_static = std::make_shared(params, imu_data); + + // TODO: create the feature tracker here + // TODO: create dynamic initialize class object +} + +bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, + std::shared_ptr t_imu, bool wait_for_jerk) { + + // TODO: calculate the current disparity to see if we are stationary + + // TODO: if not stationary, first try to do our dynamic initialization, return on success + + // TODO: return if we need to initalize any of the calibration (only dynamic can do this) + + // TODO: return if we are wait_for_jerk and disparity says we are moving (handles constant accel case) + + // TODO: if we have calibration, check if we are still, and use our static initalizer + + // TEMP: use our static initializer! + return init_static->initialize(timestamp, covariance, order, t_imu, wait_for_jerk); +} diff --git a/ov_init/src/init/InertialInitializer.h b/ov_init/src/init/InertialInitializer.h new file mode 100644 index 000000000..a594ea223 --- /dev/null +++ b/ov_init/src/init/InertialInitializer.h @@ -0,0 +1,104 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OV_INIT_INERTIALINITIALIZER_H +#define OV_INIT_INERTIALINITIALIZER_H + +#include "init/InertialInitializerOptions.h" +#include "static/StaticInitializer.h" + +#include "types/Type.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" +#include "utils/sensor_data.h" + +namespace ov_init { + +/** + * @brief Initializer for visual-inertial system. + * + * This will try to do both dynamic and state initialization of the state. + * The user can request to wait for a jump in our IMU readings (i.e. device is picked up) or to initialize as soon as possible. + * For state initialization, the user needs to specify the calibration beforehand, otherwise dynamic is always used. + * + * The logic is as follows: + * 1. Try to perform dynamic initialization of state elements. + * 2. If this fails and we have calibration then we can try to do static initialization + * 3. If the unit is stationary and we are waiting for a jerk, just return, otherwise initialize the state! + * + * The dynamic system is based on an implementation and extension of the work [Estimator initialization in vision-aided inertial navigation + * with unknown camera-IMU calibration](https://ieeexplore.ieee.org/document/6386235) @cite Dong2012IROS which solves the initialization + * problem by first creating a linear system for recovering the camera to IMU rotation, then for velocity, gravity, and feature positions, + * and finally a full optimization to allow for covariance recovery. + */ +class InertialInitializer { + +public: + /** + * @brief Default constructor + * @param params_ Parameters loaded from either ROS or CMDLINE + */ + explicit InertialInitializer(InertialInitializerOptions ¶ms_); + + /** + * @brief Feed function for inertial data + * @param message Contains our timestamp and inertial information + */ + void feed_imu(const ov_core::ImuData &message) { + + // Append it to our vector + imu_data->push_back(message); + + // Delete all measurements older than three of our initialization windows + auto it0 = imu_data->begin(); + while (it0 != imu_data->end() && it0->timestamp < message.timestamp - 3 * params.init_window_time) { + it0 = imu_data->erase(it0); + } + } + + /** + * @brief Try to get the initialized system + * + * @param[out] timestamp Timestamp we have initialized the state at + * @param[out] covariance Calculated covariance of the returned state + * @param[out] order Order of the covariance matrix + * @param[out] t_imu Our imu type (need to have correct ids) + * @param wait_for_jerk If true we will wait for a "jerk" + * @return True if we have successfully initialized our system + */ + bool initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, + std::shared_ptr t_imu, bool wait_for_jerk = true); + +protected: + /// Initialization parameters + InertialInitializerOptions params; + + /// Our history of IMU messages (time, angular, linear) + std::shared_ptr> imu_data; + + /// Static initialization helper class + std::shared_ptr init_static; +}; + +} // namespace ov_init + +#endif // OV_INIT_INERTIALINITIALIZER_H diff --git a/ov_init/src/init/InertialInitializerOptions.h b/ov_init/src/init/InertialInitializerOptions.h new file mode 100644 index 000000000..323b8ee32 --- /dev/null +++ b/ov_init/src/init/InertialInitializerOptions.h @@ -0,0 +1,483 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OV_INIT_INERTIALINITIALIZEROPTIONS_H +#define OV_INIT_INERTIALINITIALIZEROPTIONS_H + +#include +#include +#include +#include +#include + +#include "feat/FeatureInitializerOptions.h" +#include "track/TrackBase.h" +#include "utils/colors.h" +#include "utils/opencv_yaml_parse.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + +namespace ov_init { + +/** + * @brief Struct which stores all options needed for state estimation. + * + * This is broken into a few different parts: estimator, trackers, and simulation. + * If you are going to add a parameter here you will need to add it to the parsers. + * You will also need to add it to the print statement at the bottom of each. + */ +struct InertialInitializerOptions { + + /** + * @brief This function will load the non-simulation parameters of the system and print. + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load(const std::shared_ptr &parser = nullptr) { + print_and_load_initializer(parser); + print_and_load_noise(parser); + print_and_load_state(parser); + print_and_load_trackers(parser); + } + + // CALIBRATION ============================ + + /// If we should only do dynamic state initialization + bool init_only_use_dynamic = false; + + /// If we should only do static state initialization + bool init_only_use_static = false; + + /// Bool to determine whether or not to recover imu-to-camera pose + bool init_calib_camera_pose = false; + + /// Bool to determine whether or not to recover camera to IMU time offset + bool init_calib_camera_timeoffset = false; + + /// Bool to determine whether or not to recover acceleration bias + bool init_state_bias_accel = true; + + /// Bool to determine whether or not to enforce gravity magnitude + bool init_enforce_gravity_magnitude = true; + + /// What camera rate we should track features at + double init_camera_rate = 10.0; + + /// Amount of time we will initialize over (seconds) + double init_window_time = 1.0; + + /// Variance threshold on our acceleration to be classified as moving + double init_imu_thresh = 1.0; + + /// Max disparity we will consider the unit to be stationary + double init_max_disparity = 1.0; + + /** + * @brief This function will load print out all initializer settings loaded. + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load_initializer(const std::shared_ptr &parser = nullptr) { + PRINT_DEBUG("INITIALIZATION SETTINGS:\n"); + if (parser != nullptr) { + parser->parse_config("init_only_use_dynamic", init_only_use_dynamic); + parser->parse_config("init_only_use_static", init_only_use_static); + parser->parse_config("init_camera_pose", init_calib_camera_pose); + parser->parse_config("init_camera_timeoffset", init_calib_camera_timeoffset); + parser->parse_config("init_state_bias_accel", init_state_bias_accel); + parser->parse_config("init_enforce_gravity_magnitude", init_enforce_gravity_magnitude); + parser->parse_config("init_camera_rate", init_camera_rate); + parser->parse_config("init_window_time", init_window_time); + parser->parse_config("init_imu_thresh", init_imu_thresh); + parser->parse_config("init_max_disparity", init_max_disparity); + } + PRINT_DEBUG(" - init_only_use_dynamic: %d\n", init_only_use_dynamic); + PRINT_DEBUG(" - init_only_use_static: %d\n", init_only_use_static); + if (init_only_use_dynamic && init_only_use_static) { + PRINT_ERROR(RED "cannot only use static and dynamic initialization!\n" RESET); + PRINT_ERROR(RED " init_only_use_dynamic = %d\n" RESET, init_only_use_dynamic); + PRINT_ERROR(RED " init_only_use_dynamic = %d\n" RESET, init_only_use_static); + std::exit(EXIT_FAILURE); + } + PRINT_DEBUG(" - init_calib_camera_pose: %d\n", init_calib_camera_pose); + PRINT_DEBUG(" - init_calib_camera_timeoffset: %d\n", init_calib_camera_timeoffset); + PRINT_DEBUG(" - init_state_ba: %d\n", init_state_bias_accel); + PRINT_DEBUG(" - init_enforce_gravity_magnitude: %d\n", init_enforce_gravity_magnitude); + PRINT_DEBUG(" - init_camera_rate: %.2f\n", init_camera_rate); + PRINT_DEBUG(" - init_window_time: %.2f\n", init_window_time); + PRINT_DEBUG(" - init_imu_thresh: %.2f\n", init_imu_thresh); + PRINT_DEBUG(" - init_max_disparity: %.2f\n", init_max_disparity); + // Ensure we have enough frames to work with + double dt = 1.0 / init_camera_rate; + int num_frames = std::floor(init_window_time / dt); + if (num_frames < 4) { + PRINT_ERROR(RED "number of requested frames to init not enough!!\n" RESET); + PRINT_ERROR(RED " init_camera_rate = %.2f (%.2f seconds)\n" RESET, init_camera_rate, dt); + PRINT_ERROR(RED " init_window_time = %.2f seconds\n" RESET, init_window_time); + PRINT_ERROR(RED " num init frames = %d\n" RESET, num_frames); + std::exit(EXIT_FAILURE); + } + } + + // NOISE / CHI2 ============================ + + /// Gyroscope white noise (rad/s/sqrt(hz)) + double sigma_w = 1.6968e-04; + double sigma_w_2 = pow(1.6968e-04, 2); + + /// Gyroscope random walk (rad/s^2/sqrt(hz)) + double sigma_wb = 1.9393e-05; + double sigma_wb_2 = pow(1.9393e-05, 2); + + /// Accelerometer white noise (m/s^2/sqrt(hz)) + double sigma_a = 2.0000e-3; + double sigma_a_2 = pow(2.0000e-3, 2); + + /// Accelerometer random walk (m/s^3/sqrt(hz)) + double sigma_ab = 3.0000e-03; + double sigma_ab_2 = pow(3.0000e-03, 2); + + /// Noise sigma for our raw pixel measurements + double sigma_pix = 1; + double sigma_pix_sq = 1; + + /** + * @brief This function will load print out all noise parameters loaded. + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load_noise(const std::shared_ptr &parser = nullptr) { + PRINT_DEBUG("NOISE PARAMETERS:\n"); + if (parser != nullptr) { + parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", sigma_w); + parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", sigma_wb); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", sigma_a); + parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", sigma_ab); + sigma_w_2 = std::pow(sigma_w, 2); + sigma_wb_2 = std::pow(sigma_wb, 2); + sigma_a_2 = std::pow(sigma_a, 2); + sigma_ab_2 = std::pow(sigma_ab, 2); + parser->parse_config("up_slam_sigma_px", sigma_pix); + sigma_pix_sq = std::pow(sigma_pix, 2); + } + PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); + PRINT_DEBUG(" - sigma_pix: %.2f\n", sigma_pix); + } + + // STATE DEFAULTS ========================== + + /// Gravity magnitude in the global frame (i.e. should be 9.81 typically) + double gravity_mag = 9.81; + + /// Number of distinct cameras that we will observe features in + int num_cameras = 1; + +// /// Time offset between camera and IMU. +// double calib_camimu_dt = 0.0; +// +// /// Map between camid and camera model (true=fisheye, false=radtan) +// std::map camera_fisheye; +// +// /// Map between camid and intrinsics. Values depends on the model but each should be a 4x1 vector normally. +// std::map camera_intrinsics; +// +// /// Map between camid and camera extrinsics (q_ItoC, p_IinC). +// std::map camera_extrinsics; +// +// /// Map between camid and the dimensions of incoming images (width/cols, height/rows). This is normally only used during simulation. +// std::map> camera_wh; +// +// /// If we should try to load a mask and use it to reject invalid features +// bool use_mask = false; +// +// /// Mask images for each camera +// std::map masks; + + /** + * @brief This function will load and print all state parameters (e.g. sensor extrinsics) + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load_state(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("gravity_mag", gravity_mag); + parser->parse_config("max_cameras", num_cameras); // might be redundant +// for (int i = 0; i < num_cameras; i++) { +// +// // Time offset (use the first one) +// // TODO: support multiple time offsets between cameras +// if (i == 0) { +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "timeshift_cam_imu", calib_camimu_dt, false); +// } +// +// // Distortion model +// std::string dist_model = "radtan"; +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_model", dist_model); +// +// // Distortion parameters +// std::vector cam_calib1 = {1, 1, 0, 0}; +// std::vector cam_calib2 = {0, 0, 0, 0}; +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "intrinsics", cam_calib1); +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_coeffs", cam_calib2); +// Eigen::VectorXd cam_calib = Eigen::VectorXd::Zero(8); +// cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1), +// cam_calib2.at(2), cam_calib2.at(3); +// cam_calib(0) /= (downsample_cameras) ? 2.0 : 1.0; +// cam_calib(1) /= (downsample_cameras) ? 2.0 : 1.0; +// cam_calib(2) /= (downsample_cameras) ? 2.0 : 1.0; +// cam_calib(3) /= (downsample_cameras) ? 2.0 : 1.0; +// +// // FOV / resolution +// std::vector matrix_wh = {1, 1}; +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "resolution", matrix_wh); +// matrix_wh.at(0) /= (downsample_cameras) ? 2.0 : 1.0; +// matrix_wh.at(1) /= (downsample_cameras) ? 2.0 : 1.0; +// std::pair wh(matrix_wh.at(0), matrix_wh.at(1)); +// +// // Extrinsics +// Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "T_imu_cam", T_CtoI); +// +// // Load these into our state +// Eigen::Matrix cam_eigen; +// cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); +// cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); +// +// // Insert +// camera_fisheye.insert({i, dist_model == "equidistant"}); +// camera_intrinsics.insert({i, cam_calib}); +// camera_extrinsics.insert({i, cam_eigen}); +// camera_wh.insert({i, wh}); +// } +// parser->parse_config("use_mask", use_mask); +// if (use_mask) { +// for (int i = 0; i < num_cameras; i++) { +// std::string mask_path; +// std::string mask_node = "mask" + std::to_string(i); +// parser->parse_config(mask_node, mask_path); +// std::string total_mask_path = parser->get_config_folder() + mask_path; +// if (!boost::filesystem::exists(total_mask_path)) { +// PRINT_ERROR(RED "Initializer(): invalid mask path:\n" RESET); +// PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); +// std::exit(EXIT_FAILURE); +// } +// masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); +// } +// } + } + PRINT_DEBUG("STATE PARAMETERS:\n"); + PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); + PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); + PRINT_DEBUG(" - num_cameras: %d\n", num_cameras); +// PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); +// PRINT_DEBUG(" - camera masks?: %d\n", use_mask); +// assert(num_cameras == (int)camera_fisheye.size()); +// for (int n = 0; n < num_cameras; n++) { +// std::stringstream ss; +// ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; +// ss << "cam_" << n << "_wh:" << std::endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; +// ss << "cam_" << n << "_intrinsic(0:3):" << std::endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; +// ss << "cam_" << n << "_intrinsic(4:7):" << std::endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; +// ss << "cam_" << n << "_extrinsic(0:3):" << std::endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; +// ss << "cam_" << n << "_extrinsic(4:6):" << std::endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; +// Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); +// T_CtoI.block(0, 0, 3, 3) = ov_core::quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose(); +// T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1); +// ss << "T_C" << n << "toI:" << std::endl << T_CtoI << std::endl << std::endl; +// PRINT_DEBUG(ss.str().c_str()); +// } + } + + // TRACKERS =============================== + + /// If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. + bool use_stereo = true; + + /// If we should use KLT tracking, or descriptor matcher + bool use_klt = true; + + /// If should extract aruco tags and estimate them + bool use_aruco = true; + + /// Will half the resolution of the aruco tag image (will be faster) + bool downsize_aruco = true; + + /// Will half the resolution all tracking image (aruco will be 1/4 instead of halved if dowsize_aruoc also enabled) + bool downsample_cameras = false; + + /// If our front-end should try to use some multi-threading for stereo matching + bool use_multi_threading = true; + + /// The number of points we should extract and track in *each* image frame. This highly effects the computation required for tracking. + int num_pts = 150; + + /// Fast extraction threshold + int fast_threshold = 20; + + /// Number of grids we should split column-wise to do feature extraction in + int grid_x = 5; + + /// Number of grids we should split row-wise to do feature extraction in + int grid_y = 5; + + /// Will check after doing KLT track and remove any features closer than this + int min_px_dist = 10; + + /// What type of pre-processing histogram method should be applied to images + ov_core::TrackBase::HistogramMethod histogram_method = ov_core::TrackBase::HistogramMethod::HISTOGRAM; + + /// KNN ration between top two descriptor matcher which is required to be a good match + double knn_ratio = 0.85; + + /// Parameters used by our feature initialize / triangulator + ov_core::FeatureInitializerOptions featinit_options; + + /** + * @brief This function will load print out all parameters related to visual tracking + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load_trackers(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("use_stereo", use_stereo); + parser->parse_config("use_klt", use_klt); + parser->parse_config("use_aruco", use_aruco); + parser->parse_config("downsize_aruco", downsize_aruco); + parser->parse_config("multi_threading", use_multi_threading); + parser->parse_config("num_pts", num_pts); + parser->parse_config("fast_threshold", fast_threshold); + parser->parse_config("grid_x", grid_x); + parser->parse_config("grid_y", grid_y); + parser->parse_config("min_px_dist", min_px_dist); + std::string histogram_method_str = "HISTOGRAM"; + parser->parse_config("histogram_method", histogram_method_str); + if (histogram_method_str == "NONE") { + histogram_method = ov_core::TrackBase::NONE; + } else if (histogram_method_str == "HISTOGRAM") { + histogram_method = ov_core::TrackBase::HISTOGRAM; + } else if (histogram_method_str == "CLAHE") { + histogram_method = ov_core::TrackBase::CLAHE; + } else { + printf(RED "Initializer(): invalid feature histogram specified:\n" RESET); + printf(RED "\t- NONE\n" RESET); + printf(RED "\t- HISTOGRAM\n" RESET); + printf(RED "\t- CLAHE\n" RESET); + std::exit(EXIT_FAILURE); + } + parser->parse_config("knn_ratio", knn_ratio); + } + PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n"); + PRINT_DEBUG(" - use_stereo: %d\n", use_stereo); + PRINT_DEBUG(" - use_klt: %d\n", use_klt); + PRINT_DEBUG(" - use_aruco: %d\n", use_aruco); + PRINT_DEBUG(" - downsize aruco: %d\n", downsize_aruco); + PRINT_DEBUG(" - downsize cameras: %d\n", downsample_cameras); + PRINT_DEBUG(" - use multi-threading: %d\n", use_multi_threading); + PRINT_DEBUG(" - num_pts: %d\n", num_pts); + PRINT_DEBUG(" - fast threshold: %d\n", fast_threshold); + PRINT_DEBUG(" - grid X by Y: %d by %d\n", grid_x, grid_y); + PRINT_DEBUG(" - min px dist: %d\n", min_px_dist); + PRINT_DEBUG(" - hist method: %d\n", (int)histogram_method); + PRINT_DEBUG(" - knn ratio: %.3f\n", knn_ratio); + featinit_options.print(parser); + if (num_pts < 15) { + PRINT_ERROR(RED "number of requested features tracked not enough!!\n" RESET); + PRINT_ERROR(RED " num_pts = %d\n" RESET, num_pts); + std::exit(EXIT_FAILURE); + } + } + + // SIMULATOR =============================== + + /// Seed for initial states (i.e. random feature 3d positions in the generated map) + int sim_seed_state_init = 0; + + /// Seed for calibration perturbations. Change this to perturb by different random values if perturbations are enabled. + int sim_seed_preturb = 0; + + /// Measurement noise seed. This should be incremented for each run in the Monte-Carlo simulation to generate the same true measurements, + /// but diffferent noise values. + int sim_seed_measurements = 0; + + /// If we should perturb the calibration that the estimator starts with + bool sim_do_perturbation = false; + + /// Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),ori(xyzw) format. + std::string sim_traj_path = "../ov_data/sim/udel_gore.txt"; + + /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in + /// simulation. + double sim_distance_threshold = 1.2; + + /// Frequency (Hz) that we will simulate our cameras + double sim_freq_cam = 10.0; + + /// Frequency (Hz) that we will simulate our inertial measurement unit + double sim_freq_imu = 400.0; + + /// Feature distance we generate features from (minimum) + double sim_min_feature_gen_distance = 5; + + /// Feature distance we generate features from (maximum) + double sim_max_feature_gen_distance = 10; + + /** + * @brief This function will load print out all simulated parameters. + * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. + * + * @param parser If not null, this parser will be used to load our parameters + */ + void print_and_load_simulation(const std::shared_ptr &parser = nullptr) { + if (parser != nullptr) { + parser->parse_config("sim_seed_state_init", sim_seed_state_init); + parser->parse_config("sim_seed_preturb", sim_seed_preturb); + parser->parse_config("sim_seed_measurements", sim_seed_measurements); + parser->parse_config("sim_do_perturbation", sim_do_perturbation); + parser->parse_config("sim_traj_path", sim_traj_path); + parser->parse_config("sim_distance_threshold", sim_distance_threshold); + parser->parse_config("sim_freq_cam", sim_freq_cam); + parser->parse_config("sim_freq_imu", sim_freq_imu); + parser->parse_config("sim_min_feature_gen_dist", sim_min_feature_gen_distance); + parser->parse_config("sim_max_feature_gen_dist", sim_max_feature_gen_distance); + } + PRINT_DEBUG("SIMULATION PARAMETERS:\n"); + PRINT_WARNING(BOLDRED " - state init seed: %d \n" RESET, sim_seed_state_init); + PRINT_WARNING(BOLDRED " - perturb seed: %d \n" RESET, sim_seed_preturb); + PRINT_WARNING(BOLDRED " - measurement seed: %d \n" RESET, sim_seed_measurements); + PRINT_WARNING(BOLDRED " - do perturb?: %d\n" RESET, sim_do_perturbation); + PRINT_DEBUG(" - traj path: %s\n", sim_traj_path.c_str()); + PRINT_DEBUG(" - dist thresh: %.2f\n", sim_distance_threshold); + PRINT_DEBUG(" - cam feq: %.2f\n", sim_freq_cam); + PRINT_DEBUG(" - imu feq: %.2f\n", sim_freq_imu); + PRINT_DEBUG(" - min feat dist: %.2f\n", sim_min_feature_gen_distance); + PRINT_DEBUG(" - max feat dist: %.2f\n", sim_max_feature_gen_distance); + } +}; + +} // namespace ov_init + +#endif // OV_INIT_INERTIALINITIALIZEROPTIONS_H \ No newline at end of file diff --git a/ov_core/src/init/InertialInitializer.cpp b/ov_init/src/static/StaticInitializer.cpp similarity index 57% rename from ov_core/src/init/InertialInitializer.cpp rename to ov_init/src/static/StaticInitializer.cpp index 2873194f5..f78725fee 100644 --- a/ov_core/src/init/InertialInitializer.cpp +++ b/ov_init/src/static/StaticInitializer.cpp @@ -19,48 +19,37 @@ * along with this program. If not, see . */ -#include "InertialInitializer.h" +#include "StaticInitializer.h" using namespace ov_core; +using namespace ov_type; +using namespace ov_init; -void InertialInitializer::feed_imu(const ImuData &message) { - - // Append it to our vector - imu_data.push_back(message); - - // Delete all measurements older than three of our initialization windows - auto it0 = imu_data.begin(); - while (it0 != imu_data.end() && it0->timestamp < message.timestamp - 3 * _window_length) { - it0 = imu_data.erase(it0); - } -} - -bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix &q_GtoI0, Eigen::Matrix &b_w0, - Eigen::Matrix &v_I0inG, Eigen::Matrix &b_a0, - Eigen::Matrix &p_I0inG, bool wait_for_jerk) { +bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, + std::shared_ptr t_imu, bool wait_for_jerk) { // Return if we don't have any measurements - if (imu_data.size() < 2) { + if (imu_data->size() < 2) { return false; } // Newest and oldest imu timestamp - double newesttime = imu_data.at(imu_data.size() - 1).timestamp; - double oldesttime = imu_data.at(0).timestamp; + double newesttime = imu_data->at(imu_data->size() - 1).timestamp; + double oldesttime = imu_data->at(0).timestamp; // Return if we don't have enough for two windows - if (newesttime - oldesttime < 2 * _window_length) { + if (newesttime - oldesttime < 2 * params.init_window_time) { // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); return false; } // First lets collect a window of IMU readings from the newest measurement to the oldest std::vector window_1to0, window_2to1; - for (const ImuData &data : imu_data) { - if (data.timestamp > newesttime - 1 * _window_length && data.timestamp <= newesttime - 0 * _window_length) { + for (const ImuData &data : *imu_data) { + if (data.timestamp > newesttime - 1 * params.init_window_time && data.timestamp <= newesttime - 0 * params.init_window_time) { window_1to0.push_back(data); } - if (data.timestamp > newesttime - 2 * _window_length && data.timestamp <= newesttime - 1 * _window_length) { + if (data.timestamp > newesttime - 2 * params.init_window_time && data.timestamp <= newesttime - 1 * params.init_window_time) { window_2to1.push_back(data); } } @@ -86,9 +75,9 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix _imu_excite_threshold && wait_for_jerk) { - PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, _imu_excite_threshold); + if (a_var_2to1 > params.init_imu_thresh && wait_for_jerk) { + PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, params.init_imu_thresh); return false; } // If it is above the threshold and we are not waiting for a jerk // Then we are not stationary (i.e. moving) so we should wait till we are - if ((a_var_1to0 > _imu_excite_threshold || a_var_2to1 > _imu_excite_threshold) && !wait_for_jerk) { + if ((a_var_1to0 > params.init_imu_thresh || a_var_2to1 > params.init_imu_thresh) && !wait_for_jerk) { PRINT_DEBUG(YELLOW "[INIT-IMU]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, - _imu_excite_threshold); + params.init_imu_thresh); return false; } - // Get z axis, which alines with -g (z_in_G=0,0,1) + // Get z axis, which aligns with -g (z_in_G=0,0,1) Eigen::Vector3d z_axis = a_avg_2to1 / a_avg_2to1.norm(); // Create an x_axis @@ -131,10 +120,10 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix y_axis = skew_x(z_axis) * x_axis; + Eigen::Vector3d y_axis = skew_x(z_axis) * x_axis; // From these axes get rotation - Eigen::Matrix Ro; + Eigen::Matrix3d Ro; Ro.block(0, 0, 3, 1) = x_axis; Ro.block(0, 1, 3, 1) = y_axis; Ro.block(0, 2, 3, 1) = z_axis; @@ -143,17 +132,42 @@ bool InertialInitializer::initialize_with_imu(double &time0, Eigen::Matrix q_GtoI = rot_2_quat(Ro); // Set our biases equal to our noise (subtract our gravity from accelerometer bias) - Eigen::Matrix bg = w_avg_2to1; - Eigen::Matrix ba = a_avg_2to1 - quat_2_Rot(q_GtoI) * _gravity; + Eigen::Vector3d gravity_inG; + gravity_inG << 0.0, 0.0, params.gravity_mag; + Eigen::Vector3d bg = w_avg_2to1; + Eigen::Vector3d ba = a_avg_2to1 - quat_2_Rot(q_GtoI) * gravity_inG; // Set our state variables - time0 = window_2to1.at(window_2to1.size() - 1).timestamp; - q_GtoI0 = q_GtoI; - b_w0 = bg; - v_I0inG = Eigen::Matrix::Zero(); - b_a0 = ba; - p_I0inG = Eigen::Matrix::Zero(); - - // Done!!! + timestamp = window_2to1.at(window_2to1.size() - 1).timestamp; + Eigen::VectorXd imu_state = Eigen::VectorXd::Zero(16); + imu_state.block(0, 0, 4, 1) = q_GtoI; + imu_state.block(10, 0, 3, 1) = bg; + imu_state.block(13, 0, 3, 1) = ba; + assert(t_imu != nullptr); + t_imu->set_value(imu_state); + t_imu->set_fej(imu_state); + + // Create base covariance and its covariance ordering + order.clear(); + order.push_back(t_imu); + covariance = 1e-3 * Eigen::MatrixXd::Identity(t_imu->size(), t_imu->size()); + + // Make velocity uncertainty a bit bigger + covariance.block(t_imu->v()->id(), t_imu->v()->id(), 3, 3) *= 2; + + // A VIO system has 4dof unobservabile directions which can be arbitarily picked. + // This means that on startup, we can fix the yaw and position to be 100 percent known. + // Thus, after determining the global to current IMU orientation after initialization, we can propagate the global error + // into the new IMU pose. In this case the position is directly equivalent, but the orientation needs to be propagated. + covariance(t_imu->q()->id() + 2, t_imu->q()->id() + 2) = 0.0; + covariance.block(t_imu->p()->id(), t_imu->p()->id(), 3, 3).setZero(); + + // Propagate into the current local IMU frame + // R_GtoI = R_GtoI*R_GtoG -> H = R_GtoI + Eigen::Matrix3d R_GtoI = quat_2_Rot(q_GtoI); + covariance.block(t_imu->q()->id(), t_imu->q()->id(), 3, 3) = + R_GtoI * covariance.block(t_imu->q()->id(), t_imu->q()->id(), 3, 3) * R_GtoI.transpose(); + + // Return :D return true; } diff --git a/ov_core/src/init/InertialInitializer.h b/ov_init/src/static/StaticInitializer.h similarity index 52% rename from ov_core/src/init/InertialInitializer.h rename to ov_init/src/static/StaticInitializer.h index 1fb0d8a21..0313a8d3d 100644 --- a/ov_core/src/init/InertialInitializer.h +++ b/ov_init/src/static/StaticInitializer.h @@ -19,22 +19,23 @@ * along with this program. If not, see . */ -#ifndef OV_CORE_INERTIALINITIALIZER_H -#define OV_CORE_INERTIALINITIALIZER_H +#ifndef OV_INIT_STATICINITIALIZER_H +#define OV_INIT_STATICINITIALIZER_H +#include "init/InertialInitializerOptions.h" + +#include "types/IMU.h" #include "utils/colors.h" #include "utils/print.h" #include "utils/quat_ops.h" #include "utils/sensor_data.h" -namespace ov_core { +namespace ov_init { /** - * @brief Initializer for visual-inertial system. + * @brief Initializer for a static visual-inertial system. * - * This class has a series of functions that can be used to initialize your system. - * Right now we have our implementation that assumes that the imu starts from standing still. - * In the future we plan to add support for structure-from-motion dynamic initialization. + * This implementation that assumes that the imu starts from standing still. * * To initialize from standstill: * 1. Collect all inertial measurements @@ -44,28 +45,19 @@ namespace ov_core { * 5. Return a roll and pitch aligned with gravity and biases. * */ -class InertialInitializer { +class StaticInitializer { public: /** * @brief Default constructor - * @param gravity_mag Global gravity magnitude of the system (normally 9.81) - * @param window_length Amount of time we will initialize over (seconds) - * @param imu_excite_threshold Variance threshold on our acceleration to be classified as moving - */ - InertialInitializer(double gravity_mag, double window_length, double imu_excite_threshold) - : _window_length(window_length), _imu_excite_threshold(imu_excite_threshold) { - _gravity << 0.0, 0.0, gravity_mag; - } - - /** - * @brief Feed function for inertial data - * @param message Contains our timestamp and inertial information + * @param params_ Parameters loaded from either ROS or CMDLINE + * @param imu_data_ Shared pointer to our IMU vector of historical information */ - void feed_imu(const ImuData &message); + explicit StaticInitializer(InertialInitializerOptions ¶ms_, std::shared_ptr> imu_data_) + : params(params_), imu_data(imu_data_) {} /** - * @brief Try to initialize the system using just the imu + * @brief Try to get the initialized system using just the imu * * This will check if we have had a large enough jump in our acceleration. * If we have then we will use the period of time before this jump to initialize the state. @@ -75,33 +67,24 @@ class InertialInitializer { * This is only recommended if you have zero velocity update enabled to handle the stationary cases. * To initialize in this case, we need to have the average angular variance be below the set threshold (i.e. we need to be stationary). * - * @param[out] time0 Timestamp that the returned state is at - * @param[out] q_GtoI0 Orientation at initialization - * @param[out] b_w0 Gyro bias at initialization - * @param[out] v_I0inG Velocity at initialization - * @param[out] b_a0 Acceleration bias at initialization - * @param[out] p_I0inG Position at initialization + * @param[out] timestamp Timestamp we have initialized the state at + * @param[out] covariance Calculated covariance of the returned state + * @param[out] order Order of the covariance matrix + * @param[out] t_imu Our imu type element * @param wait_for_jerk If true we will wait for a "jerk" * @return True if we have successfully initialized our system */ - bool initialize_with_imu(double &time0, Eigen::Matrix &q_GtoI0, Eigen::Matrix &b_w0, - Eigen::Matrix &v_I0inG, Eigen::Matrix &b_a0, Eigen::Matrix &p_I0inG, - bool wait_for_jerk = true); - -protected: - /// Gravity vector - Eigen::Matrix _gravity; - - /// Amount of time we will initialize over (seconds) - double _window_length; + bool initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, + std::shared_ptr t_imu, bool wait_for_jerk = true); - /// Variance threshold on our acceleration to be classified as moving - double _imu_excite_threshold; +private: + /// Initialization parameters + InertialInitializerOptions params; /// Our history of IMU messages (time, angular, linear) - std::vector imu_data; + std::shared_ptr> imu_data; }; -} // namespace ov_core +} // namespace ov_init -#endif // OV_CORE_INERTIALINITIALIZER_H +#endif // OV_INIT_STATICINITIALIZER_H diff --git a/ov_init/src/test_orientation.cpp b/ov_init/src/test_orientation.cpp new file mode 100644 index 000000000..17232e2f4 --- /dev/null +++ b/ov_init/src/test_orientation.cpp @@ -0,0 +1,36 @@ +/* +* OpenVINS: An Open Platform for Visual-Inertial Research +* Copyright (C) 2021 Patrick Geneva +* Copyright (C) 2021 Guoquan Huang +* Copyright (C) 2021 OpenVINS Contributors +* Copyright (C) 2019 Kevin Eckenhoff +* +* This program 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. +* +* This program 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 this program. If not, see . +*/ + + + +#include + + + +int main(int argc, char **argv) { + + // Launch our ros node + ros::init(argc, argv, "test_orientation"); + ros::NodeHandle nh("~"); + + + +} \ No newline at end of file diff --git a/ov_init/src/test_static.cpp b/ov_init/src/test_static.cpp new file mode 100644 index 000000000..255760596 --- /dev/null +++ b/ov_init/src/test_static.cpp @@ -0,0 +1,36 @@ +/* +* OpenVINS: An Open Platform for Visual-Inertial Research +* Copyright (C) 2021 Patrick Geneva +* Copyright (C) 2021 Guoquan Huang +* Copyright (C) 2021 OpenVINS Contributors +* Copyright (C) 2019 Kevin Eckenhoff +* +* This program 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. +* +* This program 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 this program. If not, see . +*/ + + + +#include + + + +int main(int argc, char **argv) { + + // Launch our ros node + ros::init(argc, argv, "test_static"); + ros::NodeHandle nh("~"); + + + +} \ No newline at end of file diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 89158018e..1c1147876 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -11,7 +11,7 @@ project(ov_msckf) #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/) # Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core) +find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core ov_init) # Include libraries (if we don't have opencv 4, then fallback to opencv 3) # The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! @@ -37,7 +37,7 @@ option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_definitions(-DROS_AVAILABLE=1) catkin_package( - CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core + CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core ov_init INCLUDE_DIRS src LIBRARIES ov_msckf_lib ) @@ -87,6 +87,9 @@ if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") include_directories(${ov_core_SOURCE_DIR}/src/) list(APPEND thirdparty_libraries ov_core_lib) + message(WARNING "MANUALLY LINKING TO OV_INIT LIBRARY....") + include_directories(${ov_init_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_init_lib) endif () ################################################## diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index d463f557b..95875d24e 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -19,8 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 40 dt_slam_delay: 2 -init_window_time: 0.75 -init_imu_thresh: 1.5 gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +37,22 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 0.75 +init_imu_thresh: 1.5 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml index 7398d2b9b..d4a1149f0 100644 --- a/ov_msckf/config/rpng_aruco/estimator_config.yaml +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -19,8 +19,7 @@ max_slam_in_update: 25 max_msckf_in_update: 40 dt_slam_delay: 2 -init_window_time: 0.5 -init_imu_thresh: 1.5 + gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +38,22 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 0.5 +init_imu_thresh: 1.5 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml index 3fb1fe42d..ae2900320 100644 --- a/ov_msckf/config/rpng_ironsides/estimator_config.yaml +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -19,11 +19,9 @@ max_slam_in_update: 25 max_msckf_in_update: 40 dt_slam_delay: 1 -init_window_time: 1.0 -init_imu_thresh: 0.5 gravity_mag: 9.80114 -feat_rep_msckf: "GLOBAL_3D" +feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -39,6 +37,22 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 1.0 +init_imu_thresh: 0.5 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" @@ -61,11 +75,11 @@ min_px_dist: 20 knn_ratio: 0.65 downsample_cameras: false multi_threading: true -histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE +histogram_method: "CLAHE" # NONE, HISTOGRAM, CLAHE -fi_max_dist: 100 -fi_max_baseline: 100 -fi_max_cond_number: 10000 +fi_max_dist: 150 +fi_max_baseline: 200 +fi_max_cond_number: 20000 fi_triangulate_1d: false # aruco tag tracker for the system diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index be1e73e9e..b308b6159 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -19,8 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 10 dt_slam_delay: 2 -init_window_time: 0.75 -init_imu_thresh: 1.0 gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +37,22 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 0.75 +init_imu_thresh: 1.0 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index 0fe205ff7..95b447ab9 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -19,8 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 40 dt_slam_delay: 5 -init_window_time: 1.0 -init_imu_thresh: 0.60 # room1-5:0.60, room6:0.45 gravity_mag: 9.80766 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +37,22 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 1.0 +init_imu_thresh: 0.60 # room1-5:0.60, room6:0.45 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml index 4053f46ec..b9fd5b83c 100644 --- a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml @@ -19,8 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 30 dt_slam_delay: 2 -init_window_time: 1.0 -init_imu_thresh: 0.5 gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +37,22 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 1.0 +init_imu_thresh: 0.5 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml index 4053f46ec..b9fd5b83c 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml @@ -19,8 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 30 dt_slam_delay: 2 -init_window_time: 1.0 -init_imu_thresh: 0.5 gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -39,6 +37,22 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 1.0 +init_imu_thresh: 0.5 +init_max_disparity: 0.5 + +# ================================================================== +# ================================================================== + record_timing_information: false record_timing_filepath: "/tmp/traj_timing.txt" diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 4268d4441..78d0017cb 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -12,10 +12,6 @@ - - - - @@ -43,8 +39,6 @@ - - diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 89904b861..f7add9630 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -12,10 +12,6 @@ - - - - @@ -38,8 +34,6 @@ - - diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 3cc2b2639..371dbe6de 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -30,6 +30,7 @@ visualization_msgs cv_bridge ov_core + ov_init roscpp @@ -42,6 +43,7 @@ visualization_msgs cv_bridge ov_core + ov_init ov_eval ov_data diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 4614a5427..25e58a97a 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -125,7 +125,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { propagator = std::make_shared(params.imu_noises, params.gravity_mag); // Our state initialize - initializer = std::make_shared(params.gravity_mag, params.init_window_time, params.init_imu_thresh); + initializer = std::make_shared(params.init_options); // Make the updater! updaterMSCKF = std::make_shared(params.msckf_options, params.featinit_options); @@ -706,40 +706,30 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) bool VioManager::try_to_initialize() { // Returns from our initializer - double time0; - Eigen::Matrix q_GtoI0; - Eigen::Matrix b_w0, v_I0inG, b_a0, p_I0inG; + double timestamp; + Eigen::MatrixXd covariance; + std::vector> order; // Try to initialize the system // We will wait for a jerk if we do not have the zero velocity update enabled // Otherwise we can initialize right away as the zero velocity will handle the stationary case bool wait_for_jerk = (updaterZUPT == nullptr); - bool success = initializer->initialize_with_imu(time0, q_GtoI0, b_w0, v_I0inG, b_a0, p_I0inG, wait_for_jerk); + bool success = initializer->initialize(timestamp, covariance, order, state->_imu, wait_for_jerk); // Return if it failed if (!success) { return false; } - // Make big vector (q,p,v,bg,ba), and update our state - // Note: start from zero position, as this is what our covariance is based off of - Eigen::Matrix imu_val; - imu_val.block(0, 0, 4, 1) = q_GtoI0; - imu_val.block(4, 0, 3, 1) << 0, 0, 0; - imu_val.block(7, 0, 3, 1) = v_I0inG; - imu_val.block(10, 0, 3, 1) = b_w0; - imu_val.block(13, 0, 3, 1) = b_a0; - // imu_val.block(10,0,3,1) << 0,0,0; - // imu_val.block(13,0,3,1) << 0,0,0; - state->_imu->set_value(imu_val); - state->_imu->set_fej(imu_val); - state->_timestamp = time0; - startup_time = time0; - - // Fix the global yaw and position gauge freedoms - StateHelper::fix_4dof_gauge_freedoms(state, q_GtoI0); + // Set our covariance (state should already be set in the initializer) + StateHelper::set_initial_covariance(state, covariance, order); + + // Set the state time + state->_timestamp = timestamp; + startup_time = timestamp; // Cleanup any features older then the initialization time + // TODO: ensure we keep the feature tracks from our dynamic init so we can match to the init'ed features! trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); if (trackARUCO != nullptr) { trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index ff1885e67..01e4fc161 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -32,7 +32,6 @@ #include "cam/CamBase.h" #include "cam/CamEqui.h" #include "cam/CamRadtan.h" -#include "init/InertialInitializer.h" #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" @@ -43,6 +42,8 @@ #include "utils/print.h" #include "utils/sensor_data.h" +#include "init/InertialInitializer.h" + #include "state/Propagator.h" #include "state/State.h" #include "state/StateHelper.h" @@ -103,13 +104,22 @@ class VioManager { // Initialize the system state->_imu->set_value(imustate.block(1, 0, 16, 1)); state->_imu->set_fej(imustate.block(1, 0, 16, 1)); + + // Fix the global yaw and position gauge freedoms + std::vector> order = {state->_imu}; + Eigen::MatrixXd Cov = 1e-2 * Eigen::MatrixXd::Identity(state->_imu->size(), state->_imu->size()); + Cov.block(state->_imu->v()->id(), state->_imu->v()->id(), 3, 3) *= 10; + Cov(state->_imu->q()->id() + 2, state->_imu->q()->id() + 2) = 0.0; + Cov.block(state->_imu->p()->id(), state->_imu->p()->id(), 3, 3).setZero(); + Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) = + state->_imu->Rot() * Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) * state->_imu->Rot().transpose(); + StateHelper::set_initial_covariance(state, Cov, order); + + // Set the state time state->_timestamp = imustate(0, 0); startup_time = imustate(0, 0); is_initialized_vio = true; - // Fix the global yaw and position gauge freedoms - StateHelper::fix_4dof_gauge_freedoms(state, imustate.block(1, 0, 4, 1)); - // Cleanup any features older then the initialization time trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); if (trackARUCO != nullptr) { @@ -293,7 +303,7 @@ class VioManager { std::shared_ptr trackARUCO; /// State initializer - std::shared_ptr initializer; + std::shared_ptr initializer; /// Boolean if we are initialized or not bool is_initialized_vio = false; diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 70ef176ab..36dcfbef0 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -28,11 +28,14 @@ #include #include -#include "feat/FeatureInitializerOptions.h" #include "state/Propagator.h" #include "state/StateOptions.h" -#include "track/TrackBase.h" #include "update/UpdaterOptions.h" + +#include "init/InertialInitializerOptions.h" + +#include "feat/FeatureInitializerOptions.h" +#include "track/TrackBase.h" #include "utils/colors.h" #include "utils/opencv_yaml_parse.h" #include "utils/print.h" @@ -65,15 +68,12 @@ struct VioManagerOptions { /// Core state options (e.g. number of cameras, use fej, stereo, what calibration to enable etc) StateOptions state_options; + /// Our state initialization options (e.g. window size, num features, if we should get the calibration) + ov_init::InertialInitializerOptions init_options; + /// Delay, in seconds, that we should wait from init before we start estimating SLAM features double dt_slam_delay = 2.0; - /// Amount of time we will initialize over (seconds) - double init_window_time = 1.0; - - /// Variance threshold on our acceleration to be classified as moving - double init_imu_thresh = 1.0; - /// If we should try to use zero velocity update bool try_zupt = false; @@ -104,10 +104,9 @@ struct VioManagerOptions { void print_and_load_estimator(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("ESTIMATOR PARAMETERS:\n"); state_options.print(parser); + init_options.print_and_load(parser); if (parser != nullptr) { parser->parse_config("dt_slam_delay", dt_slam_delay); - parser->parse_config("init_window_time", init_window_time); - parser->parse_config("init_imu_thresh", init_imu_thresh); parser->parse_config("try_zupt", try_zupt); parser->parse_config("zupt_max_velocity", zupt_max_velocity); parser->parse_config("zupt_noise_multiplier", zupt_noise_multiplier); @@ -117,8 +116,6 @@ struct VioManagerOptions { parser->parse_config("record_timing_filepath", record_timing_filepath); } PRINT_DEBUG(" - dt_slam_delay: %.1f\n", dt_slam_delay); - PRINT_DEBUG(" - init_window_time: %.2f\n", init_window_time); - PRINT_DEBUG(" - init_imu_thresh: %.2f\n", init_imu_thresh); PRINT_DEBUG(" - zero_velocity_update: %d\n", try_zupt); PRINT_DEBUG(" - zupt_max_velocity: %.2f\n", zupt_max_velocity); PRINT_DEBUG(" - zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier); @@ -224,7 +221,8 @@ struct VioManagerOptions { parser->parse_config("max_cameras", state_options.num_cameras); // might be redundant for (int i = 0; i < state_options.num_cameras; i++) { - // Time offset (use the first one, todo: support multiple!) + // Time offset (use the first one) + // TODO: support multiple time offsets between cameras if (i == 0) { parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "timeshift_cam_imu", calib_camimu_dt, false); } diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 1a2ffb737..99f824996 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -79,7 +79,7 @@ class State { int max_covariance_size() { return (int)_Cov.rows(); } /// Current timestamp (should be the last update time!) - double _timestamp; + double _timestamp = -1; /// Struct containing filter options StateOptions _options; diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp index 94556cf0b..259793ee4 100644 --- a/ov_msckf/src/state/StateHelper.cpp +++ b/ov_msckf/src/state/StateHelper.cpp @@ -184,17 +184,31 @@ void StateHelper::EKFUpdate(std::shared_ptr state, const std::vector state, const Eigen::Vector4d &q_GtoI) { +void StateHelper::set_initial_covariance(std::shared_ptr state, const Eigen::MatrixXd &covariance, + const std::vector> &order) { + + // We need to loop through each element and overwrite the current covariance values + // For example consider the following: + // x = [ ori pos ] -> insert into -> x = [ ori bias pos ] + // P = [ P_oo P_op ] -> P = [ P_oo 0 P_op ] + // [ P_po P_pp ] [ 0 P* 0 ] + // [ P_po 0 P_pp ] + // The key assumption here is that the covariance is block diagonal (cross-terms zero with P* can be dense) + // This is normally the care on startup (for example between calibration and the initial state - // Fix our global yaw and position - state->_Cov(state->_imu->q()->id() + 2, state->_imu->q()->id() + 2) = 0.0; - state->_Cov.block(state->_imu->p()->id(), state->_imu->p()->id(), 3, 3).setZero(); - - // Propagate into the current local IMU frame - // R_GtoI = R_GtoI*R_GtoG -> H = R_GtoI - Eigen::Matrix3d R_GtoI = quat_2_Rot(q_GtoI); - state->_Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) = - R_GtoI * state->_Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) * R_GtoI.transpose(); + // For each variable, lets copy over all other variable cross terms + // Note: this copies over itself to when i_index=k_index + int i_index = 0; + for (size_t i = 0; i < order.size(); i++) { + int k_index = 0; + for (size_t k = 0; k < order.size(); k++) { + state->_Cov.block(order[i]->id(), order[k]->id(), order[i]->size(), order[k]->size()) = + covariance.block(i_index, k_index, order[i]->size(), order[k]->size()); + k_index += order[k]->size(); + } + i_index += order[i]->size(); + } + state->_Cov = state->_Cov.selfadjointView(); } Eigen::MatrixXd StateHelper::get_marginal_covariance(std::shared_ptr state, diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index 7a693a5bb..ebebecd33 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -72,7 +72,8 @@ class StateHelper { * @param Q Additive state propagation noise matrix (size order_NEW by size order_NEW) */ static void EKFPropagation(std::shared_ptr state, const std::vector> &order_NEW, - const std::vector> &order_OLD, const Eigen::MatrixXd &Phi, const Eigen::MatrixXd &Q); + const std::vector> &order_OLD, const Eigen::MatrixXd &Phi, + const Eigen::MatrixXd &Q); /** * @brief Performs EKF update of the state (see @ref linear-meas page) @@ -86,17 +87,14 @@ class StateHelper { const Eigen::VectorXd &res, const Eigen::MatrixXd &R); /** - * @brief This will fix the initial covariance matrix gauge freedoms (4dof, yaw and position). - * - * A VIO system has 4dof unobservabile directions which can be arbitarily picked. - * This means that on startup, we can fix the yaw and position to be 100 percent known. - * Thus, after determining the global to current IMU orientation after initialization, we can propagate the global error - * into the new IMU pose. In this case the position is directly equivilent, but the orientation needs to be propagated. - * + * @brief This will set the initial covaraince of the specified state elements. + * Will also ensure that proper cross-covariances are inserted. * @param state Pointer to state - * @param q_GtoI Initial rotation from global frame to the first ever IMU orientation. + * @param covariance The covariance of the system state + * @param order Order of the covariance matrix */ - static void fix_4dof_gauge_freedoms(std::shared_ptr state, const Eigen::Vector4d &q_GtoI); + static void set_initial_covariance(std::shared_ptr state, const Eigen::MatrixXd &covariance, + const std::vector> &order); /** * @brief For a given set of variables, this will this will calculate a smaller covariance. @@ -109,7 +107,8 @@ class StateHelper { * @param small_variables Vector of variables whose marginal covariance is desired * @return marginal covariance of the passed variables */ - static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr state, const std::vector> &small_variables); + static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr state, + const std::vector> &small_variables); /** * @brief This gets the full covariance matrix. @@ -162,8 +161,8 @@ class StateHelper { * @param chi_2_mult Value we should multiply the chi2 threshold by (larger means it will be accepted more measurements) */ static bool initialize(std::shared_ptr state, std::shared_ptr new_variable, - const std::vector> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L, Eigen::MatrixXd &R, - Eigen::VectorXd &res, double chi_2_mult); + const std::vector> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L, + Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult); /** * @brief Initializes new variable into covariance (H_L must be invertible) From f4f7a102cafe7b4cc615e63e6a8a4383641def82 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 12 Nov 2021 03:16:52 -0500 Subject: [PATCH 15/55] add logic to also check disparity on static initialization --- ov_core/src/feat/FeatureHelper.h | 117 ++++++++++++++++++ ov_core/src/track/Grider_FAST.h | 19 ++- ov_core/src/track/TrackAruco.cpp | 4 + ov_core/src/track/TrackBase.h | 6 + ov_core/src/track/TrackKLT.cpp | 6 +- ov_init/src/init/InertialInitializer.cpp | 5 +- ov_init/src/init/InertialInitializer.h | 6 +- ov_init/src/init/InertialInitializerOptions.h | 111 ++--------------- ov_init/src/static/StaticInitializer.cpp | 50 +++++++- ov_init/src/static/StaticInitializer.h | 10 +- .../config/euroc_mav/estimator_config.yaml | 7 +- .../config/rpng_aruco/estimator_config.yaml | 4 +- .../rpng_ironsides/estimator_config.yaml | 3 +- .../config/rpng_sim/estimator_config.yaml | 3 +- ov_msckf/config/tum_vi/estimator_config.yaml | 9 +- .../uzhfpv_indoor/estimator_config.yaml | 3 +- .../uzhfpv_indoor_45/estimator_config.yaml | 3 +- ov_msckf/launch/serial.launch | 2 +- ov_msckf/src/core/RosVisualizer.cpp | 2 +- ov_msckf/src/core/VioManager.cpp | 45 +++++-- ov_msckf/src/update/UpdaterZeroVelocity.cpp | 43 ++----- ov_msckf/src/update/UpdaterZeroVelocity.h | 3 +- 22 files changed, 287 insertions(+), 174 deletions(-) create mode 100644 ov_core/src/feat/FeatureHelper.h diff --git a/ov_core/src/feat/FeatureHelper.h b/ov_core/src/feat/FeatureHelper.h new file mode 100644 index 000000000..593dc19b5 --- /dev/null +++ b/ov_core/src/feat/FeatureHelper.h @@ -0,0 +1,117 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OV_CORE_FEATURE_HELPER_H +#define OV_CORE_FEATURE_HELPER_H + +#include +#include +#include +#include + +#include "Feature.h" +#include "FeatureDatabase.h" +#include "utils/print.h" + +namespace ov_core { + +/** + * @brief Contains some nice helper functions for features. + * + * These functions should only depend on feature and the feature database. + */ +class FeatureHelper { + +public: + /** + * @brief This functions will compute the disparity between common features in the two frames. + * + * First we find all features in the first frame. + * Then we loop through each and find the uv of it in the next requested frame. + * Features are skipped if no tracked feature is found (it was lost). + * NOTE: this is on the RAW coordinates of the feature not the normalized ones. + * NOTE: This computes the disparity over all cameras! + * + * @param db Feature database pointer + * @param time0 First camera frame timestamp + * @param time1 Second camera frame timestamp + * @param disp_mean[out] Average raw disparity + * @param disp_var[out] Variance of the disparities + * @param total_feats[out] Total number of common features + */ + static void compute_disparity(std::shared_ptr db, double time0, double time1, double &disp_mean, + double &disp_var, int &total_feats) { + + // Get features seen from the first image + std::vector> feats0 = db->features_containing(time0, false, true); + + // Compute the disparity + std::vector disparities; + for (auto &feat : feats0) { + + // Get the two uvs for both times + for (auto &campairs : feat->timestamps) { + + // First find the two timestamps + size_t camid = campairs.first; + auto it0 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time0); + auto it1 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time1); + if (it0 == feat->timestamps.at(camid).end() || it1 == feat->timestamps.at(camid).end()) + continue; + auto idx0 = std::distance(feat->timestamps.at(camid).begin(), it0); + auto idx1 = std::distance(feat->timestamps.at(camid).begin(), it1); + + // Now lets calculate the disparity + Eigen::Vector2f uv0 = feat->uvs.at(camid).at(idx0).block(0, 0, 2, 1); + Eigen::Vector2f uv1 = feat->uvs.at(camid).at(idx1).block(0, 0, 2, 1); + disparities.push_back((uv1 - uv0).norm()); + } + } + + // If no disparities, just return + if (disparities.size() < 2) { + disp_mean = -1; + disp_var = -1; + total_feats = 0; + } + + // Compute mean and standard deviation in respect to it + disp_mean = 0; + for (double disp_i : disparities) { + disp_mean += disp_i; + } + disp_mean /= (double)disparities.size(); + disp_var = 0; + for (double &disp_i : disparities) { + disp_var += std::pow(disp_i - disp_mean, 2); + } + disp_var = std::sqrt(disp_var / (double)(disparities.size() - 1)); + total_feats = (int)disparities.size(); + } + +private: + // Cannot construct this class + FeatureHelper() {} +}; + +} // namespace ov_core + +#endif /* OV_CORE_FEATURE_HELPER_H */ \ No newline at end of file diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h index 7f585e76c..5e41e1fce 100644 --- a/ov_core/src/track/Grider_FAST.h +++ b/ov_core/src/track/Grider_FAST.h @@ -73,6 +73,22 @@ class Grider_FAST { static void perform_griding(const cv::Mat &img, const cv::Mat &mask, std::vector &pts, int num_features, int grid_x, int grid_y, int threshold, bool nonmaxSuppression) { + // We want to have equally distributed features + // NOTE: If we have more grids than number of total points, we calc the biggest grid we can do + // NOTE: Thus if we extract 1 point per grid we have + // NOTE: -> 1 = num_features / (grid_x * grid_y)) + // NOTE: -> grid_x = ratio * grid_y (keep the original grid ratio) + // NOTE: -> grid_y = sqrt(num_features / ratio) + if(num_features < grid_x * grid_y) { + double ratio = (double)grid_x / (double)grid_y; + grid_y = std::ceil(std::sqrt(num_features / ratio)); + grid_x = std::ceil(grid_y * ratio); + } + int num_features_grid = (int)((double)num_features / (double)(grid_x * grid_y)) + 1; + assert(grid_x > 0); + assert(grid_y > 0); + assert(num_features_grid > 0); + // Calculate the size our extraction boxes should be int size_x = img.cols / grid_x; int size_y = img.rows / grid_y; @@ -81,9 +97,6 @@ class Grider_FAST { assert(size_x > 0); assert(size_y > 0); - // We want to have equally distributed features - auto num_features_grid = (int)(num_features / (grid_x * grid_y)) + 1; - // Parallelize our 2d grid extraction!! int ct_cols = std::floor(img.cols / size_x); int ct_rows = std::floor(img.rows / size_y); diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp index 4ed35ce1d..567e1e971 100644 --- a/ov_core/src/track/TrackAruco.cpp +++ b/ov_core/src/track/TrackAruco.cpp @@ -174,6 +174,10 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2 max_height = pair.second.rows; } + // Return if we didn't have a last image + if (max_width == -1 || max_height == -1) + return; + // If the image is "small" thus we shoudl use smaller display codes bool is_small = (std::min(max_width, max_height) < 400); diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h index 5bcaa4d47..4a4affd25 100644 --- a/ov_core/src/track/TrackBase.h +++ b/ov_core/src/track/TrackBase.h @@ -157,6 +157,12 @@ class TrackBase { } } + /// Getter method for number of active features + int get_num_features() { return num_features; } + + /// Setter method for number of active features + void set_num_features(int _num_features) { num_features = _num_features; } + protected: /// Camera object which has all calibration in it std::unordered_map> camera_calib; diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp index 80c3d0b40..3d72f3417 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -410,7 +410,7 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, // Extract our features (use fast with griding) std::vector pts0_ext; - Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_featsneeded, grid_x, grid_y, threshold, true); + Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature std::vector kpts0_new; @@ -499,7 +499,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Extract our features (use fast with griding) std::vector pts0_ext; - Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_featsneeded_0, grid_x, grid_y, threshold, true); + Grider_FAST::perform_griding(img0pyr.at(0), mask0, pts0_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature std::vector kpts0_new; @@ -633,7 +633,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Extract our features (use fast with griding) std::vector pts1_ext; - Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_featsneeded_1, grid_x, grid_y, threshold, true); + Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature for (auto &kpt : pts1_ext) { diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp index db766eee5..6ec61e35e 100644 --- a/ov_init/src/init/InertialInitializer.cpp +++ b/ov_init/src/init/InertialInitializer.cpp @@ -25,13 +25,14 @@ using namespace ov_core; using namespace ov_type; using namespace ov_init; -InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_) : params(params_) { +InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_, std::shared_ptr db) + : params(params_), _db(db) { // Vector of our IMU data imu_data = std::make_shared>(); // Create static initializer - init_static = std::make_shared(params, imu_data); + init_static = std::make_shared(params, _db, imu_data); // TODO: create the feature tracker here // TODO: create dynamic initialize class object diff --git a/ov_init/src/init/InertialInitializer.h b/ov_init/src/init/InertialInitializer.h index a594ea223..9b42f7c75 100644 --- a/ov_init/src/init/InertialInitializer.h +++ b/ov_init/src/init/InertialInitializer.h @@ -56,8 +56,9 @@ class InertialInitializer { /** * @brief Default constructor * @param params_ Parameters loaded from either ROS or CMDLINE + * @param db Feature tracker database with all features in it */ - explicit InertialInitializer(InertialInitializerOptions ¶ms_); + explicit InertialInitializer(InertialInitializerOptions ¶ms_, std::shared_ptr db); /** * @brief Feed function for inertial data @@ -92,6 +93,9 @@ class InertialInitializer { /// Initialization parameters InertialInitializerOptions params; + /// Feature tracker database with all features in it + std::shared_ptr _db; + /// Our history of IMU messages (time, angular, linear) std::shared_ptr> imu_data; diff --git a/ov_init/src/init/InertialInitializerOptions.h b/ov_init/src/init/InertialInitializerOptions.h index 323b8ee32..f5b3da8bf 100644 --- a/ov_init/src/init/InertialInitializerOptions.h +++ b/ov_init/src/init/InertialInitializerOptions.h @@ -54,7 +54,6 @@ struct InertialInitializerOptions { print_and_load_initializer(parser); print_and_load_noise(parser); print_and_load_state(parser); - print_and_load_trackers(parser); } // CALIBRATION ============================ @@ -89,6 +88,9 @@ struct InertialInitializerOptions { /// Max disparity we will consider the unit to be stationary double init_max_disparity = 1.0; + /// Number of features we should try to track + int init_max_features = 20; + /** * @brief This function will load print out all initializer settings loaded. * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -108,6 +110,7 @@ struct InertialInitializerOptions { parser->parse_config("init_window_time", init_window_time); parser->parse_config("init_imu_thresh", init_imu_thresh); parser->parse_config("init_max_disparity", init_max_disparity); + parser->parse_config("init_max_features", init_max_features); } PRINT_DEBUG(" - init_only_use_dynamic: %d\n", init_only_use_dynamic); PRINT_DEBUG(" - init_only_use_static: %d\n", init_only_use_static); @@ -125,6 +128,7 @@ struct InertialInitializerOptions { PRINT_DEBUG(" - init_window_time: %.2f\n", init_window_time); PRINT_DEBUG(" - init_imu_thresh: %.2f\n", init_imu_thresh); PRINT_DEBUG(" - init_max_disparity: %.2f\n", init_max_disparity); + PRINT_DEBUG(" - init_max_features: %.2f\n", init_max_features); // Ensure we have enough frames to work with double dt = 1.0 / init_camera_rate; int num_frames = std::floor(init_window_time / dt); @@ -135,6 +139,11 @@ struct InertialInitializerOptions { PRINT_ERROR(RED " num init frames = %d\n" RESET, num_frames); std::exit(EXIT_FAILURE); } + if (init_max_features < 15) { + PRINT_ERROR(RED "number of requested feature tracks to init not enough!!\n" RESET); + PRINT_ERROR(RED " init_max_features = %d\n" RESET, init_max_features); + std::exit(EXIT_FAILURE); + } } // NOISE / CHI2 ============================ @@ -311,106 +320,6 @@ struct InertialInitializerOptions { // } } - // TRACKERS =============================== - - /// If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature tracking on each image. - bool use_stereo = true; - - /// If we should use KLT tracking, or descriptor matcher - bool use_klt = true; - - /// If should extract aruco tags and estimate them - bool use_aruco = true; - - /// Will half the resolution of the aruco tag image (will be faster) - bool downsize_aruco = true; - - /// Will half the resolution all tracking image (aruco will be 1/4 instead of halved if dowsize_aruoc also enabled) - bool downsample_cameras = false; - - /// If our front-end should try to use some multi-threading for stereo matching - bool use_multi_threading = true; - - /// The number of points we should extract and track in *each* image frame. This highly effects the computation required for tracking. - int num_pts = 150; - - /// Fast extraction threshold - int fast_threshold = 20; - - /// Number of grids we should split column-wise to do feature extraction in - int grid_x = 5; - - /// Number of grids we should split row-wise to do feature extraction in - int grid_y = 5; - - /// Will check after doing KLT track and remove any features closer than this - int min_px_dist = 10; - - /// What type of pre-processing histogram method should be applied to images - ov_core::TrackBase::HistogramMethod histogram_method = ov_core::TrackBase::HistogramMethod::HISTOGRAM; - - /// KNN ration between top two descriptor matcher which is required to be a good match - double knn_ratio = 0.85; - - /// Parameters used by our feature initialize / triangulator - ov_core::FeatureInitializerOptions featinit_options; - - /** - * @brief This function will load print out all parameters related to visual tracking - * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. - * - * @param parser If not null, this parser will be used to load our parameters - */ - void print_and_load_trackers(const std::shared_ptr &parser = nullptr) { - if (parser != nullptr) { - parser->parse_config("use_stereo", use_stereo); - parser->parse_config("use_klt", use_klt); - parser->parse_config("use_aruco", use_aruco); - parser->parse_config("downsize_aruco", downsize_aruco); - parser->parse_config("multi_threading", use_multi_threading); - parser->parse_config("num_pts", num_pts); - parser->parse_config("fast_threshold", fast_threshold); - parser->parse_config("grid_x", grid_x); - parser->parse_config("grid_y", grid_y); - parser->parse_config("min_px_dist", min_px_dist); - std::string histogram_method_str = "HISTOGRAM"; - parser->parse_config("histogram_method", histogram_method_str); - if (histogram_method_str == "NONE") { - histogram_method = ov_core::TrackBase::NONE; - } else if (histogram_method_str == "HISTOGRAM") { - histogram_method = ov_core::TrackBase::HISTOGRAM; - } else if (histogram_method_str == "CLAHE") { - histogram_method = ov_core::TrackBase::CLAHE; - } else { - printf(RED "Initializer(): invalid feature histogram specified:\n" RESET); - printf(RED "\t- NONE\n" RESET); - printf(RED "\t- HISTOGRAM\n" RESET); - printf(RED "\t- CLAHE\n" RESET); - std::exit(EXIT_FAILURE); - } - parser->parse_config("knn_ratio", knn_ratio); - } - PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n"); - PRINT_DEBUG(" - use_stereo: %d\n", use_stereo); - PRINT_DEBUG(" - use_klt: %d\n", use_klt); - PRINT_DEBUG(" - use_aruco: %d\n", use_aruco); - PRINT_DEBUG(" - downsize aruco: %d\n", downsize_aruco); - PRINT_DEBUG(" - downsize cameras: %d\n", downsample_cameras); - PRINT_DEBUG(" - use multi-threading: %d\n", use_multi_threading); - PRINT_DEBUG(" - num_pts: %d\n", num_pts); - PRINT_DEBUG(" - fast threshold: %d\n", fast_threshold); - PRINT_DEBUG(" - grid X by Y: %d by %d\n", grid_x, grid_y); - PRINT_DEBUG(" - min px dist: %d\n", min_px_dist); - PRINT_DEBUG(" - hist method: %d\n", (int)histogram_method); - PRINT_DEBUG(" - knn ratio: %.3f\n", knn_ratio); - featinit_options.print(parser); - if (num_pts < 15) { - PRINT_ERROR(RED "number of requested features tracked not enough!!\n" RESET); - PRINT_ERROR(RED " num_pts = %d\n" RESET, num_pts); - std::exit(EXIT_FAILURE); - } - } - // SIMULATOR =============================== /// Seed for initial states (i.e. random feature 3d positions in the generated map) diff --git a/ov_init/src/static/StaticInitializer.cpp b/ov_init/src/static/StaticInitializer.cpp index f78725fee..2327c0c1c 100644 --- a/ov_init/src/static/StaticInitializer.cpp +++ b/ov_init/src/static/StaticInitializer.cpp @@ -55,11 +55,59 @@ bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarianc } // Return if both of these failed - if (window_1to0.empty() || window_2to1.empty()) { + if (window_1to0.size() < 2 || window_2to1.size() < 2) { // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); return false; } + // First we can check the disparity of the two + if (params.init_max_disparity > 0) { + + // Get time in the camera + double time0_imu_in_cam = window_2to1.at(0).timestamp; + double time1_imu_in_cam = window_2to1.at(window_2to1.size() - 1).timestamp; + + // Find the closest camera timestamps + std::map camera_times; + for (auto const &row : _db->get_internal_data()) { + for (auto const × : row.second->timestamps) { + for (auto const &time : times.second) { + camera_times[time] = true; + } + } + } + auto it0 = camera_times.lower_bound(time0_imu_in_cam); + auto it1 = camera_times.lower_bound(time1_imu_in_cam); + if (it0 == camera_times.end() || it1 == camera_times.end()) { + PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to find camera timestamps to compute disparity (have you tracked features?)\n" RESET); + return false; + } + + // Get the disparity statistics from this image to the previous + double time0_cam = it0->first; + double time1_cam = it1->first; + int num_features = 0; + double average_disparity = 0.0; + double variance_disparity = 0.0; + FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, average_disparity, variance_disparity, num_features); + + // Remove old timestamps + _db->cleanup_measurements(time0_imu_in_cam); + + // Return if we can't compute the disparity + if (num_features < 10) { + PRINT_DEBUG(YELLOW "[INIT-IMU]: not enough features to compute disparity %d < 10\n" RESET, num_features); + return false; + } + + // Check if it passed our check! + if (average_disparity > params.init_max_disparity) { + PRINT_DEBUG(YELLOW "[INIT-IMU]: disparity says the platform is moving %.4f < %.4f\n" RESET, average_disparity, + params.init_max_disparity); + return false; + } + } + // Calculate the sample variance for the newest window from 1 to 0 Eigen::Vector3d a_avg_1to0 = Eigen::Vector3d::Zero(); for (const ImuData &data : window_1to0) { diff --git a/ov_init/src/static/StaticInitializer.h b/ov_init/src/static/StaticInitializer.h index 0313a8d3d..293c696ad 100644 --- a/ov_init/src/static/StaticInitializer.h +++ b/ov_init/src/static/StaticInitializer.h @@ -22,6 +22,7 @@ #ifndef OV_INIT_STATICINITIALIZER_H #define OV_INIT_STATICINITIALIZER_H +#include "feat/FeatureHelper.h" #include "init/InertialInitializerOptions.h" #include "types/IMU.h" @@ -51,10 +52,12 @@ class StaticInitializer { /** * @brief Default constructor * @param params_ Parameters loaded from either ROS or CMDLINE + * @param db Feature tracker database with all features in it * @param imu_data_ Shared pointer to our IMU vector of historical information */ - explicit StaticInitializer(InertialInitializerOptions ¶ms_, std::shared_ptr> imu_data_) - : params(params_), imu_data(imu_data_) {} + explicit StaticInitializer(InertialInitializerOptions ¶ms_, std::shared_ptr db, + std::shared_ptr> imu_data_) + : params(params_), _db(db), imu_data(imu_data_) {} /** * @brief Try to get the initialized system using just the imu @@ -81,6 +84,9 @@ class StaticInitializer { /// Initialization parameters InertialInitializerOptions params; + /// Feature tracker database with all features in it + std::shared_ptr _db; + /// Our history of IMU messages (time, angular, linear) std::shared_ptr> imu_data; }; diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index 95875d24e..9cf61059c 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -31,7 +31,7 @@ try_zupt: true zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 50 -zupt_max_disparity: 0.5 # set to 0 for only imu-based +zupt_max_disparity: 1.5 # set to 0 for only imu-based zupt_only_at_beginning: true # ================================================================== @@ -46,9 +46,10 @@ init_state_bias_accel: true init_enforce_gravity_magnitude: true init_camera_rate: 10.0 -init_window_time: 0.75 +init_window_time: 1.5 init_imu_thresh: 1.5 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/ov_msckf/config/rpng_aruco/estimator_config.yaml index d4a1149f0..57738acf8 100644 --- a/ov_msckf/config/rpng_aruco/estimator_config.yaml +++ b/ov_msckf/config/rpng_aruco/estimator_config.yaml @@ -19,7 +19,6 @@ max_slam_in_update: 25 max_msckf_in_update: 40 dt_slam_delay: 2 - gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" @@ -49,7 +48,8 @@ init_enforce_gravity_magnitude: true init_camera_rate: 10.0 init_window_time: 0.5 init_imu_thresh: 1.5 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/ov_msckf/config/rpng_ironsides/estimator_config.yaml index ae2900320..9bc2908d9 100644 --- a/ov_msckf/config/rpng_ironsides/estimator_config.yaml +++ b/ov_msckf/config/rpng_ironsides/estimator_config.yaml @@ -48,7 +48,8 @@ init_enforce_gravity_magnitude: true init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index b308b6159..1e9fa0906 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -48,7 +48,8 @@ init_enforce_gravity_magnitude: true init_camera_rate: 10.0 init_window_time: 0.75 init_imu_thresh: 1.0 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/ov_msckf/config/tum_vi/estimator_config.yaml index 95b447ab9..53a6fbaf6 100644 --- a/ov_msckf/config/tum_vi/estimator_config.yaml +++ b/ov_msckf/config/tum_vi/estimator_config.yaml @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 50 @@ -46,9 +46,10 @@ init_state_bias_accel: true init_enforce_gravity_magnitude: true init_camera_rate: 10.0 -init_window_time: 1.0 -init_imu_thresh: 0.60 # room1-5:0.60, room6:0.45 -init_max_disparity: 0.5 +init_window_time: 0.85 +init_imu_thresh: 0.45 # room1-5:0.45, room6:0.25 +init_max_disparity: 15.0 +init_max_features: 45.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml index b9fd5b83c..1adcde617 100644 --- a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml @@ -48,7 +48,8 @@ init_enforce_gravity_magnitude: true init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml index b9fd5b83c..1adcde617 100644 --- a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml @@ -48,7 +48,8 @@ init_enforce_gravity_magnitude: true init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 -init_max_disparity: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 # ================================================================== # ================================================================== diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 78d0017cb..f5d40e012 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -10,7 +10,7 @@ - + diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index 585e49f5d..d1543b7cc 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -117,7 +117,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap void RosVisualizer::visualize() { // Return if we have already visualized - if (last_visualization_timestamp == _app->get_state()->_timestamp) + if (last_visualization_timestamp == _app->get_state()->_timestamp && _app->initialized()) return; last_visualization_timestamp = _app->get_state()->_timestamp; diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 25e58a97a..c533a08fe 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -103,16 +103,17 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { //=================================================================================== //=================================================================================== - // Lets make a feature extractor + // Let's make a feature extractor + // NOTE: after we initialize we will increase the total number of feature tracks trackDATABASE = std::make_shared(); if (params.use_klt) { - trackFEATS = std::shared_ptr(new TrackKLT(state->_cam_intrinsics_cameras, params.num_pts, state->_options.max_aruco_features, - params.use_stereo, params.histogram_method, params.fast_threshold, params.grid_x, - params.grid_y, params.min_px_dist)); + trackFEATS = std::shared_ptr(new TrackKLT(state->_cam_intrinsics_cameras, params.init_options.init_max_features, + state->_options.max_aruco_features, params.use_stereo, params.histogram_method, + params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist)); } else { trackFEATS = std::shared_ptr(new TrackDescriptor( - state->_cam_intrinsics_cameras, params.num_pts, state->_options.max_aruco_features, params.use_stereo, params.histogram_method, - params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist, params.knn_ratio)); + state->_cam_intrinsics_cameras, params.init_options.init_max_features, state->_options.max_aruco_features, params.use_stereo, + params.histogram_method, params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist, params.knn_ratio)); } // Initialize our aruco tag extractor @@ -125,7 +126,7 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { propagator = std::make_shared(params.imu_noises, params.gravity_mag); // Our state initialize - initializer = std::make_shared(params.init_options); + initializer = std::make_shared(params.init_options, trackFEATS->get_feature_database()); // Make the updater! updaterMSCKF = std::make_shared(params.msckf_options, params.featinit_options); @@ -286,12 +287,14 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const // Perform our feature tracking! trackFEATS->feed_new_camera(message); - trackDATABASE->append_new_measurements(trackFEATS->get_feature_database()); + if (is_initialized_vio) { + trackDATABASE->append_new_measurements(trackFEATS->get_feature_database()); + } // If the aruco tracker is available, the also pass to it // NOTE: binocular tracking for aruco doesn't make sense as we by default have the ids // NOTE: thus we just call the stereo tracking if we are doing binocular! - if (trackARUCO != nullptr) { + if (is_initialized_vio && trackARUCO != nullptr) { trackARUCO->feed_new_camera(message); trackDATABASE->append_new_measurements(trackARUCO->get_feature_database()); } @@ -339,8 +342,27 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const // TODO: Or if we are trying to reset the system, then do that here! if (!is_initialized_vio) { is_initialized_vio = try_to_initialize(); - if (!is_initialized_vio) + if (!is_initialized_vio) { + + // Get timing statitics information + rT3 = boost::posix_time::microsec_clock::local_time(); + double time_track = (rT2 - rT1).total_microseconds() * 1e-6; + double time_init = (rT3 - rT2).total_microseconds() * 1e-6; + double time_total = (rT3 - rT1).total_microseconds() * 1e-6; + + // Timing information + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track); + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for initialization\n" RESET, time_init); + std::stringstream ss; + ss << "[TIME]: " << std::setprecision(4) << time_total << " seconds for total (camera"; + for (const auto &id : message.sensor_ids) { + ss << " " << id; + } + ss << ")" << std::endl; + PRINT_INFO(BLUE "%s" RESET, ss.str().c_str()); + return; + } } // Call on our propagate and update function @@ -729,8 +751,9 @@ bool VioManager::try_to_initialize() { startup_time = timestamp; // Cleanup any features older then the initialization time - // TODO: ensure we keep the feature tracks from our dynamic init so we can match to the init'ed features! + // Also increase the number of features to the desired amount during estimation trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); + trackFEATS->set_num_features(params.num_pts); if (trackARUCO != nullptr) { trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); } diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index e632efbe9..21bab94da 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -168,47 +168,22 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest bool disparity_passed = false; if (override_with_disparity_check) { - // Get features seen from this image, and the previous image + // Get the disparity statistics from this image to the previous double time0_cam = state->_timestamp; double time1_cam = timestamp; - std::vector> feats0 = _db->features_containing(time0_cam, false, true); - - // Compute the disparity + int num_features = 0; double average_disparity = 0.0; - double num_features = 0.0; - for (auto &feat : feats0) { - - // Get the two uvs for both times - for (auto &campairs : feat->timestamps) { - - // First find the two timestamps - size_t camid = campairs.first; - auto it0 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time0_cam); - auto it1 = std::find(feat->timestamps.at(camid).begin(), feat->timestamps.at(camid).end(), time1_cam); - if (it0 == feat->timestamps.at(camid).end() || it1 == feat->timestamps.at(camid).end()) - continue; - auto idx0 = std::distance(feat->timestamps.at(camid).begin(), it0); - auto idx1 = std::distance(feat->timestamps.at(camid).begin(), it1); - - // Now lets calculate the disparity - Eigen::Vector2f uv0 = feat->uvs.at(camid).at(idx0).block(0, 0, 2, 1); - Eigen::Vector2f uv1 = feat->uvs.at(camid).at(idx1).block(0, 0, 2, 1); - average_disparity += (uv1 - uv0).norm(); - num_features += 1; - } - } + double variance_disparity = 0.0; + FeatureHelper::compute_disparity(_db, time0_cam, time1_cam, average_disparity, variance_disparity, num_features); - // Now check if we have passed the threshold - if (num_features > 0) { - average_disparity /= num_features; - } + // Check if this disparity is enough to be classified as moving disparity_passed = (average_disparity < _zupt_max_disparity && num_features > 20); if (disparity_passed) { PRINT_INFO(CYAN "[ZUPT]: passed disparity (%.3f < %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, - (int)num_features); + (int)num_features); } else { PRINT_DEBUG(YELLOW "[ZUPT]: failed disparity (%.3f > %.3f, %d features)\n" RESET, average_disparity, _zupt_max_disparity, - (int)num_features); + (int)num_features); } } @@ -217,11 +192,11 @@ bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timest if (!disparity_passed && (chi2 > _options.chi2_multipler * chi2_check || state->_imu->vel().norm() > _zupt_max_velocity)) { last_zupt_state_timestamp = 0.0; PRINT_DEBUG(YELLOW "[ZUPT]: rejected |v_IinG| = %.3f (chi2 %.3f > %.3f)\n" RESET, state->_imu->vel().norm(), chi2, - _options.chi2_multipler * chi2_check); + _options.chi2_multipler * chi2_check); return false; } PRINT_INFO(CYAN "[ZUPT]: accepted |v_IinG| = %.3f (chi2 %.3f < %.3f)\n" RESET, state->_imu->vel().norm(), chi2, - _options.chi2_multipler * chi2_check); + _options.chi2_multipler * chi2_check); // Do our update, only do this update if we have previously detected // If we have succeeded, then we should remove the current timestamp feature tracks diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index c9eec9d30..ecd81cc88 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -23,13 +23,14 @@ #define OV_MSCKF_UPDATER_ZEROVELOCITY_H #include "feat/FeatureDatabase.h" +#include "feat/FeatureHelper.h" #include "state/Propagator.h" #include "state/State.h" #include "state/StateHelper.h" #include "utils/colors.h" +#include "utils/print.h" #include "utils/quat_ops.h" #include "utils/sensor_data.h" -#include "utils/print.h" #include "UpdaterHelper.h" #include "UpdaterOptions.h" From 35708de13a7cb2382bf14315873238a2c59fce0a Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 12 Nov 2021 03:34:09 -0500 Subject: [PATCH 16/55] no need to marg features behind the anchor pose (can be the case in fast rotation) --- ov_core/src/types/Landmark.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ov_core/src/types/Landmark.h b/ov_core/src/types/Landmark.h index c6fff6d44..f43dea60c 100644 --- a/ov_core/src/types/Landmark.h +++ b/ov_core/src/types/Landmark.h @@ -79,10 +79,10 @@ class Landmark : public Vec { assert(dx.rows() == _size); set_value(_value + dx); // Ensure we are not near zero in the z-direction - if (LandmarkRepresentation::is_relative_representation(_feat_representation) && _value(_value.rows() - 1) < 1e-8) { - PRINT_WARNING(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1)); - should_marg = true; - } + // if (LandmarkRepresentation::is_relative_representation(_feat_representation) && _value(_value.rows() - 1) < 1e-8) { + // PRINT_DEBUG(YELLOW "WARNING DEPTH %.8f BECAME CLOSE TO ZERO IN UPDATE!!!\n" RESET, _value(_value.rows() - 1)); + // should_marg = true; + // } } /** From adc6e04998516a30a6384a1df6a33fe45941356c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 12 Nov 2021 04:12:53 -0500 Subject: [PATCH 17/55] don't fix gauge freedoms in sim --- ov_msckf/config/rpng_sim/estimator_config.yaml | 4 ++-- ov_msckf/src/core/VioManager.h | 13 +++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index 1e9fa0906..8143940dd 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -22,8 +22,8 @@ dt_slam_delay: 2 gravity_mag: 9.81 feat_rep_msckf: "GLOBAL_3D" -feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" -feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "GLOBAL_3D" +feat_rep_aruco: "GLOBAL_3D" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 01e4fc161..4288a445f 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -106,13 +106,14 @@ class VioManager { state->_imu->set_fej(imustate.block(1, 0, 16, 1)); // Fix the global yaw and position gauge freedoms + // TODO: Why does this break out simulation consistency metrics? std::vector> order = {state->_imu}; - Eigen::MatrixXd Cov = 1e-2 * Eigen::MatrixXd::Identity(state->_imu->size(), state->_imu->size()); - Cov.block(state->_imu->v()->id(), state->_imu->v()->id(), 3, 3) *= 10; - Cov(state->_imu->q()->id() + 2, state->_imu->q()->id() + 2) = 0.0; - Cov.block(state->_imu->p()->id(), state->_imu->p()->id(), 3, 3).setZero(); - Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) = - state->_imu->Rot() * Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) * state->_imu->Rot().transpose(); + Eigen::MatrixXd Cov = 1e-4 * Eigen::MatrixXd::Identity(state->_imu->size(), state->_imu->size()); + // Cov.block(state->_imu->v()->id(), state->_imu->v()->id(), 3, 3) *= 10; + // Cov(state->_imu->q()->id() + 2, state->_imu->q()->id() + 2) = 0.0; + // Cov.block(state->_imu->p()->id(), state->_imu->p()->id(), 3, 3).setZero(); + // Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) = + // state->_imu->Rot() * Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) * state->_imu->Rot().transpose(); StateHelper::set_initial_covariance(state, Cov, order); // Set the state time From 5f0e8c7edfcc14be4a4d3bbd7c39b28958ea5491 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 14 Nov 2021 19:43:33 -0500 Subject: [PATCH 18/55] move helper functions into rosviz helper class, switch image publishers to image_transport, fix spelling error in get_true_parameters() in simulator --- ov_msckf/CMakeLists.txt | 6 +- ov_msckf/package.xml | 2 + ov_msckf/src/{core => ros}/RosVisualizer.cpp | 358 ++----------------- ov_msckf/src/{core => ros}/RosVisualizer.h | 10 +- ov_msckf/src/ros/RosVisualizerHelper.h | 278 ++++++++++++++ ov_msckf/src/ros_serial_msckf.cpp | 2 +- ov_msckf/src/ros_subscribe_msckf.cpp | 2 +- ov_msckf/src/run_simulation.cpp | 4 +- ov_msckf/src/sim/Simulator.h | 10 +- 9 files changed, 333 insertions(+), 339 deletions(-) rename ov_msckf/src/{core => ros}/RosVisualizer.cpp (65%) rename ov_msckf/src/{core => ros}/RosVisualizer.h (95%) create mode 100644 ov_msckf/src/ros/RosVisualizerHelper.h diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 1c1147876..7c8b223a9 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -11,7 +11,7 @@ project(ov_msckf) #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/) # Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core ov_init) +find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init) # Include libraries (if we don't have opencv 4, then fallback to opencv 3) # The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! @@ -37,7 +37,7 @@ option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" if (catkin_FOUND AND ENABLE_CATKIN_ROS) add_definitions(-DROS_AVAILABLE=1) catkin_package( - CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core ov_init + CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init INCLUDE_DIRS src LIBRARIES ov_msckf_lib ) @@ -109,7 +109,7 @@ list(APPEND library_source_files ) if (catkin_FOUND AND ENABLE_CATKIN_ROS) list(APPEND library_source_files - src/core/RosVisualizer.cpp + src/ros/RosVisualizer.cpp ) endif () add_library(ov_msckf_lib SHARED ${library_source_files}) diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 371dbe6de..2f0b36133 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -28,6 +28,7 @@ sensor_msgs nav_msgs visualization_msgs + image_transport cv_bridge ov_core ov_init @@ -41,6 +42,7 @@ sensor_msgs nav_msgs visualization_msgs + image_transport cv_bridge ov_core ov_init diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/ros/RosVisualizer.cpp similarity index 65% rename from ov_msckf/src/core/RosVisualizer.cpp rename to ov_msckf/src/ros/RosVisualizer.cpp index d1543b7cc..b6435a61d 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/ros/RosVisualizer.cpp @@ -30,6 +30,9 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap // Setup our transform broadcaster mTfBr = new tf::TransformBroadcaster(); + // Create image transport + image_transport::ImageTransport it(nh); + // Setup pose and path publisher pub_poseimu = nh.advertise("/ov_msckf/poseimu", 2); PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str()); @@ -49,8 +52,8 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str()); // Our tracking image - pub_tracks = nh.advertise("/ov_msckf/trackhist", 2); - PRINT_DEBUG("Publishing: %s\n", pub_tracks.getTopic().c_str()); + it_pub_tracks = it.advertise("/ov_msckf/trackhist", 2); + PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str()); // Groundtruth publishers pub_posegt = nh.advertise("/ov_msckf/posegt", 2); @@ -63,8 +66,8 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap pub_loop_point = nh.advertise("/ov_msckf/loop_feats", 2); pub_loop_extrinsic = nh.advertise("/ov_msckf/loop_extrinsic", 2); pub_loop_intrinsics = nh.advertise("/ov_msckf/loop_intrinsics", 2); - pub_loop_img_depth = nh.advertise("/ov_msckf/loop_depth", 2); - pub_loop_img_depth_color = nh.advertise("/ov_msckf/loop_depth_colored", 2); + it_pub_loop_img_depth = it.advertise("/ov_msckf/loop_depth", 2); + it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2); // option to enable publishing of global to IMU transformation nh.param("publish_global_to_imu_tf", publish_global2imu_tf, true); @@ -150,9 +153,10 @@ void RosVisualizer::visualize() { // Publish keyframe information publish_loopclosure_information(); - // save total state - if (save_total_state) - sim_save_total_state_to_file(); + // Save total state + if (save_total_state) { + RosVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); + } // Print how much time it took to publish / displaying things rT0_2 = boost::posix_time::microsec_clock::local_time(); @@ -331,7 +335,7 @@ void RosVisualizer::publish_state() { arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_imu; arrIMU.header.frame_id = "global"; - for (size_t i = 0; i < poses_imu.size(); i += std::floor(poses_imu.size() / 16384.0) + 1) { + for (size_t i = 0; i < poses_imu.size(); i += std::floor((double)poses_imu.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_imu.at(i)); } pub_pathimu.publish(arrIMU); @@ -342,36 +346,20 @@ void RosVisualizer::publish_state() { // Publish our transform on TF // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation - tf::StampedTransform trans; - trans.stamp_ = ros::Time::now(); + tf::StampedTransform trans = RosVisualizerHelper::get_stamped_transform_from_pose(state->_imu->pose(), false); trans.frame_id_ = "global"; trans.child_frame_id_ = "imu"; - tf::Quaternion quat(state->_imu->quat()(0), state->_imu->quat()(1), state->_imu->quat()(2), state->_imu->quat()(3)); - trans.setRotation(quat); - tf::Vector3 orig(state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); - trans.setOrigin(orig); if (publish_global2imu_tf) { mTfBr->sendTransform(trans); } // Loop through each camera calibration and publish it for (const auto &calib : state->_calib_IMUtoCAM) { - // need to flip the transform to the IMU frame - Eigen::Vector4d q_ItoC = calib.second->quat(); - Eigen::Vector3d p_CinI = -calib.second->Rot().transpose() * calib.second->pos(); - // publish our transform on TF - // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish - // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation - tf::StampedTransform trans; - trans.stamp_ = ros::Time::now(); - trans.frame_id_ = "imu"; - trans.child_frame_id_ = "cam" + std::to_string(calib.first); - tf::Quaternion quat(q_ItoC(0), q_ItoC(1), q_ItoC(2), q_ItoC(3)); - trans.setRotation(quat); - tf::Vector3 orig(p_CinI(0), p_CinI(1), p_CinI(2)); - trans.setOrigin(orig); + tf::StampedTransform trans_calib = RosVisualizerHelper::get_stamped_transform_from_pose(calib.second, true); + trans_calib.frame_id_ = "imu"; + trans_calib.child_frame_id_ = "cam" + std::to_string(calib.first); if (publish_calibration_tf) { - mTfBr->sendTransform(trans); + mTfBr->sendTransform(trans_calib); } } } @@ -379,7 +367,7 @@ void RosVisualizer::publish_state() { void RosVisualizer::publish_images() { // Check if we have subscribers - if (pub_tracks.getNumSubscribers() == 0) + if (it_pub_tracks.getNumSubscribers() == 0) return; // Get our image of history tracks @@ -391,7 +379,7 @@ void RosVisualizer::publish_images() { sensor_msgs::ImagePtr exl_msg = cv_bridge::CvImage(header, "bgr8", img_history).toImageMsg(); // Publish - pub_tracks.publish(exl_msg); + it_pub_tracks.publish(exl_msg); } void RosVisualizer::publish_features() { @@ -401,157 +389,28 @@ void RosVisualizer::publish_features() { pub_points_sim.getNumSubscribers() == 0) return; - // Get our good features + // Get our good MSCKF features std::vector feats_msckf = _app->get_good_features_MSCKF(); - - // Declare message and sizes - sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "global"; - cloud.header.stamp = ros::Time::now(); - cloud.width = 3 * feats_msckf.size(); - cloud.height = 1; - cloud.is_bigendian = false; - cloud.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier(cloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * feats_msckf.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); - sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); - sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); - - // Fill our iterators - for (const auto &pt : feats_msckf) { - *out_x = pt(0); - ++out_x; - *out_y = pt(1); - ++out_y; - *out_z = pt(2); - ++out_z; - } - - // Publish + sensor_msgs::PointCloud2 cloud = RosVisualizerHelper::get_ros_pointcloud(feats_msckf); pub_points_msckf.publish(cloud); - //==================================================================== - //==================================================================== - - // Get our good features + // Get our good SLAM features std::vector feats_slam = _app->get_features_SLAM(); - - // Declare message and sizes - sensor_msgs::PointCloud2 cloud_SLAM; - cloud_SLAM.header.frame_id = "global"; - cloud_SLAM.header.stamp = ros::Time::now(); - cloud_SLAM.width = 3 * feats_slam.size(); - cloud_SLAM.height = 1; - cloud_SLAM.is_bigendian = false; - cloud_SLAM.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier_SLAM(cloud_SLAM); - modifier_SLAM.setPointCloud2FieldsByString(1, "xyz"); - modifier_SLAM.resize(3 * feats_slam.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x_SLAM(cloud_SLAM, "x"); - sensor_msgs::PointCloud2Iterator out_y_SLAM(cloud_SLAM, "y"); - sensor_msgs::PointCloud2Iterator out_z_SLAM(cloud_SLAM, "z"); - - // Fill our iterators - for (const auto &pt : feats_slam) { - *out_x_SLAM = pt(0); - ++out_x_SLAM; - *out_y_SLAM = pt(1); - ++out_y_SLAM; - *out_z_SLAM = pt(2); - ++out_z_SLAM; - } - - // Publish + sensor_msgs::PointCloud2 cloud_SLAM = RosVisualizerHelper::get_ros_pointcloud(feats_slam); pub_points_slam.publish(cloud_SLAM); - //==================================================================== - //==================================================================== - - // Get our good features + // Get our good ARUCO features std::vector feats_aruco = _app->get_features_ARUCO(); - - // Declare message and sizes - sensor_msgs::PointCloud2 cloud_ARUCO; - cloud_ARUCO.header.frame_id = "global"; - cloud_ARUCO.header.stamp = ros::Time::now(); - cloud_ARUCO.width = 3 * feats_aruco.size(); - cloud_ARUCO.height = 1; - cloud_ARUCO.is_bigendian = false; - cloud_ARUCO.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier_ARUCO(cloud_ARUCO); - modifier_ARUCO.setPointCloud2FieldsByString(1, "xyz"); - modifier_ARUCO.resize(3 * feats_aruco.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x_ARUCO(cloud_ARUCO, "x"); - sensor_msgs::PointCloud2Iterator out_y_ARUCO(cloud_ARUCO, "y"); - sensor_msgs::PointCloud2Iterator out_z_ARUCO(cloud_ARUCO, "z"); - - // Fill our iterators - for (const auto &pt : feats_aruco) { - *out_x_ARUCO = pt(0); - ++out_x_ARUCO; - *out_y_ARUCO = pt(1); - ++out_y_ARUCO; - *out_z_ARUCO = pt(2); - ++out_z_ARUCO; - } - - // Publish + sensor_msgs::PointCloud2 cloud_ARUCO = RosVisualizerHelper::get_ros_pointcloud(feats_aruco); pub_points_aruco.publish(cloud_ARUCO); - //==================================================================== - //==================================================================== - // Skip the rest of we are not doing simulation if (_sim == nullptr) return; - // Get our good features - std::unordered_map feats_sim = _sim->get_map(); - - // Declare message and sizes - sensor_msgs::PointCloud2 cloud_SIM; - cloud_SIM.header.frame_id = "global"; - cloud_SIM.header.stamp = ros::Time::now(); - cloud_SIM.width = 3 * feats_sim.size(); - cloud_SIM.height = 1; - cloud_SIM.is_bigendian = false; - cloud_SIM.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier_SIM(cloud_SIM); - modifier_SIM.setPointCloud2FieldsByString(1, "xyz"); - modifier_SIM.resize(3 * feats_sim.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x_SIM(cloud_SIM, "x"); - sensor_msgs::PointCloud2Iterator out_y_SIM(cloud_SIM, "y"); - sensor_msgs::PointCloud2Iterator out_z_SIM(cloud_SIM, "z"); - - // Fill our iterators - for (const auto &pt : feats_sim) { - *out_x_SIM = pt.second(0); - ++out_x_SIM; - *out_y_SIM = pt.second(1); - ++out_y_SIM; - *out_z_SIM = pt.second(2); - ++out_z_SIM; - } - - // Publish + // Get our good SIMULATION features + std::vector feats_sim = _sim->get_map_vec(); + sensor_msgs::PointCloud2 cloud_SIM = RosVisualizerHelper::get_ros_pointcloud(feats_sim); pub_points_sim.publish(cloud_SIM); } @@ -573,7 +432,7 @@ void RosVisualizer::publish_groundtruth() { // Get the simulated groundtruth // NOTE: we get the true time in the IMU clock frame if (_sim != nullptr) { - timestamp_inI = _app->get_state()->_timestamp + _sim->get_true_paramters().calib_camimu_dt; + timestamp_inI = _app->get_state()->_timestamp + _sim->get_true_parameters().calib_camimu_dt; if (!_sim->get_state(timestamp_inI, state_gt)) return; } @@ -605,7 +464,7 @@ void RosVisualizer::publish_groundtruth() { arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_gt; arrIMU.header.frame_id = "global"; - for (size_t i = 0; i < poses_gt.size(); i += std::floor(poses_gt.size() / 16384.0) + 1) { + for (size_t i = 0; i < poses_gt.size(); i += std::floor((double)poses_gt.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_gt.at(i)); } pub_pathgt.publish(arrIMU); @@ -785,7 +644,7 @@ void RosVisualizer::publish_loopclosure_information() { //====================================================== // Depth images of sparse points and its colorized version - if (pub_loop_img_depth.getNumSubscribers() != 0 || pub_loop_img_depth_color.getNumSubscribers() != 0) { + if (it_pub_loop_img_depth.getNumSubscribers() != 0 || it_pub_loop_img_depth_color.getNumSubscribers() != 0) { // Create the images we will populate with the depths std::pair wh_pair = {active_cam0_image.cols, active_cam0_image.rows}; @@ -836,159 +695,8 @@ void RosVisualizer::publish_loopclosure_information() { // Create our messages sensor_msgs::ImagePtr exl_msg1 = cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_16UC1, depthmap).toImageMsg(); - pub_loop_img_depth.publish(exl_msg1); + it_pub_loop_img_depth.publish(exl_msg1); sensor_msgs::ImagePtr exl_msg2 = cv_bridge::CvImage(header, "bgr8", depthmap_viz).toImageMsg(); - pub_loop_img_depth_color.publish(exl_msg2); + it_pub_loop_img_depth_color.publish(exl_msg2); } } - -void RosVisualizer::sim_save_total_state_to_file() { - - // Get current state - std::shared_ptr state = _app->get_state(); - - // We want to publish in the IMU clock frame - // The timestamp in the state will be the last camera time - double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0); - double timestamp_inI = state->_timestamp + t_ItoC; - - // If we have our simulator, then save it to our groundtruth file - if (_sim != nullptr) { - - // Note that we get the true time in the IMU clock frame - // NOTE: we record both the estimate and groundtruth with the same "true" timestamp if we are doing simulation - Eigen::Matrix state_gt; - timestamp_inI = state->_timestamp + _sim->get_true_paramters().calib_camimu_dt; - if (_sim->get_state(timestamp_inI, state_gt)) { - // STATE: write current true state - of_state_gt.precision(5); - of_state_gt.setf(std::ios::fixed, std::ios::floatfield); - of_state_gt << state_gt(0) << " "; - of_state_gt.precision(6); - of_state_gt << state_gt(1) << " " << state_gt(2) << " " << state_gt(3) << " " << state_gt(4) << " "; - of_state_gt << state_gt(5) << " " << state_gt(6) << " " << state_gt(7) << " "; - of_state_gt << state_gt(8) << " " << state_gt(9) << " " << state_gt(10) << " "; - of_state_gt << state_gt(11) << " " << state_gt(12) << " " << state_gt(13) << " "; - of_state_gt << state_gt(14) << " " << state_gt(15) << " " << state_gt(16) << " "; - - // TIMEOFF: Get the current true time offset - of_state_gt.precision(7); - of_state_gt << _sim->get_true_paramters().calib_camimu_dt << " "; - of_state_gt.precision(0); - of_state_gt << state->_options.num_cameras << " "; - of_state_gt.precision(6); - - // CALIBRATION: Write the camera values to file - assert(state->_options.num_cameras == _sim->get_true_paramters().state_options.num_cameras); - for (int i = 0; i < state->_options.num_cameras; i++) { - // Intrinsics values - of_state_gt << _sim->get_true_paramters().camera_intrinsics.at(i)(0) << " " << _sim->get_true_paramters().camera_intrinsics.at(i)(1) - << " " << _sim->get_true_paramters().camera_intrinsics.at(i)(2) << " " - << _sim->get_true_paramters().camera_intrinsics.at(i)(3) << " "; - of_state_gt << _sim->get_true_paramters().camera_intrinsics.at(i)(4) << " " << _sim->get_true_paramters().camera_intrinsics.at(i)(5) - << " " << _sim->get_true_paramters().camera_intrinsics.at(i)(6) << " " - << _sim->get_true_paramters().camera_intrinsics.at(i)(7) << " "; - // Rotation and position - of_state_gt << _sim->get_true_paramters().camera_extrinsics.at(i)(0) << " " << _sim->get_true_paramters().camera_extrinsics.at(i)(1) - << " " << _sim->get_true_paramters().camera_extrinsics.at(i)(2) << " " - << _sim->get_true_paramters().camera_extrinsics.at(i)(3) << " "; - of_state_gt << _sim->get_true_paramters().camera_extrinsics.at(i)(4) << " " << _sim->get_true_paramters().camera_extrinsics.at(i)(5) - << " " << _sim->get_true_paramters().camera_extrinsics.at(i)(6) << " "; - } - - // New line - of_state_gt << endl; - } - } - - //========================================================================== - //========================================================================== - //========================================================================== - - // Get the covariance of the whole system - Eigen::MatrixXd cov = StateHelper::get_full_covariance(state); - - // STATE: Write the current state to file - of_state_est.precision(5); - of_state_est.setf(std::ios::fixed, std::ios::floatfield); - of_state_est << timestamp_inI << " "; - of_state_est.precision(6); - of_state_est << state->_imu->quat()(0) << " " << state->_imu->quat()(1) << " " << state->_imu->quat()(2) << " " << state->_imu->quat()(3) - << " "; - of_state_est << state->_imu->pos()(0) << " " << state->_imu->pos()(1) << " " << state->_imu->pos()(2) << " "; - of_state_est << state->_imu->vel()(0) << " " << state->_imu->vel()(1) << " " << state->_imu->vel()(2) << " "; - of_state_est << state->_imu->bias_g()(0) << " " << state->_imu->bias_g()(1) << " " << state->_imu->bias_g()(2) << " "; - of_state_est << state->_imu->bias_a()(0) << " " << state->_imu->bias_a()(1) << " " << state->_imu->bias_a()(2) << " "; - - // STATE: Write current uncertainty to file - of_state_std.precision(5); - of_state_std.setf(std::ios::fixed, std::ios::floatfield); - of_state_std << timestamp_inI << " "; - of_state_std.precision(6); - int id = state->_imu->q()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->p()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->v()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->bg()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->ba()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - - // TIMEOFF: Get the current estimate time offset - of_state_est.precision(7); - of_state_est << state->_calib_dt_CAMtoIMU->value()(0) << " "; - of_state_est.precision(0); - of_state_est << state->_options.num_cameras << " "; - of_state_est.precision(6); - - // TIMEOFF: Get the current std values - if (state->_options.do_calib_camera_timeoffset) { - of_state_std << std::sqrt(cov(state->_calib_dt_CAMtoIMU->id(), state->_calib_dt_CAMtoIMU->id())) << " "; - } else { - of_state_std << 0.0 << " "; - } - of_state_std.precision(0); - of_state_std << state->_options.num_cameras << " "; - of_state_std.precision(6); - - // CALIBRATION: Write the camera values to file - for (int i = 0; i < state->_options.num_cameras; i++) { - // Intrinsics values - of_state_est << state->_cam_intrinsics.at(i)->value()(0) << " " << state->_cam_intrinsics.at(i)->value()(1) << " " - << state->_cam_intrinsics.at(i)->value()(2) << " " << state->_cam_intrinsics.at(i)->value()(3) << " "; - of_state_est << state->_cam_intrinsics.at(i)->value()(4) << " " << state->_cam_intrinsics.at(i)->value()(5) << " " - << state->_cam_intrinsics.at(i)->value()(6) << " " << state->_cam_intrinsics.at(i)->value()(7) << " "; - // Rotation and position - of_state_est << state->_calib_IMUtoCAM.at(i)->value()(0) << " " << state->_calib_IMUtoCAM.at(i)->value()(1) << " " - << state->_calib_IMUtoCAM.at(i)->value()(2) << " " << state->_calib_IMUtoCAM.at(i)->value()(3) << " "; - of_state_est << state->_calib_IMUtoCAM.at(i)->value()(4) << " " << state->_calib_IMUtoCAM.at(i)->value()(5) << " " - << state->_calib_IMUtoCAM.at(i)->value()(6) << " "; - // Covariance - if (state->_options.do_calib_camera_intrinsics) { - int index_in = state->_cam_intrinsics.at(i)->id(); - of_state_std << std::sqrt(cov(index_in + 0, index_in + 0)) << " " << std::sqrt(cov(index_in + 1, index_in + 1)) << " " - << std::sqrt(cov(index_in + 2, index_in + 2)) << " " << std::sqrt(cov(index_in + 3, index_in + 3)) << " "; - of_state_std << std::sqrt(cov(index_in + 4, index_in + 4)) << " " << std::sqrt(cov(index_in + 5, index_in + 5)) << " " - << std::sqrt(cov(index_in + 6, index_in + 6)) << " " << std::sqrt(cov(index_in + 7, index_in + 7)) << " "; - } else { - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; - } - if (state->_options.do_calib_camera_pose) { - int index_ex = state->_calib_IMUtoCAM.at(i)->id(); - of_state_std << std::sqrt(cov(index_ex + 0, index_ex + 0)) << " " << std::sqrt(cov(index_ex + 1, index_ex + 1)) << " " - << std::sqrt(cov(index_ex + 2, index_ex + 2)) << " "; - of_state_std << std::sqrt(cov(index_ex + 3, index_ex + 3)) << " " << std::sqrt(cov(index_ex + 4, index_ex + 4)) << " " - << std::sqrt(cov(index_ex + 5, index_ex + 5)) << " "; - } else { - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; - } - } - - // Done with the estimates! - of_state_est << endl; - of_state_std << endl; -} diff --git a/ov_msckf/src/core/RosVisualizer.h b/ov_msckf/src/ros/RosVisualizer.h similarity index 95% rename from ov_msckf/src/core/RosVisualizer.h rename to ov_msckf/src/ros/RosVisualizer.h index 4e17cddf5..4c46312b9 100644 --- a/ov_msckf/src/core/RosVisualizer.h +++ b/ov_msckf/src/ros/RosVisualizer.h @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -40,7 +41,8 @@ #include #include -#include "VioManager.h" +#include "core/VioManager.h" +#include "ros/RosVisualizerHelper.h" #include "sim/Simulator.h" #include "utils/dataset_reader.h" #include "utils/print.h" @@ -101,9 +103,6 @@ class RosVisualizer { /// Publish loop-closure information of current pose and active track information void publish_loopclosure_information(); - /// Save current estimate state and groundtruth including calibration - void sim_save_total_state_to_file(); - /// Core application of the filter system std::shared_ptr _app; @@ -111,11 +110,10 @@ class RosVisualizer { std::shared_ptr _sim; // Our publishers + image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color; ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu; ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim; - ros::Publisher pub_tracks; ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics; - ros::Publisher pub_loop_img_depth, pub_loop_img_depth_color; tf::TransformBroadcaster *mTfBr; // For path viz diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h new file mode 100644 index 000000000..9c41fb2d0 --- /dev/null +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -0,0 +1,278 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OV_MSCKF_ROSVISUALIZER_HELPER_H +#define OV_MSCKF_ROSVISUALIZER_HELPER_H + +#include +#include +#include +#include + +#include "core/VioManager.h" +#include "sim/Simulator.h" + +namespace ov_msckf { + +/** + * @brief Helper class that handles some common versions into and out of ROS formats + */ +class RosVisualizerHelper { + +public: + /** + * @brief Will visualize the system if we have new things + * @param feats Vector of features we will convert into ros format + * @return ROS pointcloud + */ + static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector &feats) { + + // Declare message and sizes + sensor_msgs::PointCloud2 cloud; + cloud.header.frame_id = "global"; + cloud.header.stamp = ros::Time::now(); + cloud.width = 3 * feats.size(); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; // there may be invalid points + + // Setup pointcloud fields + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(3 * feats.size()); + + // Iterators + sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); + + // Fill our iterators + for (const auto &pt : feats) { + *out_x = (float)pt(0); + ++out_x; + *out_y = (float)pt(1); + ++out_y; + *out_z = (float)pt(2); + ++out_z; + } + + return cloud; + } + + /** + * @brief Given a ov_type::PoseJPL this will convert into the ros format. + * + * NOTE: frame ids need to be handled externally! + * + * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) + * @param flip_trans If we should flip / inverse the translation + * @return TF of our pose in global (e.g. q_ItoG, p_IinG) + */ + static tf::StampedTransform get_stamped_transform_from_pose(const std::shared_ptr &pose, bool flip_trans) { + + // Need to flip the transform to the IMU frame + Eigen::Vector4d q_ItoC = pose->quat(); + Eigen::Vector3d p_CinI = pose->pos(); + if (flip_trans) { + p_CinI = -pose->Rot().transpose() * pose->pos(); + } + + // publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation + tf::StampedTransform trans; + trans.stamp_ = ros::Time::now(); + tf::Quaternion quat(q_ItoC(0), q_ItoC(1), q_ItoC(2), q_ItoC(3)); + trans.setRotation(quat); + tf::Vector3 orig(p_CinI(0), p_CinI(1), p_CinI(2)); + trans.setOrigin(orig); + return trans; + } + + /** + * @brief Save current estimate state and groundtruth including calibration + * @param state Pointer to the state + * @param sim Pointer to the simulator (or null) + * @param of_state_est Output file for state estimate + * @param of_state_std Output file for covariance + * @param of_state_gt Output file for groundtruth (if we have it from sim) + */ + static void sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, std::ofstream &of_state_est, + std::ofstream &of_state_std, std::ofstream &of_state_gt) { + + // We want to publish in the IMU clock frame + // The timestamp in the state will be the last camera time + double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0); + double timestamp_inI = state->_timestamp + t_ItoC; + + // If we have our simulator, then save it to our groundtruth file + if (sim != nullptr) { + + // Note that we get the true time in the IMU clock frame + // NOTE: we record both the estimate and groundtruth with the same "true" timestamp if we are doing simulation + Eigen::Matrix state_gt; + timestamp_inI = state->_timestamp + sim->get_true_parameters().calib_camimu_dt; + if (sim->get_state(timestamp_inI, state_gt)) { + // STATE: write current true state + of_state_gt.precision(5); + of_state_gt.setf(std::ios::fixed, std::ios::floatfield); + of_state_gt << state_gt(0) << " "; + of_state_gt.precision(6); + of_state_gt << state_gt(1) << " " << state_gt(2) << " " << state_gt(3) << " " << state_gt(4) << " "; + of_state_gt << state_gt(5) << " " << state_gt(6) << " " << state_gt(7) << " "; + of_state_gt << state_gt(8) << " " << state_gt(9) << " " << state_gt(10) << " "; + of_state_gt << state_gt(11) << " " << state_gt(12) << " " << state_gt(13) << " "; + of_state_gt << state_gt(14) << " " << state_gt(15) << " " << state_gt(16) << " "; + + // TIMEOFF: Get the current true time offset + of_state_gt.precision(7); + of_state_gt << sim->get_true_parameters().calib_camimu_dt << " "; + of_state_gt.precision(0); + of_state_gt << state->_options.num_cameras << " "; + of_state_gt.precision(6); + + // CALIBRATION: Write the camera values to file + assert(state->_options.num_cameras == sim->get_true_parameters().state_options.num_cameras); + for (int i = 0; i < state->_options.num_cameras; i++) { + // Intrinsics values + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)(0) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(1) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(2) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(3) << " "; + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)(4) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(5) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(6) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)(7) << " "; + // Rotation and position + of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(0) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(1) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(2) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(3) << " "; + of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(4) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(5) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " "; + } + + // New line + of_state_gt << endl; + } + } + + //========================================================================== + //========================================================================== + //========================================================================== + + // Get the covariance of the whole system + Eigen::MatrixXd cov = StateHelper::get_full_covariance(state); + + // STATE: Write the current state to file + of_state_est.precision(5); + of_state_est.setf(std::ios::fixed, std::ios::floatfield); + of_state_est << timestamp_inI << " "; + of_state_est.precision(6); + of_state_est << state->_imu->quat()(0) << " " << state->_imu->quat()(1) << " " << state->_imu->quat()(2) << " " + << state->_imu->quat()(3) << " "; + of_state_est << state->_imu->pos()(0) << " " << state->_imu->pos()(1) << " " << state->_imu->pos()(2) << " "; + of_state_est << state->_imu->vel()(0) << " " << state->_imu->vel()(1) << " " << state->_imu->vel()(2) << " "; + of_state_est << state->_imu->bias_g()(0) << " " << state->_imu->bias_g()(1) << " " << state->_imu->bias_g()(2) << " "; + of_state_est << state->_imu->bias_a()(0) << " " << state->_imu->bias_a()(1) << " " << state->_imu->bias_a()(2) << " "; + + // STATE: Write current uncertainty to file + of_state_std.precision(5); + of_state_std.setf(std::ios::fixed, std::ios::floatfield); + of_state_std << timestamp_inI << " "; + of_state_std.precision(6); + int id = state->_imu->q()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->p()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->v()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->bg()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->ba()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + + // TIMEOFF: Get the current estimate time offset + of_state_est.precision(7); + of_state_est << state->_calib_dt_CAMtoIMU->value()(0) << " "; + of_state_est.precision(0); + of_state_est << state->_options.num_cameras << " "; + of_state_est.precision(6); + + // TIMEOFF: Get the current std values + if (state->_options.do_calib_camera_timeoffset) { + of_state_std << std::sqrt(cov(state->_calib_dt_CAMtoIMU->id(), state->_calib_dt_CAMtoIMU->id())) << " "; + } else { + of_state_std << 0.0 << " "; + } + of_state_std.precision(0); + of_state_std << state->_options.num_cameras << " "; + of_state_std.precision(6); + + // CALIBRATION: Write the camera values to file + for (int i = 0; i < state->_options.num_cameras; i++) { + // Intrinsics values + of_state_est << state->_cam_intrinsics.at(i)->value()(0) << " " << state->_cam_intrinsics.at(i)->value()(1) << " " + << state->_cam_intrinsics.at(i)->value()(2) << " " << state->_cam_intrinsics.at(i)->value()(3) << " "; + of_state_est << state->_cam_intrinsics.at(i)->value()(4) << " " << state->_cam_intrinsics.at(i)->value()(5) << " " + << state->_cam_intrinsics.at(i)->value()(6) << " " << state->_cam_intrinsics.at(i)->value()(7) << " "; + // Rotation and position + of_state_est << state->_calib_IMUtoCAM.at(i)->value()(0) << " " << state->_calib_IMUtoCAM.at(i)->value()(1) << " " + << state->_calib_IMUtoCAM.at(i)->value()(2) << " " << state->_calib_IMUtoCAM.at(i)->value()(3) << " "; + of_state_est << state->_calib_IMUtoCAM.at(i)->value()(4) << " " << state->_calib_IMUtoCAM.at(i)->value()(5) << " " + << state->_calib_IMUtoCAM.at(i)->value()(6) << " "; + // Covariance + if (state->_options.do_calib_camera_intrinsics) { + int index_in = state->_cam_intrinsics.at(i)->id(); + of_state_std << std::sqrt(cov(index_in + 0, index_in + 0)) << " " << std::sqrt(cov(index_in + 1, index_in + 1)) << " " + << std::sqrt(cov(index_in + 2, index_in + 2)) << " " << std::sqrt(cov(index_in + 3, index_in + 3)) << " "; + of_state_std << std::sqrt(cov(index_in + 4, index_in + 4)) << " " << std::sqrt(cov(index_in + 5, index_in + 5)) << " " + << std::sqrt(cov(index_in + 6, index_in + 6)) << " " << std::sqrt(cov(index_in + 7, index_in + 7)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + if (state->_options.do_calib_camera_pose) { + int index_ex = state->_calib_IMUtoCAM.at(i)->id(); + of_state_std << std::sqrt(cov(index_ex + 0, index_ex + 0)) << " " << std::sqrt(cov(index_ex + 1, index_ex + 1)) << " " + << std::sqrt(cov(index_ex + 2, index_ex + 2)) << " "; + of_state_std << std::sqrt(cov(index_ex + 3, index_ex + 3)) << " " << std::sqrt(cov(index_ex + 4, index_ex + 4)) << " " + << std::sqrt(cov(index_ex + 5, index_ex + 5)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + } + + // Done with the estimates! + of_state_est << endl; + of_state_std << endl; + } + +private: + // Cannot create this class + RosVisualizerHelper() = default; +}; + +} // namespace ov_msckf + +#endif // OV_MSCKF_ROSVISUALIZER_HELPER_H diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index 091ed041c..2e69b5305 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -28,9 +28,9 @@ #include -#include "core/RosVisualizer.h" #include "core/VioManager.h" #include "core/VioManagerOptions.h" +#include "ros/RosVisualizer.h" #include "utils/dataset_reader.h" #include "utils/sensor_data.h" diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp index 6bcc5429e..388dce877 100644 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ b/ov_msckf/src/ros_subscribe_msckf.cpp @@ -31,9 +31,9 @@ #include -#include "core/RosVisualizer.h" #include "core/VioManager.h" #include "core/VioManagerOptions.h" +#include "ros/RosVisualizer.h" #include "utils/dataset_reader.h" #include "utils/sensor_data.h" diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index f35d3c97f..8e3c58c66 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -30,7 +30,7 @@ #include "utils/sensor_data.h" #if ROS_AVAILABLE -#include "core/RosVisualizer.h" +#include "ros/RosVisualizer.h" #include #endif @@ -103,7 +103,7 @@ int main(int argc, char **argv) { // Since the state time is in the camera frame of reference // Subtract out the imu to camera time offset - imustate(0, 0) -= sim->get_true_paramters().calib_camimu_dt; + imustate(0, 0) -= sim->get_true_parameters().calib_camimu_dt; // Initialize our filter with the groundtruth sys->initialize_with_gt(imustate); diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index 77a9cbb98..3686a008b 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -101,8 +101,16 @@ class Simulator { /// Returns the true 3d map of features std::unordered_map get_map() { return featmap; } + /// Returns the true 3d map of features + std::vector get_map_vec() { + std::vector feats; + for (auto const &feat : featmap) + feats.push_back(feat.second); + return feats; + } + /// Access function to get the true parameters (i.e. calibration and settings) - VioManagerOptions get_true_paramters() { return params; } + VioManagerOptions get_true_parameters() { return params; } protected: /** From dd85c8e3f330ce79c8e323c2a9a994afaf07a329 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 14 Nov 2021 23:41:25 -0500 Subject: [PATCH 19/55] initial splitting of cmake into ROS1 and ROS2, support buildin in both (no ros subscribers for the ros2 have been implemented yet, so doesn't do much...) --- CMakeLists.txt | 16 -- docs/gs-installing.dox | 9 +- ov_core/CMakeLists.txt | 90 ++------- ov_core/cmake/ROS1.cmake | 81 ++++++++ ov_core/cmake/ROS2.cmake | 71 +++++++ ov_core/package.xml | 48 +++-- ov_data/CMakeLists.txt | 20 +- ov_data/package.xml | 24 ++- ov_eval/CMakeLists.txt | 178 ++---------------- ov_eval/cmake/ROS1.cmake | 163 ++++++++++++++++ ov_eval/cmake/ROS2.cmake | 123 ++++++++++++ ov_eval/package.xml | 43 ++--- ov_init/CMakeLists.txt | 87 ++------- ov_init/cmake/ROS1.cmake | 68 +++++++ ov_init/cmake/ROS2.cmake | 53 ++++++ ov_init/package.xml | 38 ++-- ov_init/src/test_orientation.cpp | 36 ---- ov_init/src/test_static.cpp | 36 ---- ov_msckf/CMakeLists.txt | 129 ++----------- ov_msckf/cmake/ROS1.cmake | 113 +++++++++++ ov_msckf/cmake/ROS2.cmake | 92 +++++++++ .../config/rpng_sim/estimator_config.yaml | 1 + ov_msckf/package.xml | 73 +++---- ov_msckf/src/core/VioManagerOptions.h | 2 +- ov_msckf/src/ros_serial_msckf.cpp | 2 +- ov_msckf/src/ros_subscribe_msckf.cpp | 2 +- ov_msckf/src/run_simulation.cpp | 2 +- ov_msckf/src/test_sim_meas.cpp | 20 ++ ov_msckf/src/test_sim_repeat.cpp | 24 ++- 29 files changed, 1005 insertions(+), 639 deletions(-) delete mode 100644 CMakeLists.txt create mode 100644 ov_core/cmake/ROS1.cmake create mode 100644 ov_core/cmake/ROS2.cmake create mode 100644 ov_eval/cmake/ROS1.cmake create mode 100644 ov_eval/cmake/ROS2.cmake create mode 100644 ov_init/cmake/ROS1.cmake create mode 100644 ov_init/cmake/ROS2.cmake delete mode 100644 ov_init/src/test_orientation.cpp delete mode 100644 ov_init/src/test_static.cpp create mode 100644 ov_msckf/cmake/ROS1.cmake create mode 100644 ov_msckf/cmake/ROS2.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 8eada6a3e..000000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -cmake_minimum_required(VERSION 3.3) - -# Project name -project(open_vins) - -# Main required modules that are needed to build the whole system -add_subdirectory(ov_core) -add_subdirectory(ov_init) -add_subdirectory(ov_msckf) - -# Optionally build the eval module, might not needed if building for a drone/robotic system -option(BUILD_OV_EVAL "Enable or disable building of ov_eval" ON) -if (BUILD_OV_EVAL) - add_subdirectory(ov_eval) -endif() - diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox index 15a2227dc..5e93539fa 100644 --- a/docs/gs-installing.dox +++ b/docs/gs-installing.dox @@ -23,7 +23,7 @@ Please see the official instructions to install ROS: @par ROS Usage in OpenVINS We do support ROS-free builds, but don't recommend using this interface as we have limited support for it. You will need to ensure you have installed OpenCV and Eigen3 which are the only dependencies. - If ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_CATKIN_ROS=OFF ..`. + If ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_ROS=OFF ..`. If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions. @code{.shell-session} @@ -46,18 +46,19 @@ If you run into any problems please google search the issue first and if you are After the build is successful please following the @ref gs-tutorial guide on getting a dataset and running the system. @code{.shell-session} -sudo apt-get install python-catkin-tools mkdir -p ~/workspace/catkin_ws_ov/src/ cd ~/workspace/catkin_ws_ov/src/ git clone https://github.com/rpng/open_vins/ cd .. -catkin build +catkin build # ROS1 +colcon build # ROS2 +colcon build --event-handlers console_cohesion+ # ROS2 with verbose output @endcode There are additional options that users might be interested in. Configure these with `catkin build -D=OFF` or `cmake -D=ON ..` in the ROS free case. -* `ENABLE_CATKIN_ROS` - (default ON) - Enable or disable building with ROS (if it is found) +* `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (if it is found) * `ENABLE_ARUCO_TAGS` - (default ON) - Enable or disable aruco tag (disable if no contrib modules) * `BUILD_OV_EVAL` - (default ON) - Enable or disable building of ov_eval * `DISABLE_MATPLOTLIB` - (default OFF) - Disable or enable matplotlib plot scripts in ov_eval diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 2a8e0d3d8..08fd0c916 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -7,9 +7,6 @@ include(GNUInstallDirs) # Project name project(ov_core) -# Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge) - # Include libraries (if we don't have opencv 4, then fallback to opencv 3) # The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! find_package(Eigen3 REQUIRED) @@ -40,20 +37,6 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Describe catkin project -option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - add_definitions(-DROS_AVAILABLE=1) - catkin_package( - CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge - INCLUDE_DIRS src - LIBRARIES ov_core_lib - ) -else () - add_definitions(-DROS_AVAILABLE=0) - message(WARNING "BUILDING WITHOUT ROS!") -endif () - # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) @@ -73,65 +56,22 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized -fno-omit-frame-pointer") -# Include our header files -include_directories( - src - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include - ${catkin_INCLUDE_DIRS} -) - -# Set link libraries used by all binaries -list(APPEND thirdparty_libraries - ${Boost_LIBRARIES} - ${OpenCV_LIBRARIES} - ${catkin_LIBRARIES} -) - -################################################## -# Make the core library -################################################## +# Find our ROS version! +# NOTE: Default to using the ROS1 package if both are in our enviroment +# NOTE: https://github.com/romainreignier/share_ros1_ros2_lib_demo +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if(catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif(ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else() + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif() -add_library(ov_core_lib SHARED - src/dummy.cpp - src/sim/BsplineSE3.cpp - src/track/TrackBase.cpp - src/track/TrackAruco.cpp - src/track/TrackDescriptor.cpp - src/track/TrackKLT.cpp - src/track/TrackSIM.cpp - src/types/Landmark.cpp - src/feat/Feature.cpp - src/feat/FeatureInitializer.cpp - src/utils/print.cpp -) -target_link_libraries(ov_core_lib ${thirdparty_libraries}) -target_include_directories(ov_core_lib PUBLIC src) -install(TARGETS ov_core_lib - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -################################################## -# Make binary files! -################################################## - -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - - add_executable(test_tracking src/test_tracking.cpp) - target_link_libraries(test_tracking ov_core_lib ${thirdparty_libraries}) - -endif () -add_executable(test_webcam src/test_webcam.cpp) -target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) -install(TARGETS test_webcam - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) diff --git a/ov_core/cmake/ROS1.cmake b/ov_core/cmake/ROS1.cmake new file mode 100644 index 000000000..ce38e8a7f --- /dev/null +++ b/ov_core/cmake/ROS1.cmake @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(catkin QUIET COMPONENTS roscpp rosbag sensor_msgs cv_bridge) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (catkin_FOUND AND ENABLE_ROS) + add_definitions(-DROS_AVAILABLE=1) + catkin_package( + CATKIN_DEPENDS roscpp rosbag sensor_msgs cv_bridge + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src/ + LIBRARIES ov_core_lib + ) +else () + add_definitions(-DROS_AVAILABLE=0) + message(WARNING "BUILDING WITHOUT ROS!") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} + ${catkin_LIBRARIES} +) + +################################################## +# Make the core library +################################################## + +add_library(ov_core_lib SHARED + src/dummy.cpp + src/sim/BsplineSE3.cpp + src/track/TrackBase.cpp + src/track/TrackAruco.cpp + src/track/TrackDescriptor.cpp + src/track/TrackKLT.cpp + src/track/TrackSIM.cpp + src/types/Landmark.cpp + src/feat/Feature.cpp + src/feat/FeatureInitializer.cpp + src/utils/print.cpp +) +target_link_libraries(ov_core_lib ${thirdparty_libraries}) +target_include_directories(ov_core_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_core_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +################################################## +# Make binary files! +################################################## + +if (catkin_FOUND AND ENABLE_ROS) + + add_executable(test_tracking src/test_tracking.cpp) + target_link_libraries(test_tracking ov_core_lib ${thirdparty_libraries}) + +endif () + +add_executable(test_webcam src/test_webcam.cpp) +target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) +install(TARGETS test_webcam + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + + diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake new file mode 100644 index 000000000..bf1289c9c --- /dev/null +++ b/ov_core/cmake/ROS2.cmake @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ros dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (NOT ENABLE_ROS) + message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +################################################## +# Make the core library +################################################## + +add_library(ov_core_lib SHARED + src/dummy.cpp + src/sim/BsplineSE3.cpp + src/track/TrackBase.cpp + src/track/TrackAruco.cpp + src/track/TrackDescriptor.cpp + src/track/TrackKLT.cpp + src/track/TrackSIM.cpp + src/types/Landmark.cpp + src/feat/Feature.cpp + src/feat/FeatureInitializer.cpp + src/utils/print.cpp +) +target_link_libraries(ov_core_lib ${thirdparty_libraries}) +target_include_directories(ov_core_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_core_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) +ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) +ament_export_libraries(ov_core_lib) + +################################################## +# Make binary files! +################################################## + +# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! +#if (catkin_FOUND AND ENABLE_ROS) +# add_executable(test_tracking src/test_tracking.cpp) +# target_link_libraries(test_tracking ov_core_lib ${thirdparty_libraries}) +#endif () + +add_executable(test_webcam src/test_webcam.cpp) +ament_target_dependencies(test_webcam rclcpp) +target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) +install(TARGETS test_webcam DESTINATION lib/${PROJECT_NAME}) + +# finally define this as the package +ament_package() \ No newline at end of file diff --git a/ov_core/package.xml b/ov_core/package.xml index 19dffc50a..2879bdc56 100644 --- a/ov_core/package.xml +++ b/ov_core/package.xml @@ -1,11 +1,13 @@ - + + ov_core - 1.0.0 + 2.4.0 Core algorithms for visual-inertial navigation algorithms. + https://github.com/rpng/open_vins Patrick Geneva @@ -15,31 +17,23 @@ GNU General Public License v3.0 - - catkin - - - cmake_modules - roscpp - rosbag - tf - std_msgs - geometry_msgs - sensor_msgs - nav_msgs - visualization_msgs - cv_bridge - - - roscpp - rosbag - tf - std_msgs - geometry_msgs - sensor_msgs - nav_msgs - visualization_msgs - cv_bridge + + catkin + cmake_modules + roscpp + rosbag + sensor_msgs + cv_bridge + + + ament_cmake + rclcpp + + + + catkin + ament_cmake + \ No newline at end of file diff --git a/ov_data/CMakeLists.txt b/ov_data/CMakeLists.txt index b131724a6..96c9f9a5d 100644 --- a/ov_data/CMakeLists.txt +++ b/ov_data/CMakeLists.txt @@ -3,9 +3,17 @@ cmake_minimum_required(VERSION 3.3) # Project name project(ov_data) -# Find catkin (the ROS build system) -find_package(catkin REQUIRED) - - -# Describe catkin Project -catkin_package() +# Find our ROS version! +# NOTE: Default to using the ROS1 package if both are in our enviroment +# NOTE: https://github.com/romainreignier/share_ros1_ros2_lib_demo +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if(catkin_FOUND) + message(STATUS "ROS *1* version found") + catkin_package() +elseif(ament_cmake_FOUND) + message(STATUS "ROS *2* version found") + ament_package() +else() + message(STATUS "No ROS versions found, doing nothing...") +endif() diff --git a/ov_data/package.xml b/ov_data/package.xml index 5d449b4cd..f26641b16 100644 --- a/ov_data/package.xml +++ b/ov_data/package.xml @@ -1,23 +1,31 @@ - + + ov_data - 0.0.0 + 2.4.0 - Data for the MSCKF, mostly just groundtruth files... + Data for the OpenVINS project, mostly just groundtruth files... + https://github.com/rpng/open_vins Patrick Geneva Patrick Geneva - MIT + GNU General Public License v3.0 + + + catkin - - catkin + + ament_cmake - - cmake_modules + + + catkin + ament_cmake + diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 9c1418fc3..11e00fd7e 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -7,9 +7,6 @@ include(GNUInstallDirs) # Project name project(ov_eval) -# Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core) - # Include libraries find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) @@ -25,20 +22,6 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Describe catkin project -option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - add_definitions(-DROS_AVAILABLE=1) - catkin_package( - CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core - INCLUDE_DIRS src - LIBRARIES ov_eval_lib - ) -else () - add_definitions(-DROS_AVAILABLE=0) - message(WARNING "BUILDING WITHOUT ROS!") -endif () - # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) @@ -58,150 +41,19 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") -# Include our header files -include_directories( - src - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include - ${catkin_INCLUDE_DIRS} -) - -# Set link libraries used by all binaries -list(APPEND thirdparty_libraries - ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} - ${catkin_LIBRARIES} -) - -# If we are not building with ROS then we need to manually link to its headers -# This isn't that elegant of a way, but this at least allows for building without ROS -# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 -if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) - message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") - include_directories(${ov_core_SOURCE_DIR}/src/) - list(APPEND thirdparty_libraries ov_core_lib) -endif () - -################################################## -# Make the shared library -################################################## - -add_library(ov_eval_lib SHARED - src/dummy.cpp - src/alignment/AlignTrajectory.cpp - src/alignment/AlignUtils.cpp - src/calc/ResultTrajectory.cpp - src/calc/ResultSimulation.cpp - src/utils/Loader.cpp -) -target_link_libraries(ov_eval_lib ${thirdparty_libraries}) -install(TARGETS ov_eval_lib - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -################################################## -# Make binary files! -################################################## - -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - - add_executable(pose_to_file src/pose_to_file.cpp) - target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) - - add_executable(live_align_trajectory src/live_align_trajectory.cpp) - target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) - -endif () - -add_executable(format_converter src/format_converter.cpp) -target_link_libraries(format_converter ov_eval_lib ${thirdparty_libraries}) -install(TARGETS format_converter - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(error_comparison src/error_comparison.cpp) -target_link_libraries(error_comparison ov_eval_lib ${thirdparty_libraries}) -install(TARGETS error_comparison - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(error_dataset src/error_dataset.cpp) -target_link_libraries(error_dataset ov_eval_lib ${thirdparty_libraries}) -install(TARGETS error_dataset - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(error_singlerun src/error_singlerun.cpp) -target_link_libraries(error_singlerun ov_eval_lib ${thirdparty_libraries}) -install(TARGETS error_singlerun - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(error_simulation src/error_simulation.cpp) -target_link_libraries(error_simulation ov_eval_lib ${thirdparty_libraries}) -install(TARGETS error_simulation - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(timing_flamegraph src/timing_flamegraph.cpp) -target_link_libraries(timing_flamegraph ov_eval_lib ${thirdparty_libraries}) -install(TARGETS timing_flamegraph - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(timing_comparison src/timing_comparison.cpp) -target_link_libraries(timing_comparison ov_eval_lib ${thirdparty_libraries}) -install(TARGETS timing_comparison - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(timing_percentages src/timing_percentages.cpp) -target_link_libraries(timing_percentages ov_eval_lib ${thirdparty_libraries}) -install(TARGETS timing_percentages - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -add_executable(plot_trajectories src/plot_trajectories.cpp) -target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) -install(TARGETS plot_trajectories - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - - -################################################## -# Python scripts! -################################################## - -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - - catkin_install_python(PROGRAMS python/pid_ros.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - catkin_install_python(PROGRAMS python/pid_sys.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -endif () - - - +# Find our ROS version! +# NOTE: Default to using the ROS1 package if both are in our enviroment +# NOTE: https://github.com/romainreignier/share_ros1_ros2_lib_demo +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if(catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif(ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else() + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif() diff --git a/ov_eval/cmake/ROS1.cmake b/ov_eval/cmake/ROS1.cmake new file mode 100644 index 000000000..9355c71a4 --- /dev/null +++ b/ov_eval/cmake/ROS1.cmake @@ -0,0 +1,163 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (catkin_FOUND AND ENABLE_ROS) + add_definitions(-DROS_AVAILABLE=1) + catkin_package( + CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src/ + LIBRARIES ov_eval_lib + ) +else () + add_definitions(-DROS_AVAILABLE=0) + message(WARNING "BUILDING WITHOUT ROS!") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${PYTHON_LIBRARIES} + ${catkin_LIBRARIES} +) + +# If we are not building with ROS then we need to manually link to its headers +# This isn't that elegant of a way, but this at least allows for building without ROS +# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +if (NOT catkin_FOUND OR NOT ENABLE_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") + include_directories(${ov_core_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_core_lib) +endif () + +################################################## +# Make the shared library +################################################## + +add_library(ov_eval_lib SHARED + src/dummy.cpp + src/alignment/AlignTrajectory.cpp + src/alignment/AlignUtils.cpp + src/calc/ResultTrajectory.cpp + src/calc/ResultSimulation.cpp + src/utils/Loader.cpp +) +target_link_libraries(ov_eval_lib ${thirdparty_libraries}) +target_include_directories(ov_eval_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_eval_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +################################################## +# Make binary files! +################################################## + +if (catkin_FOUND AND ENABLE_ROS) + + add_executable(pose_to_file src/pose_to_file.cpp) + target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) + + add_executable(live_align_trajectory src/live_align_trajectory.cpp) + target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) + +endif () + +add_executable(format_converter src/format_converter.cpp) +target_link_libraries(format_converter ov_eval_lib ${thirdparty_libraries}) +install(TARGETS format_converter + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(error_comparison src/error_comparison.cpp) +target_link_libraries(error_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_comparison + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(error_dataset src/error_dataset.cpp) +target_link_libraries(error_dataset ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_dataset + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(error_singlerun src/error_singlerun.cpp) +target_link_libraries(error_singlerun ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_singlerun + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(error_simulation src/error_simulation.cpp) +target_link_libraries(error_simulation ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_simulation + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(timing_flamegraph src/timing_flamegraph.cpp) +target_link_libraries(timing_flamegraph ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_flamegraph + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(timing_comparison src/timing_comparison.cpp) +target_link_libraries(timing_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_comparison + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(timing_percentages src/timing_percentages.cpp) +target_link_libraries(timing_percentages ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_percentages + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + ) + +add_executable(plot_trajectories src/plot_trajectories.cpp) +target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) +install(TARGETS plot_trajectories + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + + +################################################## +# Python scripts! +################################################## + +if (catkin_FOUND AND ENABLE_ROS) + + catkin_install_python(PROGRAMS python/pid_ros.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + catkin_install_python(PROGRAMS python/pid_sys.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +endif () diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake new file mode 100644 index 000000000..9d3e99b6c --- /dev/null +++ b/ov_eval/cmake/ROS2.cmake @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ov_core REQUIRED) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (NOT ENABLE_ROS) + message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${PYTHON_LIBRARIES} +) + +################################################## +# Make the shared library +################################################## + +add_library(ov_eval_lib SHARED + src/dummy.cpp + src/alignment/AlignTrajectory.cpp + src/alignment/AlignUtils.cpp + src/calc/ResultTrajectory.cpp + src/calc/ResultSimulation.cpp + src/utils/Loader.cpp +) +ament_target_dependencies(ov_eval_lib rclcpp ov_core) +target_link_libraries(ov_eval_lib ${thirdparty_libraries}) +target_include_directories(ov_eval_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_eval_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) +ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) +ament_export_libraries(ov_eval_lib) + +################################################## +# Make binary files! +################################################## + +# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! +#if (catkin_FOUND AND ENABLE_ROS) +# add_executable(pose_to_file src/pose_to_file.cpp) +# target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) +# add_executable(live_align_trajectory src/live_align_trajectory.cpp) +# target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) +#endif () + +add_executable(format_converter src/format_converter.cpp) +ament_target_dependencies(format_converter rclcpp ov_core) +target_link_libraries(format_converter ov_eval_lib ${thirdparty_libraries}) +install(TARGETS format_converter DESTINATION lib/${PROJECT_NAME}) + +add_executable(error_comparison src/error_comparison.cpp) +ament_target_dependencies(error_comparison rclcpp ov_core) +target_link_libraries(error_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_comparison DESTINATION lib/${PROJECT_NAME}) + +add_executable(error_dataset src/error_dataset.cpp) +ament_target_dependencies(error_dataset rclcpp ov_core) +target_link_libraries(error_dataset ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_dataset DESTINATION lib/${PROJECT_NAME}) + +add_executable(error_singlerun src/error_singlerun.cpp) +ament_target_dependencies(error_singlerun rclcpp ov_core) +target_link_libraries(error_singlerun ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_singlerun DESTINATION lib/${PROJECT_NAME}) + +add_executable(error_simulation src/error_simulation.cpp) +ament_target_dependencies(error_simulation rclcpp ov_core) +target_link_libraries(error_simulation ov_eval_lib ${thirdparty_libraries}) +install(TARGETS error_simulation DESTINATION lib/${PROJECT_NAME}) + +add_executable(timing_flamegraph src/timing_flamegraph.cpp) +ament_target_dependencies(timing_flamegraph rclcpp ov_core) +target_link_libraries(timing_flamegraph ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_flamegraph DESTINATION lib/${PROJECT_NAME}) + +add_executable(timing_comparison src/timing_comparison.cpp) +ament_target_dependencies(timing_comparison rclcpp ov_core) +target_link_libraries(timing_comparison ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_comparison DESTINATION lib/${PROJECT_NAME}) + +add_executable(timing_percentages src/timing_percentages.cpp) +ament_target_dependencies(timing_percentages rclcpp ov_core) +target_link_libraries(timing_percentages ov_eval_lib ${thirdparty_libraries}) +install(TARGETS timing_percentages DESTINATION lib/${PROJECT_NAME}) + +add_executable(plot_trajectories src/plot_trajectories.cpp) +ament_target_dependencies(plot_trajectories rclcpp ov_core) +target_link_libraries(plot_trajectories ov_eval_lib ${thirdparty_libraries}) +install(TARGETS plot_trajectories DESTINATION lib/${PROJECT_NAME}) + + +################################################## +# Python scripts! +################################################## + +# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! +#if (catkin_FOUND AND ENABLE_ROS) +# catkin_install_python(PROGRAMS python/pid_ros.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# catkin_install_python(PROGRAMS python/pid_sys.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +#endif () + + +# finally define this as the package +ament_package() \ No newline at end of file diff --git a/ov_eval/package.xml b/ov_eval/package.xml index ebcb85ae9..0c52c0a39 100644 --- a/ov_eval/package.xml +++ b/ov_eval/package.xml @@ -1,40 +1,41 @@ - + + ov_eval - 1.0.0 + 2.4.0 Evaluation methods and scripts for visual-inertial odometry systems. + https://github.com/rpng/open_vins Patrick Geneva Kevin Eckenhoff Patrick Geneva - Kevin Eckenhoff GNU General Public License v3.0 - - catkin - - - cmake_modules - roscpp - rospy - geometry_msgs - nav_msgs - sensor_msgs - ov_core + + catkin + cmake_modules + roscpp + rospy + geometry_msgs + nav_msgs + sensor_msgs + ov_core - - roscpp - rospy - geometry_msgs - nav_msgs - sensor_msgs - ov_core + + ament_cmake + rclcpp + ov_core + + + catkin + ament_cmake + \ No newline at end of file diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 6ae813744..53890e8fb 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -7,9 +7,6 @@ include(GNUInstallDirs) # Project name project(ov_init) -# Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core) - # Include libraries find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) @@ -25,20 +22,6 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Describe catkin project -option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - add_definitions(-DROS_AVAILABLE=1) - catkin_package( - CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core - INCLUDE_DIRS src - LIBRARIES ov_init_lib - ) -else () - add_definitions(-DROS_AVAILABLE=0) - message(WARNING "BUILDING WITHOUT ROS!") -endif () - # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) @@ -58,59 +41,19 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") -# Include our header files -include_directories( - src - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include - ${catkin_INCLUDE_DIRS} -) - -# Set link libraries used by all binaries -list(APPEND thirdparty_libraries - ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} - ${catkin_LIBRARIES} -) - -# If we are not building with ROS then we need to manually link to its headers -# This isn't that elegant of a way, but this at least allows for building without ROS -# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 -if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) - message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") - include_directories(${ov_core_SOURCE_DIR}/src/) - list(APPEND thirdparty_libraries ov_core_lib) -endif () - -################################################## -# Make the shared library -################################################## - -add_library(ov_init_lib SHARED - src/dummy.cpp - src/init/InertialInitializer.cpp - src/static/StaticInitializer.cpp -) -target_link_libraries(ov_init_lib ${thirdparty_libraries}) -install(TARGETS ov_init_lib - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - -################################################## -# Make binary files! -################################################## - -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - - add_executable(test_orientation src/test_orientation.cpp) - target_link_libraries(test_orientation ov_init_lib ${thirdparty_libraries}) - -endif () - - +# Find our ROS version! +# NOTE: Default to using the ROS1 package if both are in our enviroment +# NOTE: https://github.com/romainreignier/share_ros1_ros2_lib_demo +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if(catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif(ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else() + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif() diff --git a/ov_init/cmake/ROS1.cmake b/ov_init/cmake/ROS1.cmake new file mode 100644 index 000000000..efd176921 --- /dev/null +++ b/ov_init/cmake/ROS1.cmake @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(catkin QUIET COMPONENTS roscpp ov_core) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (catkin_FOUND AND ENABLE_ROS) + add_definitions(-DROS_AVAILABLE=1) + catkin_package( + CATKIN_DEPENDS roscpp ov_core + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src/ + LIBRARIES ov_init_lib + ) +else () + add_definitions(-DROS_AVAILABLE=0) + message(WARNING "BUILDING WITHOUT ROS!") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${PYTHON_LIBRARIES} + ${catkin_LIBRARIES} +) + +# If we are not building with ROS then we need to manually link to its headers +# This isn't that elegant of a way, but this at least allows for building without ROS +# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +if (NOT catkin_FOUND OR NOT ENABLE_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") + include_directories(${ov_core_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_core_lib) +endif () + +################################################## +# Make the shared library +################################################## + +add_library(ov_init_lib SHARED + src/dummy.cpp + src/init/InertialInitializer.cpp + src/static/StaticInitializer.cpp +) +target_link_libraries(ov_init_lib ${thirdparty_libraries}) +target_include_directories(ov_init_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_init_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + + + + + + diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake new file mode 100644 index 000000000..2b3c46abc --- /dev/null +++ b/ov_init/cmake/ROS2.cmake @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ros dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ov_core REQUIRED) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (NOT ENABLE_ROS) + message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${PYTHON_LIBRARIES} +) + +################################################## +# Make the shared library +################################################## + +add_library(ov_init_lib SHARED + src/dummy.cpp + src/init/InertialInitializer.cpp + src/static/StaticInitializer.cpp +) +ament_target_dependencies(ov_init_lib rclcpp ov_core) +target_link_libraries(ov_init_lib ${thirdparty_libraries}) +target_include_directories(ov_init_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_init_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) +ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) +ament_export_libraries(ov_init_lib) + + +# finally define this as the package +ament_package() + diff --git a/ov_init/package.xml b/ov_init/package.xml index 91b782a8c..7eee11350 100644 --- a/ov_init/package.xml +++ b/ov_init/package.xml @@ -1,11 +1,13 @@ - + + ov_init - 0.0.0 + 2.4.0 Initialization package which handles dynamic and static initialization. + https://github.com/rpng/open_vins Patrick Geneva @@ -14,25 +16,21 @@ GNU General Public License v3.0 - - catkin - - - cmake_modules - roscpp - rospy - geometry_msgs - nav_msgs - sensor_msgs - ov_core + + catkin + cmake_modules + roscpp + ov_core - - roscpp - rospy - geometry_msgs - nav_msgs - sensor_msgs - ov_core + + ament_cmake + rclcpp + ov_core + + + catkin + ament_cmake + \ No newline at end of file diff --git a/ov_init/src/test_orientation.cpp b/ov_init/src/test_orientation.cpp deleted file mode 100644 index 17232e2f4..000000000 --- a/ov_init/src/test_orientation.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* -* OpenVINS: An Open Platform for Visual-Inertial Research -* Copyright (C) 2021 Patrick Geneva -* Copyright (C) 2021 Guoquan Huang -* Copyright (C) 2021 OpenVINS Contributors -* Copyright (C) 2019 Kevin Eckenhoff -* -* This program 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. -* -* This program 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 this program. If not, see . -*/ - - - -#include - - - -int main(int argc, char **argv) { - - // Launch our ros node - ros::init(argc, argv, "test_orientation"); - ros::NodeHandle nh("~"); - - - -} \ No newline at end of file diff --git a/ov_init/src/test_static.cpp b/ov_init/src/test_static.cpp deleted file mode 100644 index 255760596..000000000 --- a/ov_init/src/test_static.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* -* OpenVINS: An Open Platform for Visual-Inertial Research -* Copyright (C) 2021 Patrick Geneva -* Copyright (C) 2021 Guoquan Huang -* Copyright (C) 2021 OpenVINS Contributors -* Copyright (C) 2019 Kevin Eckenhoff -* -* This program 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. -* -* This program 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 this program. If not, see . -*/ - - - -#include - - - -int main(int argc, char **argv) { - - // Launch our ros node - ros::init(argc, argv, "test_static"); - ros::NodeHandle nh("~"); - - - -} \ No newline at end of file diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 7c8b223a9..aad78764d 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -7,12 +7,6 @@ include(GNUInstallDirs) # Project name project(ov_msckf) -# Include our cmake files -#set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/) - -# Find catkin (the ROS build system) -find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init) - # Include libraries (if we don't have opencv 4, then fallback to opencv 3) # The OpenCV version needs to match the one used by cv_bridge otherwise you will get a segmentation fault! find_package(Eigen3 REQUIRED) @@ -32,20 +26,6 @@ else () add_definitions(-DENABLE_ARUCO_TAGS=1) endif () -# Describe catkin project -option(ENABLE_CATKIN_ROS "Enable or disable building with ROS (if it is found)" ON) -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - add_definitions(-DROS_AVAILABLE=1) - catkin_package( - CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init - INCLUDE_DIRS src - LIBRARIES ov_msckf_lib - ) -else () - add_definitions(-DROS_AVAILABLE=0) - message(WARNING "BUILDING WITHOUT ROS!") -endif () - # Try to compile with c++11 # http://stackoverflow.com/a/25836953 include(CheckCXXCompilerFlag) @@ -65,100 +45,21 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized -fno-omit-frame-pointer") -# Include our header files -include_directories( - src - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -# Set link libraries used by all binaries -list(APPEND thirdparty_libraries - ${Boost_LIBRARIES} - ${OpenCV_LIBRARIES} - ${catkin_LIBRARIES} -) - -# If we are not building with ROS then we need to manually link to its headers -# This isn't that elegant of a way, but this at least allows for building without ROS -# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 -if (NOT catkin_FOUND OR NOT ENABLE_CATKIN_ROS) - message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") - include_directories(${ov_core_SOURCE_DIR}/src/) - list(APPEND thirdparty_libraries ov_core_lib) - message(WARNING "MANUALLY LINKING TO OV_INIT LIBRARY....") - include_directories(${ov_init_SOURCE_DIR}/src/) - list(APPEND thirdparty_libraries ov_init_lib) -endif () - -################################################## -# Make the shared library -################################################## - -list(APPEND library_source_files - src/sim/Simulator.cpp - src/state/State.cpp - src/state/StateHelper.cpp - src/state/Propagator.cpp - src/core/VioManager.cpp - src/update/UpdaterHelper.cpp - src/update/UpdaterMSCKF.cpp - src/update/UpdaterSLAM.cpp - src/update/UpdaterZeroVelocity.cpp -) -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - list(APPEND library_source_files - src/ros/RosVisualizer.cpp - ) -endif () -add_library(ov_msckf_lib SHARED ${library_source_files}) -target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) -target_include_directories(ov_msckf_lib PUBLIC src) -install(TARGETS ov_msckf_lib - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) - - - -################################################## -# Make binary files! -################################################## - -if (catkin_FOUND AND ENABLE_CATKIN_ROS) - - add_executable(run_serial_msckf src/ros_serial_msckf.cpp) - target_link_libraries(run_serial_msckf ov_msckf_lib ${thirdparty_libraries}) - - add_executable(run_subscribe_msckf src/ros_subscribe_msckf.cpp) - target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) - - add_executable(run_simulation src/run_simulation.cpp) - target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) - -else () - - add_executable(run_simulation src/run_simulation.cpp) - target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) - -endif () - -add_executable(test_sim_meas src/test_sim_meas.cpp) -target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) -install(TARGETS test_sim_meas - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) +# Find our ROS version! +# NOTE: Default to using the ROS1 package if both are in our enviroment +# NOTE: https://github.com/romainreignier/share_ros1_ros2_lib_demo +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if(catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif(ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else() + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif() -add_executable(test_sim_repeat src/test_sim_repeat.cpp) -target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) -install(TARGETS test_sim_repeat - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake new file mode 100644 index 000000000..9a03207bb --- /dev/null +++ b/ov_msckf/cmake/ROS1.cmake @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (catkin_FOUND AND ENABLE_ROS) + add_definitions(-DROS_AVAILABLE=1) + catkin_package( + CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs image_transport cv_bridge ov_core ov_init + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src/ + LIBRARIES ov_msckf_lib + ) +else () + add_definitions(-DROS_AVAILABLE=0) + message(WARNING "BUILDING WITHOUT ROS!") +endif () + + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} + ${catkin_LIBRARIES} +) + +# If we are not building with ROS then we need to manually link to its headers +# This isn't that elegant of a way, but this at least allows for building without ROS +# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +if (NOT catkin_FOUND OR NOT ENABLE_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") + include_directories(${ov_core_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_core_lib) + message(WARNING "MANUALLY LINKING TO OV_INIT LIBRARY....") + include_directories(${ov_init_SOURCE_DIR}/src/) + list(APPEND thirdparty_libraries ov_init_lib) +endif () + +################################################## +# Make the shared library +################################################## + +list(APPEND library_source_files + src/sim/Simulator.cpp + src/state/State.cpp + src/state/StateHelper.cpp + src/state/Propagator.cpp + src/core/VioManager.cpp + src/update/UpdaterHelper.cpp + src/update/UpdaterMSCKF.cpp + src/update/UpdaterSLAM.cpp + src/update/UpdaterZeroVelocity.cpp +) +if (catkin_FOUND AND ENABLE_ROS) + list(APPEND library_source_files + src/ros/RosVisualizer.cpp + ) +endif () +add_library(ov_msckf_lib SHARED ${library_source_files}) +target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) +target_include_directories(ov_msckf_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_msckf_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +################################################## +# Make binary files! +################################################## + +if (catkin_FOUND AND ENABLE_ROS) + + add_executable(run_serial_msckf src/ros_serial_msckf.cpp) + target_link_libraries(run_serial_msckf ov_msckf_lib ${thirdparty_libraries}) + + add_executable(run_subscribe_msckf src/ros_subscribe_msckf.cpp) + target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) + +endif () + +add_executable(run_simulation src/run_simulation.cpp) +target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) + +add_executable(test_sim_meas src/test_sim_meas.cpp) +target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_meas + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +add_executable(test_sim_repeat src/test_sim_repeat.cpp) +target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_repeat + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + + + + + diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake new file mode 100644 index 000000000..34bfa1656 --- /dev/null +++ b/ov_msckf/cmake/ROS2.cmake @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.3) + +# Find ROS build system +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(ov_core REQUIRED) +find_package(ov_init REQUIRED) + +# Describe ROS project +option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) +if (NOT ENABLE_ROS) + message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") +endif () + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} +) + +################################################## +# Make the shared library +################################################## + +list(APPEND library_source_files + src/sim/Simulator.cpp + src/state/State.cpp + src/state/StateHelper.cpp + src/state/Propagator.cpp + src/core/VioManager.cpp + src/update/UpdaterHelper.cpp + src/update/UpdaterMSCKF.cpp + src/update/UpdaterSLAM.cpp + src/update/UpdaterZeroVelocity.cpp +) +# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! +#if (catkin_FOUND AND ENABLE_ROS) +# list(APPEND library_source_files +# src/ros/RosVisualizer.cpp +# ) +#endif () +add_library(ov_msckf_lib SHARED ${library_source_files}) +ament_target_dependencies(ov_msckf_lib rclcpp ov_core ov_init) +target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) +target_include_directories(ov_msckf_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS ov_msckf_lib + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) +ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/) +ament_export_libraries(ov_msckf_lib) + +################################################## +# Make binary files! +################################################## + +# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! +#if (catkin_FOUND AND ENABLE_ROS) +# +# add_executable(run_serial_msckf src/ros_serial_msckf.cpp) +# target_link_libraries(run_serial_msckf ov_msckf_lib ${thirdparty_libraries}) +# +# add_executable(run_subscribe_msckf src/ros_subscribe_msckf.cpp) +# target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) +# +#endif () + +add_executable(run_simulation src/run_simulation.cpp) +ament_target_dependencies(run_simulation rclcpp ov_core ov_init) +target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS run_simulation DESTINATION lib/${PROJECT_NAME}) + +add_executable(test_sim_meas src/test_sim_meas.cpp) +ament_target_dependencies(test_sim_meas rclcpp ov_core ov_init) +target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_meas DESTINATION lib/${PROJECT_NAME}) + +add_executable(test_sim_repeat src/test_sim_repeat.cpp) +ament_target_dependencies(test_sim_repeat rclcpp ov_core ov_init) +target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS test_sim_repeat DESTINATION lib/${PROJECT_NAME}) + +# finally define this as the package +ament_package() diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index 8143940dd..7ee39f6b7 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -113,6 +113,7 @@ sim_seed_preturb: 0 sim_seed_measurements: 0 sim_do_perturbation: false sim_traj_path: "/tmp/trajectory_not_set.txt" +#sim_traj_path: "/home/patrick/workspace/catkin_ws_init2/src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" sim_distance_threshold: 1.2 sim_freq_cam: 10 sim_freq_imu: 400 diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 2f0b36133..d082f9006 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -1,11 +1,13 @@ - + + ov_msckf - 1.0.0 + 2.4.0 - Implementation of the MSCKF. + Implementation of a type-based error-state Kalman filter. + https://github.com/rpng/open_vins Patrick Geneva @@ -15,39 +17,38 @@ GNU General Public License v3.0 - - catkin - - - cmake_modules - roscpp - rosbag - tf - std_msgs - geometry_msgs - sensor_msgs - nav_msgs - visualization_msgs - image_transport - cv_bridge - ov_core - ov_init - - - roscpp - rosbag - tf - std_msgs - geometry_msgs - sensor_msgs - nav_msgs - visualization_msgs - image_transport - cv_bridge - ov_core - ov_init - ov_eval - ov_data + + catkin + cmake_modules + roscpp + rosbag + tf + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + visualization_msgs + image_transport + cv_bridge + ov_core + ov_init + + + + + + + + + ament_cmake + rclcpp + ov_core + ov_init + + + catkin + ament_cmake + \ No newline at end of file diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 36dcfbef0..347ed1f09 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -414,7 +414,7 @@ struct VioManagerOptions { bool sim_do_perturbation = false; /// Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),ori(xyzw) format. - std::string sim_traj_path = "../ov_data/sim/udel_gore.txt"; + std::string sim_traj_path = "src/open_vins/ov_data/sim/udel_gore.txt"; /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in /// simulation. diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index 2e69b5305..57fd4106a 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -43,7 +43,7 @@ std::shared_ptr viz; int main(int argc, char **argv) { // Ensure we have a path, if the user passes it then we should use it - std::string config_path = "unset_path.txt"; + std::string config_path = "unset_path_to_config.yaml"; if (argc > 1) { config_path = argv[1]; } diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp index 388dce877..8000807c0 100644 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ b/ov_msckf/src/ros_subscribe_msckf.cpp @@ -51,7 +51,7 @@ void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs:: int main(int argc, char **argv) { // Ensure we have a path, if the user passes it then we should use it - std::string config_path = "unset_path.txt"; + std::string config_path = "unset_path_to_config.yaml"; if (argc > 1) { config_path = argv[1]; } diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 8e3c58c66..1f4e3e3fd 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -49,7 +49,7 @@ void signal_callback_handler(int signum) { std::exit(signum); } int main(int argc, char **argv) { // Ensure we have a path, if the user passes it then we should use it - std::string config_path = "unset_path.txt"; + std::string config_path = "unset_path_to_config.yaml"; if (argc > 1) { config_path = argv[1]; } diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp index 47e6e797b..5d824e350 100644 --- a/ov_msckf/src/test_sim_meas.cpp +++ b/ov_msckf/src/test_sim_meas.cpp @@ -45,14 +45,34 @@ void signal_callback_handler(int signum) { std::exit(signum); } // Main function int main(int argc, char **argv) { + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path_to_config.yaml"; + if (argc > 1) { + config_path = argv[1]; + } + #if ROS_AVAILABLE // Launch our ros node ros::init(argc, argv, "test_sim_meas"); ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); +#endif + + // Load the config + auto parser = std::make_shared(config_path); +#if ROS_AVAILABLE + parser->set_node_handler(nh); #endif + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + // Create the simulator VioManagerOptions params; + params.print_and_load(parser); + params.print_and_load_simulation(parser); Simulator sim(params); // Continue to simulate until we have processed all the measurements diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp index 5a1e5acfb..e3fe1f5aa 100644 --- a/ov_msckf/src/test_sim_repeat.cpp +++ b/ov_msckf/src/test_sim_repeat.cpp @@ -48,17 +48,37 @@ int main(int argc, char **argv) { // Register failure handler signal(SIGINT, signal_callback_handler); + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path_to_config.yaml"; + if (argc > 1) { + config_path = argv[1]; + } + #if ROS_AVAILABLE // Launch our ros node ros::init(argc, argv, "test_sim_repeat"); - ros::NodeHandle nh1("~"); + ros::NodeHandle nh("~"); + nh.param("config_path", config_path, config_path); +#endif + + // Load the config + auto parser = std::make_shared(config_path); +#if ROS_AVAILABLE + parser->set_node_handler(nh); #endif + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + //=================================================== //=================================================== // Create the simulator VioManagerOptions params1; + params1.print_and_load(parser); + params1.print_and_load_simulation(parser); Simulator sim1(params1); // Vector of stored measurements @@ -97,6 +117,8 @@ int main(int argc, char **argv) { // Create the simulator VioManagerOptions params2; + params2.print_and_load(parser); + params2.print_and_load_simulation(parser); Simulator sim2(params2); size_t ct_imu = 0; size_t ct_cam = 0; From 9beb92d2d43e472b14530065eec2cbf200be7a9d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 14 Nov 2021 23:42:49 -0500 Subject: [PATCH 20/55] fix header define names --- ov_core/src/utils/opencv_lambda_body.h | 6 +++--- ov_core/src/utils/print.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ov_core/src/utils/opencv_lambda_body.h b/ov_core/src/utils/opencv_lambda_body.h index 32b0cdcb7..cb8f97923 100644 --- a/ov_core/src/utils/opencv_lambda_body.h +++ b/ov_core/src/utils/opencv_lambda_body.h @@ -19,8 +19,8 @@ * along with this program. If not, see . */ -#ifndef OPENCV_LAMBDA_BODY_H -#define OPENCV_LAMBDA_BODY_H +#ifndef OV_CORE_OPENCV_LAMBDA_BODY_H +#define OV_CORE_OPENCV_LAMBDA_BODY_H #include #include @@ -45,4 +45,4 @@ class LambdaBody : public cv::ParallelLoopBody { } /* namespace ov_core */ -#endif /* OPENCV_LAMBDA_BODY_H */ +#endif /* OV_CORE_OPENCV_LAMBDA_BODY_H */ diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h index 30d5add17..b30f054f8 100644 --- a/ov_core/src/utils/print.h +++ b/ov_core/src/utils/print.h @@ -19,8 +19,8 @@ * along with this program. If not, see . */ -#ifndef PRINT_H -#define PRINT_H +#ifndef OV_CORE_PRINT_H +#define OV_CORE_PRINT_H #include #include @@ -99,4 +99,4 @@ class Printer { #define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x); #define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x); -#endif /* PRINT_H */ +#endif /* OV_CORE_PRINT_H */ From e4e17bf0b39a5022b7c9840d2130802bb54e1a28 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 15 Nov 2021 01:50:47 -0500 Subject: [PATCH 21/55] require c++14 to compile (required for ROS2), actually load from ROS2 params --- ov_core/CMakeLists.txt | 17 +- ov_core/cmake/ROS2.cmake | 2 + ov_core/src/test_webcam.cpp | 6 +- ov_core/src/utils/opencv_yaml_parse.h | 244 ++++++++++++------ ov_eval/CMakeLists.txt | 17 +- ov_eval/cmake/ROS2.cmake | 1 + ov_init/CMakeLists.txt | 17 +- ov_init/cmake/ROS2.cmake | 1 + ov_msckf/CMakeLists.txt | 17 +- ov_msckf/cmake/ROS1.cmake | 2 +- ov_msckf/cmake/ROS2.cmake | 1 + .../config/rpng_sim/estimator_config.yaml | 4 +- .../{RosVisualizer.cpp => ROS1Visualizer.cpp} | 20 +- .../ros/{RosVisualizer.h => ROS1Visualizer.h} | 10 +- ov_msckf/src/ros/RosVisualizerHelper.h | 4 +- ov_msckf/src/ros_serial_msckf.cpp | 6 +- ov_msckf/src/ros_subscribe_msckf.cpp | 6 +- ov_msckf/src/run_simulation.cpp | 39 ++- ov_msckf/src/test_sim_meas.cpp | 6 +- ov_msckf/src/test_sim_repeat.cpp | 6 +- 20 files changed, 248 insertions(+), 178 deletions(-) rename ov_msckf/src/ros/{RosVisualizer.cpp => ROS1Visualizer.cpp} (98%) rename ov_msckf/src/ros/{RosVisualizer.h => ROS1Visualizer.h} (95%) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index 08fd0c916..e1f4c7455 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -37,18 +37,11 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Try to compile with c++11 -# http://stackoverflow.com/a/25836953 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if (COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif (COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else () - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif () +# We need c++14 for ROS2, thus just require it for everybody +# NOTE: To future self, hope this isn't an issue... +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake index bf1289c9c..3516dedbe 100644 --- a/ov_core/cmake/ROS2.cmake +++ b/ov_core/cmake/ROS2.cmake @@ -9,6 +9,7 @@ option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) if (NOT ENABLE_ROS) message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") endif () +add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( @@ -42,6 +43,7 @@ add_library(ov_core_lib SHARED src/feat/FeatureInitializer.cpp src/utils/print.cpp ) +ament_target_dependencies(ov_core_lib rclcpp) target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) install(TARGETS ov_core_lib diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 569cc091d..bc0165c19 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -40,7 +40,7 @@ #include "utils/print.h" #include "utils/opencv_yaml_parse.h" -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 #include #endif @@ -58,7 +58,7 @@ int main(int argc, char **argv) { config_path = argv[1]; } -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 // Initialize this as a ROS node ros::init(argc, argv, "test_webcam"); ros::NodeHandle nh("~"); @@ -67,7 +67,7 @@ int main(int argc, char **argv) { // Load parameters auto parser = std::make_shared(config_path, false); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 parser->set_node_handler(nh); #endif diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index bf1de9c52..a26ca8eaf 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -24,10 +24,13 @@ #include #include +#include #include -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 #include +#elif ROS_AVAILABLE == 2 +#include #endif #include "colors.h" @@ -79,11 +82,16 @@ class YamlParser { } } -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 /// Allows setting of the node handler if we have ROS to override parameters void set_node_handler(ros::NodeHandle &nh_) { this->nh = nh_; } #endif +#if ROS_AVAILABLE == 2 + /// Allows setting of the node if we have ROS to override parameters + void set_node(std::shared_ptr &node_) { this->node = node_; } +#endif + /** * @brief Will get the folder this config file is in * @return Config folder @@ -109,27 +117,22 @@ class YamlParser { */ template void parse_config(const std::string &node_name, T &node_result, bool required = true) { -#if ROS_AVAILABLE - // If we have the ROS parameter, we should just get that one +#if ROS_AVAILABLE == 1 if (nh.getParam(node_name, node_result)) { nh.param(node_name, node_result); PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); return; } -#endif - - // Directly return if the config hasn't been opened - if (config == nullptr) +#elif ROS_AVAILABLE == 2 + if (node != nullptr && node->has_parameter(node_name)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); + node->get_parameter(node_name, node_result); return; - - // Else lets get the one from the config - try { - parse(config->root(), node_name, node_result, required); - } catch (...) { - PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), - typeid(node_result).name()); - all_params_found_successfully = false; } +#endif + + // Else we just parse from the YAML file! + parse_config_yaml(node_name, node_result, required); } /** @@ -151,51 +154,24 @@ class YamlParser { void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, T &node_result, bool required = true) { -#if ROS_AVAILABLE - // If we have the ROS parameter, we should just get that one +#if ROS_AVAILABLE == 1 std::string rosnode = sensor_name + "_" + node_name; if (nh.getParam(rosnode, node_result)) { nh.param(rosnode, node_result); PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); return; } -#endif - - // Directly return if the config hasn't been opened - if (config == nullptr) - return; - - // Create the path the external yaml file - std::string path; - if (!node_found(config->root(), external_node_name)) { - PRINT_ERROR(RED "the external node %s could not be found!\n" RESET, external_node_name.c_str()); - std::exit(EXIT_FAILURE); - } - (*config)[external_node_name] >> path; - std::string relative_folder = get_config_folder(); - - // Now actually try to load them from file! - auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); - if (!config_external->isOpened()) { - PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str()); - std::exit(EXIT_FAILURE); - } - - // Check that we have the requested node - if (!node_found(config_external->root(), sensor_name)) { - PRINT_WARNING(YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(), typeid(node_result).name()); - all_params_found_successfully = false; +#elif ROS_AVAILABLE == 2 + std::string rosnode = sensor_name + "_" + node_name; + if (node != nullptr && node->has_parameter(rosnode)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + node->get_parameter(rosnode, node_result); return; } +#endif - // Else lets get it! - try { - parse((*config_external)[sensor_name], node_name, node_result, required); - } catch (...) { - PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(), - typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str()); - all_params_found_successfully = false; - } + // Else we just parse from the YAML file! + parse_external_yaml(external_node_name, sensor_name, node_name, node_result, required); } /** @@ -215,7 +191,7 @@ class YamlParser { void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, Eigen::Matrix4d &node_result, bool required = true) { -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 // If we have the ROS parameter, we should just get that one // NOTE: for our 4x4 matrix we should read it as an array from ROS then covert it back into the 4x4 std::string rosnode = sensor_name + "_" + node_name; @@ -228,44 +204,57 @@ class YamlParser { matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15); return; } -#endif - - // Directly return if the config hasn't been opened - if (config == nullptr) +#elif ROS_AVAILABLE == 2 + // If we have the ROS parameter, we should just get that one + // NOTE: for our 4x4 matrix we should read it as an array from ROS then covert it back into the 4x4 + std::string rosnode = sensor_name + "_" + node_name; + std::vector matrix_TCtoI; + if (node != nullptr && node->has_parameter(rosnode)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + node->get_parameter>(rosnode, matrix_TCtoI); + node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5), + matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11), + matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15); return; - - // Create the path the external yaml file - std::string path; - if (!node_found(config->root(), external_node_name)) { - PRINT_ERROR(RED "the external node %s could not be found!\n" RESET, external_node_name.c_str()); - std::exit(EXIT_FAILURE); } - (*config)[external_node_name] >> path; - std::string relative_folder = config_path_.substr(0, config_path_.find_last_of('/')) + "/"; +#endif - // Now actually try to load them from file! - auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); - if (!config_external->isOpened()) { - PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str()); - std::exit(EXIT_FAILURE); - } + // Else we just parse from the YAML file! + parse_external_yaml(external_node_name, sensor_name, node_name, node_result, required); + } - // Check that we have the requested node - if (!node_found(config_external->root(), sensor_name)) { - PRINT_WARNING(YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(), typeid(node_result).name()); - all_params_found_successfully = false; +#if ROS_AVAILABLE == 2 + /// For ROS2 we need to override the int since it seems to only support int64_t types + /// https://docs.ros2.org/bouncy/api/rclcpp/classrclcpp_1_1_parameter.html + void parse_config(const std::string &node_name, int &node_result, bool required = true) { + int64_t val = node_result; + if (node != nullptr && node->has_parameter(node_name)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); + node->get_parameter(node_name, val); + node_result = (int)val; return; } - - // Else lets get it! - try { - parse((*config_external)[sensor_name], node_name, node_result, required); - } catch (...) { - PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(), - typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str()); - all_params_found_successfully = false; + parse_config_yaml(node_name, node_result, required); + } + /// For ROS2 we need to override the int since it seems to only support int64_t types + /// https://docs.ros2.org/bouncy/api/rclcpp/classrclcpp_1_1_parameter.html + void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, + std::vector &node_result, bool required = true) { + std::vector val; + for (auto tmp : node_result) + val.push_back(tmp); + std::string rosnode = sensor_name + "_" + node_name; + if (node != nullptr && node->has_parameter(rosnode)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + node->get_parameter>(rosnode, val); + node_result.clear(); + for (auto tmp : val) + node_result.push_back((int)tmp); + return; } + parse_external_yaml(external_node_name, sensor_name, node_name, node_result, required); } +#endif private: /// Path to the config file @@ -277,11 +266,16 @@ class YamlParser { /// Record if all parameters were found bool all_params_found_successfully = true; -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 /// Ros node handler that will override values ros::NodeHandle nh; #endif +#if ROS_AVAILABLE == 2 + /// Our rclcpp node pointer + std::shared_ptr node = nullptr; +#endif + /** * @brief Given a YAML node object, this check to see if we have a valid key * @param file_node OpenCV file node we will get the data from @@ -448,6 +442,88 @@ class YamlParser { node_result = ov_core::Inv_se3(tmp); } } + + /** + * @brief Custom parser for the ESTIMATOR parameters. + * + * This will load the data from the main config file. + * If it is unable it will give a warning to the user it couldn't be found. + * + * @tparam T Type of parameter we are looking for. + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + template void parse_config_yaml(const std::string &node_name, T &node_result, bool required = true) { + + // Directly return if the config hasn't been opened + if (config == nullptr) + return; + + // Else lets get the one from the config + try { + parse(config->root(), node_name, node_result, required); + } catch (...) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } + } + + /** + * @brief Custom parser for the EXTERNAL parameter files with levels. + * + * This will first load the external file requested. + * From there it will try to find the first level requested (e.g. imu0, cam0, cam1). + * Then the requested node can be found under this sensor name. + * ROS can override the config with `_` (e.g., cam0_distortion). + * + * @tparam T Type of parameter we are looking for. + * @param external_node_name Name of the node we will get our relative path from + * @param sensor_name The first level node we will try to get the requested node under + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + template + void parse_external_yaml(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, + T &node_result, bool required = true) { + // Directly return if the config hasn't been opened + if (config == nullptr) + return; + + // Create the path the external yaml file + std::string path; + if (!node_found(config->root(), external_node_name)) { + PRINT_ERROR(RED "the external node %s could not be found!\n" RESET, external_node_name.c_str()); + std::exit(EXIT_FAILURE); + } + (*config)[external_node_name] >> path; + std::string relative_folder = config_path_.substr(0, config_path_.find_last_of('/')) + "/"; + + // Now actually try to load them from file! + auto config_external = std::make_shared(relative_folder + path, cv::FileStorage::READ); + if (!config_external->isOpened()) { + PRINT_ERROR(RED "unable to open the configuration file!\n%s\n" RESET, (relative_folder + path).c_str()); + std::exit(EXIT_FAILURE); + } + + // Check that we have the requested node + if (!node_found(config_external->root(), sensor_name)) { + PRINT_WARNING(YELLOW "the sensor %s of type [%s] was not found...\n" RESET, sensor_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + return; + } + + // Else lets get it! + try { + parse((*config_external)[sensor_name], node_name, node_result, required); + } catch (...) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in [%s] in the external %s config file!\n" RESET, node_name.c_str(), + typeid(node_result).name(), sensor_name.c_str(), external_node_name.c_str()); + all_params_found_successfully = false; + } + } }; } /* namespace ov_core */ diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 11e00fd7e..f7a3c5fee 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -22,18 +22,11 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Try to compile with c++11 -# http://stackoverflow.com/a/25836953 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if (COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif (COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else () - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif () +# We need c++14 for ROS2, thus just require it for everybody +# NOTE: To future self, hope this isn't an issue... +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake index 9d3e99b6c..91d3d1bdc 100644 --- a/ov_eval/cmake/ROS2.cmake +++ b/ov_eval/cmake/ROS2.cmake @@ -10,6 +10,7 @@ option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) if (NOT ENABLE_ROS) message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") endif () +add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 53890e8fb..8f55af339 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -22,18 +22,11 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) endif () -# Try to compile with c++11 -# http://stackoverflow.com/a/25836953 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if (COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif (COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else () - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif () +# We need c++14 for ROS2, thus just require it for everybody +# NOTE: To future self, hope this isn't an issue... +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake index 2b3c46abc..dbc6edfbf 100644 --- a/ov_init/cmake/ROS2.cmake +++ b/ov_init/cmake/ROS2.cmake @@ -10,6 +10,7 @@ option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) if (NOT ENABLE_ROS) message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") endif () +add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index aad78764d..dd24355a9 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -26,18 +26,11 @@ else () add_definitions(-DENABLE_ARUCO_TAGS=1) endif () -# Try to compile with c++11 -# http://stackoverflow.com/a/25836953 -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if (COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif (COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") -else () - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif () +# We need c++14 for ROS2, thus just require it for everybody +# NOTE: To future self, hope this isn't an issue... +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 9a03207bb..c476ed609 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -62,7 +62,7 @@ list(APPEND library_source_files ) if (catkin_FOUND AND ENABLE_ROS) list(APPEND library_source_files - src/ros/RosVisualizer.cpp + src/ros/ROS1Visualizer.cpp ) endif () add_library(ov_msckf_lib SHARED ${library_source_files}) diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index 34bfa1656..b9ef2e34b 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -11,6 +11,7 @@ option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) if (NOT ENABLE_ROS) message(FATAL_ERROR "Build with ROS1.cmake if you don't have ROS.") endif () +add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/ov_msckf/config/rpng_sim/estimator_config.yaml index 7ee39f6b7..ffed39420 100644 --- a/ov_msckf/config/rpng_sim/estimator_config.yaml +++ b/ov_msckf/config/rpng_sim/estimator_config.yaml @@ -112,8 +112,8 @@ sim_seed_state_init: 0 sim_seed_preturb: 0 sim_seed_measurements: 0 sim_do_perturbation: false -sim_traj_path: "/tmp/trajectory_not_set.txt" -#sim_traj_path: "/home/patrick/workspace/catkin_ws_init2/src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" +#sim_traj_path: "/tmp/trajectory_not_set.txt" +sim_traj_path: "/home/patrick/workspace/catkin_ws_init2/src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" sim_distance_threshold: 1.2 sim_freq_cam: 10 sim_freq_imu: 400 diff --git a/ov_msckf/src/ros/RosVisualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp similarity index 98% rename from ov_msckf/src/ros/RosVisualizer.cpp rename to ov_msckf/src/ros/ROS1Visualizer.cpp index b6435a61d..48f3eb2d3 100644 --- a/ov_msckf/src/ros/RosVisualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -19,13 +19,13 @@ * along with this program. If not, see . */ -#include "RosVisualizer.h" +#include "ROS1Visualizer.h" using namespace ov_core; using namespace ov_type; using namespace ov_msckf; -RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim) : _app(app), _sim(sim) { +ROS1Visualizer::ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim) : _app(app), _sim(sim) { // Setup our transform broadcaster mTfBr = new tf::TransformBroadcaster(); @@ -117,7 +117,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap } } -void RosVisualizer::visualize() { +void ROS1Visualizer::visualize() { // Return if we have already visualized if (last_visualization_timestamp == _app->get_state()->_timestamp && _app->initialized()) @@ -164,7 +164,7 @@ void RosVisualizer::visualize() { PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); } -void RosVisualizer::visualize_odometry(double timestamp) { +void ROS1Visualizer::visualize_odometry(double timestamp) { // Check if we have subscribers if (pub_odomimu.getNumSubscribers() == 0) @@ -233,7 +233,7 @@ void RosVisualizer::visualize_odometry(double timestamp) { pub_odomimu.publish(odomIinM); } -void RosVisualizer::visualize_final() { +void ROS1Visualizer::visualize_final() { // Final time offset value if (_app->get_state()->_options.do_calib_camera_timeoffset) { @@ -284,7 +284,7 @@ void RosVisualizer::visualize_final() { PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); } -void RosVisualizer::publish_state() { +void ROS1Visualizer::publish_state() { // Get the current state std::shared_ptr state = _app->get_state(); @@ -364,7 +364,7 @@ void RosVisualizer::publish_state() { } } -void RosVisualizer::publish_images() { +void ROS1Visualizer::publish_images() { // Check if we have subscribers if (it_pub_tracks.getNumSubscribers() == 0) @@ -382,7 +382,7 @@ void RosVisualizer::publish_images() { it_pub_tracks.publish(exl_msg); } -void RosVisualizer::publish_features() { +void ROS1Visualizer::publish_features() { // Check if we have subscribers if (pub_points_msckf.getNumSubscribers() == 0 && pub_points_slam.getNumSubscribers() == 0 && pub_points_aruco.getNumSubscribers() == 0 && @@ -414,7 +414,7 @@ void RosVisualizer::publish_features() { pub_points_sim.publish(cloud_SIM); } -void RosVisualizer::publish_groundtruth() { +void ROS1Visualizer::publish_groundtruth() { // Our groundtruth state Eigen::Matrix state_gt; @@ -537,7 +537,7 @@ void RosVisualizer::publish_groundtruth() { //========================================================================== } -void RosVisualizer::publish_loopclosure_information() { +void ROS1Visualizer::publish_loopclosure_information() { // Get the current tracks in this frame double active_tracks_time1 = -1; diff --git a/ov_msckf/src/ros/RosVisualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h similarity index 95% rename from ov_msckf/src/ros/RosVisualizer.h rename to ov_msckf/src/ros/ROS1Visualizer.h index 4c46312b9..7a981147a 100644 --- a/ov_msckf/src/ros/RosVisualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -19,8 +19,8 @@ * along with this program. If not, see . */ -#ifndef OV_MSCKF_ROSVISUALIZER_H -#define OV_MSCKF_ROSVISUALIZER_H +#ifndef OV_MSCKF_ROS1VISUALIZER_H +#define OV_MSCKF_ROS1VISUALIZER_H #include #include @@ -59,7 +59,7 @@ namespace ov_msckf { * - Our different features (SLAM, MSCKF, ARUCO) * - Groundtruth trajectory if we have it */ -class RosVisualizer { +class ROS1Visualizer { public: /** @@ -68,7 +68,7 @@ class RosVisualizer { * @param app Core estimator manager * @param sim Simulator if we are simulating */ - RosVisualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim = nullptr); + ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim = nullptr); /** * @brief Will visualize the system if we have new things @@ -151,4 +151,4 @@ class RosVisualizer { } // namespace ov_msckf -#endif // OV_MSCKF_ROSVISUALIZER_H +#endif // OV_MSCKF_ROS1VISUALIZER_H diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 9c41fb2d0..b97e610fc 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -38,6 +38,7 @@ namespace ov_msckf { class RosVisualizerHelper { public: +#if ROS_AVAILABLE == 1 /** * @brief Will visualize the system if we have new things * @param feats Vector of features we will convert into ros format @@ -106,6 +107,7 @@ class RosVisualizerHelper { trans.setOrigin(orig); return trans; } +#endif /** * @brief Save current estimate state and groundtruth including calibration @@ -116,7 +118,7 @@ class RosVisualizerHelper { * @param of_state_gt Output file for groundtruth (if we have it from sim) */ static void sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, std::ofstream &of_state_est, - std::ofstream &of_state_std, std::ofstream &of_state_gt) { + std::ofstream &of_state_std, std::ofstream &of_state_gt) { // We want to publish in the IMU clock frame // The timestamp in the state will be the last camera time diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros_serial_msckf.cpp index 57fd4106a..d9f5beadc 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros_serial_msckf.cpp @@ -30,14 +30,14 @@ #include "core/VioManager.h" #include "core/VioManagerOptions.h" -#include "ros/RosVisualizer.h" +#include "ros/ROS1Visualizer.h" #include "utils/dataset_reader.h" #include "utils/sensor_data.h" using namespace ov_msckf; std::shared_ptr sys; -std::shared_ptr viz; +std::shared_ptr viz; // Main function int main(int argc, char **argv) { @@ -66,7 +66,7 @@ int main(int argc, char **argv) { VioManagerOptions params; params.print_and_load(parser); sys = std::make_shared(params); - viz = std::make_shared(nh, sys); + viz = std::make_shared(nh, sys); // Ensure we read in all parameters required if (!parser->successful()) { diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp index 8000807c0..0625be363 100644 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ b/ov_msckf/src/ros_subscribe_msckf.cpp @@ -33,14 +33,14 @@ #include "core/VioManager.h" #include "core/VioManagerOptions.h" -#include "ros/RosVisualizer.h" +#include "ros/ROS1Visualizer.h" #include "utils/dataset_reader.h" #include "utils/sensor_data.h" using namespace ov_msckf; std::shared_ptr sys; -std::shared_ptr viz; +std::shared_ptr viz; // Callback functions void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); @@ -75,7 +75,7 @@ int main(int argc, char **argv) { VioManagerOptions params; params.print_and_load(parser); sys = std::make_shared(params); - viz = std::make_shared(nh, sys); + viz = std::make_shared(nh, sys); // Ensure we read in all parameters required if (!parser->successful()) { diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 1f4e3e3fd..9ea19affe 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -29,17 +29,19 @@ #include "utils/print.h" #include "utils/sensor_data.h" -#if ROS_AVAILABLE -#include "ros/RosVisualizer.h" +#if ROS_AVAILABLE == 1 +#include "ros/ROS1Visualizer.h" #include +#elif ROS_AVAILABLE == 2 +#include #endif using namespace ov_msckf; std::shared_ptr sim; std::shared_ptr sys; -#if ROS_AVAILABLE -std::shared_ptr viz; +#if ROS_AVAILABLE == 1 +std::shared_ptr viz; #endif // Define the function to be called when ctrl-c (SIGINT) is sent to process @@ -54,17 +56,28 @@ int main(int argc, char **argv) { config_path = argv[1]; } -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "run_simulation"); ros::NodeHandle nh("~"); nh.param("config_path", config_path, config_path); +#elif ROS_AVAILABLE == 2 + // Launch our ros node + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("run_simulation", options); + //node->declare_parameter("config_path", config_path); + node->get_parameter("config_path", config_path); #endif // Load the config auto parser = std::make_shared(config_path); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 parser->set_node_handler(nh); +#elif ROS_AVAILABLE == 2 + parser->set_node(node); #endif // Verbosity @@ -78,8 +91,8 @@ int main(int argc, char **argv) { params.print_and_load_simulation(parser); sim = std::make_shared(params); sys = std::make_shared(params); -#if ROS_AVAILABLE - viz = std::make_shared(nh, sys, sim); +#if ROS_AVAILABLE == 1 + viz = std::make_shared(nh, sys, sim); #endif // Ensure we read in all parameters required @@ -119,8 +132,10 @@ int main(int argc, char **argv) { // Step through the rosbag signal(SIGINT, signal_callback_handler); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 while (sim->ok() && ros::ok()) { +#elif ROS_AVAILABLE == 2 + while (sim->ok() && rclcpp::ok()) { #else while (sim->ok()) { #endif @@ -130,7 +145,7 @@ int main(int argc, char **argv) { bool hasimu = sim->get_next_imu(message_imu.timestamp, message_imu.wm, message_imu.am); if (hasimu) { sys->feed_measurement_imu(message_imu); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 viz->visualize_odometry(message_imu.timestamp); #endif } @@ -143,7 +158,7 @@ int main(int argc, char **argv) { if (hascam) { if (buffer_timecam != -1) { sys->feed_measurement_simulation(buffer_timecam, buffer_camids, buffer_feats); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 viz->visualize(); #endif } @@ -158,7 +173,7 @@ int main(int argc, char **argv) { //=================================================================================== // Final visualization -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 viz->visualize_final(); #endif diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp index 5d824e350..86ff803bc 100644 --- a/ov_msckf/src/test_sim_meas.cpp +++ b/ov_msckf/src/test_sim_meas.cpp @@ -30,7 +30,7 @@ #include #include -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 #include #endif @@ -51,7 +51,7 @@ int main(int argc, char **argv) { config_path = argv[1]; } -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "test_sim_meas"); ros::NodeHandle nh("~"); @@ -60,7 +60,7 @@ int main(int argc, char **argv) { // Load the config auto parser = std::make_shared(config_path); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 parser->set_node_handler(nh); #endif diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp index e3fe1f5aa..aedaa8d2e 100644 --- a/ov_msckf/src/test_sim_repeat.cpp +++ b/ov_msckf/src/test_sim_repeat.cpp @@ -29,7 +29,7 @@ #include #include -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 #include #endif @@ -54,7 +54,7 @@ int main(int argc, char **argv) { config_path = argv[1]; } -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "test_sim_repeat"); ros::NodeHandle nh("~"); @@ -63,7 +63,7 @@ int main(int argc, char **argv) { // Load the config auto parser = std::make_shared(config_path); -#if ROS_AVAILABLE +#if ROS_AVAILABLE == 1 parser->set_node_handler(nh); #endif From 35db4fa7b3254b6912de185ea4c7826cfc381981 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 15 Nov 2021 14:32:18 -0500 Subject: [PATCH 22/55] switch the sharedptr for nodehandler, move subscriber logic into ROS1 viz class --- ov_core/src/test_tracking.cpp | 16 +- ov_core/src/test_webcam.cpp | 4 +- ov_core/src/utils/opencv_yaml_parse.h | 20 +- ov_msckf/cmake/ROS1.cmake | 8 +- ov_msckf/cmake/ROS2.cmake | 8 +- .../config/euroc_mav/estimator_config.yaml | 2 +- ov_msckf/launch/serial.launch | 4 +- ov_msckf/launch/subscribe.launch | 6 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 182 +++++++++++-- ov_msckf/src/ros/ROS1Visualizer.h | 31 ++- ...serial_msckf.cpp => ros1_serial_msckf.cpp} | 101 ++------ ov_msckf/src/ros1_subscribe_msckf.cpp | 83 ++++++ ov_msckf/src/ros_subscribe_msckf.cpp | 239 ------------------ ov_msckf/src/run_simulation.cpp | 4 +- ov_msckf/src/test_sim_meas.cpp | 4 +- ov_msckf/src/test_sim_repeat.cpp | 4 +- 16 files changed, 329 insertions(+), 387 deletions(-) rename ov_msckf/src/{ros_serial_msckf.cpp => ros1_serial_msckf.cpp} (72%) create mode 100644 ov_msckf/src/ros1_subscribe_msckf.cpp delete mode 100644 ov_msckf/src/ros_subscribe_msckf.cpp diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index ef600eccc..eae190a4f 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -70,8 +70,8 @@ int main(int argc, char **argv) { // Initialize this as a ROS node ros::init(argc, argv, "test_tracking"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); // Load parameters auto parser = std::make_shared(config_path, false); @@ -84,22 +84,22 @@ int main(int argc, char **argv) { // Our camera topics (left and right stereo) std::string topic_camera0, topic_camera1; - nh.param("topic_camera0", topic_camera0, "/cam0/image_raw"); - nh.param("topic_camera1", topic_camera1, "/cam1/image_raw"); + nh->param("topic_camera0", topic_camera0, "/cam0/image_raw"); + nh->param("topic_camera1", topic_camera1, "/cam1/image_raw"); parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", topic_camera0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", topic_camera1); // Location of the ROS bag we want to read in std::string path_to_bag; - nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); - // nh.param("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag"); + nh->param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); + // nh->param("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag"); PRINT_INFO("ros bag path is: %s\n", path_to_bag.c_str()); // Get our start location and how much of the bag we want to play // Make the bag duration < 0 to just process to the end of the bag double bag_start, bag_durr; - nh.param("bag_start", bag_start, 0); - nh.param("bag_durr", bag_durr, -1); + nh->param("bag_start", bag_start, 0); + nh->param("bag_durr", bag_durr, -1); //=================================================================================== //=================================================================================== diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index bc0165c19..7c7ae1175 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -61,8 +61,8 @@ int main(int argc, char **argv) { #if ROS_AVAILABLE == 1 // Initialize this as a ROS node ros::init(argc, argv, "test_webcam"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); #endif // Load parameters diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index a26ca8eaf..360fd4e32 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -84,7 +84,7 @@ class YamlParser { #if ROS_AVAILABLE == 1 /// Allows setting of the node handler if we have ROS to override parameters - void set_node_handler(ros::NodeHandle &nh_) { this->nh = nh_; } + void set_node_handler(std::shared_ptr nh_) { this->nh = nh_; } #endif #if ROS_AVAILABLE == 2 @@ -118,8 +118,8 @@ class YamlParser { template void parse_config(const std::string &node_name, T &node_result, bool required = true) { #if ROS_AVAILABLE == 1 - if (nh.getParam(node_name, node_result)) { - nh.param(node_name, node_result); + if (nh->getParam(node_name, node_result)) { + nh->param(node_name, node_result); PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); return; } @@ -156,8 +156,8 @@ class YamlParser { #if ROS_AVAILABLE == 1 std::string rosnode = sensor_name + "_" + node_name; - if (nh.getParam(rosnode, node_result)) { - nh.param(rosnode, node_result); + if (nh->getParam(rosnode, node_result)) { + nh->param(rosnode, node_result); PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); return; } @@ -196,8 +196,8 @@ class YamlParser { // NOTE: for our 4x4 matrix we should read it as an array from ROS then covert it back into the 4x4 std::string rosnode = sensor_name + "_" + node_name; std::vector matrix_TCtoI; - if (nh.getParam(rosnode, matrix_TCtoI)) { - nh.param>(rosnode, matrix_TCtoI); + if (nh->getParam(rosnode, matrix_TCtoI)) { + nh->param>(rosnode, matrix_TCtoI); PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5), matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11), @@ -267,12 +267,12 @@ class YamlParser { bool all_params_found_successfully = true; #if ROS_AVAILABLE == 1 - /// Ros node handler that will override values - ros::NodeHandle nh; + /// ROS1 node handler that will override values + std::shared_ptr nh; #endif #if ROS_AVAILABLE == 2 - /// Our rclcpp node pointer + /// Our ROS2 rclcpp node pointer std::shared_ptr node = nullptr; #endif diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index c476ed609..ea160835d 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -80,11 +80,11 @@ install(TARGETS ov_msckf_lib if (catkin_FOUND AND ENABLE_ROS) - add_executable(run_serial_msckf src/ros_serial_msckf.cpp) - target_link_libraries(run_serial_msckf ov_msckf_lib ${thirdparty_libraries}) + add_executable(ros1_serial_msckf src/ros1_serial_msckf.cpp) + target_link_libraries(ros1_serial_msckf ov_msckf_lib ${thirdparty_libraries}) - add_executable(run_subscribe_msckf src/ros_subscribe_msckf.cpp) - target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) + add_executable(ros1_subscribe_msckf src/ros1_subscribe_msckf.cpp) + target_link_libraries(ros1_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) endif () diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index b9ef2e34b..0791c37b2 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -66,11 +66,11 @@ ament_export_libraries(ov_msckf_lib) # TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! #if (catkin_FOUND AND ENABLE_ROS) # -# add_executable(run_serial_msckf src/ros_serial_msckf.cpp) -# target_link_libraries(run_serial_msckf ov_msckf_lib ${thirdparty_libraries}) +# add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp) +# target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries}) # -# add_executable(run_subscribe_msckf src/ros_subscribe_msckf.cpp) -# target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) +# add_executable(ros2_subscribe_msckf src/ros2_subscribe_msckf.cpp) +# target_link_libraries(ros2_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) # #endif () diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/ov_msckf/config/euroc_mav/estimator_config.yaml index 9cf61059c..f73586437 100644 --- a/ov_msckf/config/euroc_mav/estimator_config.yaml +++ b/ov_msckf/config/euroc_mav/estimator_config.yaml @@ -46,7 +46,7 @@ init_state_bias_accel: true init_enforce_gravity_magnitude: true init_camera_rate: 10.0 -init_window_time: 1.5 +init_window_time: 0.75 #1.5 init_imu_thresh: 1.5 init_max_disparity: 1.5 init_max_features: 25.0 diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index f5d40e012..77315395e 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -24,8 +24,8 @@ - - + + diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index f7add9630..29f6770f2 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -10,7 +10,7 @@ - + @@ -24,8 +24,8 @@ - - + + diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 48f3eb2d3..40e721406 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -25,30 +25,31 @@ using namespace ov_core; using namespace ov_type; using namespace ov_msckf; -ROS1Visualizer::ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim) : _app(app), _sim(sim) { +ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ptr app, std::shared_ptr sim) + : _nh(nh), _app(app), _sim(sim) { // Setup our transform broadcaster mTfBr = new tf::TransformBroadcaster(); // Create image transport - image_transport::ImageTransport it(nh); + image_transport::ImageTransport it(*_nh); // Setup pose and path publisher - pub_poseimu = nh.advertise("/ov_msckf/poseimu", 2); + pub_poseimu = nh->advertise("/ov_msckf/poseimu", 2); PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str()); - pub_odomimu = nh.advertise("/ov_msckf/odomimu", 2); + pub_odomimu = nh->advertise("/ov_msckf/odomimu", 2); PRINT_DEBUG("Publishing: %s\n", pub_odomimu.getTopic().c_str()); - pub_pathimu = nh.advertise("/ov_msckf/pathimu", 2); + pub_pathimu = nh->advertise("/ov_msckf/pathimu", 2); PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str()); // 3D points publishing - pub_points_msckf = nh.advertise("/ov_msckf/points_msckf", 2); + pub_points_msckf = nh->advertise("/ov_msckf/points_msckf", 2); PRINT_DEBUG("Publishing: %s\n", pub_points_msckf.getTopic().c_str()); - pub_points_slam = nh.advertise("/ov_msckf/points_slam", 2); + pub_points_slam = nh->advertise("/ov_msckf/points_slam", 2); PRINT_DEBUG("Publishing: %s\n", pub_points_msckf.getTopic().c_str()); - pub_points_aruco = nh.advertise("/ov_msckf/points_aruco", 2); + pub_points_aruco = nh->advertise("/ov_msckf/points_aruco", 2); PRINT_DEBUG("Publishing: %s\n", pub_points_aruco.getTopic().c_str()); - pub_points_sim = nh.advertise("/ov_msckf/points_sim", 2); + pub_points_sim = nh->advertise("/ov_msckf/points_sim", 2); PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str()); // Our tracking image @@ -56,27 +57,27 @@ ROS1Visualizer::ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str()); // Groundtruth publishers - pub_posegt = nh.advertise("/ov_msckf/posegt", 2); + pub_posegt = nh->advertise("/ov_msckf/posegt", 2); PRINT_DEBUG("Publishing: %s\n", pub_posegt.getTopic().c_str()); - pub_pathgt = nh.advertise("/ov_msckf/pathgt", 2); + pub_pathgt = nh->advertise("/ov_msckf/pathgt", 2); PRINT_DEBUG("Publishing: %s\n", pub_pathgt.getTopic().c_str()); // Loop closure publishers - pub_loop_pose = nh.advertise("/ov_msckf/loop_pose", 2); - pub_loop_point = nh.advertise("/ov_msckf/loop_feats", 2); - pub_loop_extrinsic = nh.advertise("/ov_msckf/loop_extrinsic", 2); - pub_loop_intrinsics = nh.advertise("/ov_msckf/loop_intrinsics", 2); + pub_loop_pose = nh->advertise("/ov_msckf/loop_pose", 2); + pub_loop_point = nh->advertise("/ov_msckf/loop_feats", 2); + pub_loop_extrinsic = nh->advertise("/ov_msckf/loop_extrinsic", 2); + pub_loop_intrinsics = nh->advertise("/ov_msckf/loop_intrinsics", 2); it_pub_loop_img_depth = it.advertise("/ov_msckf/loop_depth", 2); it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2); // option to enable publishing of global to IMU transformation - nh.param("publish_global_to_imu_tf", publish_global2imu_tf, true); - nh.param("publish_calibration_tf", publish_calibration_tf, true); + nh->param("publish_global_to_imu_tf", publish_global2imu_tf, true); + nh->param("publish_calibration_tf", publish_calibration_tf, true); // Load groundtruth if we have it and are not doing simulation - if (nh.hasParam("path_gt") && _sim == nullptr) { + if (nh->hasParam("path_gt") && _sim == nullptr) { std::string path_to_gt; - nh.param("path_gt", path_to_gt, ""); + nh->param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { DatasetReader::load_gt_file(path_to_gt, gt_states); PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); @@ -84,16 +85,16 @@ ROS1Visualizer::ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr } // Load if we should save the total state to file - nh.param("save_total_state", save_total_state, false); + nh->param("save_total_state", save_total_state, false); // If the file is not open, then open the file if (save_total_state) { // files we will open std::string filepath_est, filepath_std, filepath_gt; - nh.param("filepath_est", filepath_est, "state_estimate.txt"); - nh.param("filepath_std", filepath_std, "state_deviation.txt"); - nh.param("filepath_gt", filepath_gt, "state_groundtruth.txt"); + nh->param("filepath_est", filepath_est, "state_estimate.txt"); + nh->param("filepath_std", filepath_std, "state_deviation.txt"); + nh->param("filepath_gt", filepath_gt, "state_groundtruth.txt"); // If it exists, then delete it if (boost::filesystem::exists(filepath_est)) @@ -117,6 +118,54 @@ ROS1Visualizer::ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr } } +void ROS1Visualizer::setup_subscribers(std::shared_ptr parser) { + + // We need a valid parser + assert(parser != nullptr); + + // Create imu subscriber (handle legacy ros param info) + std::string topic_imu; + _nh->param("topic_imu", topic_imu, "/imu0"); + parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); + sub_imu = _nh->subscribe(topic_imu, 9999999, &ROS1Visualizer::callback_inertial, this); + + // Logic for sync stereo subscriber + // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 + if (_app->get_params().state_options.num_cameras == 2) { + // Read in the topics + std::string cam_topic0, cam_topic1; + _nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); + _nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); + // Create sync filter (they have unique pointers internally, so we have to use move logic here...) + auto image_sub0 = std::unique_ptr>( + new message_filters::Subscriber(*_nh, cam_topic0, 5)); + auto image_sub1 = std::unique_ptr>( + new message_filters::Subscriber(*_nh, cam_topic1, 5)); + auto sync = std::unique_ptr>( + new message_filters::Synchronizer(sync_pol(5), *image_sub0, *image_sub1)); + sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo, this, _1, _2, 0, 1)); + // Append to our vector of subscribers + sync_cam.push_back(std::move(sync)); + sync_subs_cam.push_back(std::move(image_sub0)); + sync_subs_cam.push_back(std::move(image_sub1)); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); + } else { + // Now we should add any non-stereo callbacks here + for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { + // read in the topic + std::string cam_topic; + _nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); + // create subscriber + subs_cam.push_back(_nh->subscribe(cam_topic, 5, boost::bind(&ROS1Visualizer::callback_monocular, this, _1, i))); + PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); + } + } +} + void ROS1Visualizer::visualize() { // Return if we have already visualized @@ -284,6 +333,93 @@ void ROS1Visualizer::visualize_final() { PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); } +void ROS1Visualizer::callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { + + // convert into correct format + ov_core::ImuData message; + message.timestamp = msg->header.stamp.toSec(); + message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; + + // send it to our VIO system + _app->feed_measurement_imu(message); + visualize(); + visualize_odometry(message.timestamp); +} + +void ROS1Visualizer::callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { + + // Get the image + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr->header.stamp.toSec(); + message.sensor_ids.push_back(cam_id0); + message.images.push_back(cv_ptr->image.clone()); + + // Load the mask if we are using it, else it is empty + // TODO: in the future we should get this from external pixel segmentation + if (_app->get_params().use_mask) { + message.masks.push_back(_app->get_params().masks.at(cam_id0)); + } else { + message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); + } + + // send it to our VIO system + _app->feed_measurement_camera(message); +} + +void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1) { + + // Get the image + cv_bridge::CvImageConstPtr cv_ptr0; + try { + cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Get the image + cv_bridge::CvImageConstPtr cv_ptr1; + try { + cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr0->header.stamp.toSec(); + message.sensor_ids.push_back(cam_id0); + message.sensor_ids.push_back(cam_id1); + message.images.push_back(cv_ptr0->image.clone()); + message.images.push_back(cv_ptr1->image.clone()); + + // Load the mask if we are using it, else it is empty + // TODO: in the future we should get this from external pixel segmentation + if (_app->get_params().use_mask) { + message.masks.push_back(_app->get_params().masks.at(cam_id0)); + message.masks.push_back(_app->get_params().masks.at(cam_id1)); + } else { + // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); + message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); + message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); + } + + // send it to our VIO system + _app->feed_measurement_camera(message); +} + + void ROS1Visualizer::publish_state() { // Get the current state diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index 7a981147a..c9cce9982 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -25,6 +25,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -46,6 +49,7 @@ #include "sim/Simulator.h" #include "utils/dataset_reader.h" #include "utils/print.h" +#include "utils/sensor_data.h" namespace ov_msckf { @@ -68,7 +72,13 @@ class ROS1Visualizer { * @param app Core estimator manager * @param sim Simulator if we are simulating */ - ROS1Visualizer(ros::NodeHandle &nh, std::shared_ptr app, std::shared_ptr sim = nullptr); + ROS1Visualizer(std::shared_ptr nh, std::shared_ptr app, std::shared_ptr sim = nullptr); + + /** + * @brief Will setup ROS subscribers and callbacks + * @param parser Configuration file parser + */ + void setup_subscribers(std::shared_ptr parser); /** * @brief Will visualize the system if we have new things @@ -87,6 +97,15 @@ class ROS1Visualizer { */ void visualize_final(); + /// Callback for inertial information + void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); + + /// Callback for monocular cameras information + void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); + + /// Callback for synchronized stereo camera information + void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); + protected: /// Publish the current state void publish_state(); @@ -103,6 +122,9 @@ class ROS1Visualizer { /// Publish loop-closure information of current pose and active track information void publish_loopclosure_information(); + /// Global node handler + std::shared_ptr _nh; + /// Core application of the filter system std::shared_ptr _app; @@ -116,6 +138,13 @@ class ROS1Visualizer { ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics; tf::TransformBroadcaster *mTfBr; + // Our subscribers and camera synchronizers + ros::Subscriber sub_imu; + std::vector subs_cam; + typedef message_filters::sync_policies::ApproximateTime sync_pol; + std::vector>> sync_cam; + std::vector>> sync_subs_cam; + // For path viz unsigned int poses_seq_imu = 0; vector poses_imu; diff --git a/ov_msckf/src/ros_serial_msckf.cpp b/ov_msckf/src/ros1_serial_msckf.cpp similarity index 72% rename from ov_msckf/src/ros_serial_msckf.cpp rename to ov_msckf/src/ros1_serial_msckf.cpp index d9f5beadc..7ab5a331f 100644 --- a/ov_msckf/src/ros_serial_msckf.cpp +++ b/ov_msckf/src/ros1_serial_msckf.cpp @@ -19,7 +19,6 @@ * along with this program. If not, see . */ -#include #include #include #include @@ -32,7 +31,6 @@ #include "core/VioManagerOptions.h" #include "ros/ROS1Visualizer.h" #include "utils/dataset_reader.h" -#include "utils/sensor_data.h" using namespace ov_msckf; @@ -50,8 +48,8 @@ int main(int argc, char **argv) { // Launch our ros node ros::init(argc, argv, "run_serial_msckf"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); // Load the config auto parser = std::make_shared(config_path); @@ -80,7 +78,7 @@ int main(int argc, char **argv) { // Our imu topic std::string topic_imu; - nh.param("topic_imu", topic_imu, "/imu0"); + nh->param("topic_imu", topic_imu, "/imu0"); parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); // Our camera topics (stereo pairs and non-stereo mono) @@ -88,8 +86,8 @@ int main(int argc, char **argv) { if (params.use_stereo && params.state_options.num_cameras == 2) { // Read in the topics std::string cam_topic0, cam_topic1; - nh.param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); - nh.param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); + nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); + nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); topic_cameras.emplace_back(0, cam_topic0); @@ -100,7 +98,7 @@ int main(int argc, char **argv) { for (int i = 0; i < params.state_options.num_cameras; i++) { // read in the topic std::string cam_topic; - nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); topic_cameras.emplace_back(i, cam_topic); PRINT_DEBUG("serial cam (mono): %s\n", cam_topic.c_str()); @@ -109,14 +107,14 @@ int main(int argc, char **argv) { // Location of the ROS bag we want to read in std::string path_to_bag; - nh.param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); + nh->param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); PRINT_DEBUG("ros bag path is: %s\n", path_to_bag.c_str()); // Load groundtruth if we have it std::map> gt_states; - if (nh.hasParam("path_gt")) { + if (nh->hasParam("path_gt")) { std::string path_to_gt; - nh.param("path_gt", path_to_gt, ""); + nh->param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { ov_core::DatasetReader::load_gt_file(path_to_gt, gt_states); PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); @@ -126,8 +124,8 @@ int main(int argc, char **argv) { // Get our start location and how much of the bag we want to play // Make the bag duration < 0 to just process to the end of the bag double bag_start, bag_durr; - nh.param("bag_start", bag_start, 0); - nh.param("bag_durr", bag_durr, -1); + nh->param("bag_start", bag_start, 0); + nh->param("bag_durr", bag_durr, -1); PRINT_DEBUG("bag start: %.1f\n", bag_start); PRINT_DEBUG("bag duration: %.1f\n", bag_durr); @@ -216,17 +214,7 @@ int main(int argc, char **argv) { } } if (should_process_imu) { - // convert into correct format - ov_core::ImuData message; - message.timestamp = msg_imu_current->header.stamp.toSec(); - message.wm << msg_imu_current->angular_velocity.x, msg_imu_current->angular_velocity.y, msg_imu_current->angular_velocity.z; - message.am << msg_imu_current->linear_acceleration.x, msg_imu_current->linear_acceleration.y, msg_imu_current->linear_acceleration.z; - // send it to our VIO system - // PRINT_DEBUG("%.15f = imu time",msg_imu_current->header.stamp.toSec()-time_init.toSec()); - sys->feed_measurement_imu(message); - viz->visualize(); - viz->visualize_odometry(message.timestamp); - // move forward in time + viz->callback_inertial(msg_imu_current); msg_imu_current = msg_imu_next; view_imu_iter++; msg_imu_next = view_imu_iter->instantiate(); @@ -289,43 +277,10 @@ int main(int argc, char **argv) { sys->initialize_with_gt(imustate); } - // Get the image - cv_bridge::CvImageConstPtr cv_ptr0, cv_ptr1; - try { - cv_ptr0 = cv_bridge::toCvShare(msg_images_current.at(0), sensor_msgs::image_encodings::MONO8); - cv_ptr1 = cv_bridge::toCvShare(msg_images_current.at(1), sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s\n", e.what()); - msg_images_current.at(0) = msg_images_next.at(0); - view_cameras_iterators.at(0)++; - msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); - msg_images_current.at(1) = msg_images_next.at(1); - view_cameras_iterators.at(1)++; - msg_images_next.at(1) = view_cameras_iterators.at(1)->instantiate(); - continue; - } - - // Create viomanager message datatype - ov_core::CameraData message; - message.timestamp = msg_images_current.at(0)->header.stamp.toSec(); - message.sensor_ids.push_back(0); - message.sensor_ids.push_back(1); - message.images.push_back(cv_ptr0->image.clone()); - message.images.push_back(cv_ptr1->image.clone()); - if (sys->get_params().use_mask) { - message.masks.push_back(sys->get_params().masks.at(0)); - message.masks.push_back(sys->get_params().masks.at(1)); - } else { - // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); - message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); - message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); - } - // PRINT_DEBUG("%.15f = cam %d time",msg_images_current.at(0)->header.stamp.toSec()-time_init.toSec(), 0); - // PRINT_DEBUG("%.15f = cam %d time",msg_images_current.at(1)->header.stamp.toSec()-time_init.toSec(), 1); - // PRINT_DEBUG("(difference is %.15f)",msg_images_current.at(1)->header.stamp.toSec()-msg_images_current.at(0)->header.stamp.toSec()); - sys->feed_measurement_camera(message); + // Feed it into our system + viz->callback_stereo(msg_images_current.at(0), msg_images_current.at(1), 0, 1); - // move forward in time + // Move forward in time msg_images_current.at(0) = msg_images_next.at(0); view_cameras_iterators.at(0)++; msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); @@ -355,30 +310,8 @@ int main(int argc, char **argv) { sys->initialize_with_gt(imustate); } - // Get the image - cv_bridge::CvImageConstPtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvShare(msg_camera, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s\n", e.what()); - msg_images_current.at(smallest_cam) = msg_images_next.at(smallest_cam); - view_cameras_iterators.at(smallest_cam)++; - msg_images_next.at(smallest_cam) = view_cameras_iterators.at(smallest_cam)->instantiate(); - continue; - } - - // Create viomanager message datatype - ov_core::CameraData message; - message.timestamp = msg_camera->header.stamp.toSec(); - message.sensor_ids.push_back(smallest_cam); - message.images.push_back(cv_ptr->image.clone()); - if (sys->get_params().use_mask) { - message.masks.push_back(sys->get_params().masks.at(smallest_cam)); - } else { - message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); - } - // PRINT_DEBUG("%.15f = cam %d time",msg_camera->header.stamp.toSec()-time_init.toSec(), smallest_cam); - sys->feed_measurement_camera(message); + // Feed it into our system + viz->callback_monocular(msg_camera, smallest_cam); // move forward msg_images_current.at(smallest_cam) = msg_images_next.at(smallest_cam); diff --git a/ov_msckf/src/ros1_subscribe_msckf.cpp b/ov_msckf/src/ros1_subscribe_msckf.cpp new file mode 100644 index 000000000..d89245b75 --- /dev/null +++ b/ov_msckf/src/ros1_subscribe_msckf.cpp @@ -0,0 +1,83 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#include + +#include + +#include "core/VioManager.h" +#include "core/VioManagerOptions.h" +#include "ros/ROS1Visualizer.h" +#include "utils/dataset_reader.h" + +using namespace ov_msckf; + +std::shared_ptr sys; +std::shared_ptr viz; + +// Main function +int main(int argc, char **argv) { + + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path_to_config.yaml"; + if (argc > 1) { + config_path = argv[1]; + } + + // Launch our ros node + ros::init(argc, argv, "run_subscribe_msckf"); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); + + // Load the config + auto parser = std::make_shared(config_path); + parser->set_node_handler(nh); + + // Verbosity + std::string verbosity = "DEBUG"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + + // Create our VIO system + VioManagerOptions params; + params.print_and_load(parser); + sys = std::make_shared(params); + viz = std::make_shared(nh, sys); + viz->setup_subscribers(parser); + + // Ensure we read in all parameters required + if (!parser->successful()) { + PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + std::exit(EXIT_FAILURE); + } + + // Spin off to ROS + // TODO: maybe should use multi-thread spinner + // TODO: but need to support multi-threaded calls to viomanager + PRINT_DEBUG("done...spinning to ros"); + ros::spin(); + + // Final visualization + viz->visualize_final(); + + // Done! + return EXIT_SUCCESS; +} diff --git a/ov_msckf/src/ros_subscribe_msckf.cpp b/ov_msckf/src/ros_subscribe_msckf.cpp deleted file mode 100644 index 0625be363..000000000 --- a/ov_msckf/src/ros_subscribe_msckf.cpp +++ /dev/null @@ -1,239 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2021 Patrick Geneva - * Copyright (C) 2021 Guoquan Huang - * Copyright (C) 2021 OpenVINS Contributors - * Copyright (C) 2019 Kevin Eckenhoff - * - * This program 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. - * - * This program 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 this program. If not, see . - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "core/VioManager.h" -#include "core/VioManagerOptions.h" -#include "ros/ROS1Visualizer.h" -#include "utils/dataset_reader.h" -#include "utils/sensor_data.h" - -using namespace ov_msckf; - -std::shared_ptr sys; -std::shared_ptr viz; - -// Callback functions -void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); -void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); -void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); - -// Main function -int main(int argc, char **argv) { - - // Ensure we have a path, if the user passes it then we should use it - std::string config_path = "unset_path_to_config.yaml"; - if (argc > 1) { - config_path = argv[1]; - } - - // Launch our ros node - ros::init(argc, argv, "run_subscribe_msckf"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); - - // Load the config - std::cout << config_path << std::endl; - auto parser = std::make_shared(config_path); - parser->set_node_handler(nh); - - // Verbosity - std::string verbosity = "DEBUG"; - parser->parse_config("verbosity", verbosity); - ov_core::Printer::setPrintLevel(verbosity); - - // Create our VIO system - VioManagerOptions params; - params.print_and_load(parser); - sys = std::make_shared(params); - viz = std::make_shared(nh, sys); - - // Ensure we read in all parameters required - if (!parser->successful()) { - PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); - std::exit(EXIT_FAILURE); - } - - //=================================================================================== - //=================================================================================== - //=================================================================================== - - // Create imu subscriber - std::string topic_imu; - nh.param("topic_imu", topic_imu, "/imu0"); - parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); - ros::Subscriber subimu = nh.subscribe(topic_imu, 9999999, callback_inertial); - - // Create camera subscriber data vectors - std::vector subs_cam; - typedef message_filters::sync_policies::ApproximateTime sync_pol; - std::vector>> sync_cam; - std::vector>> sync_subs_cam; - - // Logic for sync stereo subscriber - // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 - if (params.state_options.num_cameras == 2) { - // Read in the topics - std::string cam_topic0, cam_topic1; - nh.param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); - nh.param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); - // Create sync filter (they have unique pointers internally, so we have to use move logic here...) - auto image_sub0 = std::unique_ptr>( - new message_filters::Subscriber(nh, cam_topic0, 5)); - auto image_sub1 = std::unique_ptr>( - new message_filters::Subscriber(nh, cam_topic1, 5)); - auto sync = std::unique_ptr>( - new message_filters::Synchronizer(sync_pol(5), *image_sub0, *image_sub1)); - sync->registerCallback(boost::bind(&callback_stereo, _1, _2, 0, 1)); - // Append to our vector of subscribers - sync_cam.push_back(std::move(sync)); - sync_subs_cam.push_back(std::move(image_sub0)); - sync_subs_cam.push_back(std::move(image_sub1)); - PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); - PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); - } else { - // Now we should add any non-stereo callbacks here - for (int i = 0; i < params.state_options.num_cameras; i++) { - // read in the topic - std::string cam_topic; - nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); - // create subscriber - subs_cam.push_back(nh.subscribe(cam_topic, 5, boost::bind(callback_monocular, _1, i))); - PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); - } - } - - //=================================================================================== - //=================================================================================== - //=================================================================================== - - // Spin off to ROS - // TODO: maybe should use multi-thread spinner - // TODO: but need to support multi-threaded calls to viomanager - PRINT_DEBUG("done...spinning to ros"); - ros::spin(); - - // Final visualization - viz->visualize_final(); - - // Done! - return EXIT_SUCCESS; -} - -void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { - - // convert into correct format - ov_core::ImuData message; - message.timestamp = msg->header.stamp.toSec(); - message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; - message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; - - // send it to our VIO system - sys->feed_measurement_imu(message); - viz->visualize(); - viz->visualize_odometry(message.timestamp); -} - -void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { - - // Get the image - cv_bridge::CvImageConstPtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Create the measurement - ov_core::CameraData message; - message.timestamp = cv_ptr->header.stamp.toSec(); - message.sensor_ids.push_back(cam_id0); - message.images.push_back(cv_ptr->image.clone()); - - // Load the mask if we are using it, else it is empty - // TODO: in the future we should get this from external pixel segmentation - if (sys->get_params().use_mask) { - message.masks.push_back(sys->get_params().masks.at(cam_id0)); - } else { - message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); - } - - // send it to our VIO system - sys->feed_measurement_camera(message); -} - -void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1) { - - // Get the image - cv_bridge::CvImageConstPtr cv_ptr0; - try { - cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Get the image - cv_bridge::CvImageConstPtr cv_ptr1; - try { - cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); - } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Create the measurement - ov_core::CameraData message; - message.timestamp = cv_ptr0->header.stamp.toSec(); - message.sensor_ids.push_back(cam_id0); - message.sensor_ids.push_back(cam_id1); - message.images.push_back(cv_ptr0->image.clone()); - message.images.push_back(cv_ptr1->image.clone()); - - // Load the mask if we are using it, else it is empty - // TODO: in the future we should get this from external pixel segmentation - if (sys->get_params().use_mask) { - message.masks.push_back(sys->get_params().masks.at(cam_id0)); - message.masks.push_back(sys->get_params().masks.at(cam_id1)); - } else { - // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); - message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); - message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); - } - - // send it to our VIO system - sys->feed_measurement_camera(message); -} diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 9ea19affe..fa7b7f98b 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -59,8 +59,8 @@ int main(int argc, char **argv) { #if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "run_simulation"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); #elif ROS_AVAILABLE == 2 // Launch our ros node rclcpp::init(argc, argv); diff --git a/ov_msckf/src/test_sim_meas.cpp b/ov_msckf/src/test_sim_meas.cpp index 86ff803bc..edf20655e 100644 --- a/ov_msckf/src/test_sim_meas.cpp +++ b/ov_msckf/src/test_sim_meas.cpp @@ -54,8 +54,8 @@ int main(int argc, char **argv) { #if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "test_sim_meas"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); #endif // Load the config diff --git a/ov_msckf/src/test_sim_repeat.cpp b/ov_msckf/src/test_sim_repeat.cpp index aedaa8d2e..cc01e7d2a 100644 --- a/ov_msckf/src/test_sim_repeat.cpp +++ b/ov_msckf/src/test_sim_repeat.cpp @@ -57,8 +57,8 @@ int main(int argc, char **argv) { #if ROS_AVAILABLE == 1 // Launch our ros node ros::init(argc, argv, "test_sim_repeat"); - ros::NodeHandle nh("~"); - nh.param("config_path", config_path, config_path); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); #endif // Load the config From 426e1b43a704094a334c3e2607d6a72071478847 Mon Sep 17 00:00:00 2001 From: Jesse Bloecker Date: Mon, 15 Nov 2021 17:35:46 -0500 Subject: [PATCH 23/55] fixed python build issues --- ov_core/cmake/ROS2.cmake | 5 +++-- ov_core/package.xml | 1 + ov_eval/CMakeLists.txt | 2 +- ov_eval/cmake/ROS2.cmake | 6 +++--- ov_init/CMakeLists.txt | 2 +- ov_init/cmake/ROS2.cmake | 9 +++++---- ov_init/package.xml | 1 + 7 files changed, 15 insertions(+), 11 deletions(-) diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake index 3516dedbe..6640f6575 100644 --- a/ov_core/cmake/ROS2.cmake +++ b/ov_core/cmake/ROS2.cmake @@ -3,6 +3,7 @@ cmake_minimum_required(VERSION 3.3) # Find ros dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(cv_bridge REQUIRED) # Describe ROS project option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) @@ -43,7 +44,7 @@ add_library(ov_core_lib SHARED src/feat/FeatureInitializer.cpp src/utils/print.cpp ) -ament_target_dependencies(ov_core_lib rclcpp) +ament_target_dependencies(ov_core_lib rclcpp cv_bridge) target_link_libraries(ov_core_lib ${thirdparty_libraries}) target_include_directories(ov_core_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) install(TARGETS ov_core_lib @@ -65,7 +66,7 @@ ament_export_libraries(ov_core_lib) #endif () add_executable(test_webcam src/test_webcam.cpp) -ament_target_dependencies(test_webcam rclcpp) +ament_target_dependencies(test_webcam rclcpp cv_bridge) target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries}) install(TARGETS test_webcam DESTINATION lib/${PROJECT_NAME}) diff --git a/ov_core/package.xml b/ov_core/package.xml index 2879bdc56..e3d7914c8 100644 --- a/ov_core/package.xml +++ b/ov_core/package.xml @@ -28,6 +28,7 @@ ament_cmake rclcpp + cv_bridge diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index f7a3c5fee..c3f3fc149 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) # check if we have our python libs files # sudo apt-get install python-matplotlib python-numpy python2.7-dev -find_package(PythonLibs 2.7) +find_package(PythonLibs 2.7 QUIET) option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake index 91d3d1bdc..62d9a7ded 100644 --- a/ov_eval/cmake/ROS2.cmake +++ b/ov_eval/cmake/ROS2.cmake @@ -18,14 +18,14 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include +# /usr/local/lib/python2.7/dist-packages/numpy/core/include +# /usr/local/lib/python2.7/site-packages/numpy/core/include ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} +# ${PYTHON_LIBRARIES} ) ################################################## diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 8f55af339..94afe49d2 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) # check if we have our python libs files # sudo apt-get install python-matplotlib python-numpy python2.7-dev -find_package(PythonLibs 2.7) +find_package(PythonLibs 2.7 QUIET) option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake index dbc6edfbf..1981058d8 100644 --- a/ov_init/cmake/ROS2.cmake +++ b/ov_init/cmake/ROS2.cmake @@ -4,6 +4,7 @@ cmake_minimum_required(VERSION 3.3) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(ov_core REQUIRED) +find_package(cv_bridge REQUIRED) # Describe ROS project option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) @@ -18,14 +19,14 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include +# /usr/local/lib/python2.7/dist-packages/numpy/core/include +# /usr/local/lib/python2.7/site-packages/numpy/core/include ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} +# ${PYTHON_LIBRARIES} ) ################################################## @@ -37,7 +38,7 @@ add_library(ov_init_lib SHARED src/init/InertialInitializer.cpp src/static/StaticInitializer.cpp ) -ament_target_dependencies(ov_init_lib rclcpp ov_core) +ament_target_dependencies(ov_init_lib rclcpp ov_core cv_bridge) target_link_libraries(ov_init_lib ${thirdparty_libraries}) target_include_directories(ov_init_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) install(TARGETS ov_init_lib diff --git a/ov_init/package.xml b/ov_init/package.xml index 7eee11350..4045f94ef 100644 --- a/ov_init/package.xml +++ b/ov_init/package.xml @@ -26,6 +26,7 @@ ament_cmake rclcpp ov_core + cv_bridge From de7ecc3b9962272fcbd78f06a5167c3e7d175216 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 15 Nov 2021 17:39:04 -0500 Subject: [PATCH 24/55] small changes --- ov_core/src/utils/opencv_yaml_parse.h | 12 ++++++------ ov_msckf/cmake/ROS1.cmake | 5 +++++ ov_msckf/src/run_simulation.cpp | 1 - 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index 360fd4e32..b8d52cd8c 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -118,9 +118,9 @@ class YamlParser { template void parse_config(const std::string &node_name, T &node_result, bool required = true) { #if ROS_AVAILABLE == 1 - if (nh->getParam(node_name, node_result)) { - nh->param(node_name, node_result); + if (nh != nullptr && nh->getParam(node_name, node_result)) { PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, node_name.c_str()); + nh->param(node_name, node_result); return; } #elif ROS_AVAILABLE == 2 @@ -156,9 +156,9 @@ class YamlParser { #if ROS_AVAILABLE == 1 std::string rosnode = sensor_name + "_" + node_name; - if (nh->getParam(rosnode, node_result)) { - nh->param(rosnode, node_result); + if (nh != nullptr && nh->getParam(rosnode, node_result)) { PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + nh->param(rosnode, node_result); return; } #elif ROS_AVAILABLE == 2 @@ -196,9 +196,9 @@ class YamlParser { // NOTE: for our 4x4 matrix we should read it as an array from ROS then covert it back into the 4x4 std::string rosnode = sensor_name + "_" + node_name; std::vector matrix_TCtoI; - if (nh->getParam(rosnode, matrix_TCtoI)) { - nh->param>(rosnode, matrix_TCtoI); + if (nh != nullptr && nh->getParam(rosnode, matrix_TCtoI)) { PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + nh->param>(rosnode, matrix_TCtoI); node_result << matrix_TCtoI.at(0), matrix_TCtoI.at(1), matrix_TCtoI.at(2), matrix_TCtoI.at(3), matrix_TCtoI.at(4), matrix_TCtoI.at(5), matrix_TCtoI.at(6), matrix_TCtoI.at(7), matrix_TCtoI.at(8), matrix_TCtoI.at(9), matrix_TCtoI.at(10), matrix_TCtoI.at(11), matrix_TCtoI.at(12), matrix_TCtoI.at(13), matrix_TCtoI.at(14), matrix_TCtoI.at(15); diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index ea160835d..23bebce68 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -90,6 +90,11 @@ endif () add_executable(run_simulation src/run_simulation.cpp) target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS run_simulation + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) add_executable(test_sim_meas src/test_sim_meas.cpp) target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index fa7b7f98b..71b649824 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -68,7 +68,6 @@ int main(int argc, char **argv) { options.allow_undeclared_parameters(true); options.automatically_declare_parameters_from_overrides(true); auto node = std::make_shared("run_simulation", options); - //node->declare_parameter("config_path", config_path); node->get_parameter("config_path", config_path); #endif From 32eacc7cf9fb860d86c984cbaea6ba15b26346f5 Mon Sep 17 00:00:00 2001 From: Jesse Bloecker Date: Mon, 15 Nov 2021 22:55:49 -0500 Subject: [PATCH 25/55] ROS2 viz class and working simulation --- ov_eval/CMakeLists.txt | 7 + ov_eval/cmake/ROS1.cmake | 3 - ov_eval/cmake/ROS2.cmake | 3 - ov_init/CMakeLists.txt | 7 + ov_init/cmake/ROS1.cmake | 3 - ov_init/cmake/ROS2.cmake | 3 - ov_msckf/cmake/ROS1.cmake | 4 +- ov_msckf/cmake/ROS2.cmake | 52 +- ov_msckf/launch/display.rviz | 62 +- ov_msckf/launch/display_ros2.rviz | 353 ++++++++ ov_msckf/launch/subscribe.launch | 4 +- ov_msckf/package.xml | 21 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 2 +- ov_msckf/src/ros/ROS1Visualizer.h | 4 +- ov_msckf/src/ros/ROS2Visualizer.cpp | 835 ++++++++++++++++++ ov_msckf/src/ros/ROS2Visualizer.h | 190 ++++ ov_msckf/src/ros/RosVisualizerHelper.h | 108 +++ ov_msckf/src/run_simulation.cpp | 15 +- ...ribe_msckf.cpp => run_subscribe_msckf.cpp} | 2 +- 19 files changed, 1589 insertions(+), 89 deletions(-) create mode 100644 ov_msckf/launch/display_ros2.rviz create mode 100644 ov_msckf/src/ros/ROS2Visualizer.cpp create mode 100644 ov_msckf/src/ros/ROS2Visualizer.h rename ov_msckf/src/{ros1_subscribe_msckf.cpp => run_subscribe_msckf.cpp} (98%) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index c3f3fc149..4fe53e6e0 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -20,6 +20,13 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) + include_directories( + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ) + list(APPEND thirdparty_libraries + ${PYTHON_LIBRARIES} + ) endif () # We need c++14 for ROS2, thus just require it for everybody diff --git a/ov_eval/cmake/ROS1.cmake b/ov_eval/cmake/ROS1.cmake index 9355c71a4..2f27442aa 100644 --- a/ov_eval/cmake/ROS1.cmake +++ b/ov_eval/cmake/ROS1.cmake @@ -23,15 +23,12 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include ${catkin_INCLUDE_DIRS} ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ) diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake index 62d9a7ded..45b621b2f 100644 --- a/ov_eval/cmake/ROS2.cmake +++ b/ov_eval/cmake/ROS2.cmake @@ -18,14 +18,11 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} -# /usr/local/lib/python2.7/dist-packages/numpy/core/include -# /usr/local/lib/python2.7/site-packages/numpy/core/include ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} -# ${PYTHON_LIBRARIES} ) ################################################## diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 94afe49d2..52fca0ddb 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -20,6 +20,13 @@ if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) + include_directories( + /usr/local/lib/python2.7/dist-packages/numpy/core/include + /usr/local/lib/python2.7/site-packages/numpy/core/include + ) + list(APPEND thirdparty_libraries + ${PYTHON_LIBRARIES} + ) endif () # We need c++14 for ROS2, thus just require it for everybody diff --git a/ov_init/cmake/ROS1.cmake b/ov_init/cmake/ROS1.cmake index efd176921..16aa531ae 100644 --- a/ov_init/cmake/ROS1.cmake +++ b/ov_init/cmake/ROS1.cmake @@ -23,15 +23,12 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include ${catkin_INCLUDE_DIRS} ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} - ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ) diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake index 1981058d8..6db8c367e 100644 --- a/ov_init/cmake/ROS2.cmake +++ b/ov_init/cmake/ROS2.cmake @@ -19,14 +19,11 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} -# /usr/local/lib/python2.7/dist-packages/numpy/core/include -# /usr/local/lib/python2.7/site-packages/numpy/core/include ) # Set link libraries used by all binaries list(APPEND thirdparty_libraries ${Boost_LIBRARIES} -# ${PYTHON_LIBRARIES} ) ################################################## diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 23bebce68..dccdf3d88 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -83,8 +83,8 @@ if (catkin_FOUND AND ENABLE_ROS) add_executable(ros1_serial_msckf src/ros1_serial_msckf.cpp) target_link_libraries(ros1_serial_msckf ov_msckf_lib ${thirdparty_libraries}) - add_executable(ros1_subscribe_msckf src/ros1_subscribe_msckf.cpp) - target_link_libraries(ros1_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) + add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) + target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) endif () diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index 0791c37b2..ec2782af3 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -3,6 +3,13 @@ cmake_minimum_required(VERSION 3.3) # Find ROS build system find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) find_package(ov_core REQUIRED) find_package(ov_init REQUIRED) @@ -25,6 +32,18 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ) +list(APPEND ament_libraries + rclcpp + tf2_ros + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + ov_core + ov_init +) ################################################## # Make the shared library @@ -41,14 +60,11 @@ list(APPEND library_source_files src/update/UpdaterSLAM.cpp src/update/UpdaterZeroVelocity.cpp ) -# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! -#if (catkin_FOUND AND ENABLE_ROS) -# list(APPEND library_source_files -# src/ros/RosVisualizer.cpp -# ) -#endif () +list(APPEND library_source_files + src/ros/ROS2Visualizer.cpp +) add_library(ov_msckf_lib SHARED ${library_source_files}) -ament_target_dependencies(ov_msckf_lib rclcpp ov_core ov_init) +ament_target_dependencies(ov_msckf_lib ${ament_libraries}) target_link_libraries(ov_msckf_lib ${thirdparty_libraries}) target_include_directories(ov_msckf_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) install(TARGETS ov_msckf_lib @@ -64,28 +80,26 @@ ament_export_libraries(ov_msckf_lib) ################################################## # TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! -#if (catkin_FOUND AND ENABLE_ROS) -# -# add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp) -# target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries}) -# -# add_executable(ros2_subscribe_msckf src/ros2_subscribe_msckf.cpp) -# target_link_libraries(ros2_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) -# -#endif () +#add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp) +#target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries}) + +#add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) +#ament_target_dependencies(run_subscribe_msckf ${ament_libraries}) +#target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) +#install(TARGETS run_subscribe_msckf DESTINATION lib/${PROJECT_NAME}) add_executable(run_simulation src/run_simulation.cpp) -ament_target_dependencies(run_simulation rclcpp ov_core ov_init) +ament_target_dependencies(run_simulation ${ament_libraries}) target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) install(TARGETS run_simulation DESTINATION lib/${PROJECT_NAME}) add_executable(test_sim_meas src/test_sim_meas.cpp) -ament_target_dependencies(test_sim_meas rclcpp ov_core ov_init) +ament_target_dependencies(test_sim_meas ${ament_libraries}) target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) install(TARGETS test_sim_meas DESTINATION lib/${PROJECT_NAME}) add_executable(test_sim_repeat src/test_sim_repeat.cpp) -ament_target_dependencies(test_sim_repeat rclcpp ov_core ov_init) +ament_target_dependencies(test_sim_repeat ${ament_libraries}) target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) install(TARGETS test_sim_repeat DESTINATION lib/${PROJECT_NAME}) diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz index 399a7ecf1..d5274388c 100644 --- a/ov_msckf/launch/display.rviz +++ b/ov_msckf/launch/display.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.6011396050453186 - Tree Height: 218 + Tree Height: 381 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -52,7 +52,7 @@ Visualization Manager: Value: true - Class: rviz/TF Enabled: true - Frame Timeout: 999 + Frame Timeout: 120 Frames: All Enabled: true cam0: @@ -87,12 +87,24 @@ Visualization Manager: Max Value: 1 Median window: 5 Min Value: 0 - Name: Image + Name: Image Tracks Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /ov_msckf/loop_depth_colored + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Current Depths + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -154,9 +166,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: MSCKF Points Position Transformer: XYZ Queue Size: 10 @@ -184,9 +194,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: SLAM Points Position Transformer: XYZ Queue Size: 10 @@ -214,9 +222,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: ARUCO Points Position Transformer: XYZ Queue Size: 10 @@ -239,14 +245,12 @@ Visualization Manager: Channel Name: intensity Class: rviz/PointCloud Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: ACTIVE Features Position Transformer: XYZ Queue Size: 10 @@ -274,9 +278,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: SIM Points Position Transformer: XYZ Queue Size: 10 @@ -289,24 +291,12 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /ov_msckf/loop_depth_colored - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Current Depths - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true Enabled: true Global Options: Background Color: 44; 44; 44 Default Light: true Fixed Frame: global - Frame Rate: 30 + Frame Rate: 60 Name: root Tools: - Class: rviz/Interact @@ -329,7 +319,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10 + Distance: 12.363367080688477 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -344,22 +334,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.035398006439209 + Pitch: 0.9503980278968811 Target Frame: Value: Orbit (rviz) - Yaw: 0.8153981566429138 + Yaw: 0.7903981804847717 Saved: ~ Window Geometry: Current Depths: collapsed: false Displays: collapsed: false - Height: 810 + Height: 670 Hide Left Dock: false Hide Right Dock: true - Image: + Image Tracks: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -368,6 +358,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1431 - X: 1760 - Y: 77 + Width: 1281 + X: 1944 + Y: 56 diff --git a/ov_msckf/launch/display_ros2.rviz b/ov_msckf/launch/display_ros2.rviz new file mode 100644 index 000000000..a5f72a058 --- /dev/null +++ b/ov_msckf/launch/display_ros2.rviz @@ -0,0 +1,353 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.4882352948188782 + Tree Height: 272 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 120 + Frames: + All Enabled: true + cam0: + Value: true + global: + Value: true + imu: + Value: true + truth: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + global: + imu: + cam0: + {} + truth: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image Tracks + Normalize Range: true + Queue Size: 10 + Topic: /ov_msckf/trackhist + Unreliable: false + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Current Depths + Normalize Range: true + Queue Size: 10 + Topic: /ov_msckf/loop_depth_colored + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: Path VIO + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ov_msckf/pathimu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: Path GT + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /ov_msckf/pathgt + Unreliable: false + Value: true + - Alpha: 0.800000011920929 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 252; 175; 62 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MSCKF Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 8 + Size (m): 0.009999999776482582 + Style: Points + Topic: /ov_msckf/points_msckf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: SLAM Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 8 + Size (m): 0.009999999776482582 + Style: Points + Topic: /ov_msckf/points_slam + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 117; 80; 123 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ARUCO Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /ov_msckf/points_aruco + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ACTIVE Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /ov_msckf/loop_feats + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 127 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: SIM Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /ov_msckf/points_sim + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: global + Frame Rate: 60 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9453980326652527 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Current Depths: + collapsed: false + Displays: + collapsed: false + Height: 690 + Hide Left Dock: false + Hide Right Dock: true + Image Tracks: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000258fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000014d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000180049006d00610067006500200054007200610063006b00730100000190000001050000002800fffffffb0000000a0049006d00610067006501000001c60000011c0000000000000000fb0000001c00430075007200720065006e0074002000440065007000740068007300000001eb000000b50000002800ffffff000000010000010f00000218fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000218000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039b0000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1291 + X: 2051 + Y: 110 diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 29f6770f2..f64803a66 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -24,8 +24,8 @@ - - + + diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index d082f9006..985e9e424 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -21,7 +21,6 @@ catkin cmake_modules roscpp - rosbag tf std_msgs sensor_msgs @@ -33,15 +32,17 @@ ov_core ov_init - - - - - - ament_cmake rclcpp + rosbag + tf2_ros + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport ov_core ov_init @@ -51,4 +52,10 @@ ament_cmake + + + + + + \ No newline at end of file diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 40e721406..37c366fe8 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -731,7 +731,7 @@ void ROS1Visualizer::publish_loopclosure_information() { // PUBLISH CAMERA0 INTRINSICS sensor_msgs::CameraInfo cameraparams; cameraparams.header = header; - cameraparams.header.frame_id = "imu"; + cameraparams.header.frame_id = "cam0"; cameraparams.distortion_model = (_app->get_params().camera_fisheye.at(0)) ? "equidistant" : "plumb_bob"; Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); cameraparams.D = {cparams(4), cparams(5), cparams(6), cparams(7)}; diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index c9cce9982..abfff58b5 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -147,7 +147,7 @@ class ROS1Visualizer { // For path viz unsigned int poses_seq_imu = 0; - vector poses_imu; + std::vector poses_imu; // Groundtruth infomation ros::Publisher pub_pathgt, pub_posegt; @@ -169,7 +169,7 @@ class ROS1Visualizer { // For path viz unsigned int poses_seq_gt = 0; - vector poses_gt; + std::vector poses_gt; bool publish_global2imu_tf = true; bool publish_calibration_tf = true; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp new file mode 100644 index 000000000..33525c1ce --- /dev/null +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -0,0 +1,835 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#include "ROS2Visualizer.h" + +using namespace ov_core; +using namespace ov_type; +using namespace ov_msckf; + +ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_ptr app, std::shared_ptr sim) + : _node(node), _app(app), _sim(sim) { + + // Setup our transform broadcaster + mTfBr = new tf2_ros::TransformBroadcaster(node); + + // Create image transport + image_transport::ImageTransport it(node); + + // Setup pose and path publisher + pub_poseimu = node->create_publisher("/ov_msckf/poseimu", 2); + PRINT_DEBUG("Publishing: %s\n", pub_poseimu->get_topic_name()); + pub_odomimu = node->create_publisher("/ov_msckf/odomimu", 2); + PRINT_DEBUG("Publishing: %s\n", pub_odomimu->get_topic_name()); + pub_pathimu = node->create_publisher("/ov_msckf/pathimu", 2); + PRINT_DEBUG("Publishing: %s\n", pub_pathimu->get_topic_name()); + + // 3D points publishing + pub_points_msckf = node->create_publisher("/ov_msckf/points_msckf", 2); + PRINT_DEBUG("Publishing: %s\n", pub_points_msckf->get_topic_name()); + pub_points_slam = node->create_publisher("/ov_msckf/points_slam", 2); + PRINT_DEBUG("Publishing: %s\n", pub_points_msckf->get_topic_name()); + pub_points_aruco = node->create_publisher("/ov_msckf/points_aruco", 2); + PRINT_DEBUG("Publishing: %s\n", pub_points_aruco->get_topic_name()); + pub_points_sim = node->create_publisher("/ov_msckf/points_sim", 2); + PRINT_DEBUG("Publishing: %s\n", pub_points_sim->get_topic_name()); + + // Our tracking image + it_pub_tracks = it.advertise("/ov_msckf/trackhist", 2); + PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str()); + + // Groundtruth publishers + pub_posegt = node->create_publisher("/ov_msckf/posegt", 2); + PRINT_DEBUG("Publishing: %s\n", pub_posegt->get_topic_name()); + pub_pathgt = node->create_publisher("/ov_msckf/pathgt", 2); + PRINT_DEBUG("Publishing: %s\n", pub_pathgt->get_topic_name()); + + // Loop closure publishers + pub_loop_pose = node->create_publisher("/ov_msckf/loop_pose", 2); + pub_loop_point = node->create_publisher("/ov_msckf/loop_feats", 2); + pub_loop_extrinsic = node->create_publisher("/ov_msckf/loop_extrinsic", 2); + pub_loop_intrinsics = node->create_publisher("/ov_msckf/loop_intrinsics", 2); + it_pub_loop_img_depth = it.advertise("/ov_msckf/loop_depth", 2); + it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2); + + // option to enable publishing of global to IMU transformation + node->declare_parameter("publish_global_to_imu_tf",true); + node->declare_parameter("publish_calibration_tf", true); + + // Load groundtruth if we have it and are not doing simulation + std::string path_to_gt; + bool has_gt = node->get_parameter("path_gt", path_to_gt); + if (has_gt && _sim == nullptr && !path_to_gt.empty()) { + DatasetReader::load_gt_file(path_to_gt, gt_states); + PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); + } + + // Load if we should save the total state to file + node->declare_parameter("save_total_state", false); + node->get_parameter("save_total_state", save_total_state); + + // If the file is not open, then open the file + if (save_total_state) { + + // files we will open + std::string filepath_est, filepath_std, filepath_gt; + node->declare_parameter("filepath_est", "state_estimate.txt"); + node->declare_parameter("filepath_std", "state_deviation.txt"); + node->declare_parameter("filepath_gt", "state_groundtruth.txt"); + node->get_parameter("filepath_est", filepath_est); + node->get_parameter("filepath_std", filepath_std); + node->get_parameter("filepath_gt", filepath_gt); + + // If it exists, then delete it + if (boost::filesystem::exists(filepath_est)) + boost::filesystem::remove(filepath_est); + if (boost::filesystem::exists(filepath_std)) + boost::filesystem::remove(filepath_std); + + // Open the files + of_state_est.open(filepath_est.c_str()); + of_state_std.open(filepath_std.c_str()); + of_state_est << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + of_state_std << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + + // Groundtruth if we are simulating + if (_sim != nullptr) { + if (boost::filesystem::exists(filepath_gt)) + boost::filesystem::remove(filepath_gt); + of_state_gt.open(filepath_gt.c_str()); + of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; + } + } +} + +void ROS2Visualizer::setup_subscribers(std::shared_ptr parser) { + +// // We need a valid parser +// assert(parser != nullptr); +// +// // Create imu subscriber (handle legacy ros param info) +// std::string topic_imu; +// _nh->param("topic_imu", topic_imu, "/imu0"); +// parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); +// sub_imu = _nh->subscribe(topic_imu, 9999999, &ROS2Visualizer::callback_inertial, this); +// +// // Logic for sync stereo subscriber +// // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 +// if (_app->get_params().state_options.num_cameras == 2) { +// // Read in the topics +// std::string cam_topic0, cam_topic1; +// _nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); +// _nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); +// // Create sync filter (they have unique pointers internally, so we have to use move logic here...) +// auto image_sub0 = std::unique_ptr>( +// new message_filters::Subscriber(*_nh, cam_topic0, 5)); +// auto image_sub1 = std::unique_ptr>( +// new message_filters::Subscriber(*_nh, cam_topic1, 5)); +// auto sync = std::unique_ptr>( +// new message_filters::Synchronizer(sync_pol(5), *image_sub0, *image_sub1)); +// sync->registerCallback(boost::bind(&ROS2Visualizer::callback_stereo, this, _1, _2, 0, 1)); +// // Append to our vector of subscribers +// sync_cam.push_back(std::move(sync)); +// sync_subs_cam.push_back(std::move(image_sub0)); +// sync_subs_cam.push_back(std::move(image_sub1)); +// PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); +// PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); +// } else { +// // Now we should add any non-stereo callbacks here +// for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { +// // read in the topic +// std::string cam_topic; +// _nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); +// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); +// // create subscriber +// subs_cam.push_back(_nh->subscribe(cam_topic, 5, boost::bind(&ROS2Visualizer::callback_monocular, this, _1, i))); +// PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); +// } +// } +} + +void ROS2Visualizer::visualize() { + + // Return if we have already visualized + if (last_visualization_timestamp == _app->get_state()->_timestamp && _app->initialized()) + return; + last_visualization_timestamp = _app->get_state()->_timestamp; + + // Start timing + boost::posix_time::ptime rT0_1, rT0_2; + rT0_1 = boost::posix_time::microsec_clock::local_time(); + + // publish current image + publish_images(); + + // Return if we have not inited + if (!_app->initialized()) + return; + + // Save the start time of this dataset + if (!start_time_set) { + rT1 = boost::posix_time::microsec_clock::local_time(); + start_time_set = true; + } + + // publish state + publish_state(); + + // publish points + publish_features(); + + // Publish gt if we have it + publish_groundtruth(); + + // Publish keyframe information + publish_loopclosure_information(); + + // Save total state + if (save_total_state) { + RosVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); + } + + // Print how much time it took to publish / displaying things + rT0_2 = boost::posix_time::microsec_clock::local_time(); + double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; + PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); +} + +void ROS2Visualizer::visualize_odometry(double timestamp) { + + // Check if we have subscribers + if (pub_odomimu->get_subscription_count() == 0) + return; + + // Return if we have not inited and a second has passes + if (!_app->initialized() || (timestamp - _app->initialized_time()) < 1) + return; + + // Get fast propagate state at the desired timestamp + std::shared_ptr state = _app->get_state(); + Eigen::Matrix state_plus = Eigen::Matrix::Zero(); + _app->get_propagator()->fast_state_propagate(state, timestamp, state_plus); + + // Our odometry message + nav_msgs::msg::Odometry odomIinM; + odomIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp); + odomIinM.header.frame_id = "global"; + + // The POSE component (orientation and position) + odomIinM.pose.pose.orientation.x = state_plus(0); + odomIinM.pose.pose.orientation.y = state_plus(1); + odomIinM.pose.pose.orientation.z = state_plus(2); + odomIinM.pose.pose.orientation.w = state_plus(3); + odomIinM.pose.pose.position.x = state_plus(4); + odomIinM.pose.pose.position.y = state_plus(5); + odomIinM.pose.pose.position.z = state_plus(6); + + // Finally set the covariance in the message (in the order position then orientation as per ros convention) + // TODO: this currently is an approximation since this should actually evolve over our propagation period + // TODO: but to save time we only propagate the mean and not the uncertainty, but maybe we should try to prop the covariance? + std::vector> statevars; + statevars.push_back(state->_imu->pose()->p()); + statevars.push_back(state->_imu->pose()->q()); + Eigen::Matrix covariance_posori = StateHelper::get_marginal_covariance(_app->get_state(), statevars); + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 6; c++) { + odomIinM.pose.covariance[6 * r + c] = covariance_posori(r, c); + } + } + + // The TWIST component (angular and linear velocities) + odomIinM.child_frame_id = "imu"; + odomIinM.twist.twist.linear.x = state_plus(7); + odomIinM.twist.twist.linear.y = state_plus(8); + odomIinM.twist.twist.linear.z = state_plus(9); + odomIinM.twist.twist.angular.x = state_plus(10); // we do not estimate this... + odomIinM.twist.twist.angular.y = state_plus(11); // we do not estimate this... + odomIinM.twist.twist.angular.z = state_plus(12); // we do not estimate this... + + // Velocity covariance (linear then angular) + // TODO: this currently is an approximation since this should actually evolve over our propagation period + // TODO: but to save time we only propagate the mean and not the uncertainty, but maybe we should try to prop the covariance? + // TODO: can we come up with an approx covariance for the omega based on the w_hat = w_m - b_w ?? + statevars.clear(); + statevars.push_back(state->_imu->v()); + Eigen::Matrix covariance_linang = INFINITY * Eigen::Matrix::Identity(); + covariance_linang.block(0, 0, 3, 3) = StateHelper::get_marginal_covariance(_app->get_state(), statevars); + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 6; c++) { + odomIinM.twist.covariance[6 * r + c] = (std::isnan(covariance_linang(r, c))) ? 0 : covariance_linang(r, c); + } + } + + // Finally, publish the resulting odometry message + pub_odomimu->publish(odomIinM); +} + +void ROS2Visualizer::visualize_final() { + + // Final time offset value + if (_app->get_state()->_options.do_calib_camera_timeoffset) { + PRINT_INFO(REDPURPLE "camera-imu timeoffset = %.5f\n\n" RESET, _app->get_state()->_calib_dt_CAMtoIMU->value()(0)); + } + + // Final camera intrinsics + if (_app->get_state()->_options.do_calib_camera_intrinsics) { + for (int i = 0; i < _app->get_state()->_options.num_cameras; i++) { + std::shared_ptr calib = _app->get_state()->_cam_intrinsics.at(i); + PRINT_INFO(REDPURPLE "cam%d intrinsics:\n" RESET, (int)i); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n" RESET, calib->value()(0), calib->value()(1), calib->value()(2), calib->value()(3)); + PRINT_INFO(REDPURPLE "%.5f,%.5f,%.5f,%.5f\n\n" RESET, calib->value()(4), calib->value()(5), calib->value()(6), calib->value()(7)); + } + } + + // Final camera extrinsics + if (_app->get_state()->_options.do_calib_camera_pose) { + for (int i = 0; i < _app->get_state()->_options.num_cameras; i++) { + std::shared_ptr calib = _app->get_state()->_calib_IMUtoCAM.at(i); + Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); + T_CtoI.block(0, 0, 3, 3) = quat_2_Rot(calib->quat()).transpose(); + T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * calib->pos(); + PRINT_INFO(REDPURPLE "T_C%dtoI:\n" RESET, i); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(0, 0), T_CtoI(0, 1), T_CtoI(0, 2), T_CtoI(0, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(1, 0), T_CtoI(1, 1), T_CtoI(1, 2), T_CtoI(1, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f,\n" RESET, T_CtoI(2, 0), T_CtoI(2, 1), T_CtoI(2, 2), T_CtoI(2, 3)); + PRINT_INFO(REDPURPLE "%.3f,%.3f,%.3f,%.3f\n\n" RESET, T_CtoI(3, 0), T_CtoI(3, 1), T_CtoI(3, 2), T_CtoI(3, 3)); + } + } + + // Publish RMSE if we have it + if (!gt_states.empty()) { + PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); + } + + // Publish RMSE and NEES if doing simulation + if (_sim != nullptr) { + PRINT_INFO(REDPURPLE "RMSE average: %.3f (deg) orientation\n" RESET, summed_rmse_ori / summed_number); + PRINT_INFO(REDPURPLE "RMSE average: %.3f (m) position\n\n" RESET, summed_rmse_pos / summed_number); + PRINT_INFO(REDPURPLE "NEES average: %.3f (deg) orientation\n" RESET, summed_nees_ori / summed_number); + PRINT_INFO(REDPURPLE "NEES average: %.3f (m) position\n\n" RESET, summed_nees_pos / summed_number); + } + + // Print the total time + rT2 = boost::posix_time::microsec_clock::local_time(); + PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); +} + +//void ROS2Visualizer::callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { +// +// // convert into correct format +// ov_core::ImuData message; +// message.timestamp = msg->header.stamp.toSec(); +// message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; +// message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; +// +// // send it to our VIO system +// _app->feed_measurement_imu(message); +// visualize(); +// visualize_odometry(message.timestamp); +//} +// +//void ROS2Visualizer::callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { +// +// // Get the image +// cv_bridge::CvImageConstPtr cv_ptr; +// try { +// cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); +// } catch (cv_bridge::Exception &e) { +// PRINT_ERROR("cv_bridge exception: %s", e.what()); +// return; +// } +// +// // Create the measurement +// ov_core::CameraData message; +// message.timestamp = cv_ptr->header.stamp.toSec(); +// message.sensor_ids.push_back(cam_id0); +// message.images.push_back(cv_ptr->image.clone()); +// +// // Load the mask if we are using it, else it is empty +// // TODO: in the future we should get this from external pixel segmentation +// if (_app->get_params().use_mask) { +// message.masks.push_back(_app->get_params().masks.at(cam_id0)); +// } else { +// message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); +// } +// +// // send it to our VIO system +// _app->feed_measurement_camera(message); +//} +// +//void ROS2Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1) { +// +// // Get the image +// cv_bridge::CvImageConstPtr cv_ptr0; +// try { +// cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); +// } catch (cv_bridge::Exception &e) { +// PRINT_ERROR("cv_bridge exception: %s", e.what()); +// return; +// } +// +// // Get the image +// cv_bridge::CvImageConstPtr cv_ptr1; +// try { +// cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); +// } catch (cv_bridge::Exception &e) { +// PRINT_ERROR("cv_bridge exception: %s", e.what()); +// return; +// } +// +// // Create the measurement +// ov_core::CameraData message; +// message.timestamp = cv_ptr0->header.stamp.toSec(); +// message.sensor_ids.push_back(cam_id0); +// message.sensor_ids.push_back(cam_id1); +// message.images.push_back(cv_ptr0->image.clone()); +// message.images.push_back(cv_ptr1->image.clone()); +// +// // Load the mask if we are using it, else it is empty +// // TODO: in the future we should get this from external pixel segmentation +// if (_app->get_params().use_mask) { +// message.masks.push_back(_app->get_params().masks.at(cam_id0)); +// message.masks.push_back(_app->get_params().masks.at(cam_id1)); +// } else { +// // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); +// message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); +// message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); +// } +// +// // send it to our VIO system +// _app->feed_measurement_camera(message); +//} + + +void ROS2Visualizer::publish_state() { + + // Get the current state + std::shared_ptr state = _app->get_state(); + + // We want to publish in the IMU clock frame + // The timestamp in the state will be the last camera time + double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0); + double timestamp_inI = state->_timestamp + t_ItoC; + + // Create pose of IMU (note we use the bag time) + geometry_msgs::msg::PoseWithCovarianceStamped poseIinM; + poseIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp_inI); + poseIinM.header.frame_id = "global"; + poseIinM.pose.pose.orientation.x = state->_imu->quat()(0); + poseIinM.pose.pose.orientation.y = state->_imu->quat()(1); + poseIinM.pose.pose.orientation.z = state->_imu->quat()(2); + poseIinM.pose.pose.orientation.w = state->_imu->quat()(3); + poseIinM.pose.pose.position.x = state->_imu->pos()(0); + poseIinM.pose.pose.position.y = state->_imu->pos()(1); + poseIinM.pose.pose.position.z = state->_imu->pos()(2); + + // Finally set the covariance in the message (in the order position then orientation as per ros convention) + std::vector> statevars; + statevars.push_back(state->_imu->pose()->p()); + statevars.push_back(state->_imu->pose()->q()); + Eigen::Matrix covariance_posori = StateHelper::get_marginal_covariance(_app->get_state(), statevars); + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 6; c++) { + poseIinM.pose.covariance[6 * r + c] = covariance_posori(r, c); + } + } + pub_poseimu->publish(poseIinM); + + //========================================================= + //========================================================= + + // Append to our pose vector + geometry_msgs::msg::PoseStamped posetemp; + posetemp.header = poseIinM.header; + posetemp.pose = poseIinM.pose.pose; + poses_imu.push_back(posetemp); + + // Create our path (imu) + // NOTE: We downsample the number of poses as needed to prevent rviz crashes + // NOTE: https://github.com/ros-visualization/rviz/issues/1107 + nav_msgs::msg::Path arrIMU; + arrIMU.header.stamp = _node->now(); + arrIMU.header.frame_id = "global"; + for (size_t i = 0; i < poses_imu.size(); i += std::floor((double)poses_imu.size() / 16384.0) + 1) { + arrIMU.poses.push_back(poses_imu.at(i)); + } + pub_pathimu->publish(arrIMU); + + // Publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation + geometry_msgs::msg::TransformStamped trans = RosVisualizerHelper::get_stamped_transform_from_pose(_node, state->_imu->pose(), false); + trans.header.stamp = _node->now(); + trans.header.frame_id = "global"; + trans.child_frame_id = "imu"; + if (publish_global2imu_tf) { + mTfBr->sendTransform(trans); + } + + // Loop through each camera calibration and publish it + for (const auto &calib : state->_calib_IMUtoCAM) { + geometry_msgs::msg::TransformStamped trans_calib = RosVisualizerHelper::get_stamped_transform_from_pose(_node, calib.second, true); + trans_calib.header.stamp = _node->now(); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "cam" + std::to_string(calib.first); + if (publish_calibration_tf) { + mTfBr->sendTransform(trans_calib); + } + } +} + +void ROS2Visualizer::publish_images() { + + // Check if we have subscribers + if (it_pub_tracks.getNumSubscribers() == 0) + return; + + // Get our image of history tracks + cv::Mat img_history = _app->get_historical_viz_image(); + + // Create our message + std_msgs::msg::Header header; + header.stamp = _node->now(); + sensor_msgs::msg::Image::SharedPtr exl_msg = cv_bridge::CvImage(header, "bgr8", img_history).toImageMsg(); + + // Publish + it_pub_tracks.publish(exl_msg); +} + +void ROS2Visualizer::publish_features() { + + // Check if we have subscribers + if (pub_points_msckf->get_subscription_count() == 0 && pub_points_slam->get_subscription_count() == 0 && pub_points_aruco->get_subscription_count() == 0 && + pub_points_sim->get_subscription_count() == 0) + return; + + // Get our good MSCKF features + std::vector feats_msckf = _app->get_good_features_MSCKF(); + sensor_msgs::msg::PointCloud2 cloud = RosVisualizerHelper::get_ros_pointcloud(_node, feats_msckf); + pub_points_msckf->publish(cloud); + + // Get our good SLAM features + std::vector feats_slam = _app->get_features_SLAM(); + sensor_msgs::msg::PointCloud2 cloud_SLAM = RosVisualizerHelper::get_ros_pointcloud(_node, feats_slam); + pub_points_slam->publish(cloud_SLAM); + + // Get our good ARUCO features + std::vector feats_aruco = _app->get_features_ARUCO(); + sensor_msgs::msg::PointCloud2 cloud_ARUCO = RosVisualizerHelper::get_ros_pointcloud(_node, feats_aruco); + pub_points_aruco->publish(cloud_ARUCO); + + // Skip the rest of we are not doing simulation + if (_sim == nullptr) + return; + + // Get our good SIMULATION features + std::vector feats_sim = _sim->get_map_vec(); + sensor_msgs::msg::PointCloud2 cloud_SIM = RosVisualizerHelper::get_ros_pointcloud(_node, feats_sim); + pub_points_sim->publish(cloud_SIM); +} + +void ROS2Visualizer::publish_groundtruth() { + + // Our groundtruth state + Eigen::Matrix state_gt; + + // We want to publish in the IMU clock frame + // The timestamp in the state will be the last camera time + double t_ItoC = _app->get_state()->_calib_dt_CAMtoIMU->value()(0); + double timestamp_inI = _app->get_state()->_timestamp + t_ItoC; + + // Check that we have the timestamp in our GT file [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel] + if (_sim == nullptr && (gt_states.empty() || !DatasetReader::get_gt_state(timestamp_inI, state_gt, gt_states))) { + return; + } + + // Get the simulated groundtruth + // NOTE: we get the true time in the IMU clock frame + if (_sim != nullptr) { + timestamp_inI = _app->get_state()->_timestamp + _sim->get_true_parameters().calib_camimu_dt; + if (!_sim->get_state(timestamp_inI, state_gt)) + return; + } + + // Get the GT and system state state + Eigen::Matrix state_ekf = _app->get_state()->_imu->value(); + + // Create pose of IMU + geometry_msgs::msg::PoseStamped poseIinM; + poseIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp_inI); + poseIinM.header.frame_id = "global"; + poseIinM.pose.orientation.x = state_gt(1, 0); + poseIinM.pose.orientation.y = state_gt(2, 0); + poseIinM.pose.orientation.z = state_gt(3, 0); + poseIinM.pose.orientation.w = state_gt(4, 0); + poseIinM.pose.position.x = state_gt(5, 0); + poseIinM.pose.position.y = state_gt(6, 0); + poseIinM.pose.position.z = state_gt(7, 0); + pub_posegt->publish(poseIinM); + + // Append to our pose vector + poses_gt.push_back(poseIinM); + + // Create our path (imu) + // NOTE: We downsample the number of poses as needed to prevent rviz crashes + // NOTE: https://github.com/ros-visualization/rviz/issues/1107 + nav_msgs::msg::Path arrIMU; + arrIMU.header.stamp = _node->now(); + arrIMU.header.frame_id = "global"; + for (size_t i = 0; i < poses_gt.size(); i += std::floor((double)poses_gt.size() / 16384.0) + 1) { + arrIMU.poses.push_back(poses_gt.at(i)); + } + pub_pathgt->publish(arrIMU); + + // Publish our transform on TF + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = _node->now(); + trans.header.frame_id = "global"; + trans.child_frame_id = "truth"; + trans.transform.rotation.x = state_gt(1, 0); + trans.transform.rotation.y = state_gt(2, 0); + trans.transform.rotation.z = state_gt(3, 0); + trans.transform.rotation.w = state_gt(4, 0); + trans.transform.translation.x = state_gt(5, 0); + trans.transform.translation.y = state_gt(6, 0); + trans.transform.translation.z = state_gt(7, 0); + if (publish_global2imu_tf) { + mTfBr->sendTransform(trans); + } + + //========================================================================== + //========================================================================== + + // Difference between positions + double dx = state_ekf(4, 0) - state_gt(5, 0); + double dy = state_ekf(5, 0) - state_gt(6, 0); + double dz = state_ekf(6, 0) - state_gt(7, 0); + double rmse_pos = std::sqrt(dx * dx + dy * dy + dz * dz); + + // Quaternion error + Eigen::Matrix quat_gt, quat_st, quat_diff; + quat_gt << state_gt(1, 0), state_gt(2, 0), state_gt(3, 0), state_gt(4, 0); + quat_st << state_ekf(0, 0), state_ekf(1, 0), state_ekf(2, 0), state_ekf(3, 0); + quat_diff = quat_multiply(quat_st, Inv(quat_gt)); + double rmse_ori = (180 / M_PI) * 2 * quat_diff.block(0, 0, 3, 1).norm(); + + //========================================================================== + //========================================================================== + + // Get covariance of pose + std::vector> statevars; + statevars.push_back(_app->get_state()->_imu->q()); + statevars.push_back(_app->get_state()->_imu->p()); + Eigen::Matrix covariance = StateHelper::get_marginal_covariance(_app->get_state(), statevars); + + // Calculate NEES values + double ori_nees = 2 * quat_diff.block(0, 0, 3, 1).dot(covariance.block(0, 0, 3, 3).inverse() * 2 * quat_diff.block(0, 0, 3, 1)); + Eigen::Vector3d errpos = state_ekf.block(4, 0, 3, 1) - state_gt.block(5, 0, 3, 1); + double pos_nees = errpos.transpose() * covariance.block(3, 3, 3, 3).inverse() * errpos; + + //========================================================================== + //========================================================================== + + // Update our average variables + if (!std::isnan(ori_nees) && !std::isnan(pos_nees)) { + summed_rmse_ori += rmse_ori; + summed_rmse_pos += rmse_pos; + summed_nees_ori += ori_nees; + summed_nees_pos += pos_nees; + summed_number++; + } + + // Nice display for the user + PRINT_INFO(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | average error => %.3f, %.3f (deg,m) | called %d times\n" RESET, rmse_ori, + rmse_pos, summed_rmse_ori / summed_number, summed_rmse_pos / summed_number, (int)summed_number); + PRINT_INFO(REDPURPLE "nees => %.1f, %.1f (ori,pos) | average nees = %.1f, %.1f (ori,pos)\n" RESET, ori_nees, pos_nees, + summed_nees_ori / summed_number, summed_nees_pos / summed_number); + + //========================================================================== + //========================================================================== +} + +void ROS2Visualizer::publish_loopclosure_information() { + + // Get the current tracks in this frame + double active_tracks_time1 = -1; + double active_tracks_time2 = -1; + std::unordered_map active_tracks_posinG; + std::unordered_map active_tracks_uvd; + cv::Mat active_cam0_image; + _app->get_active_tracks(active_tracks_time1, active_tracks_posinG, active_tracks_uvd); + _app->get_active_image(active_tracks_time2, active_cam0_image); + if (active_tracks_time1 == -1) + return; + if (_app->get_state()->_clones_IMU.find(active_tracks_time1) == _app->get_state()->_clones_IMU.end()) + return; + if (active_tracks_time1 != active_tracks_time2) + return; + + // Default header + std_msgs::msg::Header header; + header.stamp = RosVisualizerHelper::get_time_from_seconds(active_tracks_time1); + + //====================================================== + // Check if we have subscribers for the pose odometry, camera intrinsics, or extrinsics + if (pub_loop_pose->get_subscription_count() != 0 || pub_loop_extrinsic->get_subscription_count() != 0 || + pub_loop_intrinsics->get_subscription_count() != 0) { + + // PUBLISH HISTORICAL POSE ESTIMATE + nav_msgs::msg::Odometry odometry_pose; + odometry_pose.header = header; + odometry_pose.header.frame_id = "global"; + odometry_pose.pose.pose.position.x = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(0); + odometry_pose.pose.pose.position.y = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(1); + odometry_pose.pose.pose.position.z = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(2); + odometry_pose.pose.pose.orientation.x = _app->get_state()->_clones_IMU.at(active_tracks_time1)->quat()(0); + odometry_pose.pose.pose.orientation.y = _app->get_state()->_clones_IMU.at(active_tracks_time1)->quat()(1); + odometry_pose.pose.pose.orientation.z = _app->get_state()->_clones_IMU.at(active_tracks_time1)->quat()(2); + odometry_pose.pose.pose.orientation.w = _app->get_state()->_clones_IMU.at(active_tracks_time1)->quat()(3); + pub_loop_pose->publish(odometry_pose); + + // PUBLISH IMU TO CAMERA0 EXTRINSIC + // need to flip the transform to the IMU frame + Eigen::Vector4d q_ItoC = _app->get_state()->_calib_IMUtoCAM.at(0)->quat(); + Eigen::Vector3d p_CinI = -_app->get_state()->_calib_IMUtoCAM.at(0)->Rot().transpose() * _app->get_state()->_calib_IMUtoCAM.at(0)->pos(); + nav_msgs::msg::Odometry odometry_calib; + odometry_calib.header = header; + odometry_calib.header.frame_id = "imu"; + odometry_calib.pose.pose.position.x = p_CinI(0); + odometry_calib.pose.pose.position.y = p_CinI(1); + odometry_calib.pose.pose.position.z = p_CinI(2); + odometry_calib.pose.pose.orientation.x = q_ItoC(0); + odometry_calib.pose.pose.orientation.y = q_ItoC(1); + odometry_calib.pose.pose.orientation.z = q_ItoC(2); + odometry_calib.pose.pose.orientation.w = q_ItoC(3); + pub_loop_extrinsic->publish(odometry_calib); + + // PUBLISH CAMERA0 INTRINSICS + sensor_msgs::msg::CameraInfo cameraparams; + cameraparams.header = header; + cameraparams.header.frame_id = "cam0"; + cameraparams.distortion_model = (_app->get_params().camera_fisheye.at(0)) ? "equidistant" : "plumb_bob"; + Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); + cameraparams.d = {cparams(4), cparams(5), cparams(6), cparams(7)}; + cameraparams.k = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1}; + pub_loop_intrinsics->publish(cameraparams); + } + + //====================================================== + // PUBLISH FEATURE TRACKS IN THE GLOBAL FRAME OF REFERENCE + if (pub_loop_point->get_subscription_count() != 0) { + + // Construct the message + sensor_msgs::msg::PointCloud point_cloud; + point_cloud.header = header; + point_cloud.header.frame_id = "global"; + for (const auto &feattimes : active_tracks_posinG) { + + // Get this feature information + size_t featid = feattimes.first; + Eigen::Vector3d uvd = Eigen::Vector3d::Zero(); + if (active_tracks_uvd.find(featid) != active_tracks_uvd.end()) { + uvd = active_tracks_uvd.at(featid); + } + Eigen::Vector3d pFinG = active_tracks_posinG.at(featid); + + // Push back 3d point + geometry_msgs::msg::Point32 p; + p.x = pFinG(0); + p.y = pFinG(1); + p.z = pFinG(2); + point_cloud.points.push_back(p); + + // Push back the uv_norm, uv_raw, and feature id + // NOTE: we don't use the normalized coordinates to save time here + // NOTE: they will have to be re-normalized in the loop closure code + sensor_msgs::msg::ChannelFloat32 p_2d; + p_2d.values.push_back(0); + p_2d.values.push_back(0); + p_2d.values.push_back(uvd(0)); + p_2d.values.push_back(uvd(1)); + p_2d.values.push_back(featid); + point_cloud.channels.push_back(p_2d); + } + pub_loop_point->publish(point_cloud); + } + + //====================================================== + // Depth images of sparse points and its colorized version + if (it_pub_loop_img_depth.getNumSubscribers() != 0 || it_pub_loop_img_depth_color.getNumSubscribers() != 0) { + + // Create the images we will populate with the depths + std::pair wh_pair = {active_cam0_image.cols, active_cam0_image.rows}; + cv::Mat depthmap_viz; + cv::cvtColor(active_cam0_image, depthmap_viz, cv::COLOR_GRAY2RGB); + cv::Mat depthmap = cv::Mat::zeros(wh_pair.second, wh_pair.first, CV_16UC1); + + // Loop through all points and append + for (const auto &feattimes : active_tracks_uvd) { + + // Get this feature information + size_t featid = feattimes.first; + Eigen::Vector3d uvd = active_tracks_uvd.at(featid); + + // Skip invalid points + double dw = 3; + if (uvd(0) < dw || uvd(0) > wh_pair.first - dw || uvd(1) < dw || uvd(1) > wh_pair.second - dw) { + continue; + } + + // Append the depth + // NOTE: scaled by 1000 to fit the 16U + // NOTE: access order is y,x (stupid opencv convention stuff) + depthmap.at((int)uvd(1), (int)uvd(0)) = (uint16_t)(1000 * uvd(2)); + + // Taken from LSD-SLAM codebase segment into 0-4 meter segments: + // https://github.com/tum-vision/lsd_slam/blob/d1e6f0e1a027889985d2e6b4c0fe7a90b0c75067/lsd_slam_core/src/util/globalFuncs.cpp#L87-L96 + float id = 1.0f / (float)uvd(2); + float r = (0.0f - id) * 255 / 1.0f; + if (r < 0) + r = -r; + float g = (1.0f - id) * 255 / 1.0f; + if (g < 0) + g = -g; + float b = (2.0f - id) * 255 / 1.0f; + if (b < 0) + b = -b; + uchar rc = r < 0 ? 0 : (r > 255 ? 255 : r); + uchar gc = g < 0 ? 0 : (g > 255 ? 255 : g); + uchar bc = b < 0 ? 0 : (b > 255 ? 255 : b); + cv::Scalar color(255 - rc, 255 - gc, 255 - bc); + + // Small square around the point (note the above bound check needs to take into account this width) + cv::Point p0(uvd(0) - dw, uvd(1) - dw); + cv::Point p1(uvd(0) + dw, uvd(1) + dw); + cv::rectangle(depthmap_viz, p0, p1, color, -1); + } + + // Create our messages + sensor_msgs::msg::Image::SharedPtr exl_msg1 = cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_16UC1, depthmap).toImageMsg(); + it_pub_loop_img_depth.publish(exl_msg1); + sensor_msgs::msg::Image::SharedPtr exl_msg2 = cv_bridge::CvImage(header, "bgr8", depthmap_viz).toImageMsg(); + it_pub_loop_img_depth_color.publish(exl_msg2); + } +} diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h new file mode 100644 index 000000000..70c7ffb7c --- /dev/null +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -0,0 +1,190 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2021 Patrick Geneva + * Copyright (C) 2021 Guoquan Huang + * Copyright (C) 2021 OpenVINS Contributors + * Copyright (C) 2019 Kevin Eckenhoff + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef OV_MSCKF_ROS2VISUALIZER_H +#define OV_MSCKF_ROS2VISUALIZER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "core/VioManager.h" +#include "ros/RosVisualizerHelper.h" +#include "sim/Simulator.h" +#include "utils/dataset_reader.h" +#include "utils/print.h" +#include "utils/sensor_data.h" + +namespace ov_msckf { + +/** + * @brief Helper class that will publish results onto the ROS framework. + * + * Also save to file the current total state and covariance along with the groundtruth if we are simulating. + * We visualize the following things: + * - State of the system on TF, pose message, and path + * - Image of our tracker + * - Our different features (SLAM, MSCKF, ARUCO) + * - Groundtruth trajectory if we have it + */ +class ROS2Visualizer { + +public: + /** + * @brief Default constructor + * @param node ROS node pointer + * @param app Core estimator manager + * @param sim Simulator if we are simulating + */ + ROS2Visualizer(std::shared_ptr node, std::shared_ptr app, std::shared_ptr sim = nullptr); + + /** + * @brief Will setup ROS subscribers and callbacks + * @param parser Configuration file parser + */ + void setup_subscribers(std::shared_ptr parser); + + /** + * @brief Will visualize the system if we have new things + */ + void visualize(); + + /** + * @brief Will publish our odometry message for the current timestep. + * This will take the current state estimate and get the propagated pose to the desired time. + * This can be used to get pose estimates on systems which require high frequency pose estimates. + */ + void visualize_odometry(double timestamp); + + /** + * @brief After the run has ended, print results + */ + void visualize_final(); + + /// Callback for inertial information + //void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); + + /// Callback for monocular cameras information + //void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); + + /// Callback for synchronized stereo camera information + //void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); + +protected: + /// Publish the current state + void publish_state(); + + /// Publish the active tracking image + void publish_images(); + + /// Publish current features + void publish_features(); + + /// Publish groundtruth (if we have it) + void publish_groundtruth(); + + /// Publish loop-closure information of current pose and active track information + void publish_loopclosure_information(); + + /// Global node handler + std::shared_ptr _node; + + /// Core application of the filter system + std::shared_ptr _app; + + /// Simulator (is nullptr if we are not sim'ing) + std::shared_ptr _sim; + + // Our publishers + image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color; + rclcpp::Publisher::SharedPtr pub_poseimu; + rclcpp::Publisher::SharedPtr pub_odomimu; + rclcpp::Publisher::SharedPtr pub_pathimu; + rclcpp::Publisher::SharedPtr pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim; + rclcpp::Publisher::SharedPtr pub_loop_pose, pub_loop_extrinsic; + rclcpp::Publisher::SharedPtr pub_loop_point; + rclcpp::Publisher::SharedPtr pub_loop_intrinsics; + tf2_ros::TransformBroadcaster *mTfBr; + +// // Our subscribers and camera synchronizers +// ros::Subscriber sub_imu; +// std::vector subs_cam; +// typedef message_filters::sync_policies::ApproximateTime sync_pol; +// std::vector>> sync_cam; +// std::vector>> sync_subs_cam; + + // For path viz + std::vector poses_imu; + + // Groundtruth infomation + rclcpp::Publisher::SharedPtr pub_pathgt; + rclcpp::Publisher::SharedPtr pub_posegt; + double summed_rmse_ori = 0.0; + double summed_rmse_pos = 0.0; + double summed_nees_ori = 0.0; + double summed_nees_pos = 0.0; + size_t summed_number = 0; + + // Start and end timestamps + bool start_time_set = false; + boost::posix_time::ptime rT1, rT2; + + // Last timestamp we visualized at + double last_visualization_timestamp = 0; + + // Our groundtruth states + std::map> gt_states; + + // For path viz + std::vector poses_gt; + bool publish_global2imu_tf = true; + bool publish_calibration_tf = true; + + // Files and if we should save total state + bool save_total_state; + std::ofstream of_state_est, of_state_std, of_state_gt; +}; + +} // namespace ov_msckf + +#endif // OV_MSCKF_ROS2VISUALIZER_H diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index b97e610fc..45fc7c6fc 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -22,10 +22,18 @@ #ifndef OV_MSCKF_ROSVISUALIZER_HELPER_H #define OV_MSCKF_ROSVISUALIZER_HELPER_H +#if ROS_AVAILABLE == 1 #include #include #include #include +#elif ROS_AVAILABLE == 2 +#include +#include +#include +#include +#include +#endif #include "core/VioManager.h" #include "sim/Simulator.h" @@ -109,6 +117,106 @@ class RosVisualizerHelper { } #endif +#if ROS_AVAILABLE == 2 + /** + * @brief Will visualize the system if we have new things + * @param node ROS2 node pointer + * @param feats Vector of features we will convert into ros format + * @return ROS pointcloud + */ + static sensor_msgs::msg::PointCloud2 get_ros_pointcloud(std::shared_ptr node, const std::vector &feats) { + + // Declare message and sizes + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = "global"; + cloud.header.stamp = node->now(); + cloud.width = 3 * feats.size(); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; // there may be invalid points + + // Setup pointcloud fields + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(3 * feats.size()); + + // Iterators + sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); + + // Fill our iterators + for (const auto &pt : feats) { + *out_x = (float)pt(0); + ++out_x; + *out_y = (float)pt(1); + ++out_y; + *out_z = (float)pt(2); + ++out_z; + } + + return cloud; + } + + /** + * @brief Given a ov_type::PoseJPL this will convert into the ros format. + * + * NOTE: frame ids need to be handled externally! + * + * @param node ROS2 node pointer + * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) + * @param flip_trans If we should flip / inverse the translation + * @return TF of our pose in global (e.g. q_ItoG, p_IinG) + */ + static geometry_msgs::msg::TransformStamped + get_stamped_transform_from_pose(std::shared_ptr node, const std::shared_ptr &pose, bool flip_trans) { + + // Need to flip the transform to the IMU frame + Eigen::Vector4d q_ItoC = pose->quat(); + Eigen::Vector3d p_CinI = pose->pos(); + if (flip_trans) { + p_CinI = -pose->Rot().transpose() * pose->pos(); + } + + // publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = node->now(); + trans.transform.rotation.x = q_ItoC(0); + trans.transform.rotation.y = q_ItoC(1); + trans.transform.rotation.z = q_ItoC(2); + trans.transform.rotation.w = q_ItoC(3); + trans.transform.translation.x = p_CinI(0); + trans.transform.translation.y = p_CinI(1); + trans.transform.translation.z = p_CinI(2); + return trans; + } + + /** + * @brief Helper function that converts time in seconds to our rclcpp timestamp + * @param seconds Time in seconds + * @return Return ROS2 header timestamp + */ + static rclcpp::Time get_time_from_seconds(double seconds) { + + // ROS2 time class has no double constructor + // Extract compatible time from timestamp using ros1 implementation for now + uint32_t sec, nsec; + int64_t sec64 = static_cast(floor(seconds)); + if (sec64 < 0 || sec64 > std::numeric_limits::max()) + throw std::runtime_error("Time is out of dual 32-bit range"); + sec = static_cast(sec64); + nsec = static_cast(std::round((seconds - sec) * 1e9)); + + // Avoid rounding errors + sec += (nsec / 1000000000ul); + nsec %= 1000000000ul; + return rclcpp::Time(sec, nsec); + } + +#endif + /** * @brief Save current estimate state and groundtruth including calibration * @param state Pointer to the state diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 71b649824..9c5420ce7 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -33,6 +33,7 @@ #include "ros/ROS1Visualizer.h" #include #elif ROS_AVAILABLE == 2 +#include "ros/ROS2Visualizer.h" #include #endif @@ -42,6 +43,8 @@ std::shared_ptr sim; std::shared_ptr sys; #if ROS_AVAILABLE == 1 std::shared_ptr viz; +#elif ROS_AVAILABLE == 2 +std::shared_ptr viz; #endif // Define the function to be called when ctrl-c (SIGINT) is sent to process @@ -92,6 +95,8 @@ int main(int argc, char **argv) { sys = std::make_shared(params); #if ROS_AVAILABLE == 1 viz = std::make_shared(nh, sys, sim); +#elif ROS_AVAILABLE == 2 + viz = std::make_shared(node, sys, sim); #endif // Ensure we read in all parameters required @@ -144,7 +149,7 @@ int main(int argc, char **argv) { bool hasimu = sim->get_next_imu(message_imu.timestamp, message_imu.wm, message_imu.am); if (hasimu) { sys->feed_measurement_imu(message_imu); -#if ROS_AVAILABLE == 1 +#if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 viz->visualize_odometry(message_imu.timestamp); #endif } @@ -157,7 +162,7 @@ int main(int argc, char **argv) { if (hascam) { if (buffer_timecam != -1) { sys->feed_measurement_simulation(buffer_timecam, buffer_camids, buffer_feats); -#if ROS_AVAILABLE == 1 +#if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 viz->visualize(); #endif } @@ -167,12 +172,8 @@ int main(int argc, char **argv) { } } - //=================================================================================== - //=================================================================================== - //=================================================================================== - // Final visualization -#if ROS_AVAILABLE == 1 +#if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 viz->visualize_final(); #endif diff --git a/ov_msckf/src/ros1_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp similarity index 98% rename from ov_msckf/src/ros1_subscribe_msckf.cpp rename to ov_msckf/src/run_subscribe_msckf.cpp index d89245b75..870d1ba1c 100644 --- a/ov_msckf/src/ros1_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -43,7 +43,7 @@ int main(int argc, char **argv) { } // Launch our ros node - ros::init(argc, argv, "run_subscribe_msckf"); + ros::init(argc, argv, "subscribe_msckf"); auto nh = std::make_shared("~"); nh->param("config_path", config_path, config_path); From e51b60601b001810c8728be58f801597b3300fd9 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 16 Nov 2021 00:20:09 -0500 Subject: [PATCH 26/55] Add subscribers for ROS2 version --- ov_msckf/cmake/ROS2.cmake | 8 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 15 +- ov_msckf/src/ros/ROS1Visualizer.h | 4 +- ov_msckf/src/ros/ROS2Visualizer.cpp | 278 ++++++++++++++------------- ov_msckf/src/ros/ROS2Visualizer.h | 19 +- ov_msckf/src/run_subscribe_msckf.cpp | 41 +++- 6 files changed, 204 insertions(+), 161 deletions(-) diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index ec2782af3..8026b7072 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -83,10 +83,10 @@ ament_export_libraries(ov_msckf_lib) #add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp) #target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries}) -#add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) -#ament_target_dependencies(run_subscribe_msckf ${ament_libraries}) -#target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) -#install(TARGETS run_subscribe_msckf DESTINATION lib/${PROJECT_NAME}) +add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) +ament_target_dependencies(run_subscribe_msckf ${ament_libraries}) +target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS run_subscribe_msckf DESTINATION lib/${PROJECT_NAME}) add_executable(run_simulation src/run_simulation.cpp) ament_target_dependencies(run_simulation ${ament_libraries}) diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 37c366fe8..f836cd11a 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -139,17 +139,14 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); // Create sync filter (they have unique pointers internally, so we have to use move logic here...) - auto image_sub0 = std::unique_ptr>( - new message_filters::Subscriber(*_nh, cam_topic0, 5)); - auto image_sub1 = std::unique_ptr>( - new message_filters::Subscriber(*_nh, cam_topic1, 5)); - auto sync = std::unique_ptr>( - new message_filters::Synchronizer(sync_pol(5), *image_sub0, *image_sub1)); + auto image_sub0 = std::make_shared>(*_nh, cam_topic0, 5); + auto image_sub1 = std::make_shared>(*_nh, cam_topic1, 5); + auto sync = std::make_shared>(sync_pol(5), *image_sub0, *image_sub1); sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo, this, _1, _2, 0, 1)); // Append to our vector of subscribers - sync_cam.push_back(std::move(sync)); - sync_subs_cam.push_back(std::move(image_sub0)); - sync_subs_cam.push_back(std::move(image_sub1)); + sync_cam.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); } else { diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index abfff58b5..490105e8a 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -142,8 +142,8 @@ class ROS1Visualizer { ros::Subscriber sub_imu; std::vector subs_cam; typedef message_filters::sync_policies::ApproximateTime sync_pol; - std::vector>> sync_cam; - std::vector>> sync_subs_cam; + std::vector>> sync_cam; + std::vector>> sync_subs_cam; // For path viz unsigned int poses_seq_imu = 0; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 33525c1ce..f70e44c70 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -71,8 +71,10 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2); // option to enable publishing of global to IMU transformation - node->declare_parameter("publish_global_to_imu_tf",true); + node->declare_parameter("publish_global_to_imu_tf", true); + node->get_parameter("publish_global_to_imu_tf", publish_global2imu_tf); node->declare_parameter("publish_calibration_tf", true); + node->get_parameter("publish_calibration_tf", publish_calibration_tf); // Load groundtruth if we have it and are not doing simulation std::string path_to_gt; @@ -122,50 +124,59 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p void ROS2Visualizer::setup_subscribers(std::shared_ptr parser) { -// // We need a valid parser -// assert(parser != nullptr); -// -// // Create imu subscriber (handle legacy ros param info) -// std::string topic_imu; -// _nh->param("topic_imu", topic_imu, "/imu0"); -// parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); -// sub_imu = _nh->subscribe(topic_imu, 9999999, &ROS2Visualizer::callback_inertial, this); -// -// // Logic for sync stereo subscriber -// // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 -// if (_app->get_params().state_options.num_cameras == 2) { -// // Read in the topics -// std::string cam_topic0, cam_topic1; -// _nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); -// _nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); -// // Create sync filter (they have unique pointers internally, so we have to use move logic here...) -// auto image_sub0 = std::unique_ptr>( -// new message_filters::Subscriber(*_nh, cam_topic0, 5)); -// auto image_sub1 = std::unique_ptr>( -// new message_filters::Subscriber(*_nh, cam_topic1, 5)); -// auto sync = std::unique_ptr>( -// new message_filters::Synchronizer(sync_pol(5), *image_sub0, *image_sub1)); -// sync->registerCallback(boost::bind(&ROS2Visualizer::callback_stereo, this, _1, _2, 0, 1)); -// // Append to our vector of subscribers -// sync_cam.push_back(std::move(sync)); -// sync_subs_cam.push_back(std::move(image_sub0)); -// sync_subs_cam.push_back(std::move(image_sub1)); -// PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); -// PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); -// } else { -// // Now we should add any non-stereo callbacks here -// for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { -// // read in the topic -// std::string cam_topic; -// _nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); -// // create subscriber -// subs_cam.push_back(_nh->subscribe(cam_topic, 5, boost::bind(&ROS2Visualizer::callback_monocular, this, _1, i))); -// PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); -// } -// } + // We need a valid parser + assert(parser != nullptr); + + // Create imu subscriber (handle legacy ros param info) + std::string topic_imu; + _node->declare_parameter("topic_imu", "/imu0"); + _node->get_parameter("topic_imu", topic_imu); + parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); + sub_imu = _node->create_subscription(topic_imu, 1000, + std::bind(&ROS2Visualizer::callback_inertial, this, std::placeholders::_1)); + + // Logic for sync stereo subscriber + // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 + if (_app->get_params().state_options.num_cameras == 2) { + // Read in the topics + std::string cam_topic0, cam_topic1; + _node->declare_parameter("topic_camera" + std::to_string(0), "/cam" + std::to_string(0) + "/image_raw"); + _node->get_parameter("topic_camera" + std::to_string(0), cam_topic0); + _node->declare_parameter("topic_camera" + std::to_string(1), "/cam" + std::to_string(1) + "/image_raw"); + _node->get_parameter("topic_camera" + std::to_string(1), cam_topic0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); + // Create sync filter (they have unique pointers internally, so we have to use move logic here...) + auto image_sub0 = std::make_shared>(_node, cam_topic0); + auto image_sub1 = std::make_shared>(_node, cam_topic1); + auto sync = std::make_shared>(sync_pol(5), *image_sub0, *image_sub1); + sync->registerCallback(std::bind(&ROS2Visualizer::callback_stereo, this, std::placeholders::_1, std::placeholders::_2, 0, 1)); + // sync->registerCallback([](const sensor_msgs::msg::Image::SharedPtr msg0, const sensor_msgs::msg::Image::SharedPtr msg1) + // {callback_stereo(msg0, msg1, 0, 1);}); + // sync->registerCallback(&callback_stereo2); // since the above two alternatives fail to compile for some reason + // Append to our vector of subscribers + sync_cam.push_back(sync); + sync_subs_cam.push_back(image_sub0); + sync_subs_cam.push_back(image_sub1); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); + } else { + // Now we should add any non-stereo callbacks here + for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { + // read in the topic + std::string cam_topic; + _node->declare_parameter("topic_camera" + std::to_string(i), "/cam" + std::to_string(i) + "/image_raw"); + _node->get_parameter("topic_camera" + std::to_string(i), cam_topic); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); + // create subscriber + // auto sub = _node->create_subscription( + // cam_topic, rclcpp::SensorDataQoS(), std::bind(&ROS2Visualizer::callback_monocular, this, std::placeholders::_1, i)); + auto sub = _node->create_subscription( + cam_topic, 5, [this, i](const sensor_msgs::msg::Image::SharedPtr msg0) { callback_monocular(msg0, i); }); + subs_cam.push_back(sub); + PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); + } + } } void ROS2Visualizer::visualize() { @@ -335,92 +346,92 @@ void ROS2Visualizer::visualize_final() { PRINT_INFO(REDPURPLE "TIME: %.3f seconds\n\n" RESET, (rT2 - rT1).total_microseconds() * 1e-6); } -//void ROS2Visualizer::callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { -// -// // convert into correct format -// ov_core::ImuData message; -// message.timestamp = msg->header.stamp.toSec(); -// message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; -// message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; -// -// // send it to our VIO system -// _app->feed_measurement_imu(message); -// visualize(); -// visualize_odometry(message.timestamp); -//} -// -//void ROS2Visualizer::callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { -// -// // Get the image -// cv_bridge::CvImageConstPtr cv_ptr; -// try { -// cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); -// } catch (cv_bridge::Exception &e) { -// PRINT_ERROR("cv_bridge exception: %s", e.what()); -// return; -// } -// -// // Create the measurement -// ov_core::CameraData message; -// message.timestamp = cv_ptr->header.stamp.toSec(); -// message.sensor_ids.push_back(cam_id0); -// message.images.push_back(cv_ptr->image.clone()); -// -// // Load the mask if we are using it, else it is empty -// // TODO: in the future we should get this from external pixel segmentation -// if (_app->get_params().use_mask) { -// message.masks.push_back(_app->get_params().masks.at(cam_id0)); -// } else { -// message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); -// } -// -// // send it to our VIO system -// _app->feed_measurement_camera(message); -//} -// -//void ROS2Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1) { -// -// // Get the image -// cv_bridge::CvImageConstPtr cv_ptr0; -// try { -// cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); -// } catch (cv_bridge::Exception &e) { -// PRINT_ERROR("cv_bridge exception: %s", e.what()); -// return; -// } -// -// // Get the image -// cv_bridge::CvImageConstPtr cv_ptr1; -// try { -// cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); -// } catch (cv_bridge::Exception &e) { -// PRINT_ERROR("cv_bridge exception: %s", e.what()); -// return; -// } -// -// // Create the measurement -// ov_core::CameraData message; -// message.timestamp = cv_ptr0->header.stamp.toSec(); -// message.sensor_ids.push_back(cam_id0); -// message.sensor_ids.push_back(cam_id1); -// message.images.push_back(cv_ptr0->image.clone()); -// message.images.push_back(cv_ptr1->image.clone()); -// -// // Load the mask if we are using it, else it is empty -// // TODO: in the future we should get this from external pixel segmentation -// if (_app->get_params().use_mask) { -// message.masks.push_back(_app->get_params().masks.at(cam_id0)); -// message.masks.push_back(_app->get_params().masks.at(cam_id1)); -// } else { -// // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); -// message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); -// message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); -// } -// -// // send it to our VIO system -// _app->feed_measurement_camera(message); -//} +void ROS2Visualizer::callback_inertial(const sensor_msgs::msg::Imu::SharedPtr msg) { + + // convert into correct format + ov_core::ImuData message; + message.timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; + + // send it to our VIO system + _app->feed_measurement_imu(message); + visualize(); + visualize_odometry(message.timestamp); +} + +void ROS2Visualizer::callback_monocular(const sensor_msgs::msg::Image::SharedPtr msg0, int cam_id0) { + // Get the image + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr->header.stamp.sec + cv_ptr->header.stamp.nanosec * 1e-9; + message.sensor_ids.push_back(cam_id0); + message.images.push_back(cv_ptr->image.clone()); + + // Load the mask if we are using it, else it is empty + // TODO: in the future we should get this from external pixel segmentation + if (_app->get_params().use_mask) { + message.masks.push_back(_app->get_params().masks.at(cam_id0)); + } else { + message.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); + } + + // send it to our VIO system + _app->feed_measurement_camera(message); +} + +void ROS2Visualizer::callback_stereo(const sensor_msgs::msg::Image::ConstSharedPtr msg0, const sensor_msgs::msg::Image::ConstSharedPtr msg1, + int cam_id0, int cam_id1) { + + // Get the image + cv_bridge::CvImageConstPtr cv_ptr0; + try { + cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Get the image + cv_bridge::CvImageConstPtr cv_ptr1; + try { + cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT_ERROR("cv_bridge exception: %s", e.what()); + return; + } + + // Create the measurement + ov_core::CameraData message; + message.timestamp = cv_ptr0->header.stamp.sec + cv_ptr0->header.stamp.nanosec * 1e-9; + message.sensor_ids.push_back(cam_id0); + message.sensor_ids.push_back(cam_id1); + message.images.push_back(cv_ptr0->image.clone()); + message.images.push_back(cv_ptr1->image.clone()); + + // Load the mask if we are using it, else it is empty + // TODO: in the future we should get this from external pixel segmentation + if (_app->get_params().use_mask) { + message.masks.push_back(_app->get_params().masks.at(cam_id0)); + message.masks.push_back(_app->get_params().masks.at(cam_id1)); + } else { + // message.masks.push_back(cv::Mat(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1, cv::Scalar(255))); + message.masks.push_back(cv::Mat::zeros(cv_ptr0->image.rows, cv_ptr0->image.cols, CV_8UC1)); + message.masks.push_back(cv::Mat::zeros(cv_ptr1->image.rows, cv_ptr1->image.cols, CV_8UC1)); + } + + // send it to our VIO system + _app->feed_measurement_camera(message); +} void ROS2Visualizer::publish_state() { @@ -520,8 +531,8 @@ void ROS2Visualizer::publish_images() { void ROS2Visualizer::publish_features() { // Check if we have subscribers - if (pub_points_msckf->get_subscription_count() == 0 && pub_points_slam->get_subscription_count() == 0 && pub_points_aruco->get_subscription_count() == 0 && - pub_points_sim->get_subscription_count() == 0) + if (pub_points_msckf->get_subscription_count() == 0 && pub_points_slam->get_subscription_count() == 0 && + pub_points_aruco->get_subscription_count() == 0 && pub_points_sim->get_subscription_count() == 0) return; // Get our good MSCKF features @@ -827,7 +838,8 @@ void ROS2Visualizer::publish_loopclosure_information() { } // Create our messages - sensor_msgs::msg::Image::SharedPtr exl_msg1 = cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_16UC1, depthmap).toImageMsg(); + sensor_msgs::msg::Image::SharedPtr exl_msg1 = + cv_bridge::CvImage(header, sensor_msgs::image_encodings::TYPE_16UC1, depthmap).toImageMsg(); it_pub_loop_img_depth.publish(exl_msg1); sensor_msgs::msg::Image::SharedPtr exl_msg2 = cv_bridge::CvImage(header, "bgr8", depthmap_viz).toImageMsg(); it_pub_loop_img_depth_color.publish(exl_msg2); diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index 70c7ffb7c..2de788e96 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -102,13 +102,14 @@ class ROS2Visualizer { void visualize_final(); /// Callback for inertial information - //void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); + void callback_inertial(const sensor_msgs::msg::Imu::SharedPtr msg); /// Callback for monocular cameras information - //void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); + void callback_monocular(const sensor_msgs::msg::Image::SharedPtr msg0, int cam_id0); /// Callback for synchronized stereo camera information - //void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); + void callback_stereo(const sensor_msgs::msg::Image::ConstSharedPtr msg0, const sensor_msgs::msg::Image::ConstSharedPtr msg1, int cam_id0, + int cam_id1); protected: /// Publish the current state @@ -146,12 +147,12 @@ class ROS2Visualizer { rclcpp::Publisher::SharedPtr pub_loop_intrinsics; tf2_ros::TransformBroadcaster *mTfBr; -// // Our subscribers and camera synchronizers -// ros::Subscriber sub_imu; -// std::vector subs_cam; -// typedef message_filters::sync_policies::ApproximateTime sync_pol; -// std::vector>> sync_cam; -// std::vector>> sync_subs_cam; + // Our subscribers and camera synchronizers + rclcpp::Subscription::SharedPtr sub_imu; + std::vector::SharedPtr> subs_cam; + typedef message_filters::sync_policies::ApproximateTime sync_pol; + std::vector>> sync_cam; + std::vector>> sync_subs_cam; // For path viz std::vector poses_imu; diff --git a/ov_msckf/src/run_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp index 870d1ba1c..d91e4c75a 100644 --- a/ov_msckf/src/run_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -19,19 +19,28 @@ * along with this program. If not, see . */ -#include - #include #include "core/VioManager.h" #include "core/VioManagerOptions.h" -#include "ros/ROS1Visualizer.h" #include "utils/dataset_reader.h" +#if ROS_AVAILABLE == 1 +#include "ros/ROS1Visualizer.h" +#include +#elif ROS_AVAILABLE == 2 +#include "ros/ROS2Visualizer.h" +#include +#endif + using namespace ov_msckf; std::shared_ptr sys; +#if ROS_AVAILABLE == 1 std::shared_ptr viz; +#elif ROS_AVAILABLE == 2 +std::shared_ptr viz; +#endif // Main function int main(int argc, char **argv) { @@ -42,14 +51,28 @@ int main(int argc, char **argv) { config_path = argv[1]; } +#if ROS_AVAILABLE == 1 // Launch our ros node - ros::init(argc, argv, "subscribe_msckf"); + ros::init(argc, argv, "run_subscribe_msckf"); auto nh = std::make_shared("~"); nh->param("config_path", config_path, config_path); +#elif ROS_AVAILABLE == 2 + // Launch our ros node + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("run_subscribe_msckf", options); + node->get_parameter("config_path", config_path); +#endif // Load the config auto parser = std::make_shared(config_path); +#if ROS_AVAILABLE == 1 parser->set_node_handler(nh); +#elif ROS_AVAILABLE == 2 + parser->set_node(node); +#endif // Verbosity std::string verbosity = "DEBUG"; @@ -60,8 +83,13 @@ int main(int argc, char **argv) { VioManagerOptions params; params.print_and_load(parser); sys = std::make_shared(params); +#if ROS_AVAILABLE == 1 viz = std::make_shared(nh, sys); viz->setup_subscribers(parser); +#elif ROS_AVAILABLE == 2 + viz = std::make_shared(node, sys); + viz->setup_subscribers(parser); +#endif // Ensure we read in all parameters required if (!parser->successful()) { @@ -73,7 +101,12 @@ int main(int argc, char **argv) { // TODO: maybe should use multi-thread spinner // TODO: but need to support multi-threaded calls to viomanager PRINT_DEBUG("done...spinning to ros"); +#if ROS_AVAILABLE == 1 ros::spin(); +#elif ROS_AVAILABLE == 2 + rclcpp::spin(node); +#endif + // Final visualization viz->visualize_final(); From 7cba87ae6a99ec979d279be8e4a1a80657d2e7c7 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Tue, 16 Nov 2021 00:30:34 -0500 Subject: [PATCH 27/55] update ci file --- .github/workflows/build_catkin.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build_catkin.yml b/.github/workflows/build_catkin.yml index a1d8b83b0..c7db65b39 100644 --- a/.github/workflows/build_catkin.yml +++ b/.github/workflows/build_catkin.yml @@ -2,7 +2,7 @@ name: C/C++ CI on: push: - branches: [ master ] + branches: [ master, develop_v2.4.1 ] pull_request: jobs: @@ -26,7 +26,7 @@ jobs: sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc + touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/kinetic/setup.bash\"" >> ~/.bashrc - name: Install apt dependencies (ubuntu-18.04) if: matrix.os == 'ubuntu-18.04' @@ -35,7 +35,7 @@ jobs: sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc + touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/melodic/setup.bash\"" >> ~/.bashrc - name: Install apt dependencies (ubuntu-20.04) if: matrix.os == 'ubuntu-20.04' @@ -44,14 +44,14 @@ jobs: sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && - touch ~/.bashrc && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc + touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/noetic/setup.bash\"" >> ~/.bashrc - - name: Create catkin workspace and compile + - name: Create ROS1 catkin workspace and compile run: | export REPO=$(basename $GITHUB_REPOSITORY) && cd $GITHUB_WORKSPACE/.. && mkdir src/ && mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && - source /opt/ros/$(ls -1 /opt/ros/ | head -n1)/setup.bash && echo "ros version: $ROS_DISTRO" && + source_ros1 && echo "ros version: $ROS_DISTRO" && catkin build -j2 --no-status - name: Run simulation dataset From e507e02b7820afcbd00715156202d60d3efa1ab7 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 19 Nov 2021 15:24:09 -0500 Subject: [PATCH 28/55] fix sim feature max distance rejection threshold, add option to play rosbag in subscribe.launch --- ov_msckf/launch/subscribe.launch | 5 ++++- ov_msckf/src/sim/Simulator.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index f64803a66..3d9aaf5ae 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -10,6 +10,7 @@ + @@ -42,7 +43,9 @@ - + + + diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 3a0c4a4dc..70c13422c 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -425,7 +425,7 @@ std::vector> Simulator::project_pointcloud(co Eigen::Vector3d p_FinC = R_ItoC * p_FinI + p_IinC; // Skip cloud if too far away - if (p_FinC(2) > 15 || p_FinC(2) < 0.5) + if (p_FinC(2) > params.sim_max_feature_gen_distance || p_FinC(2) < 0.1) continue; // Project to normalized coordinates From 03246906ba612103bb84707ddd6db2115f1f2b97 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 19 Nov 2021 15:53:39 -0500 Subject: [PATCH 29/55] untested support for 3x3 matrices in the config --- ov_core/src/utils/opencv_yaml_parse.h | 88 +++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index b8d52cd8c..a84008841 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -174,6 +174,53 @@ class YamlParser { parse_external_yaml(external_node_name, sensor_name, node_name, node_result, required); } + /** + * @brief Custom parser for Matrix3d in the external parameter files with levels. + * + * This will first load the external file requested. + * From there it will try to find the first level requested (e.g. imu0, cam0, cam1). + * Then the requested node can be found under this sensor name. + * ROS can override the config with `_` (e.g., cam0_distortion). + * + * @param external_node_name Name of the node we will get our relative path from + * @param sensor_name The first level node we will try to get the requested node under + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + void parse_external(const std::string &external_node_name, const std::string &sensor_name, const std::string &node_name, + Eigen::Matrix3d &node_result, bool required = true) { + +#if ROS_AVAILABLE == 1 + // If we have the ROS parameter, we should just get that one + // NOTE: for our 3x3 matrix we should read it as an array from ROS then covert it back into the 3x3 + std::string rosnode = sensor_name + "_" + node_name; + std::vector matrix_RCtoI; + if (nh != nullptr && nh->getParam(rosnode, matrix_RCtoI)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + nh->param>(rosnode, matrix_RCtoI); + node_result << matrix_RCtoI.at(0), matrix_RCtoI.at(1), matrix_RCtoI.at(2), matrix_RCtoI.at(3), matrix_RCtoI.at(4), matrix_RCtoI.at(5), + matrix_RCtoI.at(6), matrix_RCtoI.at(7), matrix_RCtoI.at(8); + return; + } +#elif ROS_AVAILABLE == 2 + // If we have the ROS parameter, we should just get that one + // NOTE: for our 3x3 matrix we should read it as an array from ROS then covert it back into the 4x4 + std::string rosnode = sensor_name + "_" + node_name; + std::vector matrix_RCtoI; + if (node != nullptr && node->has_parameter(rosnode)) { + PRINT_INFO(GREEN "overriding node " BOLDGREEN "%s" RESET GREEN " with value from ROS!\n" RESET, rosnode.c_str()); + node->get_parameter>(rosnode, matrix_RCtoI); + node_result << matrix_RCtoI.at(0), matrix_RCtoI.at(1), matrix_RCtoI.at(2), matrix_RCtoI.at(3), matrix_RCtoI.at(4), matrix_RCtoI.at(5), + matrix_RCtoI.at(6), matrix_RCtoI.at(7), matrix_RCtoI.at(8); + return; + } +#endif + + // Else we just parse from the YAML file! + parse_external_yaml(external_node_name, sensor_name, node_name, node_result, required); + } + /** * @brief Custom parser for Matrix4d in the external parameter files with levels. * @@ -387,6 +434,47 @@ class YamlParser { } } + /** + * @brief Custom parser for camera extrinsic 3x3 transformations + * @param file_node OpenCV file node we will get the data from + * @param node_name Name of the node + * @param node_result Resulting value (should already have default value in it) + * @param required If this parameter is required by the user to set + */ + void parse(const cv::FileNode &file_node, const std::string &node_name, Eigen::Matrix3d &node_result, bool required = true) { + + // Check that we have the requested node + if (!node_found(file_node, node_name)) { + if (required) { + PRINT_WARNING(YELLOW "the node %s of type [%s] was not found...\n" RESET, node_name.c_str(), typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("the node %s of type [%s] was not found (not required)...\n", node_name.c_str(), typeid(node_result).name()); + } + return; + } + + // Now try to get it from the config + node_result = Eigen::Matrix3d::Identity(); + try { + for (int r = 0; r < (int)file_node[node_name].size() && r < 3; r++) { + for (int c = 0; c < (int)file_node[node_name][r].size() && c < 3; c++) { + node_result(r, c) = (double)file_node[node_name][r][c]; + } + } + } catch (...) { + if (required) { + PRINT_WARNING(YELLOW "unable to parse %s node of type [%s] in the config file!\n" RESET, node_name.c_str(), + typeid(node_result).name()); + all_params_found_successfully = false; + } else { + PRINT_DEBUG("unable to parse %s node of type [%s] in the config file (not required)\n", node_name.c_str(), + typeid(node_result).name()); + } + } + + } + /** * @brief Custom parser for camera extrinsic 4x4 transformations * @param file_node OpenCV file node we will get the data from From 05fc17e0699f4bb84671b1e570e04d5c04e52f6e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Fri, 19 Nov 2021 17:06:24 -0500 Subject: [PATCH 30/55] small changes, reduce queue to fix startup delays --- ov_init/src/static/StaticInitializer.cpp | 4 ++-- ov_msckf/src/ros/ROS1Visualizer.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ov_init/src/static/StaticInitializer.cpp b/ov_init/src/static/StaticInitializer.cpp index 2327c0c1c..999618d68 100644 --- a/ov_init/src/static/StaticInitializer.cpp +++ b/ov_init/src/static/StaticInitializer.cpp @@ -39,7 +39,7 @@ bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarianc // Return if we don't have enough for two windows if (newesttime - oldesttime < 2 * params.init_window_time) { - // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); + PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); return false; } @@ -56,7 +56,7 @@ bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarianc // Return if both of these failed if (window_1to0.size() < 2 || window_2to1.size() < 2) { - // PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); + PRINT_DEBUG(YELLOW "[INIT-IMU]: unable to select window of IMU readings, not enough readings\n" RESET); return false; } diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index f836cd11a..6f7375073 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -127,7 +127,7 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars std::string topic_imu; _nh->param("topic_imu", topic_imu, "/imu0"); parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); - sub_imu = _nh->subscribe(topic_imu, 9999999, &ROS1Visualizer::callback_inertial, this); + sub_imu = _nh->subscribe(topic_imu, 100, &ROS1Visualizer::callback_inertial, this); // Logic for sync stereo subscriber // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 @@ -139,16 +139,16 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); // Create sync filter (they have unique pointers internally, so we have to use move logic here...) - auto image_sub0 = std::make_shared>(*_nh, cam_topic0, 5); - auto image_sub1 = std::make_shared>(*_nh, cam_topic1, 5); + auto image_sub0 = std::make_shared>(*_nh, cam_topic0, 1); + auto image_sub1 = std::make_shared>(*_nh, cam_topic1, 1); auto sync = std::make_shared>(sync_pol(5), *image_sub0, *image_sub1); sync->registerCallback(boost::bind(&ROS1Visualizer::callback_stereo, this, _1, _2, 0, 1)); // Append to our vector of subscribers sync_cam.push_back(sync); sync_subs_cam.push_back(image_sub0); sync_subs_cam.push_back(image_sub1); - PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic0.c_str()); - PRINT_DEBUG("subscribing to cam (stereo): %s", cam_topic1.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s\n", cam_topic0.c_str()); + PRINT_DEBUG("subscribing to cam (stereo): %s\n", cam_topic1.c_str()); } else { // Now we should add any non-stereo callbacks here for (int i = 0; i < _app->get_params().state_options.num_cameras; i++) { @@ -158,7 +158,7 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); // create subscriber subs_cam.push_back(_nh->subscribe(cam_topic, 5, boost::bind(&ROS1Visualizer::callback_monocular, this, _1, i))); - PRINT_DEBUG("subscribing to cam (mono): %s", cam_topic.c_str()); + PRINT_DEBUG("subscribing to cam (mono): %s\n", cam_topic.c_str()); } } } @@ -380,7 +380,7 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con try { cv_ptr0 = cv_bridge::toCvShare(msg0, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s\n", e.what()); return; } @@ -389,7 +389,7 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con try { cv_ptr1 = cv_bridge::toCvShare(msg1, sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception &e) { - PRINT_ERROR("cv_bridge exception: %s", e.what()); + PRINT_ERROR("cv_bridge exception: %s\n", e.what()); return; } From 0951b74407cddff5dd60bd137a6a56d9ccfdd66b Mon Sep 17 00:00:00 2001 From: Woosik Lee Date: Fri, 19 Nov 2021 17:24:51 -0500 Subject: [PATCH 31/55] KAIST dataset configure --- ov_msckf/config/kaist/estimator_config.yaml | 112 ++++++++++++++++++++ ov_msckf/config/kaist/kalibr_imu_chain.yaml | 12 ++- 2 files changed, 120 insertions(+), 4 deletions(-) create mode 100644 ov_msckf/config/kaist/estimator_config.yaml diff --git a/ov_msckf/config/kaist/estimator_config.yaml b/ov_msckf/config/kaist/estimator_config.yaml new file mode 100644 index 000000000..6fbba9df3 --- /dev/null +++ b/ov_msckf/config/kaist/estimator_config.yaml @@ -0,0 +1,112 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) +use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints +max_cameras: 2 + +calib_cam_extrinsics: false +calib_cam_intrinsics: true +calib_cam_timeoffset: true + +max_clones: 25 +max_slam: 10 +max_slam_in_update: 25 +max_msckf_in_update: 100 +dt_slam_delay: 1 + +gravity_mag: 9.79858 + +feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: true +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 1 +zupt_max_disparity: 0.3 # set to 0 for only imu-based +zupt_only_at_beginning: false + +# ================================================================== +# ================================================================== + +init_only_use_dynamic: false +init_only_use_static: true + +init_camera_pose: false +init_camera_timeoffset: false +init_state_bias_accel: true +init_enforce_gravity_magnitude: true + +init_camera_rate: 10.0 +init_window_time: 1.0 +init_imu_thresh: 0.5 +init_max_disparity: 1.5 +init_max_features: 25.0 + +# ================================================================== +# ================================================================== + +record_timing_information: false +record_timing_filepath: "/tmp/traj_timing.txt" + +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true +num_pts: 300 +fast_threshold: 10 +grid_x: 20 +grid_y: 20 +min_px_dist: 20 +knn_ratio: 0.65 +downsample_cameras: false +multi_threading: true +histogram_method: "CLAHE" # NONE, HISTOGRAM, CLAHE + +fi_max_dist: 150 +fi_max_baseline: 200 +fi_max_cond_number: 20000 +fi_triangulate_1d: false + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + diff --git a/ov_msckf/config/kaist/kalibr_imu_chain.yaml b/ov_msckf/config/kaist/kalibr_imu_chain.yaml index 6fdd0ef0a..b3eea522e 100644 --- a/ov_msckf/config/kaist/kalibr_imu_chain.yaml +++ b/ov_msckf/config/kaist/kalibr_imu_chain.yaml @@ -9,10 +9,14 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 5.8860e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 1.0000e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 1.7453e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 1.0000e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + accelerometer_noise_density: 2.0000e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-03 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # + # + # + # model: calibrated rostopic: /imu/data_raw time_offset: 0.0 From da00bbdafb40ed546db98aeb3735e1138665a254 Mon Sep 17 00:00:00 2001 From: Chuchu Chen Date: Fri, 19 Nov 2021 17:24:35 -0500 Subject: [PATCH 32/55] Updated simulation scripts; --- ov_msckf/scripts/run_sim_calib.sh | 62 +++++++++++++++++++++---------- ov_msckf/scripts/run_sim_cams.sh | 40 ++++++++++---------- ov_msckf/scripts/run_sim_rep.sh | 53 ++++++++++++++++---------- ov_msckf/src/sim/Simulator.cpp | 2 +- 4 files changed, 96 insertions(+), 61 deletions(-) diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 49efda3cf..719104417 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -1,34 +1,30 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= #============================================================= - -# dataset locations +# datasets datasets=( - "udel_gore" + "udel_gore" # "udel_arl" -# "tum_corridor1" +# "udel_gore_zupt" +# "tum_corridor1_512_16_okvis" # "udel_neighborhood" ) - - # location to save log files into -save_path_est="/home/patrick/github/pubs_data/pgeneva/2020_openvins/NEW_sim_calibration/algorithms" -save_path_gt="/home/patrick/github/pubs_data/pgeneva/2020_openvins/NEW_sim_calibration/truths" - +save_path_est="/home/cc/test/openvins_pra/sim_calib/algorithms" +save_path_gt="/home/cc/test/openvins_pra/sim_calib/truths" #============================================================= -#============================================================= +# Start the Monte Carlo Simulations #============================================================= - # Loop through if use fej or not for h in "${!datasets[@]}"; do @@ -42,31 +38,61 @@ for j in {00..02}; do #=============================================== start_time="$(date -u +%s)" filename_est="$save_path_est/ov_23_static_groundtruth/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" dataset:="${datasets[h]}.txt" sim_do_calibration:="false" sim_do_perturbation:="false" max_cameras:="1" num_clones:="11" num_slam:="50" num_pts:="100" dosave_pose:="true" path_est:="$filename_est" path_gt:="$filename_gt" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$j" dataset:="${datasets[h]}.txt" \ + sim_do_calibration:="false" \ + sim_do_perturbation:="false" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" echo "BASH: ${datasets[h]} - static_groundtruth - run $j took $elapsed seconds"; + #=============================================== #=============================================== start_time="$(date -u +%s)" filename_est="$save_path_est/ov_23_calib_groundtruth/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" dataset:="${datasets[h]}.txt" sim_do_calibration:="true" sim_do_perturbation:="false" max_cameras:="1" num_clones:="11" num_slam:="50" num_pts:="100" dosave_pose:="true" path_est:="$filename_est" path_gt:="$filename_gt" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$j" \ + dataset:="${datasets[h]}.txt" \ + sim_do_calibration:="true" \ + sim_do_perturbation:="false" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" echo "BASH: ${datasets[h]} - calib_groundtruth - run $j took $elapsed seconds"; + #=============================================== #=============================================== start_time="$(date -u +%s)" filename_est="$save_path_est/ov_23_calib_perturbed/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" dataset:="${datasets[h]}.txt" sim_do_calibration:="true" sim_do_perturbation:="true" max_cameras:="1" num_clones:="11" num_slam:="50" num_pts:="100" dosave_pose:="true" path_est:="$filename_est" path_gt:="$filename_gt" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$j" \ + dataset:="${datasets[h]}.txt" \ + sim_do_calibration:="true" \ + sim_do_perturbation:="true" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" echo "BASH: ${datasets[h]} - calib_perturbed - run $j took $elapsed seconds"; + #=============================================== #=============================================== start_time="$(date -u +%s)" filename_est="$save_path_est/ov_23_static_perturbed/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" dataset:="${datasets[h]}.txt" sim_do_calibration:="false" sim_do_perturbation:="true" max_cameras:="1" num_clones:="11" num_slam:="50" num_pts:="100" dosave_pose:="true" path_est:="$filename_est" path_gt:="$filename_gt" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$j" \ + dataset:="${datasets[h]}.txt" \ + sim_do_calibration:="false" \ + sim_do_perturbation:="true" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" echo "BASH: ${datasets[h]} - static_perturbed - run $j took $elapsed seconds"; @@ -74,8 +100,6 @@ echo "BASH: ${datasets[h]} - static_perturbed - run $j took $elapsed seconds"; #=============================================== - - done done diff --git a/ov_msckf/scripts/run_sim_cams.sh b/ov_msckf/scripts/run_sim_cams.sh index 047b2e543..4d3f9249a 100755 --- a/ov_msckf/scripts/run_sim_cams.sh +++ b/ov_msckf/scripts/run_sim_cams.sh @@ -1,19 +1,19 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= #============================================================= - -# dataset locations +# datasets datasets=( - "udel_gore" - "udel_arl" -# "tum_corridor1" + "udel_gore" +# "udel_arl" +# "udel_gore_zupt" +# "tum_corridor1_512_16_okvis" # "udel_neighborhood" ) @@ -25,22 +25,17 @@ cameras=( "4" ) - # location to save log files into -save_path_est="/home/patrick/github/pubs_data/pgeneva/2020_openvins/NEW_sim_cameras/algorithms" -save_path_gt="/home/patrick/github/pubs_data/pgeneva/2020_openvins/NEW_sim_cameras/truths" +save_path_est="/home/cc/test/openvins_pra/sim_cam/algorithms" +save_path_gt="/home/cc/test/openvins_pra/sim_cam/truths" - -#============================================================= #============================================================= +# Start Monte-Carlo Simulations #============================================================= - - -# Loop through if use fej or not +# Loop through datasets for h in "${!datasets[@]}"; do -# Loop through all representations +# Loop through number of cameras we want to use for i in "${!cameras[@]}"; do - # Monte Carlo runs for this dataset for j in {00..02}; do @@ -52,7 +47,13 @@ filename_est="$save_path_est/ov_v23_cam${cameras[i]}/${datasets[h]}/estimate_$j. filename_gt="$save_path_gt/${datasets[h]}.txt" # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" max_cameras:="${cameras[i]}" dataset:="${datasets[h]}.txt" num_clones:="11" num_slam:="50" num_pts:="100" dosave_pose:="true" path_est:="$filename_est" path_gt:="$filename_gt" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$((10#$j + 1))" \ + max_cameras:="${cameras[i]}" \ + dataset:="${datasets[h]}.txt" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null # print out the time elapsed end_time="$(date -u +%s)" @@ -61,9 +62,6 @@ echo "BASH: ${datasets[h]} - ${cameras[i]} - run $j took $elapsed seconds"; done - - - done done diff --git a/ov_msckf/scripts/run_sim_rep.sh b/ov_msckf/scripts/run_sim_rep.sh index 6d8abac42..9c85198ae 100755 --- a/ov_msckf/scripts/run_sim_rep.sh +++ b/ov_msckf/scripts/run_sim_rep.sh @@ -1,45 +1,53 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= #============================================================= +# datasets +datasets=( +# "udel_gore" + "udel_arl" +# "udel_gore_zupt" +# "tum_corridor1_512_16_okvis" +# "udel_neighborhood" +) -# config locations +# estimator configurations usefej=( "false" "true" ) -# dataset locations +# feature representations representations=( -# "GLOBAL_3D" -# "GLOBAL_FULL_INVERSE_DEPTH" -# "ANCHORED_3D" -# "ANCHORED_FULL_INVERSE_DEPTH" + "GLOBAL_3D" + "GLOBAL_FULL_INVERSE_DEPTH" + "ANCHORED_3D" + "ANCHORED_FULL_INVERSE_DEPTH" "ANCHORED_MSCKF_INVERSE_DEPTH" "ANCHORED_INVERSE_DEPTH_SINGLE" ) - # location to save log files into -save_path="/home/patrick/github/pubs_data/pgeneva/2020_openvins/sim_representations/algorithms" +save_path_est="/home/cc/test/openvins_pra/sim_representations/algorithms" +save_path_gt="/home/cc/test/openvins_pra/sim_representations/truths" #============================================================= +# Start Monte-Carlo Simulations #============================================================= -#============================================================= - +# Loop through datasets +for m in "${!datasets[@]}"; do # Loop through if use fej or not for h in "${!usefej[@]}"; do # Loop through all representations for i in "${!representations[@]}"; do - # Monte Carlo runs for this dataset for j in {00..09}; do @@ -53,21 +61,26 @@ then else temp="" fi -filename="$save_path/${representations[i]}$temp/udel_gore/estimate_$j.txt" +filename_est="$save_path_est/${representations[i]}$temp/${datasets[m]}/estimate_$j.txt" +filename_gt="$save_path_gt/${datasets[m]}.txt" # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_sim.launch seed:="$j" fej:="${usefej[h]}" feat_rep:="${representations[i]}" num_clones:="6" num_slam:="50" num_pts:="50" dosave:="true" path_est:="$filename" &> /dev/null +roslaunch ov_msckf simulation.launch \ + seed:="$((10#$j + 1))" \ + dataset:="${datasets[m]}.txt" \ + fej:="${usefej[h]}" \ + feat_rep:="${representations[i]}" \ + dosave_pose:="true" \ + path_est:="$filename_est" \ + path_gt:="$filename_gt" &> /dev/null # print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" -echo "BASH: ${usefej[h]} - ${representations[i]} - run $j took $elapsed seconds"; +echo "BASH: ${datasets[m]} - ${usefej[h]} - ${representations[i]} - run $j took $elapsed seconds"; done - - - done done - +done diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 3a0c4a4dc..37a14ae69 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -223,7 +223,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { } // Nice sleep so the user can look at the printout - sleep(3); + sleep(1); } bool Simulator::get_state(double desired_time, Eigen::Matrix &imustate) { From ce63c70fba85c0aa9186b06ed60a5685d913733c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Sun, 21 Nov 2021 22:47:09 -0500 Subject: [PATCH 33/55] explictly create Camera objects instead of tracking intrinsics, width height, and model separately --- ov_core/src/cam/CamBase.h | 19 ++++++ ov_core/src/cam/CamEqui.h | 8 ++- ov_core/src/cam/CamRadtan.h | 6 +- ov_core/src/test_tracking.cpp | 2 +- ov_core/src/test_webcam.cpp | 2 +- ov_core/src/track/TrackSIM.cpp | 7 ++- ov_core/src/track/TrackSIM.h | 9 --- ov_msckf/src/core/VioManager.cpp | 43 ++++++-------- ov_msckf/src/core/VioManagerOptions.h | 46 +++++++++------ ov_msckf/src/ros/ROS1Visualizer.cpp | 12 +++- ov_msckf/src/ros/ROS2Visualizer.cpp | 3 +- ov_msckf/src/ros/RosVisualizerHelper.h | 16 ++--- ov_msckf/src/sim/Simulator.cpp | 82 +++++++++++--------------- ov_msckf/src/sim/Simulator.h | 6 +- 14 files changed, 135 insertions(+), 126 deletions(-) diff --git a/ov_core/src/cam/CamBase.h b/ov_core/src/cam/CamBase.h index 4b50d0467..47959f487 100644 --- a/ov_core/src/cam/CamBase.h +++ b/ov_core/src/cam/CamBase.h @@ -42,6 +42,13 @@ namespace ov_core { class CamBase { public: + /** + * @brief Default constructor + * @param width Width of the camera (raw pixels) + * @param height Height of the camera (raw pixels) + */ + CamBase(int width, int height) : _width(width), _height(height) {} + /** * @brief This will set and update the camera calibration values. * This should be called on startup for each camera and after update! @@ -160,6 +167,12 @@ class CamBase { /// Gets the camera distortion cv::Vec4d get_D() { return camera_d_OPENCV; } + /// Gets the width of the camera images + int w() { return _width; } + + /// Gets the height of the camera images + int h() { return _height; } + protected: // Cannot construct the base camera class, needs a distortion model CamBase() = default; @@ -172,6 +185,12 @@ class CamBase { /// Camera distortion in OpenCV format cv::Vec4d camera_d_OPENCV; + + /// Width of the camera (raw pixels) + int _width; + + /// Height of the camera (raw pixels) + int _height; }; } // namespace ov_core diff --git a/ov_core/src/cam/CamEqui.h b/ov_core/src/cam/CamEqui.h index abc44eaf6..1a6030ef0 100644 --- a/ov_core/src/cam/CamEqui.h +++ b/ov_core/src/cam/CamEqui.h @@ -91,8 +91,12 @@ namespace ov_core { class CamEqui : public CamBase { public: - // constructor - CamEqui() {} + /** + * @brief Default constructor + * @param width Width of the camera (raw pixels) + * @param height Height of the camera (raw pixels) + */ + CamEqui(int width, int height) : CamBase(width, height) {} /** * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords. diff --git a/ov_core/src/cam/CamRadtan.h b/ov_core/src/cam/CamRadtan.h index 0ee16d101..fa8bdd437 100644 --- a/ov_core/src/cam/CamRadtan.h +++ b/ov_core/src/cam/CamRadtan.h @@ -83,9 +83,11 @@ class CamRadtan : public CamBase { public: /** - * Default constructor. + * @brief Default constructor + * @param width Width of the camera (raw pixels) + * @param height Height of the camera (raw pixels) */ - CamRadtan() {} + CamRadtan(int width, int height) : CamBase(width, height) {} /** * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords. diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index eae190a4f..321a4954f 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -158,7 +158,7 @@ int main(int argc, char **argv) { for (int i = 0; i < 2; i++) { Eigen::Matrix cam0_calib; cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0; - std::shared_ptr camera_calib = std::make_shared(); + std::shared_ptr camera_calib = std::make_shared(100, 100); camera_calib->set_value(cam0_calib); cameras.insert({i, camera_calib}); } diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 7c7ae1175..71b3f6726 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -130,7 +130,7 @@ int main(int argc, char **argv) { for (int i = 0; i < 2; i++) { Eigen::Matrix cam0_calib; cam0_calib << 1, 1, 0, 0, 0, 0, 0, 0; - std::shared_ptr camera_calib = std::make_shared(); + std::shared_ptr camera_calib = std::make_shared(100, 100); camera_calib->set_value(cam0_calib); cameras.insert({i, camera_calib}); } diff --git a/ov_core/src/track/TrackSIM.cpp b/ov_core/src/track/TrackSIM.cpp index f663746b1..557f06972 100644 --- a/ov_core/src/track/TrackSIM.cpp +++ b/ov_core/src/track/TrackSIM.cpp @@ -60,11 +60,12 @@ void TrackSIM::feed_measurement_simulation(double timestamp, const std::vectorw(); + int height = camera_calib.at(cam_id)->h(); // Move forward in time - img_last[cam_id] = cv::Mat::zeros(cv::Size(wh.first, wh.second), CV_8UC1); - img_mask_last[cam_id] = cv::Mat::zeros(cv::Size(wh.first, wh.second), CV_8UC1); + img_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); + img_mask_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); pts_last[cam_id] = good_left; ids_last[cam_id] = good_ids_left; } diff --git a/ov_core/src/track/TrackSIM.h b/ov_core/src/track/TrackSIM.h index 6fcd4dfb5..135900fd4 100644 --- a/ov_core/src/track/TrackSIM.h +++ b/ov_core/src/track/TrackSIM.h @@ -43,11 +43,6 @@ class TrackSIM : public TrackBase { */ TrackSIM(std::unordered_map> cameras, int numaruco) : TrackBase(cameras, 0, numaruco, false, HistogramMethod::NONE) {} - /** - * @brief Set the width and height for the cameras - * @param _camera_wh Width and height for each camera - */ - void set_width_height(std::map> _camera_wh) { this->camera_wh = _camera_wh; } /** * @brief Process a new image @@ -68,10 +63,6 @@ class TrackSIM : public TrackBase { */ void feed_measurement_simulation(double timestamp, const std::vector &camids, const std::vector>> &feats); - -protected: - /// Width and height of our cameras - std::map> camera_wh; }; } // namespace ov_core diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index c533a08fe..69ac783a4 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -55,22 +55,10 @@ VioManager::VioManager(VioManagerOptions ¶ms_) { state->_calib_dt_CAMtoIMU->set_fej(temp_camimu_dt); // Loop through and load each of the cameras + state->_cam_intrinsics_cameras = params.camera_intrinsics; for (int i = 0; i < state->_options.num_cameras; i++) { - - // Create the actual camera object and set the values - if (params.camera_fisheye.at(i)) { - state->_cam_intrinsics_cameras.insert({i, std::make_shared()}); - state->_cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); - } else { - state->_cam_intrinsics_cameras.insert({i, std::make_shared()}); - state->_cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); - } - - // Camera intrinsic properties - state->_cam_intrinsics.at(i)->set_value(params.camera_intrinsics.at(i)); - state->_cam_intrinsics.at(i)->set_fej(params.camera_intrinsics.at(i)); - - // Our camera extrinsic transform + state->_cam_intrinsics.at(i)->set_value(params.camera_intrinsics.at(i)->get_value()); + state->_cam_intrinsics.at(i)->set_fej(params.camera_intrinsics.at(i)->get_value()); state->_calib_IMUtoCAM.at(i)->set_value(params.camera_extrinsics.at(i)); state->_calib_IMUtoCAM.at(i)->set_fej(params.camera_extrinsics.at(i)); } @@ -196,7 +184,6 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector trackFEATS = trackSIM; PRINT_WARNING(RED "[SIM]: casting our tracker to a TrackSIM object!\n" RESET); } - trackSIM->set_width_height(params.camera_wh); // Feed our simulation tracker trackSIM->feed_measurement_simulation(timestamp, camids, feats); @@ -215,11 +202,13 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector if (did_zupt_update) { int max_width = -1; int max_height = -1; - for (auto &pair : params.camera_wh) { - if (max_width < pair.second.first) - max_width = pair.second.first; - if (max_height < pair.second.second) - max_height = pair.second.second; + for (int n = 0; n < params.state_options.num_cameras; n++) { + int width = state->_cam_intrinsics_cameras.at(n)->w(); + int height = state->_cam_intrinsics_cameras.at(n)->h(); + if (max_width < width) + max_width = width; + if (max_height < height) + max_height = height; } for (int n = 0; n < params.state_options.num_cameras; n++) { cv::Mat img_outtemp0 = cv::Mat::zeros(cv::Size(max_width, max_height), CV_8UC3); @@ -248,10 +237,11 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector ov_core::CameraData message; message.timestamp = timestamp; for (auto const &camid : camids) { - auto &wh = params.camera_wh.at(camid); + int width = state->_cam_intrinsics_cameras.at(camid)->w(); + int height = state->_cam_intrinsics_cameras.at(camid)->h(); message.sensor_ids.push_back(camid); - message.images.push_back(cv::Mat::zeros(cv::Size(wh.first, wh.second), CV_8UC1)); - message.masks.push_back(cv::Mat::zeros(cv::Size(wh.first, wh.second), CV_8UC1)); + message.images.push_back(cv::Mat::zeros(cv::Size(width, height), CV_8UC1)); + message.masks.push_back(cv::Mat::zeros(cv::Size(width, height), CV_8UC1)); } do_feature_propagate_update(message); } @@ -922,8 +912,9 @@ void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) } // Skip if not valid (i.e. negative depth, or outside of image) - if (uv_dist(0) < 0 || (int)uv_dist(0) >= params.camera_wh.at(0).first || uv_dist(1) < 0 || - (int)uv_dist(1) >= params.camera_wh.at(0).second) { + int width = state->_cam_intrinsics_cameras.at(0)->w(); + int height = state->_cam_intrinsics_cameras.at(0)->h(); + if (uv_dist(0) < 0 || (int)uv_dist(0) >= width || uv_dist(1) < 0 || (int)uv_dist(1) >= height) { // PRINT_DEBUG("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1)); continue; } diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 347ed1f09..71efa6142 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -34,6 +34,8 @@ #include "init/InertialInitializerOptions.h" +#include "cam/CamEqui.h" +#include "cam/CamRadtan.h" #include "feat/FeatureInitializerOptions.h" #include "track/TrackBase.h" #include "utils/colors.h" @@ -191,18 +193,12 @@ struct VioManagerOptions { /// Time offset between camera and IMU. double calib_camimu_dt = 0.0; - /// Map between camid and camera model (true=fisheye, false=radtan) - std::map camera_fisheye; - - /// Map between camid and intrinsics. Values depends on the model but each should be a 4x1 vector normally. - std::map camera_intrinsics; + /// Map between camid and camera intrinsics (fx, fy, cx, cy, d1...d4, cam_w, cam_h) + std::unordered_map> camera_intrinsics; /// Map between camid and camera extrinsics (q_ItoC, p_IinC). std::map camera_extrinsics; - /// Map between camid and the dimensions of incoming images (width/cols, height/rows). This is normally only used during simulation. - std::map> camera_wh; - /// If we should try to load a mask and use it to reject invalid features bool use_mask = false; @@ -237,7 +233,8 @@ struct VioManagerOptions { parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "intrinsics", cam_calib1); parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_coeffs", cam_calib2); Eigen::VectorXd cam_calib = Eigen::VectorXd::Zero(8); - cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1), cam_calib2.at(2), cam_calib2.at(3); + cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1), + cam_calib2.at(2), cam_calib2.at(3); cam_calib(0) /= (downsample_cameras) ? 2.0 : 1.0; cam_calib(1) /= (downsample_cameras) ? 2.0 : 1.0; cam_calib(2) /= (downsample_cameras) ? 2.0 : 1.0; @@ -259,11 +256,15 @@ struct VioManagerOptions { cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); - // Insert - camera_fisheye.insert({i, dist_model == "equidistant"}); - camera_intrinsics.insert({i, cam_calib}); + // Create intrinsics model + if (dist_model == "equidistant") { + camera_intrinsics.insert({i, std::make_shared(matrix_wh.at(0), matrix_wh.at(1))}); + camera_intrinsics.at(i)->set_value(cam_calib); + } else { + camera_intrinsics.insert({i, std::make_shared(matrix_wh.at(0), matrix_wh.at(1))}); + camera_intrinsics.at(i)->set_value(cam_calib); + } camera_extrinsics.insert({i, cam_eigen}); - camera_wh.insert({i, wh}); } parser->parse_config("use_mask", use_mask); if (use_mask) { @@ -286,13 +287,22 @@ struct VioManagerOptions { PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); PRINT_DEBUG(" - camera masks?: %d\n", use_mask); - assert(state_options.num_cameras == (int)camera_fisheye.size()); + if (state_options.num_cameras != (int)camera_intrinsics.size() || state_options.num_cameras != (int)camera_extrinsics.size()) { + PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET); + PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras (camera_intrinsics)\n" RESET, (int)camera_intrinsics.size(), + state_options.num_cameras); + PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras (camera_extrinsics)\n" RESET, (int)camera_extrinsics.size(), + state_options.num_cameras); + std::exit(EXIT_FAILURE); + } for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; - ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; - ss << "cam_" << n << "_wh:" << std::endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; - ss << "cam_" << n << "_intrinsic(0:3):" << std::endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; - ss << "cam_" << n << "_intrinsic(4:7):" << std::endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_fisheye:" << (std::dynamic_pointer_cast(camera_intrinsics.at(n)) != nullptr) << std::endl; + ss << "cam_" << n << "_wh:" << std::endl << camera_intrinsics.at(n)->w() << " x " << camera_intrinsics.at(n)->h() << std::endl; + ss << "cam_" << n << "_intrinsic(0:3):" << std::endl + << camera_intrinsics.at(n)->get_value().block(0, 0, 4, 1).transpose() << std::endl; + ss << "cam_" << n << "_intrinsic(4:7):" << std::endl + << camera_intrinsics.at(n)->get_value().block(4, 0, 4, 1).transpose() << std::endl; ss << "cam_" << n << "_extrinsic(0:3):" << std::endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; ss << "cam_" << n << "_extrinsic(4:6):" << std::endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 6f7375073..eb56f5a2f 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -102,6 +102,11 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ if (boost::filesystem::exists(filepath_std)) boost::filesystem::remove(filepath_std); + // Create folder path to this location if not exists + boost::filesystem::create_directories(boost::filesystem::path(filepath_est.c_str()).parent_path()); + boost::filesystem::create_directories(boost::filesystem::path(filepath_std.c_str()).parent_path()); + boost::filesystem::create_directories(boost::filesystem::path(filepath_gt.c_str()).parent_path()); + // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); @@ -373,7 +378,8 @@ void ROS1Visualizer::callback_monocular(const sensor_msgs::ImageConstPtr &msg0, _app->feed_measurement_camera(message); } -void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1) { +void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, + int cam_id1) { // Get the image cv_bridge::CvImageConstPtr cv_ptr0; @@ -416,7 +422,6 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con _app->feed_measurement_camera(message); } - void ROS1Visualizer::publish_state() { // Get the current state @@ -726,10 +731,11 @@ void ROS1Visualizer::publish_loopclosure_information() { pub_loop_extrinsic.publish(odometry_calib); // PUBLISH CAMERA0 INTRINSICS + bool is_fisheye = (std::dynamic_pointer_cast(_app->get_params().camera_intrinsics.at(0)) != nullptr); sensor_msgs::CameraInfo cameraparams; cameraparams.header = header; cameraparams.header.frame_id = "cam0"; - cameraparams.distortion_model = (_app->get_params().camera_fisheye.at(0)) ? "equidistant" : "plumb_bob"; + cameraparams.distortion_model = is_fisheye ? "equidistant" : "plumb_bob"; Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); cameraparams.D = {cparams(4), cparams(5), cparams(6), cparams(7)}; cameraparams.K = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1}; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index f70e44c70..8019e1812 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -737,10 +737,11 @@ void ROS2Visualizer::publish_loopclosure_information() { pub_loop_extrinsic->publish(odometry_calib); // PUBLISH CAMERA0 INTRINSICS + bool is_fisheye = (std::dynamic_pointer_cast(_app->get_params().camera_intrinsics.at(0)) != nullptr); sensor_msgs::msg::CameraInfo cameraparams; cameraparams.header = header; cameraparams.header.frame_id = "cam0"; - cameraparams.distortion_model = (_app->get_params().camera_fisheye.at(0)) ? "equidistant" : "plumb_bob"; + cameraparams.distortion_model = is_fisheye ? "equidistant" : "plumb_bob"; Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); cameraparams.d = {cparams(4), cparams(5), cparams(6), cparams(7)}; cameraparams.k = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1}; diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h index 45fc7c6fc..0d2c8bf0e 100644 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ b/ov_msckf/src/ros/RosVisualizerHelper.h @@ -263,14 +263,14 @@ class RosVisualizerHelper { assert(state->_options.num_cameras == sim->get_true_parameters().state_options.num_cameras); for (int i = 0; i < state->_options.num_cameras; i++) { // Intrinsics values - of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)(0) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(1) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(2) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(3) << " "; - of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)(4) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(5) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(6) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)(7) << " "; + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(0) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(1) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(2) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(3) << " "; + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(4) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(5) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(6) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(7) << " "; // Rotation and position of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(0) << " " << sim->get_true_parameters().camera_extrinsics.at(i)(1) << " " diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 336f16eb2..0389c3094 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -35,29 +35,18 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { PRINT_DEBUG("=======================================\n"); // Store a copy of our params + // NOTE: We need to explicitly create a copy of our shared pointers to the camera objects + // NOTE: Otherwise if we perturb it would also change our "true" parameters this->params = params_; - params.print_and_load_estimator(); - params.print_and_load_noise(); - params.print_and_load_state(); - params.print_and_load_trackers(); - params.print_and_load_simulation(); - - // Check that the max cameras matches the size of our cam matrices - if (params.state_options.num_cameras != (int)params.camera_fisheye.size()) { - PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET); - PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras\n" RESET, (int)params.camera_fisheye.size(), - params.state_options.num_cameras); - std::exit(EXIT_FAILURE); - } - - // Loop through and load each of the cameras - for (int i = 0; i < params.state_options.num_cameras; i++) { - if (params.camera_fisheye.at(i)) { - _cam_intrinsics_cameras.insert({i, std::make_shared()}); - _cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); + params.camera_intrinsics.clear(); + for (auto const &tmp : params_.camera_intrinsics) { + auto tmp_cast = std::dynamic_pointer_cast(tmp.second); + if (tmp_cast != nullptr) { + params.camera_intrinsics.insert({tmp.first, std::make_shared(tmp.second->w(), tmp.second->h())}); + params.camera_intrinsics.at(tmp.first)->set_value(params_.camera_intrinsics.at(tmp.first)->get_value()); } else { - _cam_intrinsics_cameras.insert({i, std::make_shared()}); - _cam_intrinsics_cameras.at(i)->set_value(params.camera_intrinsics.at(i)); + params.camera_intrinsics.insert({tmp.first, std::make_shared(tmp.second->w(), tmp.second->h())}); + params.camera_intrinsics.at(tmp.first)->set_value(params_.camera_intrinsics.at(tmp.first)->get_value()); } } @@ -153,32 +142,39 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // camera intrinsics and extrinsics for (int i = 0; i < params_.state_options.num_cameras; i++) { - // Camera intrinsic properties (k1, k2, p1, p2) + // Camera intrinsic properties (k1, k2, p1, p2, r1, r2, r3, r4) + Eigen::MatrixXd intrinsics = params_.camera_intrinsics.at(i)->get_value(); for (int r = 0; r < 4; r++) { - params_.camera_intrinsics.at(i)(r) += 1.0 * w(gen_state_perturb); + intrinsics(r) += 1.0 * w(gen_state_perturb); } - - // Camera intrinsic properties (r1, r2, r3, r4) for (int r = 4; r < 8; r++) { - params_.camera_intrinsics.at(i)(r) += 0.005 * w(gen_state_perturb); - } - - // Our camera extrinsics transform (position) - for (int r = 4; r < 7; r++) { - params_.camera_extrinsics.at(i)(r) += 0.01 * w(gen_state_perturb); + intrinsics(r) += 0.005 * w(gen_state_perturb); } + params_.camera_intrinsics.at(i)->set_value(intrinsics); // Our camera extrinsics transform (orientation) Eigen::Vector3d w_vec; w_vec << 0.001 * w(gen_state_perturb), 0.001 * w(gen_state_perturb), 0.001 * w(gen_state_perturb); params_.camera_extrinsics.at(i).block(0, 0, 4, 1) = rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.camera_extrinsics.at(i).block(0, 0, 4, 1))); + + // Our camera extrinsics transform (position) + for (int r = 4; r < 7; r++) { + params_.camera_extrinsics.at(i)(r) += 0.01 * w(gen_state_perturb); + } } } //=============================================================== //=============================================================== + // Debug print simulation parameters + params.print_and_load_estimator(); + params.print_and_load_noise(); + params.print_and_load_state(); + params.print_and_load_trackers(); + params.print_and_load_simulation(); + // We will create synthetic camera frames and ensure that each has enough features // double dt = 0.25/freq_cam; double dt = 0.25; @@ -388,11 +384,6 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, feats.push_back(uvs); camids.push_back(i); } - /** - * @brief This will load the trajectory into memory. - * @param path_traj Path to the trajectory file that we want to read in. - */ - void load_data(std::string path_traj); // Return success return true; @@ -404,15 +395,13 @@ std::vector> Simulator::project_pointcloud(co // Assert we have good camera assert(camid < params.state_options.num_cameras); - assert((int)params.camera_fisheye.size() == params.state_options.num_cameras); - assert((int)params.camera_wh.size() == params.state_options.num_cameras); - assert((int)_cam_intrinsics_cameras.size() == params.state_options.num_cameras); + assert((int)params.camera_intrinsics.size() == params.state_options.num_cameras); assert((int)params.camera_extrinsics.size() == params.state_options.num_cameras); // Grab our extrinsic and intrinsic values Eigen::Matrix R_ItoC = quat_2_Rot(params.camera_extrinsics.at(camid).block(0, 0, 4, 1)); Eigen::Matrix p_IinC = params.camera_extrinsics.at(camid).block(4, 0, 3, 1); - std::shared_ptr camera = _cam_intrinsics_cameras.at(camid); + std::shared_ptr camera = params.camera_intrinsics.at(camid); // Our projected uv true measurements std::vector> uvs; @@ -437,8 +426,7 @@ std::vector> Simulator::project_pointcloud(co uv_dist = camera->distort_f(uv_norm); // Check that it is inside our bounds - if (uv_dist(0) < 0 || uv_dist(0) > params.camera_wh.at(camid).first || uv_dist(1) < 0 || - uv_dist(1) > params.camera_wh.at(camid).second) { + if (uv_dist(0) < 0 || uv_dist(0) > camera->w() || uv_dist(1) < 0 || uv_dist(1) > camera->h()) { continue; } @@ -455,22 +443,20 @@ void Simulator::generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vect // Assert we have good camera assert(camid < params.state_options.num_cameras); - assert((int)params.camera_fisheye.size() == params.state_options.num_cameras); - assert((int)params.camera_wh.size() == params.state_options.num_cameras); - assert((int)_cam_intrinsics_cameras.size() == params.state_options.num_cameras); + assert((int)params.camera_intrinsics.size() == params.state_options.num_cameras); assert((int)params.camera_extrinsics.size() == params.state_options.num_cameras); // Grab our extrinsic and intrinsic values Eigen::Matrix R_ItoC = quat_2_Rot(params.camera_extrinsics.at(camid).block(0, 0, 4, 1)); Eigen::Matrix p_IinC = params.camera_extrinsics.at(camid).block(4, 0, 3, 1); - std::shared_ptr camera = _cam_intrinsics_cameras.at(camid); + std::shared_ptr camera = params.camera_intrinsics.at(camid); // Generate the desired number of features for (int i = 0; i < numpts; i++) { // Uniformly randomly generate within our fov - std::uniform_real_distribution gen_u(0, params.camera_wh.at(camid).first); - std::uniform_real_distribution gen_v(0, params.camera_wh.at(camid).second); + std::uniform_real_distribution gen_u(0, camera->w()); + std::uniform_real_distribution gen_v(0, camera->h()); double u_dist = gen_u(gen_state_init); double v_dist = gen_v(gen_state_init); diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index 3686a008b..df6723cf5 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -33,11 +33,12 @@ #include "cam/CamBase.h" #include "cam/CamEqui.h" #include "cam/CamRadtan.h" -#include "core/VioManagerOptions.h" #include "sim/BsplineSE3.h" #include "utils/colors.h" #include "utils/dataset_reader.h" +#include "core/VioManagerOptions.h" + namespace ov_msckf { /** @@ -142,9 +143,6 @@ class Simulator { /// True vio manager params (a copy of the parsed ones) VioManagerOptions params; - /// Camera intrinsics camera objects - std::unordered_map> _cam_intrinsics_cameras; - //=================================================================== // State related variables //=================================================================== From 847bbe567c552c48de2c62c8a3ba44b58b44cdb1 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 22 Nov 2021 00:17:27 -0500 Subject: [PATCH 34/55] properly downsample cameras, move simulation perturbation logic into its own static function --- ov_msckf/src/core/VioManagerOptions.h | 4 +- ov_msckf/src/sim/Simulator.cpp | 78 ++++++++++++++------------- ov_msckf/src/sim/Simulator.h | 7 +++ 3 files changed, 52 insertions(+), 37 deletions(-) diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 71efa6142..560727289 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -215,6 +215,7 @@ struct VioManagerOptions { if (parser != nullptr) { parser->parse_config("gravity_mag", gravity_mag); parser->parse_config("max_cameras", state_options.num_cameras); // might be redundant + parser->parse_config("downsample_cameras", downsample_cameras); // might be redundant for (int i = 0; i < state_options.num_cameras; i++) { // Time offset (use the first one) @@ -285,7 +286,6 @@ struct VioManagerOptions { PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); - PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); PRINT_DEBUG(" - camera masks?: %d\n", use_mask); if (state_options.num_cameras != (int)camera_intrinsics.size() || state_options.num_cameras != (int)camera_extrinsics.size()) { PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET); @@ -295,6 +295,7 @@ struct VioManagerOptions { state_options.num_cameras); std::exit(EXIT_FAILURE); } + PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); for (int n = 0; n < state_options.num_cameras; n++) { std::stringstream ss; ss << "cam_" << n << "_fisheye:" << (std::dynamic_pointer_cast(camera_intrinsics.at(n)) != nullptr) << std::endl; @@ -369,6 +370,7 @@ struct VioManagerOptions { parser->parse_config("use_klt", use_klt); parser->parse_config("use_aruco", use_aruco); parser->parse_config("downsize_aruco", downsize_aruco); + parser->parse_config("downsample_cameras", downsample_cameras); parser->parse_config("multi_threading", use_multi_threading); parser->parse_config("num_pts", num_pts); parser->parse_config("fast_threshold", fast_threshold); diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 0389c3094..c72ff36ac 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -130,51 +130,23 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { //=============================================================== //=============================================================== - // One std generator - std::normal_distribution w(0, 1); - // Perturb all calibration if we should if (params.sim_do_perturbation) { - // cam imu offset - params_.calib_camimu_dt += 0.01 * w(gen_state_perturb); - - // camera intrinsics and extrinsics - for (int i = 0; i < params_.state_options.num_cameras; i++) { - - // Camera intrinsic properties (k1, k2, p1, p2, r1, r2, r3, r4) - Eigen::MatrixXd intrinsics = params_.camera_intrinsics.at(i)->get_value(); - for (int r = 0; r < 4; r++) { - intrinsics(r) += 1.0 * w(gen_state_perturb); - } - for (int r = 4; r < 8; r++) { - intrinsics(r) += 0.005 * w(gen_state_perturb); - } - params_.camera_intrinsics.at(i)->set_value(intrinsics); + // Do the perturbation + perturb_parameters(gen_state_perturb, params_); - // Our camera extrinsics transform (orientation) - Eigen::Vector3d w_vec; - w_vec << 0.001 * w(gen_state_perturb), 0.001 * w(gen_state_perturb), 0.001 * w(gen_state_perturb); - params_.camera_extrinsics.at(i).block(0, 0, 4, 1) = - rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.camera_extrinsics.at(i).block(0, 0, 4, 1))); - - // Our camera extrinsics transform (position) - for (int r = 4; r < 7; r++) { - params_.camera_extrinsics.at(i)(r) += 0.01 * w(gen_state_perturb); - } - } + // Debug print simulation parameters + params.print_and_load_estimator(); + params.print_and_load_noise(); + params.print_and_load_state(); + params.print_and_load_trackers(); + params.print_and_load_simulation(); } //=============================================================== //=============================================================== - // Debug print simulation parameters - params.print_and_load_estimator(); - params.print_and_load_noise(); - params.print_and_load_state(); - params.print_and_load_trackers(); - params.print_and_load_simulation(); - // We will create synthetic camera frames and ensure that each has enough features // double dt = 0.25/freq_cam; double dt = 0.25; @@ -222,6 +194,40 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { sleep(1); } +void Simulator::perturb_parameters(std::mt19937 gen_state, VioManagerOptions ¶ms_) { + + // One std generator + std::normal_distribution w(0, 1); + + // Camera IMU offset + params_.calib_camimu_dt += 0.01 * w(gen_state); + + // Camera intrinsics and extrinsics + for (int i = 0; i < params_.state_options.num_cameras; i++) { + + // Camera intrinsic properties (k1, k2, p1, p2, r1, r2, r3, r4) + Eigen::MatrixXd intrinsics = params_.camera_intrinsics.at(i)->get_value(); + for (int r = 0; r < 4; r++) { + intrinsics(r) += 1.0 * w(gen_state); + } + for (int r = 4; r < 8; r++) { + intrinsics(r) += 0.005 * w(gen_state); + } + params_.camera_intrinsics.at(i)->set_value(intrinsics); + + // Our camera extrinsics transform (orientation) + Eigen::Vector3d w_vec; + w_vec << 0.001 * w(gen_state), 0.001 * w(gen_state), 0.001 * w(gen_state); + params_.camera_extrinsics.at(i).block(0, 0, 4, 1) = + rot_2_quat(exp_so3(w_vec) * quat_2_Rot(params_.camera_extrinsics.at(i).block(0, 0, 4, 1))); + + // Our camera extrinsics transform (position) + for (int r = 4; r < 7; r++) { + params_.camera_extrinsics.at(i)(r) += 0.01 * w(gen_state); + } + } +} + bool Simulator::get_state(double desired_time, Eigen::Matrix &imustate) { // Set to default state diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index df6723cf5..6ea139832 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -61,6 +61,13 @@ class Simulator { */ Simulator(VioManagerOptions ¶ms_); + /** + * @brief Will get a set of perturbed parameters + * @param gen_state Random number gen to use + * @param params_ Parameters we will perturb + */ + static void perturb_parameters(std::mt19937 gen_state, VioManagerOptions ¶ms_); + /** * @brief Returns if we are actively simulating * @return True if we still have simulation data From 77fb454687df3a129f86827ed56e34515cfa2043 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 10:01:50 -0500 Subject: [PATCH 35/55] small cleanup --- ov_core/package.xml | 4 ++-- ov_eval/package.xml | 4 ++-- ov_init/package.xml | 4 ++-- ov_init/src/init/InertialInitializer.cpp | 14 +------------- ov_msckf/package.xml | 4 ++-- 5 files changed, 9 insertions(+), 21 deletions(-) diff --git a/ov_core/package.xml b/ov_core/package.xml index e3d7914c8..a8e95b4b5 100644 --- a/ov_core/package.xml +++ b/ov_core/package.xml @@ -3,11 +3,11 @@ ov_core - 2.4.0 + 2.5.0 Core algorithms for visual-inertial navigation algorithms. - https://github.com/rpng/open_vins + https://docs.openvins.com/ Patrick Geneva diff --git a/ov_eval/package.xml b/ov_eval/package.xml index 0c52c0a39..4dae434e2 100644 --- a/ov_eval/package.xml +++ b/ov_eval/package.xml @@ -3,11 +3,11 @@ ov_eval - 2.4.0 + 2.5.0 Evaluation methods and scripts for visual-inertial odometry systems. - https://github.com/rpng/open_vins + https://docs.openvins.com/ Patrick Geneva diff --git a/ov_init/package.xml b/ov_init/package.xml index 4045f94ef..1a0bff1b0 100644 --- a/ov_init/package.xml +++ b/ov_init/package.xml @@ -3,11 +3,11 @@ ov_init - 2.4.0 + 2.5.0 Initialization package which handles dynamic and static initialization. - https://github.com/rpng/open_vins + https://docs.openvins.com/ Patrick Geneva diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp index 6ec61e35e..910f16099 100644 --- a/ov_init/src/init/InertialInitializer.cpp +++ b/ov_init/src/init/InertialInitializer.cpp @@ -34,23 +34,11 @@ InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_, st // Create static initializer init_static = std::make_shared(params, _db, imu_data); - // TODO: create the feature tracker here - // TODO: create dynamic initialize class object } bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, std::shared_ptr t_imu, bool wait_for_jerk) { - // TODO: calculate the current disparity to see if we are stationary - - // TODO: if not stationary, first try to do our dynamic initialization, return on success - - // TODO: return if we need to initalize any of the calibration (only dynamic can do this) - - // TODO: return if we are wait_for_jerk and disparity says we are moving (handles constant accel case) - - // TODO: if we have calibration, check if we are still, and use our static initalizer - - // TEMP: use our static initializer! + // Use our static initializer! return init_static->initialize(timestamp, covariance, order, t_imu, wait_for_jerk); } diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 985e9e424..26f7cd8ec 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -3,11 +3,11 @@ ov_msckf - 2.4.0 + 2.5.0 Implementation of a type-based error-state Kalman filter. - https://github.com/rpng/open_vins + https://docs.openvins.com/ Patrick Geneva From 01204116fc162803903bb7f9b0140454735f0982 Mon Sep 17 00:00:00 2001 From: ccchu0816 Date: Tue, 23 Nov 2021 13:07:54 -0500 Subject: [PATCH 36/55] Modified all the sim scripts. tum_vi and euroc_mav scripts. Downloading the others --- ov_msckf/launch/serial.launch | 3 +- ov_msckf/scripts/run_ros_eth.sh | 30 +++-------- ov_msckf/scripts/run_ros_tumvi.sh | 30 ++++------- ov_msckf/scripts/run_sim_calib.sh | 86 ++++++++++--------------------- 4 files changed, 47 insertions(+), 102 deletions(-) diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 77315395e..d95d3d4b3 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -10,7 +10,8 @@ - + + diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh index fd7f1b7b6..f3cc03ea8 100755 --- a/ov_msckf/scripts/run_ros_eth.sh +++ b/ov_msckf/scripts/run_ros_eth.sh @@ -1,8 +1,8 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= @@ -47,31 +47,16 @@ bagstarttimes=( "18" ) -# threshold for variance to detect if the unit has moved yet -imuthreshold=( - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" - "1.5" -) # location to save log files into -save_path1="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_euroc/algorithms" -save_path2="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_euroc/timings" -bag_path="/media/patrick/RPNG\ FLASH\ 2/euroc" - +save_path1="/home/chuchu/test_ov/openvins_pra/exp_euroc/algorithms" +save_path2="/home/chuchu/test_ov/openvins_pra/exp_euroc/timings" +bag_path="/home/chuchu/datasets/euroc_mav/" #============================================================= #============================================================= #============================================================= - # Loop through all modes for h in "${!modes[@]}"; do # Loop through all datasets @@ -104,15 +89,16 @@ then fi # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_ros_eth.launch \ +roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ + config:="euroc_mav" \ bag:="$bag_path/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ - init_imu_thresh:="${imuthreshold[i]}" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ + dolivetraj:="true" \ path_time:="$filename_time" &> /dev/null # print out the time elapsed diff --git a/ov_msckf/scripts/run_ros_tumvi.sh b/ov_msckf/scripts/run_ros_tumvi.sh index 9c9130086..87989c8bd 100755 --- a/ov_msckf/scripts/run_ros_tumvi.sh +++ b/ov_msckf/scripts/run_ros_tumvi.sh @@ -1,14 +1,13 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= #============================================================= - # estimator configurations modes=( "mono" @@ -37,28 +36,16 @@ bagstarttimes=( "0" ) -# threshold for variance to detect if the unit has moved yet -# these datasets seem to have very large variablity in their starts -imuthreshold=( - "0.60" - "0.60" - "0.60" - "0.60" - "0.60" - "0.45" -) - # location to save log files into -save_path1="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_tum/algorithms" -save_path2="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_tum/timings" -bag_path="/media/patrick/RPNG\ FLASH\ 2/tumvi" +save_path1="/home/chuchu/test_ov/openvins_pra/exp_tum/algorithms" +save_path2="/home/chuchu/test_ov/openvins_pra/exp_tum/timings" +bag_path="/home/chuchu/datasets/tum_vi" #============================================================= #============================================================= #============================================================= - - +# TODO: Still need to test all, see how to get imu threshold in # Loop through all modes for h in "${!modes[@]}"; do # Loop through all datasets @@ -91,15 +78,16 @@ then fi # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_ros_tum.launch \ +roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ + config:="tum_vi" \ bag:="$bag_path/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ - init_imu_thresh:="${imuthreshold[i]}" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ + dolivetraj:="true" \ path_time:="$filename_time" &> /dev/null # print out the time elapsed diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 719104417..63bcbd4f6 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -17,89 +17,59 @@ datasets=( # "udel_neighborhood" ) +# If we want to calibrate parameters +sim_do_calibration=( + "false" + "true" +) + +# If we want to perturb the initial state values +sim_do_perturbation=( + "false" + "true" +) + # location to save log files into -save_path_est="/home/cc/test/openvins_pra/sim_calib/algorithms" -save_path_gt="/home/cc/test/openvins_pra/sim_calib/truths" +save_path_est="/home/chuchu/test_ov/openvins_pra/sim_calib/algorithms" +save_path_gt="/home/chuchu/test_ov/openvins_pra/sim_calib/truths" #============================================================= # Start the Monte Carlo Simulations #============================================================= -# Loop through if use fej or not +# Loop through the datasets for h in "${!datasets[@]}"; do - -# groundtruth file save location -filename_gt="$save_path_gt/${datasets[h]}.txt" - +# Loop through if we want to calibrate +for m in "${!sim_do_calibration[@]}"; do +# Loop through if we want to perturb +for n in "${!sim_do_perturbation[@]}"; do # Monte Carlo runs for this dataset for j in {00..02}; do -#=============================================== -#=============================================== -start_time="$(date -u +%s)" -filename_est="$save_path_est/ov_23_static_groundtruth/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf simulation.launch \ - seed:="$j" dataset:="${datasets[h]}.txt" \ - sim_do_calibration:="false" \ - sim_do_perturbation:="false" \ - dosave_pose:="true" \ - path_est:="$filename_est" \ - path_gt:="$filename_gt" &> /dev/null -end_time="$(date -u +%s)" -elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - static_groundtruth - run $j took $elapsed seconds"; -#=============================================== -#=============================================== -start_time="$(date -u +%s)" -filename_est="$save_path_est/ov_23_calib_groundtruth/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf simulation.launch \ - seed:="$j" \ - dataset:="${datasets[h]}.txt" \ - sim_do_calibration:="true" \ - sim_do_perturbation:="false" \ - dosave_pose:="true" \ - path_est:="$filename_est" \ - path_gt:="$filename_gt" &> /dev/null -end_time="$(date -u +%s)" -elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - calib_groundtruth - run $j took $elapsed seconds"; - -#=============================================== -#=============================================== -start_time="$(date -u +%s)" -filename_est="$save_path_est/ov_23_calib_perturbed/${datasets[h]}/estimate_$j.txt" -roslaunch ov_msckf simulation.launch \ - seed:="$j" \ - dataset:="${datasets[h]}.txt" \ - sim_do_calibration:="true" \ - sim_do_perturbation:="true" \ - dosave_pose:="true" \ - path_est:="$filename_est" \ - path_gt:="$filename_gt" &> /dev/null -end_time="$(date -u +%s)" -elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - calib_perturbed - run $j took $elapsed seconds"; +filename_est="$save_path_est/calib_${sim_do_calibration[m]}/perturb_${sim_do_perturbation[n]}/${datasets[h]}/estimate_$j.txt" +filename_gt="$save_path_gt/${datasets[h]}.txt" #=============================================== +# Start Monte Carlo Simulations #=============================================== start_time="$(date -u +%s)" -filename_est="$save_path_est/ov_23_static_perturbed/${datasets[h]}/estimate_$j.txt" roslaunch ov_msckf simulation.launch \ - seed:="$j" \ + seed:="$((10#$j + 1))" \ dataset:="${datasets[h]}.txt" \ - sim_do_calibration:="false" \ - sim_do_perturbation:="true" \ + sim_do_calibration:="${sim_do_calibration[m]}" \ + sim_do_perturbation:="${sim_do_perturbation[n]}" \ dosave_pose:="true" \ path_est:="$filename_est" \ path_gt:="$filename_gt" &> /dev/null end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - static_perturbed - run $j took $elapsed seconds"; +echo "BASH: ${datasets[h]} - calib_${sim_do_calibration[m]} - perturb_${sim_do_perturbation[n]} - run $j took $elapsed seconds"; #=============================================== #=============================================== - +done +done done done From d7f4f7515deed83ce7e788029a3c61ecc6cfa921 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 10:04:19 -0500 Subject: [PATCH 37/55] move config folder to root directory --- .../euroc_mav/estimator_config.yaml | 0 .../euroc_mav/kalibr_imu_chain.yaml | 0 .../euroc_mav/kalibr_imucam_chain.yaml | 0 .../kaist/estimator_config.yaml | 0 .../kaist/kalibr_imu_chain.yaml | 0 .../kaist/kalibr_imucam_chain.yaml | 0 .../kaist/pgeneva_ros_kaist.launch.old | 0 .../kaist_vio/kalibr_imu_chain.yaml | 0 .../kaist_vio/kalibr_imucam_chain.yaml | 0 .../kaist_vio/pgeneva_ros_kaistvio.launch.old | 0 .../rpng_aruco/estimator_config.yaml | 0 .../rpng_aruco/kalibr_imu_chain.yaml | 0 .../rpng_aruco/kalibr_imucam_chain.yaml | 0 .../rpng_ironsides/estimator_config.yaml | 0 .../rpng_ironsides/kalibr_imu_chain.yaml | 0 .../rpng_ironsides/kalibr_imucam_chain.yaml | 0 .../rpng_sim/estimator_config.yaml | 0 .../rpng_sim/kalibr_imu_chain.yaml | 0 .../rpng_sim/kalibr_imucam_chain.yaml | 0 .../tum_vi/estimator_config.yaml | 0 .../tum_vi/kalibr_imu_chain.yaml | 0 .../tum_vi/kalibr_imucam_chain.yaml | 0 .../uzhfpv_indoor/estimator_config.yaml | 0 .../uzhfpv_indoor/kalibr_imu_chain.yaml | 0 .../uzhfpv_indoor/kalibr_imucam_chain.yaml | 0 .../uzhfpv_indoor_45/estimator_config.yaml | 0 .../uzhfpv_indoor_45/kalibr_imu_chain.yaml | 0 .../uzhfpv_indoor_45/kalibr_imucam_chain.yaml | 0 ov_msckf/launch/serial.launch | 5 ++--- ov_msckf/launch/simulation.launch | 16 ++++++++-------- ov_msckf/launch/subscribe.launch | 2 +- 31 files changed, 11 insertions(+), 12 deletions(-) rename {ov_msckf/config => config}/euroc_mav/estimator_config.yaml (100%) rename {ov_msckf/config => config}/euroc_mav/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/euroc_mav/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/kaist/estimator_config.yaml (100%) rename {ov_msckf/config => config}/kaist/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/kaist/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/kaist/pgeneva_ros_kaist.launch.old (100%) rename {ov_msckf/config => config}/kaist_vio/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/kaist_vio/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/kaist_vio/pgeneva_ros_kaistvio.launch.old (100%) rename {ov_msckf/config => config}/rpng_aruco/estimator_config.yaml (100%) rename {ov_msckf/config => config}/rpng_aruco/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/rpng_aruco/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/rpng_ironsides/estimator_config.yaml (100%) rename {ov_msckf/config => config}/rpng_ironsides/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/rpng_ironsides/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/rpng_sim/estimator_config.yaml (100%) rename {ov_msckf/config => config}/rpng_sim/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/rpng_sim/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/tum_vi/estimator_config.yaml (100%) rename {ov_msckf/config => config}/tum_vi/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/tum_vi/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor/estimator_config.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor/kalibr_imucam_chain.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor_45/estimator_config.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor_45/kalibr_imu_chain.yaml (100%) rename {ov_msckf/config => config}/uzhfpv_indoor_45/kalibr_imucam_chain.yaml (100%) diff --git a/ov_msckf/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml similarity index 100% rename from ov_msckf/config/euroc_mav/estimator_config.yaml rename to config/euroc_mav/estimator_config.yaml diff --git a/ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml b/config/euroc_mav/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/euroc_mav/kalibr_imu_chain.yaml rename to config/euroc_mav/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml b/config/euroc_mav/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/euroc_mav/kalibr_imucam_chain.yaml rename to config/euroc_mav/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml similarity index 100% rename from ov_msckf/config/kaist/estimator_config.yaml rename to config/kaist/estimator_config.yaml diff --git a/ov_msckf/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/kaist/kalibr_imu_chain.yaml rename to config/kaist/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/kaist/kalibr_imucam_chain.yaml b/config/kaist/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/kaist/kalibr_imucam_chain.yaml rename to config/kaist/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/kaist/pgeneva_ros_kaist.launch.old b/config/kaist/pgeneva_ros_kaist.launch.old similarity index 100% rename from ov_msckf/config/kaist/pgeneva_ros_kaist.launch.old rename to config/kaist/pgeneva_ros_kaist.launch.old diff --git a/ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/kaist_vio/kalibr_imu_chain.yaml rename to config/kaist_vio/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml b/config/kaist_vio/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/kaist_vio/kalibr_imucam_chain.yaml rename to config/kaist_vio/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch.old b/config/kaist_vio/pgeneva_ros_kaistvio.launch.old similarity index 100% rename from ov_msckf/config/kaist_vio/pgeneva_ros_kaistvio.launch.old rename to config/kaist_vio/pgeneva_ros_kaistvio.launch.old diff --git a/ov_msckf/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml similarity index 100% rename from ov_msckf/config/rpng_aruco/estimator_config.yaml rename to config/rpng_aruco/estimator_config.yaml diff --git a/ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml b/config/rpng_aruco/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_aruco/kalibr_imu_chain.yaml rename to config/rpng_aruco/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml b/config/rpng_aruco/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_aruco/kalibr_imucam_chain.yaml rename to config/rpng_aruco/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml similarity index 100% rename from ov_msckf/config/rpng_ironsides/estimator_config.yaml rename to config/rpng_ironsides/estimator_config.yaml diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml b/config/rpng_ironsides/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_ironsides/kalibr_imu_chain.yaml rename to config/rpng_ironsides/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml b/config/rpng_ironsides/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_ironsides/kalibr_imucam_chain.yaml rename to config/rpng_ironsides/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml similarity index 100% rename from ov_msckf/config/rpng_sim/estimator_config.yaml rename to config/rpng_sim/estimator_config.yaml diff --git a/ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml b/config/rpng_sim/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_sim/kalibr_imu_chain.yaml rename to config/rpng_sim/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml b/config/rpng_sim/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/rpng_sim/kalibr_imucam_chain.yaml rename to config/rpng_sim/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml similarity index 100% rename from ov_msckf/config/tum_vi/estimator_config.yaml rename to config/tum_vi/estimator_config.yaml diff --git a/ov_msckf/config/tum_vi/kalibr_imu_chain.yaml b/config/tum_vi/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/tum_vi/kalibr_imu_chain.yaml rename to config/tum_vi/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml b/config/tum_vi/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/tum_vi/kalibr_imucam_chain.yaml rename to config/tum_vi/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor/estimator_config.yaml rename to config/uzhfpv_indoor/estimator_config.yaml diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml b/config/uzhfpv_indoor/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor/kalibr_imu_chain.yaml rename to config/uzhfpv_indoor/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml b/config/uzhfpv_indoor/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor/kalibr_imucam_chain.yaml rename to config/uzhfpv_indoor/kalibr_imucam_chain.yaml diff --git a/ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor_45/estimator_config.yaml rename to config/uzhfpv_indoor_45/estimator_config.yaml diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor_45/kalibr_imu_chain.yaml rename to config/uzhfpv_indoor_45/kalibr_imu_chain.yaml diff --git a/ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml b/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml similarity index 100% rename from ov_msckf/config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml rename to config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index d95d3d4b3..514cb6b6d 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -3,15 +3,14 @@ - + - - + diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 4fa0c3739..31716e9e5 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -7,7 +7,7 @@ - + @@ -82,15 +82,15 @@ - + - - - - - - + + + + + + diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 3d9aaf5ae..12e4b93a3 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -3,7 +3,7 @@ - + From a4c0f8e9171ed2be84bb01d9ae2b6cd6264299a3 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 10:18:51 -0500 Subject: [PATCH 38/55] small cleanup --- ov_msckf/launch/serial.launch | 18 ++++++++-------- ov_msckf/launch/simulation.launch | 34 ++++++++++-------------------- ov_msckf/scripts/run_ros_uzhfpv.sh | 3 ++- ov_msckf/scripts/run_sim_calib.sh | 1 - ov_msckf/scripts/run_sim_cams.sh | 1 - ov_msckf/scripts/run_sim_rep.sh | 1 - 6 files changed, 22 insertions(+), 36 deletions(-) diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index 514cb6b6d..cf1774b1e 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -28,21 +28,21 @@ - - - + + + - - + + - - + + - - + + diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 31716e9e5..b5548d38d 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -21,7 +21,7 @@ - + @@ -49,7 +49,7 @@ - + @@ -57,17 +57,17 @@ - - - - + + + + - - + + @@ -77,25 +77,13 @@ - - - + + + - - - - - - - - - - - - diff --git a/ov_msckf/scripts/run_ros_uzhfpv.sh b/ov_msckf/scripts/run_ros_uzhfpv.sh index bec63d591..9aa4a39cd 100755 --- a/ov_msckf/scripts/run_ros_uzhfpv.sh +++ b/ov_msckf/scripts/run_ros_uzhfpv.sh @@ -1,7 +1,8 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 63bcbd4f6..5cc0d463d 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -14,7 +14,6 @@ datasets=( # "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" -# "udel_neighborhood" ) # If we want to calibrate parameters diff --git a/ov_msckf/scripts/run_sim_cams.sh b/ov_msckf/scripts/run_sim_cams.sh index 4d3f9249a..c90b73dfc 100755 --- a/ov_msckf/scripts/run_sim_cams.sh +++ b/ov_msckf/scripts/run_sim_cams.sh @@ -14,7 +14,6 @@ datasets=( # "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" -# "udel_neighborhood" ) # number of cameras diff --git a/ov_msckf/scripts/run_sim_rep.sh b/ov_msckf/scripts/run_sim_rep.sh index 9c85198ae..36ea0d416 100755 --- a/ov_msckf/scripts/run_sim_rep.sh +++ b/ov_msckf/scripts/run_sim_rep.sh @@ -14,7 +14,6 @@ datasets=( "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" -# "udel_neighborhood" ) # estimator configurations From 1b7a5c9e6e6a3c92faf6bb4aaff0c0c10623b69d Mon Sep 17 00:00:00 2001 From: Jesse Bloecker Date: Wed, 8 Dec 2021 12:08:22 -0500 Subject: [PATCH 39/55] ros2 launch file --- ov_msckf/cmake/ROS2.cmake | 4 +++ ov_msckf/launch/subscribe.launch.py | 48 +++++++++++++++++++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 ov_msckf/launch/subscribe.launch.py diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index 8026b7072..9d2a5c06a 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -103,5 +103,9 @@ ament_target_dependencies(test_sim_repeat ${ament_libraries}) target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) install(TARGETS test_sim_repeat DESTINATION lib/${PROJECT_NAME}) +# Install launch and config directories +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) +install(DIRECTORY ../config/ DESTINATION share/${PROJECT_NAME}/) + # finally define this as the package ament_package() diff --git a/ov_msckf/launch/subscribe.launch.py b/ov_msckf/launch/subscribe.launch.py new file mode 100644 index 000000000..38996b316 --- /dev/null +++ b/ov_msckf/launch/subscribe.launch.py @@ -0,0 +1,48 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory, get_package_prefix +import os +import sys + +launch_args = [ + DeclareLaunchArgument(name='namespace', default_value='', description='namespace'), + DeclareLaunchArgument(name='ov_enable', default_value='true', description='enable openvins node'), + DeclareLaunchArgument(name='rviz_enable', default_value='true', description='enable rviz node'), + DeclareLaunchArgument(name='config', default_value='euroc_mav', description='euroc_mav, tum_vi, rpng_aruco...'), + DeclareLaunchArgument(name='verbosity', default_value='INFO', description='ALL, DEBUG, INFO, WARNING, ERROR, SILENT'), + DeclareLaunchArgument(name='use_stereo', default_value='true', description=''), + DeclareLaunchArgument(name='max_cameras', default_value='2', description='') +] + + +def launch_setup(context): + configs_dir=os.path.join(get_package_share_directory('ov_msckf'),'..','config') + available_configs = os.listdir(configs_dir) + config = LaunchConfiguration('config').perform(context) + if not config in available_configs: + return[LogInfo(msg='ERROR: unknown config: \'{}\' - Available configs are: {} - not starting OpenVINS'.format(config,', '.join(available_configs)))] + config_path = os.path.join(get_package_share_directory('ov_msckf'),'config',config,'estimator_config.yaml') + node1 = Node(package = 'ov_msckf', + executable = 'run_subscribe_msckf', + condition = IfCondition(LaunchConfiguration('ov_enable')), + namespace = LaunchConfiguration('namespace'), + parameters =[{'verbosity': LaunchConfiguration('verbosity')}, + {'use_stereo': LaunchConfiguration('use_stereo')}, + {'max_cameras': LaunchConfiguration('max_cameras')}, + {'config_path': config_path}]) + + node2 = Node(package = 'rviz2', + executable = 'rviz2', + condition = IfCondition(LaunchConfiguration('rviz_enable')), + arguments = ['-d'+os.path.join(get_package_share_directory('ov_msckf'),'launch','display_ros2.rviz'), '--ros-args', '--log-level', 'warn']) + + return [node1, node2] + +def generate_launch_description(): + opfunc = OpaqueFunction(function = launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld From 41a4f76b623f2fe41850905cc4811976e6258b5c Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 12:08:41 -0500 Subject: [PATCH 40/55] update documentation to include ROS 2 install and tutorial --- docs/dev-ros1-to-ros2.dox | 49 +++++++ docs/dev-welcome.dox | 1 + docs/gs-installing.dox | 112 +++++++++++++--- docs/gs-tutorial.dox | 208 ++++++++++++++++------------- ov_core/src/feat/FeatureHelper.h | 6 +- ov_core/src/utils/dataset_reader.h | 2 +- 6 files changed, 260 insertions(+), 118 deletions(-) create mode 100644 docs/dev-ros1-to-ros2.dox diff --git a/docs/dev-ros1-to-ros2.dox b/docs/dev-ros1-to-ros2.dox new file mode 100644 index 000000000..6589faaf0 --- /dev/null +++ b/docs/dev-ros1-to-ros2.dox @@ -0,0 +1,49 @@ +/** + + +@page dev-ros1-to-ros2 ROS1 to ROS2 Bag Conversion Guide + + + +@section gs-ros1-to-ros2-option-1 rosbags + +[rosbags](https://gitlab.com/ternaris/rosbags) is the simplest utility which does not depend on ROS installs at all. +ROS bag conversion is a hard problem since you need to have both ROS1 and ROS2 dependencies. +This is what was used to generate the converted ROS2 bag files for standard datasets. +To do a conversion of a bag file we can do the following: + +@code{.shell-session} +pip install rosbags +rosbags-convert V1_01_easy.bag +@endcode + + + + +@section dev-ros1-to-ros2-option-2 rosbag2 play + +To do this conversion you will need to have both ROS1 and ROS2 installed on your system. +Also ensure that you have installed all dependencies and backends required. +The main [rosbag2](https://github.com/ros2/rosbag2) readme has a lot of good details. + +@code{.shell-session} +sudo apt-get install ros-$ROS2_DISTRO-ros2bag ros-$ROS2_DISTRO-rosbag2* +sudo apt install ros-$ROS2_DISTRO-rosbag2-bag-v2-plugins +@endcode + +From here we can do the following. +This is based on [this issue](https://github.com/ros2/rosbag2/issues/139#issuecomment-516167831). +You might run into issues with the .so files being corrupted (see [this issue](https://github.com/ros2/rosbag2/issues/94)) +Not sure if there is a fix besides building it from scratch yourself. + +@code{.shell-session} +source_ros1 +source_ros2 +ros2 bag play -s rosbag_v2 V1_01_easy.bag +@endcode + + + + + +*/ diff --git a/docs/dev-welcome.dox b/docs/dev-welcome.dox index cc9bc2fa3..9cc6f966e 100644 --- a/docs/dev-welcome.dox +++ b/docs/dev-welcome.dox @@ -6,6 +6,7 @@ - @subpage dev-coding-style --- General coding styles and conventions - @subpage dev-docker --- How to use docker images to develop with - @subpage dev-docs --- Developer guide on how documentation can be built +- @subpage dev-ros1-to-ros2 --- Some notes on ROS bag conversion - @subpage dev-index --- Description of the covariance index system - @subpage dev-roadmap --- Where we plan to go in the future - @subpage dev-profiling --- Some notes on performing profiling diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox index 5e93539fa..062f8963b 100644 --- a/docs/gs-installing.dox +++ b/docs/gs-installing.dox @@ -2,6 +2,7 @@ @page gs-installing Installation Guide +@tableofcontents @section gs-install-ros ROS Dependency @@ -13,31 +14,89 @@ See the [opencv_contrib](https://github.com/opencv/opencv_contrib) readme on how We have tested building with OpenCV 3.2, 3.3, 3.4, 4.2, and 4.5. Please see the official instructions to install ROS: -- [Ubuntu 16.04 ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) (uses OpenCV 3.3) -- [Ubuntu 18.04 ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) (uses OpenCV 3.2) -- [Ubuntu 20.04 ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) (uses OpenCV 4.2) +- [Ubuntu 16.04 ROS 1 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) (uses OpenCV 3.3) +- [Ubuntu 18.04 ROS 1 Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) (uses OpenCV 3.2) +- [Ubuntu 20.04 ROS 1 Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) (uses OpenCV 4.2) +- [Ubuntu 18.04 ROS 2 Dashing](https://docs.ros.org/en/dashing/) +- [Ubuntu 20.04 ROS 2 Galactic](https://docs.ros.org/en/galactic/) -@m_class{m-block m-warning} +We do support ROS-free builds, but don't recommend using this interface as we have limited support for it. +You will need to ensure you have installed OpenCV and Eigen3 which are the only dependencies. +If ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_ROS=OFF ..`. +If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions. +The simulator can give you and example on how to do this. + +@subsection gs-install-ros-1 ROS1 Install -@par ROS Usage in OpenVINS - We do support ROS-free builds, but don't recommend using this interface as we have limited support for it. - You will need to ensure you have installed OpenCV and Eigen3 which are the only dependencies. - If ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_ROS=OFF ..`. - If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions. +To install we can perform the following: @code{.shell-session} sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt-get update -export ROS_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04 -sudo apt-get install ros-$ROS_DISTRO-desktop-full -echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc -source ~/.bashrc +export ROS1_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04 +sudo apt-get install ros-$ROS1_DISTRO-desktop-full sudo apt-get install libeigen3-dev python-catkin-tools # ubuntu 16.04 or 18.04 sudo apt-get install libeigen3-dev python3-catkin-tools python3-osrf-pycommon # ubuntu 20.04 @endcode +If you only have ROS1 on your system and are not cross installing ROS2, then you can run the following to append this to your bashrc file. +Every time a terminal is open, thus will load the ROS1 environmental variables required to find all dependencies for building and system installed packages. + +@code{.shell-session} +echo "source /opt/ros/$ROS1_DISTRO/setup.bash" >> ~/.bashrc +source ~/.bashrc +@endcode + +Otherwise, if you want to also install ROS2, you must *NOT* have a global source. +Instead we can have a nice helper command which can be used when we build a ROS1 workspace. +Additionally, the `source_devel` command can be used when in your workspace root to source built packages. +Once appended simply run `source_ros1` to load your ROS1 environmental variables. + +@code{.shell-session} +echo "alias source_ros1=\"source /opt/ros/$ROS1_DISTRO/setup.bash\"" >> ~/.bashrc +echo "alias source_devel=\"source devel/setup.bash\"" >> ~/.bashrc +source ~/.bashrc +@endcode + + +@subsection gs-install-ros-2 ROS2 Install + +To install we can perform the following: + +@code{.shell-session} +sudo apt update && sudo apt install curl gnupg lsb-release +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null +sudo apt-get update +export ROS2_DISTRO=galactic # dashing=18.04, galactic=20.04 +sudo apt install ros-$ROS2_DISTRO-desktop +sudo apt-get install ros-$ROS2_DISTRO-ros2bag ros-$ROS2_DISTRO-rosbag2* # rosbag utilities (seems to be separate) +@endcode + +If you only have ROS2 on your system and are not cross installing ROS1, then you can run the following to append this to your bashrc file. +Every time a terminal is open, thus will load the ROS2 environmental variables required to find all dependencies for building and system installed packages. + +@code{.shell-session} +echo "source /opt/ros/$ROS2_DISTRO/setup.bash" >> ~/.bashrc +source ~/.bashrc +@endcode + +Otherwise, if you want to also install ROS1, you must *NOT* have a global source. +Instead we can have a nice helper command which can be used when we build a ROS1 workspace. +Additionally, the `source_install` command can be used when in your workspace root to source built packages. +Once appended simply run `source_ros2` to load your ROS1 environmental variables. + +@code{.shell-session} +echo "alias source_ros2=\"source /opt/ros/$ROS2_DISTRO/setup.bash\"" >> ~/.bashrc +echo "alias source_install=\"source install/setup.bash\"" >> ~/.bashrc +source ~/.bashrc +@endcode + + + + @section gs-install-openvins Cloning the OpenVINS Project Now that we have ROS installed we can setup a catkin workspace and build the project! @@ -45,6 +104,14 @@ If you did not install the catkin_tools build system, you should be able to buil If you run into any problems please google search the issue first and if you are unable to find a solution please open an issue on our github page. After the build is successful please following the @ref gs-tutorial guide on getting a dataset and running the system. +There are additional options that users might be interested in. +Configure these with `catkin build -D=OFF` or `cmake -D=ON ..` in the ROS free case. + +- `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (if it is found) +- `ENABLE_ARUCO_TAGS` - (default ON) - Enable or disable aruco tag (disable if no contrib modules) +- `BUILD_OV_EVAL` - (default ON) - Enable or disable building of ov_eval +- `DISABLE_MATPLOTLIB` - (default OFF) - Disable or enable matplotlib plot scripts in ov_eval + @code{.shell-session} mkdir -p ~/workspace/catkin_ws_ov/src/ cd ~/workspace/catkin_ws_ov/src/ @@ -55,25 +122,19 @@ colcon build # ROS2 colcon build --event-handlers console_cohesion+ # ROS2 with verbose output @endcode -There are additional options that users might be interested in. -Configure these with `catkin build -D=OFF` or `cmake -D=ON ..` in the ROS free case. - -* `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (if it is found) -* `ENABLE_ARUCO_TAGS` - (default ON) - Enable or disable aruco tag (disable if no contrib modules) -* `BUILD_OV_EVAL` - (default ON) - Enable or disable building of ov_eval -* `DISABLE_MATPLOTLIB` - (default OFF) - Disable or enable matplotlib plot scripts in ov_eval @section gs-install-oveval Additional Evaluation Requirements -If you want to use the plotting utility wrapper of [matplotlib-cpp](https://github.com/lava/matplotlib-cpp) to generate plots directly from running the cpp code in ov_eval you will need to make sure you have a valid Python 2.7 install of matplotlib. +If you want to use the plotting utility wrapper of [matplotlib-cpp](https://github.com/lava/matplotlib-cpp) to generate plots directly from running the cpp code in ov_eval you will need to make sure you have a valid Python 2.7 or 3 install of matplotlib. On ubuntu 16.04 you can do the following command which should take care of everything you need. If you can't link properly, make sure you can call it from Python normally (i.e. that your Python environment is not broken). You can disable this visualization if it is broken for you by passing the -DDISABLE_MATPLOTLIB=ON parameter to your catkin build. Additionally if you wish to record CPU and memory usage of the node, you will need to install the [psutil](https://github.com/giampaolo/psutil) library. @code{.shell-session} -sudo apt-get install python-matplotlib python-numpy python2.7-dev python-psutil +sudo apt-get install python-matplotlib python-numpy python2.7-dev python-psutil # for python2 systems +sudo apt-get install python-matplotlib python3-numpy python3-dev python3-psutil # for python3 systems catkin build -DDISABLE_MATPLOTLIB=OFF # build with viz (default) catkin build -DDISABLE_MATPLOTLIB=ON # build without viz @endcode @@ -85,6 +146,13 @@ We leverage [OpenCV](https://opencv.org/) for this project which you can typical If the ROS version of [cv_bridge](http://wiki.ros.org/cv_bridge) does not work (or are using non-ROS building), then you can try building OpenCV from source ensuring you include the contrib modules. One should make sure you can see some of the "contrib" (e.g. aruco) when you cmake to ensure you have linked to the contrib modules. + +@m_class{m-block m-warning} + +@par OpenCV Install + Try to first build with your system / ROS OpenCV. + Only fall back onto this if it does not allow you to compile, or want a newer version! + @code{.shell-session} git clone https://github.com/opencv/opencv/ git clone https://github.com/opencv/opencv_contrib/ diff --git a/docs/gs-tutorial.dox b/docs/gs-tutorial.dox index b8eaf6f84..646354ca5 100644 --- a/docs/gs-tutorial.dox +++ b/docs/gs-tutorial.dox @@ -2,135 +2,159 @@ @page gs-tutorial Simple Tutorial +@tableofcontents This guide assumes that you have already built the project successfully and are now ready to run the program on some datasets. If you have not compiled the program yet please follow the @ref gs-installing guide. The first that we will download is a dataset to run the program on. In this tutorial we will run on the [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) @cite Burri2016IJRR which provides monochrome stereo images at 20Hz with a MEMS ADIS16448 IMU at 200Hz. - - -@m_div{m-button m-primary} +@m_div{m-col-m-6 m-button m-primary} -@m_div{m-big}Download ROS Bag@m_enddiv -@m_div{m-small} Vicon Room 1 01 @m_enddiv +@m_div{m-big}Download ROS 1 Bag@m_enddiv +@m_div{m-small}Vicon Room 1 01 Easy @m_enddiv + +@m_enddiv + +@m_div{m-col-m-6 m-button m-primary} + +@m_div{m-big}Download ROS 2 Bag@m_enddiv +@m_div{m-small}Vicon Room 1 01 Easy @m_enddiv @m_enddiv -All configuration information for the system is exposed to the user in the launch file. -We will create a launch file that will launch our MSCKF estimation node and feed the ROS bag into the system in serial. +All configuration information for the system is exposed to the user in the configuration file, and can be overridden in the launch file. +We will create a launch file that will launch our MSCKF estimation node and feed the ROS bag into the system. One can take a look in the [launch](https://github.com/rpng/open_vins/tree/master/ov_msckf/launch) folder for more examples. + +@section gs-tutorial-ros1 ROS 1 Tutorial + +The ROS1 system uses the [roslaunch](http://wiki.ros.org/roslaunch) system to manage and launch nodes. +These files can launch multiple nodes, and each node can their own set of parameters set. +For OpenVINS we need to define a series of files: + +- `estimator_config.yaml` - Contains OpenVINS specific configuration files. Each of these can be overridden in the launch file. +- `kalibr_imu_chain.yaml` - IMU noise parameters and topic information based on the sensor in the dataset. This should be the same as Kalibr's (see @ref gs-calib-imu-static). +- `kalibr_imucam_chain.yaml` - Camera to IMU transformation and camera intrinsics. This should be the same as Kalibr's (see @ref gs-calib-cam-static). + +Consider the below launch file. +We can see the main parameter that is being passed into the estimator is the `config_path` file which has all configuration for this specific dataset. +Additionally, we can see that we are launching the `run_subscribe_msckf` ROS 1 node, and are going to be overriding the `use_stereo` and `max_cameras` with the specificed values. +ROS parameters always have priority, and you should see in the console that they have been successfully overridden. + @m_div{m-container-inflate} @code{.xml} - - - - - - - [0,1] - - - - - - - - - - - - - [0.0,0.0,9.81] - - - - - - - - - - - - - - - - - - - - - - - [458.654,457.296,367.215,248.375] - [-0.28340811,0.07395907,0.00019359,1.76187114e-05] - [457.587,456.134,379.999,255.238] - [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] - - - - [ - 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, - 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, - -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, - 0.0, 0.0, 0.0, 1.0 - ] - - - [ - 0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556, - 0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024, - -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038, - 0.0, 0.0, 0.0, 1.0 - ] - - + + + + + + + + + + + - - - + @endcode @m_enddiv +Since the configuration file for the EurocMav dataset has already been created, we can simply do the following. +Note it is good practice to run a `roscore` that stays active so that you do not need to relaunch rviz or other packages. +@code{.shell-session} +roscore # term 0 +source devel/setup.bash # term 1 +roslaunch ov_msckf subscribe.launch config:=euroc_mav +@endcode + +In another two terminals we can run the following. +For RVIZ, one can open the `ov_msckf/launch/display.rviz` configuration file. +You should see the system publishing features and a state estimate. -One will need to edit the path to the ROS bag that was downloaded from the [EuRoC MAV Dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) website. -Lets now look at the different sections in this launch file in more detail. -A lot of the parameters that are exposed have not been included in the above file and will instead be set to their default values. -Please take a look at the other launch files or the ov_msckf::VioManager constructor for more details on what parameters are exposed. +@code{.shell-session} +rviz # term 2 +rosbag play V1_01_easy.bag # term 3 +@endcode -| Group | Description | -|---|---| -| bag topics | ROS topics that we will parse the IMU and camera data from. If we are only using one camera, i.e. monocular, then only the first camera topic is used. | -| stereo pairs | Even set of camera ids, which stereo tracking will be tried to be performed on. For example a 0,1,2,3 means that you want to perform stereo tracking of the 0,1 and 2,3 camera pairs. | -| bag params. | Location of the bag we will read along with the start time, in seconds, and duration we want to run on. | -| world/filter params. | This has most of the core parameters that can be tuned to improve filter performance including the sliding window size, representation, gravity, and number of environmental SLAM features. One can also change the number of cameras from 1 to 2 to do stereo matching and update. | -| tracker/extractor params. | For our visual front-end tracker we have a few key parameters that we can tune, most importantly is the number of features extracted. | -| sensor noise values | Since our feature measurement function is on the raw pixels (see @ref update-feat), the pixel noise should be 1 pixel if we have good calibration. We additionally have the *continuous time* white noise and random walk values for our IMU. | -| camera intrinsics | Camera intrinsic values from the EurocMav dataset. | -| camera extrinsics | Camera extrinsics values from the EurocMav dataset. Note that they have the rotation from camera to imu and position of the imu in the camera in the imu frame. | +@section gs-tutorial-ros2 ROS 2 Tutorial +For ROS 2, launch files and nodes have become a bit more combersom due to the removal of a centralized communication method. +This both allows for more distributed systems, but causes a bit more on the developer to perform integration. +The launch system is described in [this](https://design.ros2.org/articles/roslaunch.html) design article. +Consider the following launch file which does the same as the ROS 1 launch file above. +@m_div{m-container-inflate} +@code{.py} +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory, get_package_prefix +import os +import sys + +launch_args = [ + DeclareLaunchArgument(name='namespace', default_value='', description='namespace'), + DeclareLaunchArgument(name='config', default_value='euroc_mav', description='euroc_mav, tum_vi, rpng_aruco...'), + DeclareLaunchArgument(name='verbosity', default_value='INFO', description='ALL, DEBUG, INFO, WARNING, ERROR, SILENT'), + DeclareLaunchArgument(name='use_stereo', default_value='true', description=''), + DeclareLaunchArgument(name='max_cameras', default_value='2', description='') +] + +def launch_setup(context): + configs_dir=os.path.join(get_package_share_directory('ov_msckf'),'..','config') + available_configs = os.listdir(configs_dir) + config = LaunchConfiguration('config').perform(context) + if not config in available_configs: + return[LogInfo(msg='ERROR: unknown config: \'{}\' - Available configs are: {} - not starting OpenVINS'.format(config,', '.join(available_configs)))] + config_path = os.path.join(get_package_share_directory('ov_msckf'),'config',config,'estimator_config.yaml') + node1 = Node(package = 'ov_msckf', + executable = 'run_subscribe_msckf', + namespace = LaunchConfiguration('namespace'), + parameters =[{'verbosity': LaunchConfiguration('verbosity')}, + {'use_stereo': LaunchConfiguration('use_stereo')}, + {'max_cameras': LaunchConfiguration('max_cameras')}, + {'config_path': config_path}]) + return [node1] + +def generate_launch_description(): + opfunc = OpaqueFunction(function = launch_setup) + ld = LaunchDescription(launch_args) + ld.add_action(opfunc) + return ld -If we create a launch file `tutorial.launch` in our `/ov_msckf/launch/` folder with the above contents and have replaced the bag path to the downloaded bag location, we can do the following. +@endcode +@m_enddiv +We can see that first the `launch_setup` function defines the nodes that we will be launching from this file. +Then the `LaunchDescription` is created given the launch arguments and the node is added to it and returned to ROS. +We can the launch it using the following: @code{.shell-session} -source devel/setup.bash -roslaunch ov_msckf tutorial.launch +source install/setup.bash +ros2 launch ov_msckf subscribe.launch.py config:=euroc_mav @endcode +We can then use the ROS2 rosbag file. +First make sure you have installed the rosbag2 and all its backends. +If you downloaded the bag above you should already have a valid bag format. +Otherwise, you will need to convert it following @ref dev-ros1-to-ros2 . +A "bag" is now defined by a db3 sqlite database and config yaml file in a folder. +@code{.shell-session} +ros2 bag play V1_01_easy +@endcode diff --git a/ov_core/src/feat/FeatureHelper.h b/ov_core/src/feat/FeatureHelper.h index 593dc19b5..c74c9ca1d 100644 --- a/ov_core/src/feat/FeatureHelper.h +++ b/ov_core/src/feat/FeatureHelper.h @@ -53,9 +53,9 @@ class FeatureHelper { * @param db Feature database pointer * @param time0 First camera frame timestamp * @param time1 Second camera frame timestamp - * @param disp_mean[out] Average raw disparity - * @param disp_var[out] Variance of the disparities - * @param total_feats[out] Total number of common features + * @param disp_mean Average raw disparity + * @param disp_var Variance of the disparities + * @param total_feats Total number of common features */ static void compute_disparity(std::shared_ptr db, double time0, double time1, double &disp_mean, double &disp_var, int &total_feats) { diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h index 8edd6c707..8bc6fff5e 100644 --- a/ov_core/src/utils/dataset_reader.h +++ b/ov_core/src/utils/dataset_reader.h @@ -172,7 +172,7 @@ class DatasetReader { /** * @brief This will load the trajectory into memory (space separated) * @param path Path to the trajectory file that we want to read in. - * @param path_traj Will be filled with groundtruth states (timestamp(s), q_GtoI, p_IinG) + * @param traj_data Will be filled with groundtruth states (timestamp(s), q_GtoI, p_IinG) */ static void load_simulated_trajectory(std::string path, std::vector &traj_data) { From 1862fce65ed9b9e1e5dd267f504c1baac7461128 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 13:04:22 -0500 Subject: [PATCH 41/55] some documentation updates --- docs/dev-coding-style.dox | 11 ++-- docs/dev-welcome.dox | 2 +- docs/gs-datasets.dox | 109 ++++++++++++++++++++------------------ docs/gs-tutorial.dox | 12 ++--- 4 files changed, 71 insertions(+), 63 deletions(-) diff --git a/docs/dev-coding-style.dox b/docs/dev-coding-style.dox index f988e0ea2..9191506c7 100644 --- a/docs/dev-coding-style.dox +++ b/docs/dev-coding-style.dox @@ -52,13 +52,14 @@ In general the user can specify the following (see [ov_core/src/utils/print.h](o - PrintLevel::SILENT : All PRINT_XXXX will be silenced. To use these, you can specify the following using the [printf](https://www.cplusplus.com/reference/cstdio/printf/) standard input logic. +A user can also specify colors (see [ov_core/src/utils/colors.h](ov_core/src/utils/colors.h) file for details): @code{.cpp} -PRINT_ALL("the value is %.2f", variable); -PRINT_DEBUG("the value is %.2f", variable); -PRINT_INFO("the value is %.2f", variable); -PRINT_WARNING("the value is %.2f", variable); -PRINT_ERROR("the value is %.2f", variable); +PRINT_ALL("the value is %.2f\n", variable); +PRINT_DEBUG("the value is %.2f\n", variable); +PRINT_INFO("the value is %.2f\n", variable); +PRINT_WARNING("the value is %.2f\n", variable); +PRINT_ERROR(RED "the value is %.2f\n" RESET, variable); @endcode diff --git a/docs/dev-welcome.dox b/docs/dev-welcome.dox index 9cc6f966e..605d77601 100644 --- a/docs/dev-welcome.dox +++ b/docs/dev-welcome.dox @@ -8,8 +8,8 @@ - @subpage dev-docs --- Developer guide on how documentation can be built - @subpage dev-ros1-to-ros2 --- Some notes on ROS bag conversion - @subpage dev-index --- Description of the covariance index system -- @subpage dev-roadmap --- Where we plan to go in the future - @subpage dev-profiling --- Some notes on performing profiling +- @subpage dev-roadmap --- Where we plan to go in the future */ \ No newline at end of file diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox index 2f564e469..cf829e0f1 100644 --- a/docs/gs-datasets.dox +++ b/docs/gs-datasets.dox @@ -14,6 +14,7 @@ The ETH ASL [EuRoC MAV dataset](https://projects.asl.ethz.ch/datasets/doku.php?i The reason for this is the synchronised inertial+camera sensor data and the high quality groundtruth. The dataset contains different sequences of varying difficulty of a Micro Aerial Vehicle (MAV) flying in an indoor room. Monochrome stereo images are collected by a two Aptina MT9V034 global shutter cameras at 20 frames per seconds, while a ADIS16448 MEMS inertial unit provides linear accelerations and angular velocities at a rate of 200 samples per second. + We recommend that most users start testing on this dataset before moving on to the other datasets that our system support or before trying with your own collected data. The machine hall datasets have the MAV being picked up in the beginning and then set down, we normally skip this part, but it should be able to be handled by the filter if SLAM features are enabled. Please take a look at the [run_ros_eth.sh](https://github.com/rpng/open_vins/blob/master/ov_msckf/scripts/run_ros_eth.sh) script for some reasonable default values (they might still need to be tuned). @@ -27,14 +28,19 @@ Please take a look at the [run_ros_eth.sh](https://github.com/rpng/open_vins/blo You can find the output at this [link](https://drive.google.com/drive/folders/1d62Q_RQwHzKLcIdUlTeBmojr7j0UL4sM?usp=sharing) and is what we normally use to evaluate the error on this dataset. @m_div{m-text-center} -| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Example Launch | +| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Config | |-------------:|--------|--------------|------------------|------------------| -| Vicon Room 1 01 | 58 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_01_easy/V1_01_easy.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | -| Vicon Room 1 02 | 76 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_02_medium/V1_02_medium.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | -| Vicon Room 1 03 | 79 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_03_difficult/V1_03_difficult.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | -| Vicon Room 2 01 | 37 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_01_easy/V2_01_easy.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | -| Vicon Room 2 02 | 83 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_02_medium/V2_02_medium.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | -| Vicon Room 2 03 | 86 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_03_difficult/V2_03_difficult.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_eth.launch) | +| Vicon Room 1 01 | 58 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_01_easy/V1_01_easy.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Vicon Room 1 02 | 76 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_02_medium/V1_02_medium.bag) , [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing)| [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Vicon Room 1 03 | 79 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_03_difficult/V1_03_difficult.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Vicon Room 2 01 | 37 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_01_easy/V2_01_easy.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Vicon Room 2 02 | 83 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_02_medium/V2_02_medium.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Vicon Room 2 03 | 86 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room2/V2_03_difficult/V2_03_difficult.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Machine Hall 01 | 80 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Machine Hall 02 | 73 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_02_easy/MH_02_easy.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Machine Hall 03 | 131 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_03_medium/MH_03_medium.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Machine Hall 04 | 92 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_04_difficult/MH_04_difficult.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | +| Machine Hall 05 | 98 | [rosbag](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_05_difficult/MH_05_difficult.bag), [rosbag2](https://drive.google.com/drive/folders/1xQ1KcZhZ5pioPXTyrZBN6Mjxkfpcd_B3?usp=sharing) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/euroc_mav) | [config](https://github.com/rpng/open_vins/blob/master/config/euroc_mav) | @m_enddiv @@ -59,14 +65,14 @@ Note that we focus on the room datasets as full 6 dof pose collection is availab @m_div{m-text-center} -| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Example Launch | +| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Config | |-------------:|--------|--------------|------------------|------------------| -| room1 | 147 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room1_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | -| room2 | 142 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room2_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | -| room3 | 136 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room3_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | -| room4 | 69 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room4_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | -| room5 | 132 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room5_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | -| room6 | 67 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room6_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_tum.launch) | +| room1 | 147 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room1_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | +| room2 | 142 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room2_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | +| room3 | 136 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room3_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | +| room4 | 69 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room4_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | +| room5 | 132 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room5_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | +| room6 | 67 | [rosbag](http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-room6_512_16.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/tum_vi) | [config](https://github.com/rpng/open_vins/blob/master/config/tum_vi) | @m_enddiv @@ -96,14 +102,14 @@ Thus, please ensure that you have enabled online calibration of these parameters Additionally, there is no groundtruth for these datasets, but some do include GPS messages if you wish to compare relative to something. @m_div{m-text-center} -| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Example Launch | +| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Config | |-------------:|--------|--------------|------------------|------------------| -| ArUco Room 01 | 27 | [rosbag](https://drive.google.com/file/d/1ytjo8V6pCroaVd8-QSop7R4DbsvvKyRQ/view?usp=sharing) | none | [launch aruco](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_aruco.launch) | -| ArUco Room 02 | 93 | [rosbag](https://drive.google.com/file/d/1l_hnPUW6ufqxPtrLqRRHHI4mfGRZB1ha/view?usp=sharing) | none | [launch aruco](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_aruco.launch) | -| ArUco Hallway 01 | 190 | [rosbag](https://drive.google.com/file/d/1FQBo3uHqRd0qm8GUb50Q-sj5gukcwaoU/view?usp=sharing) | none | [launch aruco](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_aruco.launch) | -| ArUco Hallway 02 | 105 | [rosbag](https://drive.google.com/file/d/1oAbnV3MPOeaUSjnSc3g8t-pWV1nVjbys/view?usp=sharing) | none | [launch aruco](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_aruco.launch) | -| Neighborhood 01 | 2300 | [rosbag](https://drive.google.com/file/d/1N07SDbaLEkq9pVEvi6oiHpavaRuFs3j2/view?usp=sharing) | none | [launch ironsides](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_ironsides.launch) | -| Neighborhood 02 | 7400 | [rosbag](https://drive.google.com/file/d/1QEUi40sO8OkVXEGF5JojiiZMHMSiSqtg/view?usp=sharing) | none | [launch ironsides](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_serial_ironsides.launch) | +| ArUco Room 01 | 27 | [rosbag](https://drive.google.com/file/d/1ytjo8V6pCroaVd8-QSop7R4DbsvvKyRQ/view?usp=sharing) | none | [config aruco](https://github.com/rpng/open_vins/blob/master/config/rpng_aruco) | +| ArUco Room 02 | 93 | [rosbag](https://drive.google.com/file/d/1l_hnPUW6ufqxPtrLqRRHHI4mfGRZB1ha/view?usp=sharing) | none | [config aruco](https://github.com/rpng/open_vins/blob/master/config/rpng_aruco) | +| ArUco Hallway 01 | 190 | [rosbag](https://drive.google.com/file/d/1FQBo3uHqRd0qm8GUb50Q-sj5gukcwaoU/view?usp=sharing) | none | [config aruco](https://github.com/rpng/open_vins/blob/master/config/rpng_aruco) | +| ArUco Hallway 02 | 105 | [rosbag](https://drive.google.com/file/d/1oAbnV3MPOeaUSjnSc3g8t-pWV1nVjbys/view?usp=sharing) | none | [config aruco](https://github.com/rpng/open_vins/blob/master/config/rpng_aruco) | +| Neighborhood 01 | 2300 | [rosbag](https://drive.google.com/file/d/1N07SDbaLEkq9pVEvi6oiHpavaRuFs3j2/view?usp=sharing) | none | [config ironsides](https://github.com/rpng/open_vins/blob/master/config/rpng_ironsides) | +| Neighborhood 02 | 7400 | [rosbag](https://drive.google.com/file/d/1QEUi40sO8OkVXEGF5JojiiZMHMSiSqtg/view?usp=sharing) | none | [config ironsides](https://github.com/rpng/open_vins/blob/master/config/rpng_ironsides) | @m_enddiv @@ -113,7 +119,7 @@ Additionally, there is no groundtruth for these datasets, but some do include GP @section gs-data-uzhfpv UZH-FPV Drone Racing Dataset -The [UZH-FPV Drone Racing Dataset](http://rpg.ifi.uzh.ch/uzh-fpv.html) @cite Schubert2018IROS is a dataset focused on high-speed agressive 6dof motion with very high levels of optical flow as compared to other datasets. +The [UZH-FPV Drone Racing Dataset](https://fpv.ifi.uzh.ch/) @cite Schubert2018IROS is a dataset focused on high-speed agressive 6dof motion with very high levels of optical flow as compared to other datasets. A FPV drone racing quadrotor has on board a Qualcomm Snapdragon Flight board which can provide inertial measurement and has two 640x480 grayscale global shutter fisheye camera's attached. The groundtruth is collected with a Leica Nova MS60 laser tracker. There are four total sensor configurations and calibration provides including: indoor forward facing stereo, indoor 45 degree stereo, outdoor forward facing, and outdoor 45 degree. @@ -124,24 +130,26 @@ Please take a look at the [run_ros_uzhfpv.sh](https://github.com/rpng/open_vins/ @m_class{m-block m-warning} -@par Outdoor Dataset Support - These outdoor datasets seem to give strange relative pose errors (RPE) and thus, while we can run on them, we don't recommend comparing methods on these. - Please see the below indoor datasets that we are able to run on and the error results look reasonable. +@par Dataset Groundtruthing + Only the Absolute Trajectory Error (ATE) should be used as a metric for this dataset. + This is due to inaccurate groundtruth orientation estimates which are explain in their [report](https://fpv.ifi.uzh.ch/wp-content/uploads/2020/11/Ground-Truth-Rotation-Issue-Report.pdf) on the issue. + The basic summary is that it is hard to get an accurate orientation information due to the point-based Leica measurements used to groundtruth. + @m_div{m-text-center} -| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Example Launch | +| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Config | |-------------:|--------|--------------|------------------|------------------| -| Indoor 5 | 157 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_forward_5_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 6 | 204 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_forward_6_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 7 | 314 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_forward_7_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 9 | 136 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_forward_9_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 10 | 129 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_forward_10_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 45deg 2 | 207 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_45_2_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 45deg 4 | 164 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_45_4_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 45deg 12 | 112 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_45_12_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 45deg 13 | 159 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_45_13_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | -| Indoor 45deg 14 | 211 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv/indoor_45_14_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_uzhfpv.launch) | +| Indoor 5 | 157 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_forward_5_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor) | +| Indoor 6 | 204 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_forward_6_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor) | +| Indoor 7 | 314 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_forward_7_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor) | +| Indoor 9 | 136 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_forward_9_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor) | +| Indoor 10 | 129 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_forward_10_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor) | +| Indoor 45deg 2 | 207 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_45_2_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor_45) | +| Indoor 45deg 4 | 164 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_45_4_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor_45) | +| Indoor 45deg 12 | 112 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_45_12_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor_45) | +| Indoor 45deg 13 | 159 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_45_13_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor_45) | +| Indoor 45deg 14 | 211 | [rosbag](http://rpg.ifi.uzh.ch/datasets/uzh-fpv-newer-versions/v2/indoor_45_14_snapdragon_with_gt.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/uzh_fpv) | [config](https://github.com/rpng/open_vins/blob/master/config/uzhfpv_indoor_45) | @m_enddiv @@ -149,7 +157,6 @@ Please take a look at the [run_ros_uzhfpv.sh](https://github.com/rpng/open_vins/ - @section gs-data-kaist KAIST Urban Dataset The [KAIST urban dataset](https://sites.google.com/view/complex-urban-dataset) @cite Jeong2019IJRR is a dataset focus on autonomous driving and localization in challenging complex urban environments. @@ -181,9 +188,9 @@ Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and @m_div{m-text-center} | Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch | |-------------:|--------|--------------|------------------|------------------| -| Urban 28 | 11.47 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | -| Urban 38 | 11.42 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | -| Urban 39 | 11.06 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaist.launch) | +| Urban 28 | 11.47 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | +| Urban 38 | 11.42 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | +| Urban 39 | 11.06 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | @m_enddiv @@ -197,17 +204,17 @@ This topic has been provided in ov_data for convinces sake. @m_div{m-text-center} | Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch | |-------------:|--------|--------------|------------------|------------------| -| circle | 29.99 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| circle_fast | 64.15 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| circle_head | 35.05 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| infinite | 29.35 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| infinite_fast | 54.24 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| infinite_head | 37.45 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| rotation | 7.82 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| rotation_fast | 14.55 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| square | 41.94 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| square_fast | 44.07 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | -| square_head | 50.00 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [launch](https://github.com/rpng/open_vins/blob/master/ov_msckf/launch/pgeneva_ros_kaistvio.launch) | +| circle | 29.99 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| circle_fast | 64.15 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| circle_head | 35.05 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/circle/circle_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| infinite | 29.35 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| infinite_fast | 54.24 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| infinite_head | 37.45 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/infinite/infinite_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| rotation | 7.82 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| rotation_fast | 14.55 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/rotation/rotation_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| square | 41.94 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| square_fast | 44.07 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_fast.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | +| square_head | 50.00 | [download](https://urserver.kaist.ac.kr/publicdata/KAIST_VIO_Dataset/square/square_head.bag) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist_vio) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist_vio) | @m_enddiv diff --git a/docs/gs-tutorial.dox b/docs/gs-tutorial.dox index 646354ca5..246793389 100644 --- a/docs/gs-tutorial.dox +++ b/docs/gs-tutorial.dox @@ -27,6 +27,11 @@ In this tutorial we will run on the [EuRoC MAV Dataset](https://projects.asl.eth All configuration information for the system is exposed to the user in the configuration file, and can be overridden in the launch file. We will create a launch file that will launch our MSCKF estimation node and feed the ROS bag into the system. One can take a look in the [launch](https://github.com/rpng/open_vins/tree/master/ov_msckf/launch) folder for more examples. +For OpenVINS we need to define a series of files: + +- `estimator_config.yaml` - Contains OpenVINS specific configuration files. Each of these can be overridden in the launch file. +- `kalibr_imu_chain.yaml` - IMU noise parameters and topic information based on the sensor in the dataset. This should be the same as Kalibr's (see @ref gs-calib-imu-static). +- `kalibr_imucam_chain.yaml` - Camera to IMU transformation and camera intrinsics. This should be the same as Kalibr's (see @ref gs-calib-cam-static). @@ -34,12 +39,6 @@ One can take a look in the [launch](https://github.com/rpng/open_vins/tree/maste The ROS1 system uses the [roslaunch](http://wiki.ros.org/roslaunch) system to manage and launch nodes. These files can launch multiple nodes, and each node can their own set of parameters set. -For OpenVINS we need to define a series of files: - -- `estimator_config.yaml` - Contains OpenVINS specific configuration files. Each of these can be overridden in the launch file. -- `kalibr_imu_chain.yaml` - IMU noise parameters and topic information based on the sensor in the dataset. This should be the same as Kalibr's (see @ref gs-calib-imu-static). -- `kalibr_imucam_chain.yaml` - Camera to IMU transformation and camera intrinsics. This should be the same as Kalibr's (see @ref gs-calib-cam-static). - Consider the below launch file. We can see the main parameter that is being passed into the estimator is the `config_path` file which has all configuration for this specific dataset. Additionally, we can see that we are launching the `run_subscribe_msckf` ROS 1 node, and are going to be overriding the `use_stereo` and `max_cameras` with the specificed values. @@ -151,6 +150,7 @@ First make sure you have installed the rosbag2 and all its backends. If you downloaded the bag above you should already have a valid bag format. Otherwise, you will need to convert it following @ref dev-ros1-to-ros2 . A "bag" is now defined by a db3 sqlite database and config yaml file in a folder. +In another terminal we can run the following: @code{.shell-session} ros2 bag play V1_01_easy From 58488e148895db59b127e32212f9c51e7b95d53e Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 13:12:49 -0500 Subject: [PATCH 42/55] fix quat_ops documentation (closes #206) --- ov_core/src/utils/quat_ops.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index 58523626b..7cd2c0406 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -148,7 +148,7 @@ inline Eigen::Matrix skew_x(const Eigen::Matrix &w) * * This is based on equation 62 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf): * \f{align*}{ - * \mathbf{R} = (2q_4^2-1)\mathbf{I}_3-2q_4\lfloor\mathbf{q}\times\rfloor+2\mathbf{q}^\top\mathbf{q} + * \mathbf{R} = (2q_4^2-1)\mathbf{I}_3-2q_4\lfloor\mathbf{q}\times\rfloor+2\mathbf{q}\mathbf{q}^\top * @f} * * @param[in] q JPL quaternion @@ -180,7 +180,7 @@ inline Eigen::Matrix quat_2_Rot(const Eigen::Matrix * * @param[in] q First JPL quaternion * @param[in] p Second JPL quaternion - * @return 4x1 resulting p*q quaternion + * @return 4x1 resulting q*p quaternion */ inline Eigen::Matrix quat_multiply(const Eigen::Matrix &q, const Eigen::Matrix &p) { Eigen::Matrix q_t; @@ -267,7 +267,7 @@ inline Eigen::Matrix exp_so3(const Eigen::Matrix &w) * @f} * * @param[in] R 3x3 SO(3) rotation matrix - * @return 3x1 in the se(3) space [omegax, omegay, omegaz] + * @return 3x1 in the so(3) space [omegax, omegay, omegaz] */ inline Eigen::Matrix log_so3(const Eigen::Matrix &R) { // magnitude of the skew elements (handle edge case where we sometimes have a>1...) From 24bd2e95b0fe81e66cbe70649277b8daed65f53d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 13:55:02 -0500 Subject: [PATCH 43/55] cleaned up initialization parameters required --- Doxyfile-mcss | 2 +- config/euroc_mav/estimator_config.yaml | 53 ++-- config/kaist/estimator_config.yaml | 11 +- config/rpng_aruco/estimator_config.yaml | 13 +- config/rpng_ironsides/estimator_config.yaml | 11 +- config/rpng_sim/estimator_config.yaml | 11 +- config/tum_vi/estimator_config.yaml | 11 +- config/uzhfpv_indoor/estimator_config.yaml | 11 +- config/uzhfpv_indoor_45/estimator_config.yaml | 11 +- ov_core/src/dummy.cpp | 1 - ov_init/src/dummy.cpp | 6 +- ov_init/src/init/InertialInitializerOptions.h | 268 +----------------- ov_msckf/launch/display.rviz | 25 +- 13 files changed, 47 insertions(+), 387 deletions(-) diff --git a/Doxyfile-mcss b/Doxyfile-mcss index 68c1aee52..da1db0c85 100644 --- a/Doxyfile-mcss +++ b/Doxyfile-mcss @@ -35,7 +35,7 @@ ALIASES += \ ##! M_THEME_COLOR = #2f73a3 ##! M_FAVICON = docs/img/favicon-light.png -##! M_LINKS_NAVBAR1 = pages namespaceov__core namespaceov__type namespaceov__msckf namespaceov__eval annotated +##! M_LINKS_NAVBAR1 = pages annotated namespaceov__core namespaceov__type namespaceov__msckf namespaceov__init namespaceov__eval ##! M_LINKS_NAVBAR2 = \ ##! "GitHub" diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index f73586437..98401ddd1 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -2,24 +2,24 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT -use_fej: true # if we should use first-estimate Jacobians (enable for good consistency) -use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it +use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) +use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it use_rk4int: true # if rk4 integration should be used (overrides imu averaging) -use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints -max_cameras: 2 +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs +max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) -calib_cam_extrinsics: true -calib_cam_intrinsics: true -calib_cam_timeoffset: true +calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized -max_clones: 11 -max_slam: 50 -max_slam_in_update: 25 -max_msckf_in_update: 40 -dt_slam_delay: 2 +max_clones: 11 # how many clones in the sliding window +max_slam: 50 # number of features in our state vector +max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch +max_msckf_in_update: 40 # how many MSCKF features to use in the update +dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...) -gravity_mag: 9.81 +gravity_mag: 9.81 # magnitude of gravity in this location feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -37,26 +37,19 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 -init_window_time: 0.75 #1.5 -init_imu_thresh: 1.5 -init_max_disparity: 1.5 -init_max_features: 25.0 +init_window_time: 0.75 # how many seconds to collect initialization information +init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 50.0 # how many features to track during initialization (saves on computation) # ================================================================== # ================================================================== -record_timing_information: false -record_timing_filepath: "/tmp/traj_timing.txt" +record_timing_information: false # if we want to record timing information of the method +record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame +# if we want to save the simulation state and its diagional covariance +# use this with rosrun ov_eval error_simulation save_total_state: false filepath_est: "/tmp/ov_estimate.txt" filepath_std: "/tmp/ov_estimate_std.txt" @@ -74,8 +67,8 @@ grid_x: 20 grid_y: 20 min_px_dist: 15 knn_ratio: 0.70 -downsample_cameras: false -multi_threading: true +downsample_cameras: false # will downsample image in half if true +multi_threading: true # if should enable opencv multi threading histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE # aruco tag tracker for the system diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml index 6fbba9df3..f647dc579 100644 --- a/config/kaist/estimator_config.yaml +++ b/config/kaist/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml index 57738acf8..a17157cc6 100644 --- a/config/rpng_aruco/estimator_config.yaml +++ b/config/rpng_aruco/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 0.5 -init_imu_thresh: 1.5 +init_imu_thresh: 1.2 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index 9bc2908d9..a49a5c680 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index ffed39420..a725038b0 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 0.75 init_imu_thresh: 1.0 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index 53a6fbaf6..7e43d72e6 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 0.85 init_imu_thresh: 0.45 # room1-5:0.45, room6:0.25 init_max_disparity: 15.0 -init_max_features: 45.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml index 1adcde617..142d412c9 100644 --- a/config/uzhfpv_indoor/estimator_config.yaml +++ b/config/uzhfpv_indoor/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml index 1adcde617..142d412c9 100644 --- a/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/config/uzhfpv_indoor_45/estimator_config.yaml @@ -37,19 +37,10 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== -init_only_use_dynamic: false -init_only_use_static: true - -init_camera_pose: false -init_camera_timeoffset: false -init_state_bias_accel: true -init_enforce_gravity_magnitude: true - -init_camera_rate: 10.0 init_window_time: 1.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25.0 +init_max_features: 50.0 # ================================================================== # ================================================================== diff --git a/ov_core/src/dummy.cpp b/ov_core/src/dummy.cpp index 850e49e5f..32ca7eaa7 100644 --- a/ov_core/src/dummy.cpp +++ b/ov_core/src/dummy.cpp @@ -28,7 +28,6 @@ * key components of the ov_core codebase are the following: * * - 3d feature initialization (see @ref ov_core::FeatureInitializer) - * - Inertial state initialization (see @ref ov_core::InertialInitializer) * - SE(3) b-spline (see @ref ov_core::BsplineSE3) * - KLT, descriptor, aruco, and simulation feature trackers * - Groundtruth dataset reader (see @ref ov_core::DatasetReader) diff --git a/ov_init/src/dummy.cpp b/ov_init/src/dummy.cpp index 302c292fa..bc181886d 100644 --- a/ov_init/src/dummy.cpp +++ b/ov_init/src/dummy.cpp @@ -22,10 +22,8 @@ /** * @namespace ov_init * - * This is an implementation and extension of the work [Estimator initialization in vision-aided inertial navigation with unknown camera-IMU - * calibration](https://ieeexplore.ieee.org/document/6386235) @cite Dong2012IROS which solves the initialization problem by first creating a - * linear system for recovering the camera to IMU rotation, then for velocity, gravity, and feature positions, and finally a full - * optimization to allow for covariance recovery. Specifically, we focus on also recovering the biases of the platform and the time offset. + * Right now this contains static initialization code for a visual-inertial system. + * It will wait for the platform to stationary, and then initialize its orientation in the gravity frame. * */ namespace ov_init {} diff --git a/ov_init/src/init/InertialInitializerOptions.h b/ov_init/src/init/InertialInitializerOptions.h index f5b3da8bf..30f43e296 100644 --- a/ov_init/src/init/InertialInitializerOptions.h +++ b/ov_init/src/init/InertialInitializerOptions.h @@ -52,32 +52,10 @@ struct InertialInitializerOptions { */ void print_and_load(const std::shared_ptr &parser = nullptr) { print_and_load_initializer(parser); - print_and_load_noise(parser); print_and_load_state(parser); } - // CALIBRATION ============================ - - /// If we should only do dynamic state initialization - bool init_only_use_dynamic = false; - - /// If we should only do static state initialization - bool init_only_use_static = false; - - /// Bool to determine whether or not to recover imu-to-camera pose - bool init_calib_camera_pose = false; - - /// Bool to determine whether or not to recover camera to IMU time offset - bool init_calib_camera_timeoffset = false; - - /// Bool to determine whether or not to recover acceleration bias - bool init_state_bias_accel = true; - - /// Bool to determine whether or not to enforce gravity magnitude - bool init_enforce_gravity_magnitude = true; - - /// What camera rate we should track features at - double init_camera_rate = 10.0; + // INITIALIZATION ============================ /// Amount of time we will initialize over (seconds) double init_window_time = 1.0; @@ -100,45 +78,15 @@ struct InertialInitializerOptions { void print_and_load_initializer(const std::shared_ptr &parser = nullptr) { PRINT_DEBUG("INITIALIZATION SETTINGS:\n"); if (parser != nullptr) { - parser->parse_config("init_only_use_dynamic", init_only_use_dynamic); - parser->parse_config("init_only_use_static", init_only_use_static); - parser->parse_config("init_camera_pose", init_calib_camera_pose); - parser->parse_config("init_camera_timeoffset", init_calib_camera_timeoffset); - parser->parse_config("init_state_bias_accel", init_state_bias_accel); - parser->parse_config("init_enforce_gravity_magnitude", init_enforce_gravity_magnitude); - parser->parse_config("init_camera_rate", init_camera_rate); parser->parse_config("init_window_time", init_window_time); parser->parse_config("init_imu_thresh", init_imu_thresh); parser->parse_config("init_max_disparity", init_max_disparity); parser->parse_config("init_max_features", init_max_features); } - PRINT_DEBUG(" - init_only_use_dynamic: %d\n", init_only_use_dynamic); - PRINT_DEBUG(" - init_only_use_static: %d\n", init_only_use_static); - if (init_only_use_dynamic && init_only_use_static) { - PRINT_ERROR(RED "cannot only use static and dynamic initialization!\n" RESET); - PRINT_ERROR(RED " init_only_use_dynamic = %d\n" RESET, init_only_use_dynamic); - PRINT_ERROR(RED " init_only_use_dynamic = %d\n" RESET, init_only_use_static); - std::exit(EXIT_FAILURE); - } - PRINT_DEBUG(" - init_calib_camera_pose: %d\n", init_calib_camera_pose); - PRINT_DEBUG(" - init_calib_camera_timeoffset: %d\n", init_calib_camera_timeoffset); - PRINT_DEBUG(" - init_state_ba: %d\n", init_state_bias_accel); - PRINT_DEBUG(" - init_enforce_gravity_magnitude: %d\n", init_enforce_gravity_magnitude); - PRINT_DEBUG(" - init_camera_rate: %.2f\n", init_camera_rate); PRINT_DEBUG(" - init_window_time: %.2f\n", init_window_time); PRINT_DEBUG(" - init_imu_thresh: %.2f\n", init_imu_thresh); PRINT_DEBUG(" - init_max_disparity: %.2f\n", init_max_disparity); PRINT_DEBUG(" - init_max_features: %.2f\n", init_max_features); - // Ensure we have enough frames to work with - double dt = 1.0 / init_camera_rate; - int num_frames = std::floor(init_window_time / dt); - if (num_frames < 4) { - PRINT_ERROR(RED "number of requested frames to init not enough!!\n" RESET); - PRINT_ERROR(RED " init_camera_rate = %.2f (%.2f seconds)\n" RESET, init_camera_rate, dt); - PRINT_ERROR(RED " init_window_time = %.2f seconds\n" RESET, init_window_time); - PRINT_ERROR(RED " num init frames = %d\n" RESET, num_frames); - std::exit(EXIT_FAILURE); - } if (init_max_features < 15) { PRINT_ERROR(RED "number of requested feature tracks to init not enough!!\n" RESET); PRINT_ERROR(RED " init_max_features = %d\n" RESET, init_max_features); @@ -146,55 +94,6 @@ struct InertialInitializerOptions { } } - // NOISE / CHI2 ============================ - - /// Gyroscope white noise (rad/s/sqrt(hz)) - double sigma_w = 1.6968e-04; - double sigma_w_2 = pow(1.6968e-04, 2); - - /// Gyroscope random walk (rad/s^2/sqrt(hz)) - double sigma_wb = 1.9393e-05; - double sigma_wb_2 = pow(1.9393e-05, 2); - - /// Accelerometer white noise (m/s^2/sqrt(hz)) - double sigma_a = 2.0000e-3; - double sigma_a_2 = pow(2.0000e-3, 2); - - /// Accelerometer random walk (m/s^3/sqrt(hz)) - double sigma_ab = 3.0000e-03; - double sigma_ab_2 = pow(3.0000e-03, 2); - - /// Noise sigma for our raw pixel measurements - double sigma_pix = 1; - double sigma_pix_sq = 1; - - /** - * @brief This function will load print out all noise parameters loaded. - * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. - * - * @param parser If not null, this parser will be used to load our parameters - */ - void print_and_load_noise(const std::shared_ptr &parser = nullptr) { - PRINT_DEBUG("NOISE PARAMETERS:\n"); - if (parser != nullptr) { - parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", sigma_w); - parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", sigma_wb); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", sigma_a); - parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", sigma_ab); - sigma_w_2 = std::pow(sigma_w, 2); - sigma_wb_2 = std::pow(sigma_wb, 2); - sigma_a_2 = std::pow(sigma_a, 2); - sigma_ab_2 = std::pow(sigma_ab, 2); - parser->parse_config("up_slam_sigma_px", sigma_pix); - sigma_pix_sq = std::pow(sigma_pix, 2); - } - PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); - PRINT_DEBUG(" - sigma_pix: %.2f\n", sigma_pix); - } - // STATE DEFAULTS ========================== /// Gravity magnitude in the global frame (i.e. should be 9.81 typically) @@ -203,27 +102,6 @@ struct InertialInitializerOptions { /// Number of distinct cameras that we will observe features in int num_cameras = 1; -// /// Time offset between camera and IMU. -// double calib_camimu_dt = 0.0; -// -// /// Map between camid and camera model (true=fisheye, false=radtan) -// std::map camera_fisheye; -// -// /// Map between camid and intrinsics. Values depends on the model but each should be a 4x1 vector normally. -// std::map camera_intrinsics; -// -// /// Map between camid and camera extrinsics (q_ItoC, p_IinC). -// std::map camera_extrinsics; -// -// /// Map between camid and the dimensions of incoming images (width/cols, height/rows). This is normally only used during simulation. -// std::map> camera_wh; -// -// /// If we should try to load a mask and use it to reject invalid features -// bool use_mask = false; -// -// /// Mask images for each camera -// std::map masks; - /** * @brief This function will load and print all state parameters (e.g. sensor extrinsics) * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. @@ -234,157 +112,13 @@ struct InertialInitializerOptions { if (parser != nullptr) { parser->parse_config("gravity_mag", gravity_mag); parser->parse_config("max_cameras", num_cameras); // might be redundant -// for (int i = 0; i < num_cameras; i++) { -// -// // Time offset (use the first one) -// // TODO: support multiple time offsets between cameras -// if (i == 0) { -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "timeshift_cam_imu", calib_camimu_dt, false); -// } -// -// // Distortion model -// std::string dist_model = "radtan"; -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_model", dist_model); -// -// // Distortion parameters -// std::vector cam_calib1 = {1, 1, 0, 0}; -// std::vector cam_calib2 = {0, 0, 0, 0}; -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "intrinsics", cam_calib1); -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_coeffs", cam_calib2); -// Eigen::VectorXd cam_calib = Eigen::VectorXd::Zero(8); -// cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1), -// cam_calib2.at(2), cam_calib2.at(3); -// cam_calib(0) /= (downsample_cameras) ? 2.0 : 1.0; -// cam_calib(1) /= (downsample_cameras) ? 2.0 : 1.0; -// cam_calib(2) /= (downsample_cameras) ? 2.0 : 1.0; -// cam_calib(3) /= (downsample_cameras) ? 2.0 : 1.0; -// -// // FOV / resolution -// std::vector matrix_wh = {1, 1}; -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "resolution", matrix_wh); -// matrix_wh.at(0) /= (downsample_cameras) ? 2.0 : 1.0; -// matrix_wh.at(1) /= (downsample_cameras) ? 2.0 : 1.0; -// std::pair wh(matrix_wh.at(0), matrix_wh.at(1)); -// -// // Extrinsics -// Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); -// parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "T_imu_cam", T_CtoI); -// -// // Load these into our state -// Eigen::Matrix cam_eigen; -// cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose()); -// cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1); -// -// // Insert -// camera_fisheye.insert({i, dist_model == "equidistant"}); -// camera_intrinsics.insert({i, cam_calib}); -// camera_extrinsics.insert({i, cam_eigen}); -// camera_wh.insert({i, wh}); -// } -// parser->parse_config("use_mask", use_mask); -// if (use_mask) { -// for (int i = 0; i < num_cameras; i++) { -// std::string mask_path; -// std::string mask_node = "mask" + std::to_string(i); -// parser->parse_config(mask_node, mask_path); -// std::string total_mask_path = parser->get_config_folder() + mask_path; -// if (!boost::filesystem::exists(total_mask_path)) { -// PRINT_ERROR(RED "Initializer(): invalid mask path:\n" RESET); -// PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str()); -// std::exit(EXIT_FAILURE); -// } -// masks.insert({i, cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE)}); -// } -// } } PRINT_DEBUG("STATE PARAMETERS:\n"); PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag); PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); PRINT_DEBUG(" - num_cameras: %d\n", num_cameras); -// PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt); -// PRINT_DEBUG(" - camera masks?: %d\n", use_mask); -// assert(num_cameras == (int)camera_fisheye.size()); -// for (int n = 0; n < num_cameras; n++) { -// std::stringstream ss; -// ss << "cam_" << n << "_fisheye:" << camera_fisheye.at(n) << std::endl; -// ss << "cam_" << n << "_wh:" << std::endl << camera_wh.at(n).first << " x " << camera_wh.at(n).second << std::endl; -// ss << "cam_" << n << "_intrinsic(0:3):" << std::endl << camera_intrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; -// ss << "cam_" << n << "_intrinsic(4:7):" << std::endl << camera_intrinsics.at(n).block(4, 0, 4, 1).transpose() << std::endl; -// ss << "cam_" << n << "_extrinsic(0:3):" << std::endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl; -// ss << "cam_" << n << "_extrinsic(4:6):" << std::endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl; -// Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity(); -// T_CtoI.block(0, 0, 3, 3) = ov_core::quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose(); -// T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1); -// ss << "T_C" << n << "toI:" << std::endl << T_CtoI << std::endl << std::endl; -// PRINT_DEBUG(ss.str().c_str()); -// } } - // SIMULATOR =============================== - - /// Seed for initial states (i.e. random feature 3d positions in the generated map) - int sim_seed_state_init = 0; - - /// Seed for calibration perturbations. Change this to perturb by different random values if perturbations are enabled. - int sim_seed_preturb = 0; - - /// Measurement noise seed. This should be incremented for each run in the Monte-Carlo simulation to generate the same true measurements, - /// but diffferent noise values. - int sim_seed_measurements = 0; - - /// If we should perturb the calibration that the estimator starts with - bool sim_do_perturbation = false; - - /// Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),ori(xyzw) format. - std::string sim_traj_path = "../ov_data/sim/udel_gore.txt"; - - /// We will start simulating after we have moved this much along the b-spline. This prevents static starts as we init from groundtruth in - /// simulation. - double sim_distance_threshold = 1.2; - - /// Frequency (Hz) that we will simulate our cameras - double sim_freq_cam = 10.0; - - /// Frequency (Hz) that we will simulate our inertial measurement unit - double sim_freq_imu = 400.0; - - /// Feature distance we generate features from (minimum) - double sim_min_feature_gen_distance = 5; - - /// Feature distance we generate features from (maximum) - double sim_max_feature_gen_distance = 10; - - /** - * @brief This function will load print out all simulated parameters. - * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. - * - * @param parser If not null, this parser will be used to load our parameters - */ - void print_and_load_simulation(const std::shared_ptr &parser = nullptr) { - if (parser != nullptr) { - parser->parse_config("sim_seed_state_init", sim_seed_state_init); - parser->parse_config("sim_seed_preturb", sim_seed_preturb); - parser->parse_config("sim_seed_measurements", sim_seed_measurements); - parser->parse_config("sim_do_perturbation", sim_do_perturbation); - parser->parse_config("sim_traj_path", sim_traj_path); - parser->parse_config("sim_distance_threshold", sim_distance_threshold); - parser->parse_config("sim_freq_cam", sim_freq_cam); - parser->parse_config("sim_freq_imu", sim_freq_imu); - parser->parse_config("sim_min_feature_gen_dist", sim_min_feature_gen_distance); - parser->parse_config("sim_max_feature_gen_dist", sim_max_feature_gen_distance); - } - PRINT_DEBUG("SIMULATION PARAMETERS:\n"); - PRINT_WARNING(BOLDRED " - state init seed: %d \n" RESET, sim_seed_state_init); - PRINT_WARNING(BOLDRED " - perturb seed: %d \n" RESET, sim_seed_preturb); - PRINT_WARNING(BOLDRED " - measurement seed: %d \n" RESET, sim_seed_measurements); - PRINT_WARNING(BOLDRED " - do perturb?: %d\n" RESET, sim_do_perturbation); - PRINT_DEBUG(" - traj path: %s\n", sim_traj_path.c_str()); - PRINT_DEBUG(" - dist thresh: %.2f\n", sim_distance_threshold); - PRINT_DEBUG(" - cam feq: %.2f\n", sim_freq_cam); - PRINT_DEBUG(" - imu feq: %.2f\n", sim_freq_imu); - PRINT_DEBUG(" - min feat dist: %.2f\n", sim_min_feature_gen_distance); - PRINT_DEBUG(" - max feat dist: %.2f\n", sim_max_feature_gen_distance); - } }; } // namespace ov_init diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz index d5274388c..e847e0593 100644 --- a/ov_msckf/launch/display.rviz +++ b/ov_msckf/launch/display.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.6011396050453186 - Tree Height: 381 + Tree Height: 347 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -63,8 +63,7 @@ Visualization Manager: Value: true imu: Value: true - truth: - Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: false @@ -77,8 +76,6 @@ Visualization Manager: {} cam1: {} - truth: - {} Update Interval: 0 Value: true - Class: rviz/Image @@ -90,7 +87,7 @@ Visualization Manager: Name: Image Tracks Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: true - Class: rviz/Image @@ -102,7 +99,7 @@ Visualization Manager: Name: Current Depths Normalize Range: true Queue Size: 2 - Transport Hint: raw + Transport Hint: compressed Unreliable: false Value: false - Alpha: 1 @@ -122,6 +119,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -145,6 +143,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -325,6 +324,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -334,10 +334,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9503980278968811 + Pitch: 0.8953980803489685 Target Frame: - Value: Orbit (rviz) - Yaw: 0.7903981804847717 + Yaw: 0.8253981471061707 Saved: ~ Window Geometry: Current Depths: @@ -349,7 +348,7 @@ Window Geometry: Hide Right Dock: true Image Tracks: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001870000026bfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000016000001ba000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000180049006d00610067006500200054007200610063006b007301000001d6000000ab0000001600fffffffb0000001a0054007200610063006b00200048006900730074006f0072007901000003d0000000160000000000000000fb0000000a0049006d00610067006501000001330000011c0000000000000000fb0000001c00430075007200720065006e007400200044006500700074006800730000000255000000b80000001600fffffffb0000000a0049006d0061006700650100000214000001140000000000000000fb000000140054007200610063006b0020004c0069006e006501000003ec000000160000000000000000000000010000010f000002a4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000000000002a4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000295fc0100000005fb0000000800430041004d003000000000000000020c0000000000000000fb0000000800430041004d00310000000000000002ef0000000000000000fb0000000800430041004d00320000000000000003850000000000000000fb0000000800430041004d003300000000000000073f0000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a300000158fc0100000002fb0000000800540069006d0065000000000000000570000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003740000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -359,5 +358,5 @@ Window Geometry: Views: collapsed: true Width: 1281 - X: 1944 - Y: 56 + X: 2092 + Y: 1164 From be7a97d4f6019efe127e6c0c486535dc9636ed2f Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 17:47:23 -0500 Subject: [PATCH 44/55] add dockerfiles, and upgrade python cmake logic (requires cmake 3.12 now) --- Dockerfile_ros1_16_04 | 43 +++++++++++++++++++++++ Dockerfile_ros1_18_04 | 43 +++++++++++++++++++++++ Dockerfile => Dockerfile_ros1_20_04 | 14 ++++++-- Dockerfile_ros2_18_04 | 42 ++++++++++++++++++++++ Dockerfile_ros2_20_04 | 42 ++++++++++++++++++++++ config/rpng_sim/estimator_config.yaml | 3 +- docs/dev-docker.dox | 50 ++++++++++++++++++++------- docs/gs-installing.dox | 10 +++--- ov_core/CMakeLists.txt | 2 +- ov_data/CMakeLists.txt | 2 +- ov_eval/CMakeLists.txt | 27 +++++++-------- ov_init/CMakeLists.txt | 27 +++++++-------- ov_msckf/CMakeLists.txt | 2 +- 13 files changed, 252 insertions(+), 55 deletions(-) create mode 100644 Dockerfile_ros1_16_04 create mode 100644 Dockerfile_ros1_18_04 rename Dockerfile => Dockerfile_ros1_20_04 (68%) create mode 100644 Dockerfile_ros2_18_04 create mode 100644 Dockerfile_ros2_20_04 diff --git a/Dockerfile_ros1_16_04 b/Dockerfile_ros1_16_04 new file mode 100644 index 000000000..58b7df3db --- /dev/null +++ b/Dockerfile_ros1_16_04 @@ -0,0 +1,43 @@ +FROM osrf/ros:kinetic-desktop-full + +# ========================================================= +# ========================================================= + +# Are you are looking for how to use this docker file? +# - https://docs.openvins.com/dev-docker.html +# - https://docs.docker.com/get-started/ +# - http://wiki.ros.org/docker/Tutorials/Docker + +# ========================================================= +# ========================================================= + +# Dependencies we use, catkin tools is very good build system +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git +RUN sudo apt-get install -y python-catkin-tools + +# Seems this has Python 2.7 installed on it... +RUN sudo apt-get install -y python2.7-dev python-matplotlib python-numpy python-psutil python-tk + +# Install CMake 3.13.5 +ADD https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh /cmake-3.13.5-Linux-x86_64.sh +RUN mkdir /opt/cmake +RUN sh /cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +RUN cmake --version + +# Install deps needed for clion remote debugging +# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ +RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ + gdb clang cmake rsync tar python && apt-get clean +RUN ( \ + echo 'LogLevel DEBUG2'; \ + echo 'PermitRootLogin yes'; \ + echo 'PasswordAuthentication yes'; \ + echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ + ) > /etc/ssh/sshd_config_test_clion \ + && mkdir /run/sshd +RUN useradd -m user && yes password | passwd user +RUN usermod -s /bin/bash user +CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] + diff --git a/Dockerfile_ros1_18_04 b/Dockerfile_ros1_18_04 new file mode 100644 index 000000000..bb24fb942 --- /dev/null +++ b/Dockerfile_ros1_18_04 @@ -0,0 +1,43 @@ +FROM osrf/ros:melodic-desktop-full + +# ========================================================= +# ========================================================= + +# Are you are looking for how to use this docker file? +# - https://docs.openvins.com/dev-docker.html +# - https://docs.docker.com/get-started/ +# - http://wiki.ros.org/docker/Tutorials/Docker + +# ========================================================= +# ========================================================= + +# Dependencies we use, catkin tools is very good build system +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git +RUN sudo apt-get install -y python-catkin-tools + +# Seems this has Python 3.6 installed on it... +RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk + +# Install CMake 3.13.5 +ADD https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh /cmake-3.13.5-Linux-x86_64.sh +RUN mkdir /opt/cmake +RUN sh /cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +RUN cmake --version + +# Install deps needed for clion remote debugging +# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ +RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ + gdb clang cmake rsync tar python && apt-get clean +RUN ( \ + echo 'LogLevel DEBUG2'; \ + echo 'PermitRootLogin yes'; \ + echo 'PasswordAuthentication yes'; \ + echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ + ) > /etc/ssh/sshd_config_test_clion \ + && mkdir /run/sshd +RUN useradd -m user && yes password | passwd user +RUN usermod -s /bin/bash user +CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] + diff --git a/Dockerfile b/Dockerfile_ros1_20_04 similarity index 68% rename from Dockerfile rename to Dockerfile_ros1_20_04 index e69633365..a912b4928 100644 --- a/Dockerfile +++ b/Dockerfile_ros1_20_04 @@ -12,10 +12,20 @@ FROM osrf/ros:noetic-desktop-full # ========================================================= # Dependencies we use, catkin tools is very good build system -# Also some helper utitiles for fast in terminal edits (nano etc) -RUN apt-get update && apt-get install -y libeigen3-dev nano git cmake +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git RUN sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon +# Seems this has Python 3.8 installed on it... +RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk + +# Install CMake 3.13.5 +ADD https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh /cmake-3.13.5-Linux-x86_64.sh +RUN mkdir /opt/cmake +RUN sh /cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +RUN cmake --version + # Install deps needed for clion remote debugging # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ diff --git a/Dockerfile_ros2_18_04 b/Dockerfile_ros2_18_04 new file mode 100644 index 000000000..86baa0fa0 --- /dev/null +++ b/Dockerfile_ros2_18_04 @@ -0,0 +1,42 @@ +FROM osrf/ros:dashing-desktop + +# ========================================================= +# ========================================================= + +# Are you are looking for how to use this docker file? +# - https://docs.openvins.com/dev-docker.html +# - https://docs.docker.com/get-started/ +# - http://wiki.ros.org/docker/Tutorials/Docker + +# ========================================================= +# ========================================================= + +# Dependencies we use, catkin tools is very good build system +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git + +# Seems this has Python 3.6 installed on it... +RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk + +# Install CMake 3.13.5 +ADD https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh /cmake-3.13.5-Linux-x86_64.sh +RUN mkdir /opt/cmake +RUN sh /cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +RUN cmake --version + +# Install deps needed for clion remote debugging +# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ +RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ + gdb clang cmake rsync tar python && apt-get clean +RUN ( \ + echo 'LogLevel DEBUG2'; \ + echo 'PermitRootLogin yes'; \ + echo 'PasswordAuthentication yes'; \ + echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ + ) > /etc/ssh/sshd_config_test_clion \ + && mkdir /run/sshd +RUN useradd -m user && yes password | passwd user +RUN usermod -s /bin/bash user +CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] + diff --git a/Dockerfile_ros2_20_04 b/Dockerfile_ros2_20_04 new file mode 100644 index 000000000..6787c6c85 --- /dev/null +++ b/Dockerfile_ros2_20_04 @@ -0,0 +1,42 @@ +FROM osrf/ros:galactic-desktop + +# ========================================================= +# ========================================================= + +# Are you are looking for how to use this docker file? +# - https://docs.openvins.com/dev-docker.html +# - https://docs.docker.com/get-started/ +# - http://wiki.ros.org/docker/Tutorials/Docker + +# ========================================================= +# ========================================================= + +# Dependencies we use, catkin tools is very good build system +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git + +# Seems this has Python 3.8 installed on it... +RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk + +# Install CMake 3.13.5 +ADD https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh /cmake-3.13.5-Linux-x86_64.sh +RUN mkdir /opt/cmake +RUN sh /cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +RUN cmake --version + +# Install deps needed for clion remote debugging +# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ +RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ + gdb clang cmake rsync tar python && apt-get clean +RUN ( \ + echo 'LogLevel DEBUG2'; \ + echo 'PermitRootLogin yes'; \ + echo 'PasswordAuthentication yes'; \ + echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ + ) > /etc/ssh/sshd_config_test_clion \ + && mkdir /run/sshd +RUN useradd -m user && yes password | passwd user +RUN usermod -s /bin/bash user +CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] + diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index a725038b0..3c25232bf 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -103,8 +103,7 @@ sim_seed_state_init: 0 sim_seed_preturb: 0 sim_seed_measurements: 0 sim_do_perturbation: false -#sim_traj_path: "/tmp/trajectory_not_set.txt" -sim_traj_path: "/home/patrick/workspace/catkin_ws_init2/src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" +sim_traj_path: "src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt" sim_distance_threshold: 1.2 sim_freq_cam: 10 sim_freq_imu: 400 diff --git a/docs/dev-docker.dox b/docs/dev-docker.dox index 33d50b437..4d360e7fb 100644 --- a/docs/dev-docker.dox +++ b/docs/dev-docker.dox @@ -67,11 +67,14 @@ rviz @endcode + + @section dev-docker-openvins Running OpenVINS with Docker Clone the OpenVINS repository, build the container and then launch it. -The [Dockerfile](https://github.com/rpng/open_vins/blob/master/Dockerfile) will not build the repo by default, thus you will need to build the project. - +The [Dockerfile](https://docs.docker.com/engine/reference/builder/) will not build the repo by default, thus you will need to build the project. +We have a few docker files for each version of ROS and operating system we support. +In the following we will use the [Dockerfile_ros1_20_04](https://github.com/rpng/open_vins/blob/master/Dockerfile_ros1_20_04) which is for a ROS1 install with a 20.04 system. @m_class{m-block m-warning} @@ -85,15 +88,17 @@ mkdir -p ~/workspace/catkin_ws_ov/src cd ~/workspace/catkin_ws_ov/src git clone https://github.com/rpng/open_vins.git cd open_vins -docker build -t openvins . -cd ~/workspace/catkin_ws_ov +# which docker file version you want (ROS1 vs ROS2 and ubuntu version) +# ros1_20_04, ros1_18_04 +export VERSION=ros1_20_04 +docker build -t ov_$VERSION -f Dockerfile_$VERSION . @endcode -If the dockerfile breaks, you can remove the image and reinstall using the above +If the dockerfile breaks, you can remove the image and reinstall using the following: @code{.shell-session} docker image list -docker image rm openvins --force +docker image rm ov_ros1_20_04 --force @endcode From here it is a good idea to create a nice helper command which will launch the docker and also pass the GUI to your host machine. @@ -117,19 +122,24 @@ alias ov_docker="docker run -it --net=host --gpus all \ --env=\"NVIDIA_DRIVER_CAPABILITIES=all\" --env=\"DISPLAY\" \ --env=\"QT_X11_NO_MITSHM=1\" --volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\" \ --mount type=bind,source=$DOCKER_CATKINWS,target=/catkin_ws \ - --mount type=bind,source=$DOCKER_DATASETS,target=/datasets \ - openvins $1" + --mount type=bind,source=$DOCKER_DATASETS,target=/datasets $1" # save and exit source ~/.bashrc @endcode Now we can launch RVIZ and also compile the OpenVINS codebase. -From two different terminals on the host machine one can run the following: +From two different terminals on the host machine one can run the following (ROS 1): + +@code{.shell-session} +ov_docker ov_ros1_20_04 roscore +ov_docker ov_ros1_20_04 rosrun rviz rviz -d /catkin_ws/src/open_vins/ov_msckf/launch/display.rviz +@endcode + +To actually get a bash environment that we can use to build and run things with we can do the following. +Note that any install or changes to operating system variables will not persist, thus only edit within your workspace which is linked as a volume. @code{.shell-session} -ov_docker roscore -ov_docker rosrun rviz rviz -d /catkin_ws/src/open_vins/ov_msckf/launch/display.rviz -ov_docker bash +ov_docker ov_ros1_20_04 bash @endcode Now once inside the docker with the bash shell we can build and launch an example simulation: @@ -138,9 +148,21 @@ Now once inside the docker with the bash shell we can build and launch an exampl cd catkin_ws catkin build source devel/setup.bash -roslaunch ov_msckf pgeneva_sim.launch +rosrun ov_eval plot_trajectories none src/open_vins/ov_data/sim/udel_gore.txt +roslaunch ov_msckf simulation.launch @endcode +And a version for ROS 2 we can do the following: + +@code{.shell-session} +cd catkin_ws +colcon build --event-handlers console_cohesion+ +source install/setup.bash +ros2 run ov_eval plot_trajectories none src/open_vins/ov_data/sim/udel_gore.txt +ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml +@endcode + + @m_class{m-block m-danger} @par Real-time Performance @@ -149,6 +171,8 @@ roslaunch ov_msckf pgeneva_sim.launch the "serial" nodes which allows for the same parameters to be used as when installing directly on an OS. + + @section dev-docker-clion Using Jetbrains Clion and Docker Jetbrains provides some instructions on their side and a youtube video. diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox index 062f8963b..7046a6ce5 100644 --- a/docs/gs-installing.dox +++ b/docs/gs-installing.dox @@ -17,8 +17,8 @@ Please see the official instructions to install ROS: - [Ubuntu 16.04 ROS 1 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) (uses OpenCV 3.3) - [Ubuntu 18.04 ROS 1 Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) (uses OpenCV 3.2) - [Ubuntu 20.04 ROS 1 Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) (uses OpenCV 4.2) -- [Ubuntu 18.04 ROS 2 Dashing](https://docs.ros.org/en/dashing/) -- [Ubuntu 20.04 ROS 2 Galactic](https://docs.ros.org/en/galactic/) +- [Ubuntu 18.04 ROS 2 Dashing](https://docs.ros.org/en/dashing/) (uses OpenCV 3.2) +- [Ubuntu 20.04 ROS 2 Galactic](https://docs.ros.org/en/galactic/) (uses OpenCV 4.2) We do support ROS-free builds, but don't recommend using this interface as we have limited support for it. @@ -37,7 +37,7 @@ sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31 sudo apt-get update export ROS1_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04 sudo apt-get install ros-$ROS1_DISTRO-desktop-full -sudo apt-get install libeigen3-dev python-catkin-tools # ubuntu 16.04 or 18.04 +sudo apt-get install libeigen3-dev python-catkin-tools # ubuntu 16.04, 18.04 sudo apt-get install libeigen3-dev python3-catkin-tools python3-osrf-pycommon # ubuntu 20.04 @endcode @@ -133,8 +133,8 @@ You can disable this visualization if it is broken for you by passing the -DDISA Additionally if you wish to record CPU and memory usage of the node, you will need to install the [psutil](https://github.com/giampaolo/psutil) library. @code{.shell-session} -sudo apt-get install python-matplotlib python-numpy python2.7-dev python-psutil # for python2 systems -sudo apt-get install python-matplotlib python3-numpy python3-dev python3-psutil # for python3 systems +sudo apt-get install python2.7-dev python-matplotlib python-numpy python-psutil # for python2 systems +sudo apt-get install python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk # for python3 systems catkin build -DDISABLE_MATPLOTLIB=OFF # build with viz (default) catkin build -DDISABLE_MATPLOTLIB=ON # build without viz @endcode diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index e1f4c7455..e90738482 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.3) +cmake_minimum_required(VERSION 3.12) # Define our install directories based on GNU conventions # https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html diff --git a/ov_data/CMakeLists.txt b/ov_data/CMakeLists.txt index 96c9f9a5d..b3424b14b 100644 --- a/ov_data/CMakeLists.txt +++ b/ov_data/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.3) +cmake_minimum_required(VERSION 3.12) # Project name project(ov_data) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 4fe53e6e0..0b4fa572f 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.3) +cmake_minimum_required(VERSION 3.12) # Define our install directories based on GNU conventions # https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html @@ -11,22 +11,19 @@ project(ov_eval) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) -# check if we have our python libs files -# sudo apt-get install python-matplotlib python-numpy python2.7-dev -find_package(PythonLibs 2.7 QUIET) +# check if we have our python libs files (will search for python3 then python2 installs) +# sudo apt-get install python-matplotlib python-numpy python-dev +# https://cmake.org/cmake/help/latest/module/FindPython.html +# https://stackoverflow.com/a/34580995/7718197 +find_package(Python COMPONENTS Interpreter Development) option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) -if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) +if (Python_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) - message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) - message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) - message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) - include_directories( - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include - ) - list(APPEND thirdparty_libraries - ${PYTHON_LIBRARIES} - ) + message(STATUS "PYTHON VERSION: " ${Python_VERSION}) + message(STATUS "PYTHON INCLUDE: " ${Python_INCLUDE_DIRS}) + message(STATUS "PYTHON LIBRARIES: " ${Python_LIBRARIES}) + include_directories(${Python_INCLUDE_DIRS}) + list(APPEND thirdparty_libraries ${Python_LIBRARIES}) endif () # We need c++14 for ROS2, thus just require it for everybody diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 52fca0ddb..2bd8c87cd 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.3) +cmake_minimum_required(VERSION 3.12) # Define our install directories based on GNU conventions # https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html @@ -11,22 +11,19 @@ project(ov_init) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) -# check if we have our python libs files -# sudo apt-get install python-matplotlib python-numpy python2.7-dev -find_package(PythonLibs 2.7 QUIET) +# check if we have our python libs files (will search for python3 then python2 installs) +# sudo apt-get install python-matplotlib python-numpy python-dev +# https://cmake.org/cmake/help/latest/module/FindPython.html +# https://stackoverflow.com/a/34580995/7718197 +find_package(Python COMPONENTS Interpreter Development) option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) -if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) +if (Python_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) - message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) - message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) - message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) - include_directories( - /usr/local/lib/python2.7/dist-packages/numpy/core/include - /usr/local/lib/python2.7/site-packages/numpy/core/include - ) - list(APPEND thirdparty_libraries - ${PYTHON_LIBRARIES} - ) + message(STATUS "PYTHON VERSION: " ${Python_VERSION}) + message(STATUS "PYTHON INCLUDE: " ${Python_INCLUDE_DIRS}) + message(STATUS "PYTHON LIBRARIES: " ${Python_LIBRARIES}) + include_directories(${Python_INCLUDE_DIRS}) + list(APPEND thirdparty_libraries ${Python_LIBRARIES}) endif () # We need c++14 for ROS2, thus just require it for everybody diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index dd24355a9..5d1cd4662 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.3) +cmake_minimum_required(VERSION 3.12) # Define our install directories based on GNU conventions # https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html From ae50186543ee64d0d6bf7b492ac0ee079ef84e22 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 17:55:44 -0500 Subject: [PATCH 45/55] add instructions for cmake upgrade, move docker to getting started section --- docs/dev-docker.dox | 17 ++++++----------- docs/dev-welcome.dox | 1 - docs/getting-started.dox | 1 + docs/gs-installing.dox | 12 ++++++++++++ 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/docs/dev-docker.dox b/docs/dev-docker.dox index 4d360e7fb..f95617dc1 100644 --- a/docs/dev-docker.dox +++ b/docs/dev-docker.dox @@ -1,7 +1,7 @@ /** -@page dev-docker Developing with Docker +@page dev-docker Building with Docker @tableofcontents @@ -62,8 +62,7 @@ docker run -it --net=host --gpus all \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ osrf/ros:noetic-desktop-full \ bash -# you should be able to launch rviz once in bash -rviz +rviz # you should be able to launch rviz once in bash @endcode @@ -88,9 +87,7 @@ mkdir -p ~/workspace/catkin_ws_ov/src cd ~/workspace/catkin_ws_ov/src git clone https://github.com/rpng/open_vins.git cd open_vins -# which docker file version you want (ROS1 vs ROS2 and ubuntu version) -# ros1_20_04, ros1_18_04 -export VERSION=ros1_20_04 +export VERSION=ros1_20_04 # which docker file version you want (ROS1 vs ROS2 and ubuntu version) docker build -t ov_$VERSION -f Dockerfile_$VERSION . @endcode @@ -113,8 +110,7 @@ Here you can append it to the bottom of the ~/.bashrc so that we always have it can add and remove mounts from this command as you see the need. @code{.shell-session} -nano ~/.bashrc -# add to the bashrc file +nano ~/.bashrc # add to the bashrc file xhost + &> /dev/null export DOCKER_CATKINWS=/home/username/workspace/catkin_ws_ov export DOCKER_DATASETS=/home/username/datasets @@ -123,8 +119,7 @@ alias ov_docker="docker run -it --net=host --gpus all \ --env=\"QT_X11_NO_MITSHM=1\" --volume=\"/tmp/.X11-unix:/tmp/.X11-unix:rw\" \ --mount type=bind,source=$DOCKER_CATKINWS,target=/catkin_ws \ --mount type=bind,source=$DOCKER_DATASETS,target=/datasets $1" -# save and exit -source ~/.bashrc +source ~/.bashrc # after you save and exit @endcode Now we can launch RVIZ and also compile the OpenVINS codebase. @@ -187,7 +182,7 @@ After building the OpenVINS image (as above) we can do the following which will This process will allow us to connect Clion to it. @code{.shell-session} -docker run -d --cap-add sys_ptrace -p127.0.0.1:2222:22 --name clion_remote_env openvins +docker run -d --cap-add sys_ptrace -p127.0.0.1:2222:22 --name clion_remote_env ov_ros1_20_04 @endcode We can now change Clion to use the docker remote: diff --git a/docs/dev-welcome.dox b/docs/dev-welcome.dox index 605d77601..7e0dff983 100644 --- a/docs/dev-welcome.dox +++ b/docs/dev-welcome.dox @@ -4,7 +4,6 @@ @page dev-welcome Internals and Developers - @subpage dev-coding-style --- General coding styles and conventions -- @subpage dev-docker --- How to use docker images to develop with - @subpage dev-docs --- Developer guide on how documentation can be built - @subpage dev-ros1-to-ros2 --- Some notes on ROS bag conversion - @subpage dev-index --- Description of the covariance index system diff --git a/docs/getting-started.dox b/docs/getting-started.dox index 858259009..0098ce60a 100644 --- a/docs/getting-started.dox +++ b/docs/getting-started.dox @@ -35,6 +35,7 @@ The ov_eval library has a bunch of evaluation methods and scripts that one can u @section getting-started-more Getting Started Guides - @subpage gs-installing --- Installation guide for OpenVINS and dependencies +- @subpage dev-docker --- Installing with Docker instead of from source - @subpage gs-tutorial --- Simple tutorial on getting OpenVINS running out of the box. - @subpage gs-datasets --- Links to supported datasets and configuration files - @subpage gs-calibration --- Guide to how to calibration your own visual-inertial sensors. diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox index 7046a6ce5..f560780e7 100644 --- a/docs/gs-installing.dox +++ b/docs/gs-installing.dox @@ -95,6 +95,18 @@ source ~/.bashrc @endcode +@section gs-install-cmake Upgrading CMake to Version 3.12 + +We use a newer version of [cmake](https://cmake.org/download/) so you will probably need to upgrade your system if you are on an old ubuntu version. +You can run the following which downloads a script and will install it for you. + +@code{.shell-session} +wget https://cmake.org/files/v3.13/cmake-3.13.5-Linux-x86_64.sh +sudo mkdir /opt/cmake +sudo sh cmake-3.13.5-Linux-x86_64.sh --prefix=/opt/cmake --skip-license +sudo ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake +cmake --version +@endcode @section gs-install-openvins Cloning the OpenVINS Project From df709fe809c92b6cd014442cbe81e3a9853cee05 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Wed, 8 Dec 2021 17:57:51 -0500 Subject: [PATCH 46/55] formatting --- ov_core/src/feat/FeatureInitializerOptions.h | 2 +- ov_core/src/test_webcam.cpp | 2 +- ov_core/src/track/Grider_FAST.h | 2 +- ov_core/src/track/TrackAruco.h | 3 ++- ov_core/src/utils/opencv_yaml_parse.h | 1 - ov_core/src/utils/print.cpp | 4 ++-- ov_init/src/init/InertialInitializer.cpp | 1 - ov_init/src/init/InertialInitializerOptions.h | 1 - ov_msckf/src/run_simulation.cpp | 2 +- ov_msckf/src/run_subscribe_msckf.cpp | 1 - ov_msckf/src/state/StateOptions.h | 4 ++-- 11 files changed, 10 insertions(+), 13 deletions(-) diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h index c5252fec0..720245ecc 100644 --- a/ov_core/src/feat/FeatureInitializerOptions.h +++ b/ov_core/src/feat/FeatureInitializerOptions.h @@ -22,8 +22,8 @@ #ifndef OV_CORE_INITIALIZEROPTIONS_H #define OV_CORE_INITIALIZEROPTIONS_H -#include "utils/print.h" #include "utils/opencv_yaml_parse.h" +#include "utils/print.h" namespace ov_core { diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 71b3f6726..484ffead7 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -37,8 +37,8 @@ #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" -#include "utils/print.h" #include "utils/opencv_yaml_parse.h" +#include "utils/print.h" #if ROS_AVAILABLE == 1 #include diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h index 5e41e1fce..0bef5d3d2 100644 --- a/ov_core/src/track/Grider_FAST.h +++ b/ov_core/src/track/Grider_FAST.h @@ -79,7 +79,7 @@ class Grider_FAST { // NOTE: -> 1 = num_features / (grid_x * grid_y)) // NOTE: -> grid_x = ratio * grid_y (keep the original grid ratio) // NOTE: -> grid_y = sqrt(num_features / ratio) - if(num_features < grid_x * grid_y) { + if (num_features < grid_x * grid_y) { double ratio = (double)grid_x / (double)grid_y; grid_y = std::ceil(std::sqrt(num_features / ratio)); grid_x = std::ceil(grid_y * ratio); diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index d924e4261..e4ef4a443 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -38,7 +38,8 @@ namespace ov_core { * We track the corners of the tag as compared to the pose of the tag or any other corners. * Right now we hardcode the dictionary to be `cv::aruco::DICT_6X6_1000`, so please generate tags in this family of tags. * You can generate these tags using an online utility: https://chev.me/arucogen/ - * The actual size of the tags do not matter since we do not recover the pose and instead just use this for re-detection and tracking of the four corners of the tag. + * The actual size of the tags do not matter since we do not recover the pose and instead just use this for re-detection and tracking of the + * four corners of the tag. */ class TrackAruco : public TrackBase { diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h index a84008841..538b340c1 100644 --- a/ov_core/src/utils/opencv_yaml_parse.h +++ b/ov_core/src/utils/opencv_yaml_parse.h @@ -472,7 +472,6 @@ class YamlParser { typeid(node_result).name()); } } - } /** diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp index 4e6d15ba8..436d48efb 100644 --- a/ov_core/src/utils/print.cpp +++ b/ov_core/src/utils/print.cpp @@ -85,11 +85,11 @@ void Printer::debugPrint(PrintLevel level, const char location[], const char lin // Print the location info first for our debug output // Truncate the filename to the max size for the filepath - if(static_cast(Printer::current_print_level) <= static_cast(Printer::PrintLevel::DEBUG)) { + if (static_cast(Printer::current_print_level) <= static_cast(Printer::PrintLevel::DEBUG)) { std::string path(location); std::string base_filename = path.substr(path.find_last_of("/\\") + 1); if (base_filename.size() > MAX_FILE_PATH_LEGTH) { - printf("%s", base_filename.substr(base_filename.size()-MAX_FILE_PATH_LEGTH,base_filename.size()).c_str()); + printf("%s", base_filename.substr(base_filename.size() - MAX_FILE_PATH_LEGTH, base_filename.size()).c_str()); } else { printf("%s", base_filename.c_str()); } diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp index 910f16099..c3a410daa 100644 --- a/ov_init/src/init/InertialInitializer.cpp +++ b/ov_init/src/init/InertialInitializer.cpp @@ -33,7 +33,6 @@ InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_, st // Create static initializer init_static = std::make_shared(params, _db, imu_data); - } bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, diff --git a/ov_init/src/init/InertialInitializerOptions.h b/ov_init/src/init/InertialInitializerOptions.h index 30f43e296..8570cf37e 100644 --- a/ov_init/src/init/InertialInitializerOptions.h +++ b/ov_init/src/init/InertialInitializerOptions.h @@ -118,7 +118,6 @@ struct InertialInitializerOptions { PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag); PRINT_DEBUG(" - num_cameras: %d\n", num_cameras); } - }; } // namespace ov_init diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 9c5420ce7..081b3bea2 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -100,7 +100,7 @@ int main(int argc, char **argv) { #endif // Ensure we read in all parameters required - if(!parser->successful()) { + if (!parser->successful()) { PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); std::exit(EXIT_FAILURE); } diff --git a/ov_msckf/src/run_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp index d91e4c75a..d50a73093 100644 --- a/ov_msckf/src/run_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -107,7 +107,6 @@ int main(int argc, char **argv) { rclcpp::spin(node); #endif - // Final visualization viz->visualize_final(); diff --git a/ov_msckf/src/state/StateOptions.h b/ov_msckf/src/state/StateOptions.h index 530fb8ba9..f1a39c3f6 100644 --- a/ov_msckf/src/state/StateOptions.h +++ b/ov_msckf/src/state/StateOptions.h @@ -23,8 +23,8 @@ #define OV_MSCKF_STATE_OPTIONS_H #include "types/LandmarkRepresentation.h" -#include "utils/print.h" #include "utils/opencv_yaml_parse.h" +#include "utils/print.h" #include @@ -81,7 +81,7 @@ struct StateOptions { ov_type::LandmarkRepresentation::Representation feat_rep_aruco = ov_type::LandmarkRepresentation::Representation::GLOBAL_3D; /// Nice print function of what parameters we have loaded - void print(const std::shared_ptr& parser=nullptr) { + void print(const std::shared_ptr &parser = nullptr) { if (parser != nullptr) { parser->parse_config("use_fej", do_fej); parser->parse_config("use_imuavg", imu_avg); From 0a90ffb7c22c931c5e558c4d0ba52d098b19b346 Mon Sep 17 00:00:00 2001 From: Yulin Yang Date: Tue, 30 Nov 2021 22:16:21 -0500 Subject: [PATCH 47/55] use better matrix log functions that can handle things near identity --- ov_core/src/utils/quat_ops.h | 131 +++++++++++++++++++---------------- 1 file changed, 72 insertions(+), 59 deletions(-) diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index 7cd2c0406..7bc6d8e2b 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -230,7 +230,7 @@ inline Eigen::Matrix vee(const Eigen::Matrix &w_x) { * \mathrm{where}&\quad \theta^2 = \mathbf{v}^\top\mathbf{v} * @f} * - * @param[in] w 3x1 vector we will take the exponential of + * @param[in] w 3x1 vector in R(3) we will take the exponential of * @return SO(3) rotation matrix */ inline Eigen::Matrix exp_so3(const Eigen::Matrix &w) { @@ -239,7 +239,7 @@ inline Eigen::Matrix exp_so3(const Eigen::Matrix &w) double theta = w.norm(); // Handle small angle values double A, B; - if (theta < 1e-12) { + if (theta < 1e-7) { A = 1; B = 0.5; } else { @@ -266,30 +266,52 @@ inline Eigen::Matrix exp_so3(const Eigen::Matrix &w) * \lfloor\mathbf{v}\times\rfloor &= \frac{\theta}{2\sin{\theta}}(\mathbf{R}-\mathbf{R}^\top) * @f} * + * This function is based on the GTSAM one as the original implementation was a bit unstable. + * See the following: + * - https://github.com/borglab/gtsam/ + * - https://github.com/borglab/gtsam/issues/746 + * - https://github.com/borglab/gtsam/pull/780 + * * @param[in] R 3x3 SO(3) rotation matrix - * @return 3x1 in the so(3) space [omegax, omegay, omegaz] + * @return 3x1 in the R(3) space [omegax, omegay, omegaz] */ inline Eigen::Matrix log_so3(const Eigen::Matrix &R) { - // magnitude of the skew elements (handle edge case where we sometimes have a>1...) - double a = 0.5 * (R.trace() - 1); - double theta = (a > 1) ? acos(1) : ((a < -1) ? acos(-1) : acos(a)); - // Handle small angle values - double D; - if (theta < 1e-12) { - D = 0.5; - } else { - D = theta / (2 * sin(theta)); - } - // calculate the skew symetric matrix - Eigen::Matrix w_x = D * (R - R.transpose()); - // check if we are near the identity - if (R != Eigen::MatrixXd::Identity(3, 3)) { - Eigen::Vector3d vec; - vec << w_x(2, 1), w_x(0, 2), w_x(1, 0); - return vec; + + // note switch to base 1 + double R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); + double R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); + double R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + + // Get trace(R) + const double tr = R.trace(); + Eigen::Vector3d omega; + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (tr + 1.0 < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-5) + omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Eigen::Vector3d(R13, R23, 1.0 + R33); + else if (std::abs(R22 + 1.0) > 1e-5) + omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Eigen::Vector3d(R12, 1.0 + R22, R32); + else + // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit + omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Eigen::Vector3d(1.0 + R11, R21, R31); } else { - return Eigen::Vector3d::Zero(); + double magnitude; + const double tr_3 = tr - 3.0; // always negative + if (tr_3 < -1e-7) { + double theta = acos((tr - 1.0) / 2.0); + magnitude = theta / (2.0 * sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) + // see https://github.com/borglab/gtsam/issues/746 for details + magnitude = 0.5 - tr_3 / 12.0; + } + omega = magnitude * Eigen::Vector3d(R32 - R23, R13 - R31, R21 - R12); } + + return omega; } /** @@ -309,7 +331,7 @@ inline Eigen::Matrix log_so3(const Eigen::Matrix &R) * C &= (1-A)/\theta^2 * \f} * - * @param vec 6x1 in the se(3) space [omega, u] + * @param vec 6x1 in the R(6) space [omega, u] * @return 4x4 SE(3) matrix */ inline Eigen::Matrix4d exp_se3(Eigen::Matrix vec) { @@ -323,7 +345,7 @@ inline Eigen::Matrix4d exp_se3(Eigen::Matrix vec) { // Handle small angle values double A, B, C; - if (theta < 1e-12) { + if (theta < 1e-7) { A = 1; B = 0.5; C = 1.0 / 6.0; @@ -359,43 +381,34 @@ inline Eigen::Matrix4d exp_se3(Eigen::Matrix vec) { * \mathbf V^{-1} &= \mathbf I - \frac{1}{2} \lfloor \boldsymbol\omega \times\rfloor + \frac{1}{\theta^2}\Big(1-\frac{A}{2B}\Big)\lfloor * \boldsymbol\omega \times\rfloor^2 \f} * + * This function is based on the GTSAM one as the original implementation was a bit unstable. + * See the following: + * - https://github.com/borglab/gtsam/ + * - https://github.com/borglab/gtsam/issues/746 + * - https://github.com/borglab/gtsam/pull/780 + * * @param mat 4x4 SE(3) matrix - * @return 6x1 in the se(3) space [omega, u] + * @return 6x1 in the R(6) space [omega, u] */ inline Eigen::Matrix log_se3(Eigen::Matrix4d mat) { - - // Get sub-matrices - Eigen::Matrix3d R = mat.block(0, 0, 3, 3); - Eigen::Vector3d t = mat.block(0, 3, 3, 1); - - // Get theta (handle edge case where we sometimes have a>1...) - double a = 0.5 * (R.trace() - 1); - double theta = (a > 1) ? acos(1) : ((a < -1) ? acos(-1) : acos(a)); - - // Handle small angle values - double A, B, D, E; - if (theta < 1e-12) { - A = 1; - B = 0.5; - D = 0.5; - E = 1.0 / 12.0; + Eigen::Vector3d w = log_so3(mat.block<3, 3>(0, 0)); + Eigen::Vector3d T = mat.block<3, 1>(0, 3); + const double t = w.norm(); + if (t < 1e-10) { + Eigen::Matrix log; + log << w, T; + return log; } else { - A = sin(theta) / theta; - B = (1 - cos(theta)) / (theta * theta); - D = theta / (2 * sin(theta)); - E = 1 / (theta * theta) * (1 - 0.5 * A / B); + Eigen::Matrix3d W = skew_x(w / t); + // Formula from Agrawal06iros, equation (14) + // simplified with Mathematica, and multiplying in T to avoid matrix math + double Tan = tan(0.5 * t); + Eigen::Vector3d WT = W * T; + Eigen::Vector3d u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + Eigen::Matrix log; + log << w, u; + return log; } - - // Get the skew matrix and V inverse - Eigen::Matrix3d I_33 = Eigen::Matrix3d::Identity(); - Eigen::Matrix3d wskew = D * (R - R.transpose()); - Eigen::Matrix3d Vinv = I_33 - 0.5 * wskew + E * wskew * wskew; - - // Calculate vector - Eigen::Matrix vec; - vec.head(3) << wskew(2, 1), wskew(0, 2), wskew(1, 0); - vec.tail(3) = Vinv * t; - return vec; } /** @@ -405,7 +418,7 @@ inline Eigen::Matrix log_se3(Eigen::Matrix4d mat) { * \boldsymbol\Omega^{\wedge} = \begin{bmatrix} \lfloor \boldsymbol\omega \times\rfloor & \mathbf u \\ \mathbf 0 & 0 \end{bmatrix} * \f} * - * @param vec 6x1 in the se(3) space [omega, u] + * @param vec 6x1 in the R(6) space [omega, u] * @return Lie algebra se(3) 4x4 matrix */ inline Eigen::Matrix4d hat_se3(const Eigen::Matrix &vec) { @@ -493,9 +506,9 @@ inline Eigen::Matrix quatnorm(Eigen::Matrix q_t) { * @param w axis-angle * @return The left Jacobian of SO(3) */ -inline Eigen::Matrix Jl_so3(Eigen::Matrix w) { +inline Eigen::Matrix Jl_so3(const Eigen::Matrix &w) { double theta = w.norm(); - if (theta < 1e-12) { + if (theta < 1e-6) { return Eigen::MatrixXd::Identity(3, 3); } else { Eigen::Matrix a = w / theta; @@ -515,7 +528,7 @@ inline Eigen::Matrix Jl_so3(Eigen::Matrix w) { * @param w axis-angle * @return The right Jacobian of SO(3) */ -inline Eigen::Matrix Jr_so3(Eigen::Matrix w) { return Jl_so3(-w); } +inline Eigen::Matrix Jr_so3(const Eigen::Matrix &w) { return Jl_so3(-w); } /** * @brief Gets roll, pitch, yaw of argument rotation (in that order). From 7c863f74163a755548c07393c183de8d59dd8f4d Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 11:50:49 -0500 Subject: [PATCH 48/55] update cmake and .github ci --- .github/workflows/build_catkin.yml | 63 -------------------------- .github/workflows/build_ros1.yml | 72 ++++++++++++++++++++++++++++++ ov_core/CMakeLists.txt | 18 +++++--- 3 files changed, 83 insertions(+), 70 deletions(-) delete mode 100644 .github/workflows/build_catkin.yml create mode 100644 .github/workflows/build_ros1.yml diff --git a/.github/workflows/build_catkin.yml b/.github/workflows/build_catkin.yml deleted file mode 100644 index c7db65b39..000000000 --- a/.github/workflows/build_catkin.yml +++ /dev/null @@ -1,63 +0,0 @@ -name: C/C++ CI - -on: - push: - branches: [ master, develop_v2.4.1 ] - pull_request: - -jobs: - build: - runs-on: ${{ matrix.os }} - if: "!contains(github.event.head_commit.message, 'skip ci')" - strategy: - fail-fast: false - matrix: - os: [ ubuntu-16.04, ubuntu-18.04, ubuntu-20.04 ] - compiler: [ "/usr/bin/g++" ] - steps: - - uses: actions/checkout@v2 - - name: Checkout submodules - uses: textbook/git-checkout-submodule-action@master - - - name: Install apt dependencies (ubuntu-16.04) - if: matrix.os == 'ubuntu-16.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/kinetic/setup.bash\"" >> ~/.bashrc - - - name: Install apt dependencies (ubuntu-18.04) - if: matrix.os == 'ubuntu-18.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && - touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/melodic/setup.bash\"" >> ~/.bashrc - - - name: Install apt dependencies (ubuntu-20.04) - if: matrix.os == 'ubuntu-20.04' - run: | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && - curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && - sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && - touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/noetic/setup.bash\"" >> ~/.bashrc - - - name: Create ROS1 catkin workspace and compile - run: | - export REPO=$(basename $GITHUB_REPOSITORY) && - cd $GITHUB_WORKSPACE/.. && mkdir src/ && - mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && - source_ros1 && echo "ros version: $ROS_DISTRO" && - catkin build -j2 --no-status - - - name: Run simulation dataset - run: | - source $GITHUB_WORKSPACE/devel/setup.bash && - roscore & - export REPO=$(basename $GITHUB_REPOSITORY) && - source $GITHUB_WORKSPACE/devel/setup.bash && - rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt \ No newline at end of file diff --git a/.github/workflows/build_ros1.yml b/.github/workflows/build_ros1.yml new file mode 100644 index 000000000..1787bea35 --- /dev/null +++ b/.github/workflows/build_ros1.yml @@ -0,0 +1,72 @@ +name: C/C++ CI + +on: + push: + branches: [ master, develop_v2.4.1 ] + pull_request: + +jobs: + build: + name: "ROS1 Ubuntu 16.04" + runs-on: ubuntu-latest + steps: + - name: Build docker image + run: docker build -t local < Dockerfile_ros1_16_04 + - name: Run build + run: docker run -it -v $PWD:/catkin_ws/src -w/catkin_ws catkin build + + +# runs-on: ${{ matrix.os }} +# if: "!contains(github.event.head_commit.message, 'skip ci')" +# strategy: +# fail-fast: false +# matrix: +# os: [ ubuntu-16.04, ubuntu-18.04, ubuntu-20.04 ] +# compiler: [ "/usr/bin/g++" ] +# steps: +# - uses: actions/checkout@v2 +# - name: Checkout submodules +# uses: textbook/git-checkout-submodule-action@master +# +# - name: Install apt dependencies (ubuntu-16.04) +# if: matrix.os == 'ubuntu-16.04' +# run: | +# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && +# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && +# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && +# sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && +# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/kinetic/setup.bash\"" >> ~/.bashrc +# +# - name: Install apt dependencies (ubuntu-18.04) +# if: matrix.os == 'ubuntu-18.04' +# run: | +# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && +# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && +# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && +# sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && +# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/melodic/setup.bash\"" >> ~/.bashrc +# +# - name: Install apt dependencies (ubuntu-20.04) +# if: matrix.os == 'ubuntu-20.04' +# run: | +# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && +# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && +# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && +# sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && +# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/noetic/setup.bash\"" >> ~/.bashrc +# +# - name: Create ROS1 catkin workspace and compile +# run: | +# export REPO=$(basename $GITHUB_REPOSITORY) && +# cd $GITHUB_WORKSPACE/.. && mkdir src/ && +# mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && +# source_ros1 && echo "ros version: $ROS_DISTRO" && +# catkin build -j2 --no-status +# +# - name: Run simulation dataset +# run: | +# source $GITHUB_WORKSPACE/devel/setup.bash && +# roscore & +# export REPO=$(basename $GITHUB_REPOSITORY) && +# source $GITHUB_WORKSPACE/devel/setup.bash && +# rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt \ No newline at end of file diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index e90738482..a4af70103 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -26,15 +26,19 @@ else () add_definitions(-DENABLE_ARUCO_TAGS=1) endif () -# check if we have our python libs files -# sudo apt-get install python-matplotlib python-numpy python2.7-dev -find_package(PythonLibs 2.7) +# check if we have our python libs files (will search for python3 then python2 installs) +# sudo apt-get install python-matplotlib python-numpy python-dev +# https://cmake.org/cmake/help/latest/module/FindPython.html +# https://stackoverflow.com/a/34580995/7718197 +find_package(Python COMPONENTS Interpreter Development) option(DISABLE_MATPLOTLIB "Disable or enable matplotlib plot scripts in ov_eval" OFF) -if (PYTHONLIBS_FOUND AND NOT DISABLE_MATPLOTLIB) +if (Python_FOUND AND NOT DISABLE_MATPLOTLIB) add_definitions(-DHAVE_PYTHONLIBS=1) - message(STATUS "PYTHON VERSION: " ${PYTHONLIBS_VERSION_STRING}) - message(STATUS "PYTHON INCLUDE: " ${PYTHON_INCLUDE_DIRS}) - message(STATUS "PYTHON LIBRARIES: " ${PYTHON_LIBRARIES}) + message(STATUS "PYTHON VERSION: " ${Python_VERSION}) + message(STATUS "PYTHON INCLUDE: " ${Python_INCLUDE_DIRS}) + message(STATUS "PYTHON LIBRARIES: " ${Python_LIBRARIES}) + include_directories(${Python_INCLUDE_DIRS}) + list(APPEND thirdparty_libraries ${Python_LIBRARIES}) endif () # We need c++14 for ROS2, thus just require it for everybody From df93c2bd2e608af8bfe03c8b80993f5403f42960 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 11:52:31 -0500 Subject: [PATCH 49/55] update .github ci --- .github/workflows/build_ros1.yml | 116 ++++++++++++++----------------- ov_msckf/cmake/ROS1.cmake | 5 +- 2 files changed, 56 insertions(+), 65 deletions(-) diff --git a/.github/workflows/build_ros1.yml b/.github/workflows/build_ros1.yml index 1787bea35..a9bd836c6 100644 --- a/.github/workflows/build_ros1.yml +++ b/.github/workflows/build_ros1.yml @@ -1,72 +1,62 @@ -name: C/C++ CI +name: ROS 1 Workflow on: push: - branches: [ master, develop_v2.4.1 ] + branches: [ master ] pull_request: jobs: - build: + build_1604: name: "ROS1 Ubuntu 16.04" runs-on: ubuntu-latest steps: - - name: Build docker image - run: docker build -t local < Dockerfile_ros1_16_04 - - name: Run build - run: docker run -it -v $PWD:/catkin_ws/src -w/catkin_ws catkin build - - -# runs-on: ${{ matrix.os }} -# if: "!contains(github.event.head_commit.message, 'skip ci')" -# strategy: -# fail-fast: false -# matrix: -# os: [ ubuntu-16.04, ubuntu-18.04, ubuntu-20.04 ] -# compiler: [ "/usr/bin/g++" ] -# steps: -# - uses: actions/checkout@v2 -# - name: Checkout submodules -# uses: textbook/git-checkout-submodule-action@master -# -# - name: Install apt dependencies (ubuntu-16.04) -# if: matrix.os == 'ubuntu-16.04' -# run: | -# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && -# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && -# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && -# sudo apt update && sudo apt -y install build-essential ros-kinetic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && -# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/kinetic/setup.bash\"" >> ~/.bashrc -# -# - name: Install apt dependencies (ubuntu-18.04) -# if: matrix.os == 'ubuntu-18.04' -# run: | -# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && -# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && -# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && -# sudo apt update && sudo apt -y install build-essential ros-melodic-desktop python-catkin-pkg python-catkin-tools libeigen3-dev git && -# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/melodic/setup.bash\"" >> ~/.bashrc -# -# - name: Install apt dependencies (ubuntu-20.04) -# if: matrix.os == 'ubuntu-20.04' -# run: | -# sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && -# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 && -# curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add - && -# sudo apt update && sudo apt -y install build-essential ros-noetic-desktop python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon libeigen3-dev git && -# touch ~/.bashrc && echo "alias source_ros1=\"source /opt/ros/noetic/setup.bash\"" >> ~/.bashrc -# -# - name: Create ROS1 catkin workspace and compile -# run: | -# export REPO=$(basename $GITHUB_REPOSITORY) && -# cd $GITHUB_WORKSPACE/.. && mkdir src/ && -# mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && -# source_ros1 && echo "ros version: $ROS_DISTRO" && -# catkin build -j2 --no-status -# -# - name: Run simulation dataset -# run: | -# source $GITHUB_WORKSPACE/devel/setup.bash && -# roscore & -# export REPO=$(basename $GITHUB_REPOSITORY) && -# source $GITHUB_WORKSPACE/devel/setup.bash && -# rosrun ov_msckf run_simulation _sim_traj_path:=$GITHUB_WORKSPACE/src/$REPO/ov_data/sim/udel_gore.txt \ No newline at end of file + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_16_04 . + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source devel/setup.bash && roslaunch ov_msckf simulation.launch verbosity:=WARNING" + build_1804: + name: "ROS1 Ubuntu 18.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source devel/setup.bash && roslaunch ov_msckf simulation.launch verbosity:=WARNING" + build_2004: + name: "ROS1 Ubuntu 20.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source devel/setup.bash && roslaunch ov_msckf simulation.launch verbosity:=WARNING" diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index dccdf3d88..12ffbc85e 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -38,10 +38,11 @@ list(APPEND thirdparty_libraries # See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 if (NOT catkin_FOUND OR NOT ENABLE_ROS) message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") - include_directories(${ov_core_SOURCE_DIR}/src/) + message(WARNING ${ov_core_SOURCE_DIR}) + include_directories(${CMAKE_SOURCE_DIR}/../ov_core/src/) list(APPEND thirdparty_libraries ov_core_lib) message(WARNING "MANUALLY LINKING TO OV_INIT LIBRARY....") - include_directories(${ov_init_SOURCE_DIR}/src/) + include_directories(${CMAKE_SOURCE_DIR}/../ov_init/src/) list(APPEND thirdparty_libraries ov_init_lib) endif () From 062125b228f7f03092fbc28fd56a02feba191e8b Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 13:49:05 -0500 Subject: [PATCH 50/55] more workflows and fix ros-free build --- .github/workflows/build.yml | 20 +++++++++++++ .github/workflows/build_ros1.yml | 9 ++++++ .github/workflows/build_ros2.yml | 50 ++++++++++++++++++++++++++++++++ ov_msckf/cmake/ROS1.cmake | 27 ++++++++++++++--- ov_msckf/src/run_simulation.cpp | 4 +++ 5 files changed, 106 insertions(+), 4 deletions(-) create mode 100644 .github/workflows/build.yml create mode 100644 .github/workflows/build_ros2.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 000000000..b78df26c0 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,20 @@ +name: ROS Free Workflow + +on: + push: + branches: [ master] + pull_request: + +jobs: + build: + name: "Ubuntu Latest" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Configure and Build + run: | + sudo apt-get update && sudo apt-get install -y libeigen3-dev && + mkdir build && cd build && + cmake ../ov_msckf/ && make + diff --git a/.github/workflows/build_ros1.yml b/.github/workflows/build_ros1.yml index a9bd836c6..9564f9a00 100644 --- a/.github/workflows/build_ros1.yml +++ b/.github/workflows/build_ros1.yml @@ -18,6 +18,9 @@ jobs: cd $GITHUB_WORKSPACE/.. && mkdir src/ && mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_16_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" - name: Run Build in Docker run: | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" @@ -36,6 +39,9 @@ jobs: cd $GITHUB_WORKSPACE/.. && mkdir src/ && mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_18_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" - name: Run Build in Docker run: | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" @@ -54,6 +60,9 @@ jobs: cd $GITHUB_WORKSPACE/.. && mkdir src/ && mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros1_20_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" - name: Run Build in Docker run: | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && catkin build" diff --git a/.github/workflows/build_ros2.yml b/.github/workflows/build_ros2.yml new file mode 100644 index 000000000..751567a92 --- /dev/null +++ b/.github/workflows/build_ros2.yml @@ -0,0 +1,50 @@ +name: ROS 2 Workflow + +on: + push: + branches: [ master ] + pull_request: + +jobs: + build_1804: + name: "ROS2 Ubuntu 18.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros2_18_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" + build_2004: + name: "ROS2 Ubuntu 20.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros2_20_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 12ffbc85e..6aaebfb19 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -35,15 +35,34 @@ list(APPEND thirdparty_libraries # If we are not building with ROS then we need to manually link to its headers # This isn't that elegant of a way, but this at least allows for building without ROS -# See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 +# If we had a root cmake we could do this: https://stackoverflow.com/a/11217008/7718197 +# But since we don't we need to basically build all the cpp / h files explicitly :( if (NOT catkin_FOUND OR NOT ENABLE_ROS) + message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") - message(WARNING ${ov_core_SOURCE_DIR}) include_directories(${CMAKE_SOURCE_DIR}/../ov_core/src/) - list(APPEND thirdparty_libraries ov_core_lib) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/cam/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/cpi/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/feat/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/plot/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/sim/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/track/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/types/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + file(GLOB_RECURSE ov_core_files "${CMAKE_SOURCE_DIR}/../ov_core/src/utils/*.cpp") + list(APPEND library_source_files ${ov_core_files}) + message(WARNING "MANUALLY LINKING TO OV_INIT LIBRARY....") include_directories(${CMAKE_SOURCE_DIR}/../ov_init/src/) - list(APPEND thirdparty_libraries ov_init_lib) + file(GLOB_RECURSE ov_init_files "${CMAKE_SOURCE_DIR}/../ov_init/src/*.[hc]pp") + list(APPEND library_source_files ${ov_init_files}) + endif () ################################################## diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 081b3bea2..fa5951668 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -177,6 +177,10 @@ int main(int argc, char **argv) { viz->visualize_final(); #endif +#if ROS_AVAILABLE == 2 + rclcpp::shutdown(); +#endif + // Done! return EXIT_SUCCESS; } From fa0c654232a974e78388520bfe0d653a01808a67 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 14:25:42 -0500 Subject: [PATCH 51/55] update ros-free ci --- .github/workflows/build.yml | 2 +- .github/workflows/{build_ros1.yml => build_ros1.yml.disable} | 0 .github/workflows/{build_ros2.yml => build_ros2.yml.disable} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename .github/workflows/{build_ros1.yml => build_ros1.yml.disable} (100%) rename .github/workflows/{build_ros2.yml => build_ros2.yml.disable} (100%) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index b78df26c0..881dfec60 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -14,7 +14,7 @@ jobs: uses: actions/checkout@v2 - name: Configure and Build run: | - sudo apt-get update && sudo apt-get install -y libeigen3-dev && + sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev python-matplotlib && mkdir build && cd build && cmake ../ov_msckf/ && make diff --git a/.github/workflows/build_ros1.yml b/.github/workflows/build_ros1.yml.disable similarity index 100% rename from .github/workflows/build_ros1.yml rename to .github/workflows/build_ros1.yml.disable diff --git a/.github/workflows/build_ros2.yml b/.github/workflows/build_ros2.yml.disable similarity index 100% rename from .github/workflows/build_ros2.yml rename to .github/workflows/build_ros2.yml.disable From 323b4dfd90f28e78c0d71e366ba1b3a0d71d6265 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 14:55:21 -0500 Subject: [PATCH 52/55] tf needs to be shared to prevent segfault in ROS2 --- .github/workflows/build.yml | 2 +- .../workflows/{build_ros1.yml.disable => build_ros1.yml} | 0 .../workflows/{build_ros2.yml.disable => build_ros2.yml} | 0 ov_msckf/src/ros/ROS1Visualizer.cpp | 2 +- ov_msckf/src/ros/ROS1Visualizer.h | 2 +- ov_msckf/src/ros/ROS2Visualizer.cpp | 2 +- ov_msckf/src/ros/ROS2Visualizer.h | 2 +- ov_msckf/src/run_simulation.cpp | 8 ++++---- ov_msckf/src/run_subscribe_msckf.cpp | 5 +++++ 9 files changed, 14 insertions(+), 9 deletions(-) rename .github/workflows/{build_ros1.yml.disable => build_ros1.yml} (100%) rename .github/workflows/{build_ros2.yml.disable => build_ros2.yml} (100%) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 881dfec60..8cfef6f80 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -14,7 +14,7 @@ jobs: uses: actions/checkout@v2 - name: Configure and Build run: | - sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev python-matplotlib && + sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev && mkdir build && cd build && cmake ../ov_msckf/ && make diff --git a/.github/workflows/build_ros1.yml.disable b/.github/workflows/build_ros1.yml similarity index 100% rename from .github/workflows/build_ros1.yml.disable rename to .github/workflows/build_ros1.yml diff --git a/.github/workflows/build_ros2.yml.disable b/.github/workflows/build_ros2.yml similarity index 100% rename from .github/workflows/build_ros2.yml.disable rename to .github/workflows/build_ros2.yml diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index eb56f5a2f..7eddd8543 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -29,7 +29,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ : _nh(nh), _app(app), _sim(sim) { // Setup our transform broadcaster - mTfBr = new tf::TransformBroadcaster(); + mTfBr = std::make_shared(); // Create image transport image_transport::ImageTransport it(*_nh); diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index 490105e8a..af6c16739 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -136,7 +136,7 @@ class ROS1Visualizer { ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu; ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim; ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics; - tf::TransformBroadcaster *mTfBr; + std::shared_ptr mTfBr; // Our subscribers and camera synchronizers ros::Subscriber sub_imu; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 8019e1812..a67a0657b 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -29,7 +29,7 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p : _node(node), _app(app), _sim(sim) { // Setup our transform broadcaster - mTfBr = new tf2_ros::TransformBroadcaster(node); + mTfBr = std::make_shared(node); // Create image transport image_transport::ImageTransport it(node); diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index 2de788e96..1c4fdd3f2 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -145,7 +145,7 @@ class ROS2Visualizer { rclcpp::Publisher::SharedPtr pub_loop_pose, pub_loop_extrinsic; rclcpp::Publisher::SharedPtr pub_loop_point; rclcpp::Publisher::SharedPtr pub_loop_intrinsics; - tf2_ros::TransformBroadcaster *mTfBr; + std::shared_ptr mTfBr; // Our subscribers and camera synchronizers rclcpp::Subscription::SharedPtr sub_imu; diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index fa5951668..6c6dd8499 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -173,11 +173,11 @@ int main(int argc, char **argv) { } // Final visualization -#if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 +#if ROS_AVAILABLE == 1 + viz->visualize_final(); + ros::shutdown(); +#elif ROS_AVAILABLE == 2 viz->visualize_final(); -#endif - -#if ROS_AVAILABLE == 2 rclcpp::shutdown(); #endif diff --git a/ov_msckf/src/run_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp index d50a73093..5ce9c179d 100644 --- a/ov_msckf/src/run_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -109,6 +109,11 @@ int main(int argc, char **argv) { // Final visualization viz->visualize_final(); +#if ROS_AVAILABLE == 1 + ros::shutdown(); +#elif ROS_AVAILABLE == 2 + rclcpp::shutdown(); +#endif // Done! return EXIT_SUCCESS; From bb730bd77f15646f1faf8da64e4cea331df68771 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 15:20:17 -0500 Subject: [PATCH 53/55] update ci --- .github/workflows/build.yml | 2 +- .github/workflows/build_ros2.yml | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 8cfef6f80..6b62ee47d 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -14,7 +14,7 @@ jobs: uses: actions/checkout@v2 - name: Configure and Build run: | - sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev && + sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev libboost-all-dev && mkdir build && cd build && cmake ../ov_msckf/ && make diff --git a/.github/workflows/build_ros2.yml b/.github/workflows/build_ros2.yml index 751567a92..d94f4c660 100644 --- a/.github/workflows/build_ros2.yml +++ b/.github/workflows/build_ros2.yml @@ -24,9 +24,10 @@ jobs: - name: Run Build in Docker run: | docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build" - - name: Run OpenVINS Simulation! - run: | - docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" + # THIS SEEMS TO FAIL WITH 245 ERROR, NOT SURE WHY!!!!! + #- name: Run OpenVINS Simulation! + # run: | + # docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" build_2004: name: "ROS2 Ubuntu 20.04" runs-on: ubuntu-latest From 42a3a247f1eb6e2810f5861cd3894e0a7d38f7de Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 15:26:22 -0500 Subject: [PATCH 54/55] update readme [skip ci] --- ReadMe.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ReadMe.md b/ReadMe.md index 19c878c0e..99b1549ef 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,6 +1,8 @@ # OpenVINS -![C/C++ CI](https://github.com/rpng/open_vins/workflows/C/C++%20CI/badge.svg) +[![ROS 1 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml) +[![ROS 2 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml) +[![ROS Free Workflow](https://github.com/rpng/open_vins/actions/workflows/build.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build.yml) Welcome to the OpenVINS project! The OpenVINS project houses some core computer vision code along with a state-of-the art filter-based visual-inertial From 5cdd24ef00e2198ab1f82f695a6b13d19fa09016 Mon Sep 17 00:00:00 2001 From: Patrick Geneva Date: Mon, 13 Dec 2021 15:44:24 -0500 Subject: [PATCH 55/55] update readme [skip ci] --- ReadMe.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ReadMe.md b/ReadMe.md index 99b1549ef..bf45671c3 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -21,6 +21,8 @@ details on what the system supports. ## News / Events +* **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See + v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details. * **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See v2.4 [PR#117](https://github.com/rpng/open_vins/pull/186) for details. * **December 1, 2020** - Released improved memory management, active feature pointcloud publishing, limiting number of