From 98d194a3288acc51bb66dd96e54f3f62085ee1bb Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Wed, 21 Aug 2024 13:12:16 +0900 Subject: [PATCH] visualize odometry factors --- include/glim/viewer/standard_viewer.hpp | 18 ++- src/glim/viewer/standard_viewer.cpp | 153 ++++++++++++++++++++++-- 2 files changed, 162 insertions(+), 9 deletions(-) diff --git a/include/glim/viewer/standard_viewer.hpp b/include/glim/viewer/standard_viewer.hpp index af4a0663..0bb4607b 100644 --- a/include/glim/viewer/standard_viewer.hpp +++ b/include/glim/viewer/standard_viewer.hpp @@ -5,6 +5,9 @@ #include #include #include +#include +#include +#include #include #include @@ -15,6 +18,10 @@ namespace spdlog { class logger; } +namespace gtsam { +class NonlinearFactor; +} + namespace glim { class TrajectoryManager; @@ -55,6 +62,8 @@ class StandardViewer : public ExtensionModule { bool show_odometry_scans; bool show_odometry_keyframes; + bool show_odometry_factors; + bool show_submaps; bool show_factors; @@ -65,6 +74,11 @@ class StandardViewer : public ExtensionModule { Eigen::Vector3d last_imu_vel; Eigen::Matrix last_imu_bias; + using FactorLine = std::tuple; + using FactorLineGetter = std::function(const gtsam::NonlinearFactor*)>; + std::vector, FactorLineGetter>> odometry_factor_lines; + std::unordered_map odometry_poses; + Eigen::Vector2f z_range; Eigen::Vector2f auto_z_range; @@ -75,8 +89,8 @@ class StandardViewer : public ExtensionModule { std::mutex invoke_queue_mutex; std::vector> invoke_queue; - + // Logging std::shared_ptr logger; }; -} \ No newline at end of file +} // namespace glim \ No newline at end of file diff --git a/src/glim/viewer/standard_viewer.cpp b/src/glim/viewer/standard_viewer.cpp index ba73bcfb..062020af 100644 --- a/src/glim/viewer/standard_viewer.cpp +++ b/src/glim/viewer/standard_viewer.cpp @@ -4,9 +4,15 @@ #include #include +#include #include +#include #include +#ifdef BUILD_GTSAM_POINTS_GPU +#include +#endif + #include #include #include @@ -39,6 +45,7 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) { show_odometry_scans = true; show_odometry_keyframes = true; + show_odometry_factors = false; show_submaps = true; show_factors = true; @@ -190,9 +197,15 @@ void StandardViewer::set_callbacks() { invoke([this, frames] { auto viewer = guik::LightViewer::instance(); for (const auto& frame : frames) { + const Eigen::Isometry3f pose = resolve_pose(frame); + odometry_poses[frame->id] = pose; + + viewer->update_drawable( + "frame_coord_" + std::to_string(frame->id), + glk::Primitives::coordinate_system(), + guik::VertexColor(resolve_pose(frame) * Eigen::UniformScaling(0.5f))); auto drawable = viewer->find_drawable("frame_" + std::to_string(frame->id)); if (drawable.first) { - const Eigen::Isometry3f pose = resolve_pose(frame); drawable.first->add("model_matrix", pose.matrix()); } } @@ -206,6 +219,7 @@ void StandardViewer::set_callbacks() { for (const auto& keyframe : keyframes) { const Eigen::Isometry3f pose = resolve_pose(keyframe); + odometry_poses[keyframe->id] = pose; const std::string name = "odometry_keyframe_" + std::to_string(keyframe->id); auto drawable = viewer->find_drawable(name); @@ -221,6 +235,117 @@ void StandardViewer::set_callbacks() { }); }); + OdometryEstimationCallbacks::on_smoother_update.add([this]( + gtsam_points::IncrementalFixedLagSmootherExtWithFallback& smoother, + gtsam::NonlinearFactorGraph& new_factors, + gtsam::Values& new_values, + std::map& new_stamps) { + // + std::vector, FactorLineGetter>> new_factor_lines; + new_factor_lines.reserve(new_factors.size()); + + for (const auto& factor : new_factors) { + if (!factor) { + continue; + } + + if (factor->keys().size() == 1) { + const gtsam::Symbol symbol0(factor->keys()[0]); + if (symbol0.chr() != 'x') { + continue; + } + const int idx0 = symbol0.index(); + + const auto matching_factor = boost::dynamic_pointer_cast(factor); + if (matching_factor) { + const auto l = [this, idx0](const gtsam::NonlinearFactor* factor) -> std::optional { + const auto found0 = odometry_poses.find(idx0); + if (found0 == odometry_poses.end()) { + return std::nullopt; + } + + const Eigen::Vector3d pt1 = static_cast(factor)->get_fixed_target_pose().translation(); + return std::make_tuple(found0->second.translation(), pt1.cast(), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.5f), Eigen::Vector4f(1.0f, 0.0f, 0.0f, 0.5f)); + }; + + new_factor_lines.emplace_back(factor, l); + continue; + } + +#ifdef BUILD_GTSAM_POINTS_GPU + const auto gpu_factor = boost::dynamic_pointer_cast(factor); + if (gpu_factor) { + const auto l = [this, idx0](const gtsam::NonlinearFactor* factor) -> std::optional { + const auto found0 = odometry_poses.find(idx0); + if (found0 == odometry_poses.end()) { + return std::nullopt; + } + + const Eigen::Vector3f pt1 = static_cast(factor)->get_fixed_target_pose().translation(); + return std::make_tuple(found0->second.translation(), pt1, Eigen::Vector4f(0.0f, 1.0f, 0.0f, 0.5f), Eigen::Vector4f(1.0f, 0.0f, 0.0f, 0.5f)); + }; + + new_factor_lines.emplace_back(factor, l); + continue; + } +#endif + } else if (factor->keys().size() == 2) { + const gtsam::Symbol symbol0(factor->keys()[0]); + const gtsam::Symbol symbol1(factor->keys()[1]); + if (symbol0.chr() != 'x' || symbol1.chr() != 'x') { + continue; + } + + const int idx0 = symbol0.index(); + const int idx1 = symbol1.index(); + + const auto l = [this, idx0, idx1](const gtsam::NonlinearFactor*) -> std::optional { + const auto found0 = odometry_poses.find(idx0); + const auto found1 = odometry_poses.find(idx1); + if (found0 == odometry_poses.end() || found1 == odometry_poses.end()) { + return std::nullopt; + } + + return std::make_tuple(found0->second.translation(), found1->second.translation(), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f), Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); + }; + + new_factor_lines.emplace_back(factor, l); + } + } + + invoke([this, new_factor_lines] { + auto remove_loc = std::remove_if(odometry_factor_lines.begin(), odometry_factor_lines.end(), [](const auto& factor) { return factor.first.expired(); }); + odometry_factor_lines.erase(remove_loc, odometry_factor_lines.end()); + odometry_factor_lines.insert(odometry_factor_lines.end(), new_factor_lines.begin(), new_factor_lines.end()); + + if (!show_odometry_factors) { + return; + } + + std::vector line_vertices; + std::vector line_colors; + + for (const auto& factor_line : odometry_factor_lines) { + const auto factor = factor_line.first.lock(); + if (!factor) { + continue; + } + + const auto line = factor_line.second(factor.get()); + if (!line) { + continue; + } + line_vertices.push_back(std::get<0>(*line)); + line_vertices.push_back(std::get<1>(*line)); + line_colors.push_back(std::get<2>(*line)); + line_colors.push_back(std::get<3>(*line)); + } + + auto viewer = guik::viewer(); + viewer->update_drawable("odometry_factors", std::make_shared(line_vertices, line_colors), guik::VertexColor().make_transparent()); + }); + }); + // Marginalized frames callback OdometryEstimationCallbacks::on_marginalized_frames.add([this](const std::vector& frames) { std::vector marginalized_ids(frames.size()); @@ -236,6 +361,8 @@ void StandardViewer::set_callbacks() { auto viewer = guik::LightViewer::instance(); for (const int id : marginalized_ids) { viewer->remove_drawable("frame_" + std::to_string(id)); + viewer->remove_drawable("frame_coord_" + std::to_string(id)); + odometry_poses.erase(id); } }); }); @@ -247,6 +374,7 @@ void StandardViewer::set_callbacks() { for (const auto& keyframe : keyframes) { viewer->remove_drawable("odometry_keyframe_" + std::to_string(keyframe->id)); viewer->remove_drawable("odometry_keyframe_coord_" + std::to_string(keyframe->id)); + odometry_poses.erase(keyframe->id); } }); }); @@ -465,11 +593,15 @@ void StandardViewer::viewer_loop() { request_to_terminate = true; } - std::lock_guard lock(invoke_queue_mutex); - for (const auto& task : invoke_queue) { + std::vector> tasks; + { + std::lock_guard lock(invoke_queue_mutex); + tasks.swap(invoke_queue); + } + + for (const auto& task : tasks) { task(); } - invoke_queue.clear(); } guik::LightViewer::destroy(); @@ -500,6 +632,10 @@ bool StandardViewer::drawable_filter(const std::string& name) { return false; } + if (!show_odometry_factors && starts_with(name, "odometry_factors")) { + return false; + } + if (!show_submaps && starts_with(name, "submap_")) { return false; } @@ -546,9 +682,10 @@ void StandardViewer::drawable_selection() { } ImGui::Separator(); - bool show_odometry = show_odometry_scans || show_odometry_keyframes; + bool show_odometry = show_odometry_scans || show_odometry_keyframes || show_odometry_factors; if (ImGui::Checkbox("odometry", &show_odometry)) { show_odometry_scans = show_odometry_keyframes = show_odometry; + show_odometry_factors &= show_odometry; } ImGui::SameLine(); @@ -556,9 +693,11 @@ void StandardViewer::drawable_selection() { show_odometry_status = true; } - ImGui::Checkbox("scans", &show_odometry_scans); + ImGui::Checkbox("scans##odom", &show_odometry_scans); + ImGui::SameLine(); + ImGui::Checkbox("keyframes##odom", &show_odometry_keyframes); ImGui::SameLine(); - ImGui::Checkbox("keyframes", &show_odometry_keyframes); + ImGui::Checkbox("factors##odom", &show_odometry_factors); ImGui::Separator(); bool show_mapping = show_submaps || show_factors;