diff --git a/include/glim/mapping/async_global_mapping.hpp b/include/glim/mapping/async_global_mapping.hpp index 151efcf7..96aad073 100644 --- a/include/glim/mapping/async_global_mapping.hpp +++ b/include/glim/mapping/async_global_mapping.hpp @@ -86,6 +86,7 @@ class AsyncGlobalMapping { int optimization_interval; std::atomic_bool request_to_optimize; + std::atomic_bool request_to_recover; std::atomic request_to_find_overlapping_submaps; std::mutex global_mapping_mutex; diff --git a/include/glim/mapping/callbacks.hpp b/include/glim/mapping/callbacks.hpp index 9c2a75f5..4e85d99e 100644 --- a/include/glim/mapping/callbacks.hpp +++ b/include/glim/mapping/callbacks.hpp @@ -132,6 +132,12 @@ struct GlobalMappingCallbacks { */ static CallbackSlot request_to_optimize; + /** + * @brief Request the global mapping module to detect and recover from a graph corruption + * @note This is a special inverse-direction callback slot + */ + static CallbackSlot request_to_recover; + /** * @brief Request the global mapping module to find new overlapping submaps * @param min_overlap Minimum overlap rate between submaps diff --git a/include/glim/mapping/global_mapping.hpp b/include/glim/mapping/global_mapping.hpp index b3ea803e..c182ad34 100644 --- a/include/glim/mapping/global_mapping.hpp +++ b/include/glim/mapping/global_mapping.hpp @@ -87,6 +87,9 @@ class GlobalMapping : public GlobalMappingBase { void update_submaps(); gtsam_points::ISAM2ResultExt update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values); + void recover_graph() override; + std::pair recover_graph(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values) const; + private: using Params = GlobalMappingParams; Params params; diff --git a/include/glim/mapping/global_mapping_base.hpp b/include/glim/mapping/global_mapping_base.hpp index 494ada4d..1b45f2f2 100644 --- a/include/glim/mapping/global_mapping_base.hpp +++ b/include/glim/mapping/global_mapping_base.hpp @@ -52,6 +52,11 @@ class GlobalMappingBase { */ virtual void optimize(); + /** + * @brief Request to detect and recover graph corruption + */ + virtual void recover_graph(); + /** * @brief Save the mapping result * @param path Save path diff --git a/src/glim/mapping/async_global_mapping.cpp b/src/glim/mapping/async_global_mapping.cpp index bf578343..4849dc3a 100644 --- a/src/glim/mapping/async_global_mapping.cpp +++ b/src/glim/mapping/async_global_mapping.cpp @@ -12,9 +12,11 @@ AsyncGlobalMapping::AsyncGlobalMapping(const std::shared_ptr lock(global_mapping_mutex); + request_to_recover = false; + global_mapping->recover_graph(); + last_optimization_time = std::chrono::high_resolution_clock::now(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } diff --git a/src/glim/mapping/callbacks.cpp b/src/glim/mapping/callbacks.cpp index 99d0da99..2872f9fa 100644 --- a/src/glim/mapping/callbacks.cpp +++ b/src/glim/mapping/callbacks.cpp @@ -23,5 +23,6 @@ CallbackSlot GlobalMappingCallbacks::on_smoother_update_result; CallbackSlot GlobalMappingCallbacks::request_to_optimize; +CallbackSlot GlobalMappingCallbacks::request_to_recover; CallbackSlot GlobalMappingCallbacks::request_to_find_overlapping_submaps; } // namespace glim \ No newline at end of file diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index aa3e46fc..96b91d22 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/src/glim/mapping/global_mapping.cpp @@ -521,8 +521,8 @@ gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearF isam2.reset(new gtsam_points::ISAM2ExtDummy(isam2_params)); } - isam2->update(factors, values); - logger->warn("isam2 was reset"); + logger->warn("reset isam2"); + return update_isam2(factors, values); } return result; @@ -711,6 +711,7 @@ bool GlobalMapping::load(const std::string& path) { gtsam::Values values; gtsam::NonlinearFactorGraph graph; + bool needs_recover = false; try { logger->info("deserializing factor graph"); @@ -718,13 +719,22 @@ bool GlobalMapping::load(const std::string& path) { } catch (boost::archive::archive_exception e) { logger->error("failed to deserialize factor graph!!"); logger->error(e.what()); + } catch (std::exception& e) { + logger->error("failed to deserialize factor graph!!"); + logger->error(e.what()); + needs_recover = true; } + try { logger->info("deserializing values"); gtsam::deserializeFromBinaryFile(path + "/values.bin", values); } catch (boost::archive::archive_exception e) { - logger->error("failed to deserialize factor graph!!"); + logger->error("failed to deserialize values!!"); + logger->error(e.what()); + } catch (std::exception& e) { + logger->error("failed to deserialize values!!"); logger->error(e.what()); + needs_recover = true; } logger->info("creating matching cost factors"); @@ -756,6 +766,21 @@ bool GlobalMapping::load(const std::string& path) { } } + const size_t num_factors_before = graph.size(); + const auto remove_loc = std::remove_if(graph.begin(), graph.end(), [](const auto& factor) { return factor == nullptr; }); + graph.erase(remove_loc, graph.end()); + if (graph.size() != num_factors_before) { + logger->warn("removed {} invalid factors", num_factors_before - graph.size()); + needs_recover = true; + } + + if (needs_recover) { + logger->warn("recovering factor graph"); + const auto recovered = recover_graph(graph, values); + graph.add(recovered.first); + values.insert_or_assign(recovered.second); + } + logger->info("optimize"); Callbacks::on_smoother_update(*isam2, graph, values); auto result = update_isam2(graph, values); @@ -769,4 +794,141 @@ bool GlobalMapping::load(const std::string& path) { return true; } +void GlobalMapping::recover_graph() { + const auto recovered = recover_graph(isam2->getFactorsUnsafe(), isam2->calculateEstimate()); + update_isam2(recovered.first, recovered.second); +} + +// Recover the graph by adding missing values and factors +std::pair GlobalMapping::recover_graph(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values) const { + logger->info("recovering graph"); + bool enable_imu = false; + for (const auto& value : values) { + const char chr = gtsam::Symbol(value.key).chr(); + enable_imu |= (chr == 'e' || chr == 'v' || chr == 'b'); + } + for (const auto& factor : graph) { + enable_imu |= boost::dynamic_pointer_cast(factor) != nullptr; + } + + logger->info("enable_imu={}", enable_imu); + + logger->info("creating connectivity map"); + bool prior_exists = false; + std::unordered_map> connectivity_map; + for (const auto& factor : graph) { + if (!factor) { + continue; + } + + for (const auto key : factor->keys()) { + for (const auto key2 : factor->keys()) { + connectivity_map[key].insert(key2); + } + } + + if (factor->keys().size() == 1 && factor->keys()[0] == X(0)) { + prior_exists |= boost::dynamic_pointer_cast(factor) != nullptr; + } + } + + if (!prior_exists) { + logger->warn("X0 prior is missing"); + new_factors->emplace_shared(X(0), 6, params.init_pose_damping_scale); + } + + logger->info("fixing missing values and factors"); + const auto prior_noise3 = gtsam::noiseModel::Isotropic::Precision(3, 1e6); + const auto prior_noise6 = gtsam::noiseModel::Isotropic::Precision(6, 1e6); + + gtsam::NonlinearFactorGraph new_factors; + gtsam::Values new_values; + for (int i = 0; i < submaps.size(); i++) { + if (!values.exists(X(i))) { + logger->warn("X{} is missing", i); + new_values.insert(X(i), gtsam::Pose3(submaps[i]->T_world_origin.matrix())); + } + + if (connectivity_map[X(i)].count(X(i + 1)) == 0 && i != submaps.size() - 1) { + logger->warn("X{} -> X{} is missing", i, i + 1); + + const Eigen::Isometry3d delta = submaps[i]->origin_odom_frame()->T_world_sensor().inverse() * submaps[i + 1]->origin_odom_frame()->T_world_sensor(); + new_factors.emplace_shared>(X(i), X(i + 1), gtsam::Pose3(delta.matrix()), prior_noise6); + } + + if (!enable_imu) { + continue; + } + + const auto submap = submaps[i]; + const gtsam::imuBias::ConstantBias imu_biasL(submap->frames.front()->imu_bias); + const gtsam::imuBias::ConstantBias imu_biasR(submap->frames.back()->imu_bias); + const Eigen::Vector3d v_origin_imuL = submap->T_world_origin.linear().inverse() * submap->frames.front()->v_world_imu; + const Eigen::Vector3d v_origin_imuR = submap->T_world_origin.linear().inverse() * submap->frames.back()->v_world_imu; + + if (i != 0) { + if (!values.exists(E(i * 2))) { + logger->warn("E{} is missing", i * 2); + new_values.insert(E(i * 2), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_L).matrix())); + } + if (!values.exists(V(i * 2))) { + logger->warn("V{} is missing", i * 2); + new_values.insert(V(i * 2), (submap->T_world_origin.linear() * v_origin_imuL).eval()); + } + if (!values.exists(B(i * 2))) { + logger->warn("B{} is missing", i * 2); + new_values.insert(B(i * 2), imu_biasL); + } + + if (connectivity_map[X(i)].count(E(i * 2)) == 0) { + logger->warn("X{} -> E{} is missing", i, i * 2); + new_factors.emplace_shared>(X(i), E(i * 2), gtsam::Pose3(submap->T_origin_endpoint_L.matrix()), prior_noise6); + } + if (connectivity_map[X(i)].count(V(i * 2)) == 0) { + logger->warn("X{} -> V{} is missing", i, i * 2); + new_factors.emplace_shared(X(i), V(i * 2), v_origin_imuL, prior_noise3); + } + if (connectivity_map[B(i * 2)].count(B(i * 2)) == 0) { + logger->warn("B{} -> B{} is missing", i * 2, i * 2); + new_factors.emplace_shared>(B(i * 2), imu_biasL, prior_noise6); + } + + if (connectivity_map[B(i * 2)].count(B(i * 2 + 1)) == 0) { + logger->warn("B{} -> B{} is missing", i * 2, i * 2 + 1); + new_factors.emplace_shared>(B(i * 2), B(i * 2 + 1), gtsam::imuBias::ConstantBias(), prior_noise6); + } + } + + if (!values.exists(E(i * 2 + 1))) { + logger->warn("E{} is missing", i * 2 + 1); + new_values.insert(E(i * 2 + 1), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_R).matrix())); + } + if (!values.exists(V(i * 2 + 1))) { + logger->warn("V{} is missing", i * 2 + 1); + new_values.insert(V(i * 2 + 1), (submap->T_world_origin.linear() * v_origin_imuR).eval()); + } + if (!values.exists(B(i * 2 + 1))) { + logger->warn("B{} is missing", i * 2 + 1); + new_values.insert(B(i * 2 + 1), imu_biasR); + } + + if (connectivity_map[X(i)].count(E(i * 2 + 1)) == 0) { + logger->warn("X{} -> E{} is missing", i, i * 2 + 1); + new_factors.emplace_shared>(X(i), E(i * 2 + 1), gtsam::Pose3(submap->T_origin_endpoint_R.matrix()), prior_noise6); + } + if (connectivity_map[X(i)].count(V(i * 2 + 1)) == 0) { + logger->warn("X{} -> V{} is missing", i, i * 2 + 1); + new_factors.emplace_shared(X(i), V(i * 2 + 1), v_origin_imuR, prior_noise3); + } + if (connectivity_map[B(i * 2 + 1)].count(B(i * 2 + 1)) == 0) { + logger->warn("B{} -> B{} is missing", i * 2 + 1, i * 2 + 1); + new_factors.emplace_shared>(B(i * 2 + 1), imu_biasR, prior_noise6); + } + } + + logger->info("recovering done"); + + return {new_factors, new_values}; +} + } // namespace glim \ No newline at end of file diff --git a/src/glim/mapping/global_mapping_base.cpp b/src/glim/mapping/global_mapping_base.cpp index 9ed11177..1180dbaa 100644 --- a/src/glim/mapping/global_mapping_base.cpp +++ b/src/glim/mapping/global_mapping_base.cpp @@ -26,6 +26,8 @@ void GlobalMappingBase::find_overlapping_submaps(double min_overlap) {} void GlobalMappingBase::optimize() {} +void GlobalMappingBase::recover_graph() {} + std::shared_ptr GlobalMappingBase::load_module(const std::string& so_name) { return load_module_from_so(so_name, "create_global_mapping_module"); } diff --git a/src/glim/viewer/interactive_viewer.cpp b/src/glim/viewer/interactive_viewer.cpp index 25fe2283..febcf091 100644 --- a/src/glim/viewer/interactive_viewer.cpp +++ b/src/glim/viewer/interactive_viewer.cpp @@ -209,6 +209,11 @@ void InteractiveViewer::drawable_selection() { GlobalMappingCallbacks::request_to_find_overlapping_submaps(min_overlap); } + if (ImGui::Button("Recover graph")) { + logger->info("recovering graph..."); + GlobalMappingCallbacks::request_to_recover(); + } + if (ImGui::Button("Optimize")) { logger->info("optimizing..."); GlobalMappingCallbacks::request_to_optimize();