From 46e4e178a228ee80562740fdea51fd561cdf9a99 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Thu, 15 Aug 2024 20:15:20 +0900 Subject: [PATCH] stabilize odom visualization --- CMakeLists.txt | 1 + include/glim/mapping/sub_map.hpp | 32 ++++++++++ include/glim/util/trajectory_manager.hpp | 38 +++--------- src/glim/util/trajectory_manager.cpp | 74 ++++++++++++++++++++++++ src/glim/viewer/interactive_viewer.cpp | 2 +- src/glim/viewer/standard_viewer.cpp | 10 +++- 6 files changed, 125 insertions(+), 32 deletions(-) create mode 100644 src/glim/util/trajectory_manager.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 682f9913..0e31a608 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(glim SHARED src/glim/util/logging.cpp src/glim/util/export_factors.cpp src/glim/util/time_keeper.cpp + src/glim/util/trajectory_manager.cpp src/glim/util/extension_module.cpp src/glim/util/load_module.cpp src/glim/util/data_validator.cpp diff --git a/include/glim/mapping/sub_map.hpp b/include/glim/mapping/sub_map.hpp index 59dfe9cc..79302c68 100644 --- a/include/glim/mapping/sub_map.hpp +++ b/include/glim/mapping/sub_map.hpp @@ -31,6 +31,36 @@ struct SubMap { /// @brief Get the origin odometry frame EstimationFrame::ConstPtr origin_odom_frame() const { return odom_frames[frames.size() / 2]; } + /** + * @brief Get the custom data and cast it to the specified type. + * @note This method does not check the type of the custom data. + * @param key Key of the custom data + * @return T* Pointer to the custom data. nullptr if not found. + */ + template + T* get_custom_data(const std::string& key) { + const auto found = custom_data.find(key); + if (found == custom_data.end()) { + return nullptr; + } + return reinterpret_cast(found->second.get()); + } + + /** + * @brief Get the custom data and cast it to the specified type. + * @note This method does not check the type of the custom data. + * @param key Key of the custom data + * @return T* Pointer to the custom data. nullptr if not found. + */ + template + const T* get_custom_data(const std::string& key) const { + const auto found = custom_data.find(key); + if (found == custom_data.end()) { + return nullptr; + } + return reinterpret_cast(found->second.get()); + } + /** * @brief Save the submap * @param path Save path @@ -57,6 +87,8 @@ struct SubMap { std::vector frames; ///< Optimized odometry frames std::vector odom_frames; ///< Original odometry frames + + std::unordered_map> custom_data; ///< User-defined custom data }; } // namespace glim diff --git a/include/glim/util/trajectory_manager.hpp b/include/glim/util/trajectory_manager.hpp index 9f8f027c..12655936 100644 --- a/include/glim/util/trajectory_manager.hpp +++ b/include/glim/util/trajectory_manager.hpp @@ -10,39 +10,19 @@ namespace glim { class TrajectoryManager { public: - TrajectoryManager() { - odom_stamps.push_back(0.0); - T_odom_sensor.push_back(Eigen::Isometry3d::Identity()); + TrajectoryManager(); + ~TrajectoryManager(); - T_world_odom.setIdentity(); - } - ~TrajectoryManager() {} + void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor, int priority = 1); + void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor); - void add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor) { - this->odom_stamps.push_back(stamp); - this->T_odom_sensor.push_back(T_odom_sensor); - } - - void update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor) { - auto found = std::lower_bound(odom_stamps.begin(), odom_stamps.end(), stamp); - int idx = std::distance(odom_stamps.begin(), found); - T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse(); - - if (idx > 1) { - odom_stamps.erase(odom_stamps.begin(), odom_stamps.begin() + idx - 1); - T_odom_sensor.erase(T_odom_sensor.begin(), T_odom_sensor.begin() + idx - 1); - } - } - - Eigen::Isometry3d current_pose() const { return T_world_odom * T_odom_sensor.back(); } - - Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const { return T_world_odom * pose; } - - Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const { return T_world_odom * point; } - - const Eigen::Isometry3d get_T_world_odom() const { return T_world_odom; } + Eigen::Isometry3d current_pose() const; + Eigen::Isometry3d odom2world(const Eigen::Isometry3d& pose) const; + Eigen::Vector3d odom2world(const Eigen::Vector3d& point) const; + const Eigen::Isometry3d get_T_world_odom() const; private: + int odom_priority; std::vector odom_stamps; std::vector T_odom_sensor; diff --git a/src/glim/util/trajectory_manager.cpp b/src/glim/util/trajectory_manager.cpp new file mode 100644 index 00000000..8a2c5f9b --- /dev/null +++ b/src/glim/util/trajectory_manager.cpp @@ -0,0 +1,74 @@ +#include + +namespace glim { + +TrajectoryManager::TrajectoryManager() { + odom_priority = -1; + odom_stamps.push_back(0.0); + T_odom_sensor.push_back(Eigen::Isometry3d::Identity()); + + T_world_odom.setIdentity(); +} + +TrajectoryManager::~TrajectoryManager() {} + +void TrajectoryManager::add_odom(double stamp, const Eigen::Isometry3d& T_odom_sensor, int priority) { + if (odom_priority < priority) { + odom_priority = priority; + this->odom_stamps = {0.0}; + this->T_odom_sensor = {T_odom_sensor}; + } else if (odom_priority != priority) { + return; + } + + this->odom_stamps.push_back(stamp); + this->T_odom_sensor.push_back(T_odom_sensor); +} + +void TrajectoryManager::update_anchor(double stamp, const Eigen::Isometry3d& T_world_sensor) { + const auto found = std::lower_bound(odom_stamps.begin(), odom_stamps.end(), stamp); + const int idx = std::distance(odom_stamps.begin(), found); + + if (std::abs(stamp - odom_stamps[idx]) < 1e-6 || idx == 0) { + T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse(); + } else { + const double t0 = odom_stamps[idx - 1]; + const double t1 = odom_stamps[idx]; + if (t0 > stamp || t1 < stamp) { + T_world_odom = T_world_sensor * T_odom_sensor[idx].inverse(); + return; + } + + const double p = (stamp - t0) / (t1 - t0); + + const Eigen::Isometry3d T0 = T_odom_sensor[idx - 1]; + const Eigen::Isometry3d T1 = T_odom_sensor[idx]; + Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); + T.translation() = T0.translation() * (1.0 - p) + T1.translation() * p; + T.linear() = Eigen::Quaterniond(T0.linear()).slerp(p, Eigen::Quaterniond(T1.linear())).toRotationMatrix(); + T_world_odom = T_world_sensor * T.inverse(); + } + + if (idx > 1) { + odom_stamps.erase(odom_stamps.begin(), odom_stamps.begin() + idx - 1); + T_odom_sensor.erase(T_odom_sensor.begin(), T_odom_sensor.begin() + idx - 1); + } +} + +Eigen::Isometry3d TrajectoryManager::current_pose() const { + return T_world_odom * T_odom_sensor.back(); +} + +Eigen::Isometry3d TrajectoryManager::odom2world(const Eigen::Isometry3d& pose) const { + return T_world_odom * pose; +} + +Eigen::Vector3d TrajectoryManager::odom2world(const Eigen::Vector3d& point) const { + return T_world_odom * point; +} + +const Eigen::Isometry3d TrajectoryManager::get_T_world_odom() const { + return T_world_odom; +} + +} // namespace glim \ No newline at end of file diff --git a/src/glim/viewer/interactive_viewer.cpp b/src/glim/viewer/interactive_viewer.cpp index 9c117663..a2e38526 100644 --- a/src/glim/viewer/interactive_viewer.cpp +++ b/src/glim/viewer/interactive_viewer.cpp @@ -384,7 +384,7 @@ void InteractiveViewer::odometry_on_new_frame(const EstimationFrame::ConstPtr& n auto viewer = guik::viewer(); auto cloud_buffer = std::make_shared(new_frame->frame->points, new_frame->frame->size()); - trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor()); + trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor(), 1); const Eigen::Isometry3d pose = trajectory->odom2world(new_frame->T_world_sensor()); viewer->update_drawable("current", cloud_buffer, guik::FlatOrange(pose).set_point_scale(2.0f)); }); diff --git a/src/glim/viewer/standard_viewer.cpp b/src/glim/viewer/standard_viewer.cpp index 2f6ea239..ba73bcfb 100644 --- a/src/glim/viewer/standard_viewer.cpp +++ b/src/glim/viewer/standard_viewer.cpp @@ -139,7 +139,7 @@ void StandardViewer::set_callbacks() { last_imu_vel = new_frame->v_world_imu; last_imu_bias = new_frame->imu_bias; - trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor()); + trajectory->add_odom(new_frame->stamp, new_frame->T_world_sensor(), 1); const Eigen::Isometry3f pose = resolve_pose(new_frame); if (track) { @@ -226,7 +226,13 @@ void StandardViewer::set_callbacks() { std::vector marginalized_ids(frames.size()); std::transform(frames.begin(), frames.end(), marginalized_ids.begin(), [](const EstimationFrame::ConstPtr& frame) { return frame->id; }); - invoke([this, marginalized_ids] { + const EstimationFrame::ConstPtr last_frame = frames.empty() ? nullptr : frames.back(); + + invoke([this, marginalized_ids, last_frame] { + if (last_frame) { + trajectory->add_odom(last_frame->stamp, last_frame->T_world_sensor(), 2); + } + auto viewer = guik::LightViewer::instance(); for (const int id : marginalized_ids) { viewer->remove_drawable("frame_" + std::to_string(id));