From 768aa509cb3fe159bbfc886e7567a2bae02403aa Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 22 Oct 2024 19:02:28 +0900 Subject: [PATCH] Fix indeterminant linear system (#92) * add mapping tools for standard viewer * fix indeterminant linear system * prevent previous submap with a small overlap being isolated * quit callback for extension modules --- include/glim/util/extension_module.hpp | 5 ++ include/glim/viewer/standard_viewer.hpp | 3 ++ src/glim/mapping/global_mapping.cpp | 68 +++++++++++++++++++++---- src/glim/viewer/standard_viewer.cpp | 23 +++++++++ 4 files changed, 88 insertions(+), 11 deletions(-) diff --git a/include/glim/util/extension_module.hpp b/include/glim/util/extension_module.hpp index 62bb83bf..82efab99 100644 --- a/include/glim/util/extension_module.hpp +++ b/include/glim/util/extension_module.hpp @@ -22,6 +22,11 @@ class ExtensionModule { */ virtual bool ok() const { return true; } + /** + * @brief Called when the system is quitting. + */ + virtual void at_exit(const std::string& dump_path) {} + /** * @brief Load an extension module from a dynamic library * @param so_name Dynamic library name diff --git a/include/glim/viewer/standard_viewer.hpp b/include/glim/viewer/standard_viewer.hpp index 3b67094a..63a477b3 100644 --- a/include/glim/viewer/standard_viewer.hpp +++ b/include/glim/viewer/standard_viewer.hpp @@ -79,6 +79,9 @@ class StandardViewer : public ExtensionModule { std::vector, FactorLineGetter>> odometry_factor_lines; std::unordered_map odometry_poses; + bool show_mapping_tools; + float min_overlap; + double point_size; bool point_size_metric; bool point_shape_circle; diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index 2913ab6c..136e1b63 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/src/glim/mapping/global_mapping.cpp @@ -158,6 +158,7 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) { } if (params.enable_imu) { + logger->debug("create IMU factor"); if (submap->odom_frames.front()->frame_id != FrameID::IMU) { logger->warn("odom frames are not estimated in the IMU frame while global mapping requires IMU estimation"); } @@ -210,13 +211,9 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) { } Callbacks::on_smoother_update(*isam2, *new_factors, *new_values); - try { - auto result = update_isam2(*new_factors, *new_values); - Callbacks::on_smoother_update_result(*isam2, result); - } catch (std::exception& e) { - logger->error("an exception was caught during global map optimization!!"); - logger->error(e.what()); - } + auto result = update_isam2(*new_factors, *new_values); + Callbacks::on_smoother_update_result(*isam2, result); + new_values.reset(new gtsam::Values); new_factors.reset(new gtsam::NonlinearFactorGraph); @@ -414,6 +411,7 @@ boost::shared_ptr GlobalMapping::create_matching_co const auto& current_submap = submaps.back(); + double previous_overlap = 0.0; for (int i = 0; i < current; i++) { const double dist = (submaps[i]->T_world_origin.translation() - current_submap->T_world_origin.translation()).norm(); if (dist > params.max_implicit_loop_distance) { @@ -423,6 +421,7 @@ boost::shared_ptr GlobalMapping::create_matching_co const Eigen::Isometry3d delta = submaps[i]->T_world_origin.inverse() * current_submap->T_world_origin; const double overlap = gtsam_points::overlap_auto(submaps[i]->voxelmaps.back(), current_submap->frame, delta); + previous_overlap = i == current - 1 ? overlap : previous_overlap; if (overlap < params.min_implicit_loop_overlap) { continue; } @@ -447,6 +446,14 @@ boost::shared_ptr GlobalMapping::create_matching_co } } + if (previous_overlap < std::max(0.25, params.min_implicit_loop_overlap)) { + logger->warn("previous submap has only a small overlap with the current submap ({})", previous_overlap); + logger->warn("create a between factor to prevent the submap from being isolated"); + const int last = current - 1; + const gtsam::Pose3 init_delta = gtsam::Pose3((submaps[last]->T_world_origin.inverse() * submaps[current]->T_world_origin).matrix()); + factors->add(gtsam::make_shared>(X(last), X(current), init_delta, gtsam::noiseModel::Isotropic::Precision(6, 1e6))); + } + return factors; } @@ -459,14 +466,53 @@ void GlobalMapping::update_submaps() { gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values) { gtsam_points::ISAM2ResultExt result; + gtsam::Key indeterminant_nearby_key = 0; + try { #ifdef GTSAM_USE_TBB - auto arena = static_cast(tbb_task_arena.get()); - arena->execute([&] { + auto arena = static_cast(tbb_task_arena.get()); + arena->execute([&] { #endif - result = isam2->update(new_factors, new_values); + result = isam2->update(new_factors, new_values); #ifdef GTSAM_USE_TBB - }); + }); #endif + } catch (const gtsam::IndeterminantLinearSystemException& e) { + logger->error("an indeterminant lienar system exception was caught during global map optimization!!"); + logger->error(e.what()); + indeterminant_nearby_key = e.nearbyVariable(); + } catch (const std::exception& e) { + logger->error("an exception was caught during global map optimization!!"); + logger->error(e.what()); + } + + if (indeterminant_nearby_key != 0) { + const gtsam::Symbol symbol(indeterminant_nearby_key); + if (symbol.chr() == 'v' || symbol.chr() == 'b' || symbol.chr() == 'e') { + indeterminant_nearby_key = X(symbol.index() / 2); + } + logger->warn("insert a damping factor at {} to prevent corruption", std::string(gtsam::Symbol(indeterminant_nearby_key))); + + gtsam::Values values = isam2->getLinearizationPoint(); + gtsam::NonlinearFactorGraph factors = isam2->getFactorsUnsafe(); + factors.emplace_shared(indeterminant_nearby_key, 6, 1e4); + + gtsam::ISAM2Params isam2_params; + if (params.use_isam2_dogleg) { + gtsam::ISAM2DoglegParams dogleg_params; + isam2_params.setOptimizationParams(dogleg_params); + } + isam2_params.relinearizeSkip = params.isam2_relinearize_skip; + isam2_params.setRelinearizeThreshold(params.isam2_relinearize_thresh); + + if (params.enable_optimization) { + isam2.reset(new gtsam_points::ISAM2Ext(isam2_params)); + } else { + isam2.reset(new gtsam_points::ISAM2ExtDummy(isam2_params)); + } + + isam2->update(factors, values); + logger->warn("isam2 was reset"); + } return result; } diff --git a/src/glim/viewer/standard_viewer.cpp b/src/glim/viewer/standard_viewer.cpp index 710b8667..d716248c 100644 --- a/src/glim/viewer/standard_viewer.cpp +++ b/src/glim/viewer/standard_viewer.cpp @@ -59,6 +59,9 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) { z_range = config.param("standard_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast(); auto_z_range << 0.0f, 0.0f; + show_mapping_tools = false; + min_overlap = 0.2f; + points_alpha = config.param("standard_viewer", "points_alpha", 1.0); factors_alpha = config.param("standard_viewer", "factors_alpha", 1.0); @@ -730,6 +733,11 @@ void StandardViewer::drawable_selection() { show_submaps = show_factors = show_mapping; } + ImGui::SameLine(); + if (ImGui::Button("Tools")) { + show_mapping_tools = true; + } + ImGui::Checkbox("submaps", &show_submaps); ImGui::SameLine(); ImGui::Checkbox("factors", &show_factors); @@ -750,6 +758,21 @@ void StandardViewer::drawable_selection() { ImGui::Text("bias:%.3f %.3f %.3f %.3f %.3f %.3f", last_imu_bias[0], last_imu_bias[1], last_imu_bias[2], last_imu_bias[3], last_imu_bias[4], last_imu_bias[5]); ImGui::End(); } + + if (show_mapping_tools) { + ImGui::Begin("mapping tools", &show_mapping_tools, ImGuiWindowFlags_AlwaysAutoResize); + ImGui::DragFloat("Min overlap", &min_overlap, 0.01f, 0.01f, 1.0f); + if (ImGui::Button("Find overlapping submaps")) { + logger->info("finding overlapping submaps..."); + GlobalMappingCallbacks::request_to_find_overlapping_submaps(min_overlap); + } + + if (ImGui::Button("Optimize")) { + logger->info("optimizing..."); + GlobalMappingCallbacks::request_to_optimize(); + } + ImGui::End(); + } } } // namespace glim