diff --git a/include/glim/mapping/async_global_mapping.hpp b/include/glim/mapping/async_global_mapping.hpp index 96aad073..d2694550 100644 --- a/include/glim/mapping/async_global_mapping.hpp +++ b/include/glim/mapping/async_global_mapping.hpp @@ -72,6 +72,11 @@ class AsyncGlobalMapping { std::vector export_points(); + std::shared_ptr get_global_mapping() { + std::lock_guard lock(global_mapping_mutex); + return global_mapping; + } + private: void run(); diff --git a/include/glim/viewer/offline_viewer.hpp b/include/glim/viewer/offline_viewer.hpp index 759ebd3b..fdffb040 100644 --- a/include/glim/viewer/offline_viewer.hpp +++ b/include/glim/viewer/offline_viewer.hpp @@ -21,7 +21,7 @@ class OfflineViewer : public InteractiveViewer { void main_menu(); - std::shared_ptr load_map(guik::ProgressInterface& progress, const std::string& path); + std::shared_ptr load_map(guik::ProgressInterface& progress, const std::string& path, std::shared_ptr global_mapping); bool save_map(guik::ProgressInterface& progress, const std::string& path); bool export_map(guik::ProgressInterface& progress, const std::string& path); diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp index db1206cc..04354478 100644 --- a/src/glim/mapping/global_mapping.cpp +++ b/src/glim/mapping/global_mapping.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -214,6 +215,8 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) { } } + logger->debug("|new_factors|={} |new_values|={}", new_factors->size(), new_values->size()); + Callbacks::on_smoother_update(*isam2, *new_factors, *new_values); auto result = update_isam2(*new_factors, *new_values); Callbacks::on_smoother_update_result(*isam2, result); @@ -284,6 +287,9 @@ void GlobalMapping::find_overlapping_submaps(double min_overlap) { // Between factors are Vector2i actually. A bad use of Vector3i std::unordered_set existing_factors; for (const auto& factor : isam2->getFactorsUnsafe()) { + if (factor == nullptr) { + continue; + } if (factor->keys().size() != 2) { continue; } @@ -352,10 +358,13 @@ void GlobalMapping::optimize() { return; } - gtsam::NonlinearFactorGraph new_factors; - gtsam::Values new_values; - Callbacks::on_smoother_update(*isam2, new_factors, new_values); - auto result = update_isam2(new_factors, new_values); + logger->info("|new_factors|={} |new_values|={}", new_factors->size(), new_values->size()); + + Callbacks::on_smoother_update(*isam2, *new_factors, *new_values); + auto result = update_isam2(*new_factors, *new_values); + + new_factors.reset(new gtsam::NonlinearFactorGraph); + new_values.reset(new gtsam::Values); Callbacks::on_smoother_update_result(*isam2, result); @@ -484,6 +493,7 @@ gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearF arena->execute([&] { #endif result = isam2->update(new_factors, new_values); + #ifdef GTSAM_USE_TBB }); #endif @@ -644,6 +654,8 @@ bool GlobalMapping::load(const std::string& path) { return false; } + const int start_from_frame_id = submaps.size(); + std::string token; int num_submaps, num_all_frames, num_matching_cost_factors; @@ -658,13 +670,14 @@ bool GlobalMapping::load(const std::string& path) { } logger->info("Load submaps"); - submaps.resize(num_submaps); - subsampled_submaps.resize(num_submaps); + submaps.reserve(submaps.size() + num_submaps); + subsampled_submaps.reserve(submaps.size() + num_submaps); for (int i = 0; i < num_submaps; i++) { auto submap = SubMap::load((boost::format("%s/%06d") % path % i).str()); if (!submap) { return false; } + submap->id += start_from_frame_id; // Adaptively determine the voxel resolution based on the median distance const int max_scan_count = 256; @@ -680,19 +693,19 @@ bool GlobalMapping::load(const std::string& path) { subsampled_submap = gtsam_points::random_sampling(submap->frame, params.randomsampling_rate, mt); } - submaps[i] = submap; - submaps[i]->voxelmaps.clear(); - subsampled_submaps[i] = subsampled_submap; + submaps.push_back(submap); + submaps.back()->voxelmaps.clear(); + subsampled_submaps.push_back(subsampled_submap); if (params.enable_gpu) { #ifdef GTSAM_POINTS_USE_CUDA - subsampled_submaps[i] = gtsam_points::PointCloudGPU::clone(*subsampled_submaps[i]); + subsampled_submaps.back() = gtsam_points::PointCloudGPU::clone(*subsampled_submaps.back()); for (int j = 0; j < params.submap_voxelmap_levels; j++) { const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, j); auto voxelmap = std::make_shared(resolution); - voxelmap->insert(*subsampled_submaps[i]); - submaps[i]->voxelmaps.push_back(voxelmap); + voxelmap->insert(*subsampled_submaps.back()); + submaps.back()->voxelmaps.push_back(voxelmap); } #else logger->warn("GPU is enabled for global_mapping but gtsam_points was built without CUDA!!"); @@ -701,21 +714,21 @@ bool GlobalMapping::load(const std::string& path) { for (int j = 0; j < params.submap_voxelmap_levels; j++) { const double resolution = base_resolution * std::pow(params.submap_voxelmap_scaling_factor, j); auto voxelmap = std::make_shared(resolution); - voxelmap->insert(*subsampled_submaps[i]); - submaps[i]->voxelmaps.push_back(voxelmap); + voxelmap->insert(*subsampled_submaps.back()); + submaps.back()->voxelmaps.push_back(voxelmap); } } Callbacks::on_insert_submap(submap); } - gtsam::Values values; - gtsam::NonlinearFactorGraph graph; + gtsam::Values values, loaded_values; + gtsam::NonlinearFactorGraph graph, loaded_graph; bool needs_recover = false; + logger->info("deserializing factor graph"); try { - logger->info("deserializing factor graph"); - gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph); + gtsam::deserializeFromBinaryFile(path + "/graph.bin", loaded_graph); } catch (boost::archive::archive_exception e) { logger->error("failed to deserialize factor graph!!"); logger->error(e.what()); @@ -725,9 +738,9 @@ bool GlobalMapping::load(const std::string& path) { needs_recover = true; } + logger->info("deserializing values"); try { - logger->info("deserializing values"); - gtsam::deserializeFromBinaryFile(path + "/values.bin", values); + gtsam::deserializeFromBinaryFile(path + "/values.bin", loaded_values); } catch (boost::archive::archive_exception e) { logger->error("failed to deserialize values!!"); logger->error(e.what()); @@ -737,11 +750,51 @@ bool GlobalMapping::load(const std::string& path) { needs_recover = true; } + // remap keys in graph and values if dump previously loaded + if (start_from_frame_id > 0) { + std::map rekey_mapping; + for (int i = 0; i < num_submaps; i++) { + rekey_mapping[X(i)] = X(i + start_from_frame_id); + rekey_mapping[E(i * 2)] = E((i + start_from_frame_id) * 2); + rekey_mapping[E(i * 2 + 1)] = E((i + start_from_frame_id) * 2 + 1); + rekey_mapping[B(i * 2)] = B((i + start_from_frame_id) * 2); + rekey_mapping[B(i * 2 + 1)] = B((i + start_from_frame_id) * 2 + 1); + rekey_mapping[V(i * 2)] = V((i + start_from_frame_id) * 2); + rekey_mapping[V(i * 2 + 1)] = V((i + start_from_frame_id) * 2 + 1); + } + + auto first_factor = loaded_graph.front(); + if (boost::dynamic_pointer_cast(first_factor)) { + logger->info("First factor is a LinearContainerFactor, removing from graph before loading"); + graph = gtsam::NonlinearFactorGraph(loaded_graph.begin() + 1, loaded_graph.end()); + } else { + graph = loaded_graph; + } + + // rekey graph + logger->info("rekeying factors"); + graph = graph.rekey(rekey_mapping); + + // rekey values + for (auto it = loaded_values.begin(); it != loaded_values.end(); ++it) { + auto matched_key = rekey_mapping.find(it->key); + if (matched_key != rekey_mapping.end()) { + values.insert(matched_key->second, it->value); + } else { + logger->warn("No remapping found for Value with key {}, keeping it as is", gtsam::Symbol(it->key).string()); + values.insert(it->key, it->value); + } + } + } else { + graph = loaded_graph; + values = loaded_values; + } + logger->info("creating matching cost factors"); for (const auto& factor : matching_cost_factors) { const auto type = std::get<0>(factor); - const auto first = std::get<1>(factor); - const auto second = std::get<2>(factor); + const auto first = std::get<1>(factor) + start_from_frame_id; + const auto second = std::get<2>(factor) + start_from_frame_id; if (type == "vgicp" || type == "vgicp_gpu") { if (params.enable_gpu) { @@ -781,13 +834,19 @@ bool GlobalMapping::load(const std::string& path) { values.insert_or_assign(recovered.second); } - logger->info("optimize"); - Callbacks::on_smoother_update(*isam2, graph, values); - auto result = update_isam2(graph, values); - Callbacks::on_smoother_update_result(*isam2, result); + if (start_from_frame_id <= 0) { + logger->info("optimize"); + Callbacks::on_smoother_update(*isam2, graph, values); + auto result = update_isam2(graph, values); + Callbacks::on_smoother_update_result(*isam2, result); - update_submaps(); - Callbacks::on_update_submaps(submaps); + update_submaps(); + Callbacks::on_update_submaps(submaps); + } else { + logger->info("skip optimization"); + this->new_factors->add(graph); + this->new_values->insert(values); + } logger->info("done"); diff --git a/src/glim/mapping/sub_map.cpp b/src/glim/mapping/sub_map.cpp index 5145a05e..7afe5304 100644 --- a/src/glim/mapping/sub_map.cpp +++ b/src/glim/mapping/sub_map.cpp @@ -85,6 +85,7 @@ SubMap::Ptr SubMap::load(const std::string& path) { std::string token; ifs >> token >> submap->id; + submap->id; ifs >> token; submap->T_world_origin.matrix() = read_matrix<4, 4>(ifs); diff --git a/src/glim/viewer/interactive_viewer.cpp b/src/glim/viewer/interactive_viewer.cpp index febcf091..185b1ff5 100644 --- a/src/glim/viewer/interactive_viewer.cpp +++ b/src/glim/viewer/interactive_viewer.cpp @@ -257,6 +257,7 @@ void InteractiveViewer::context_menu() { if (type == PickType::FRAME) { const int frame_id = right_clicked_info[3]; + ImGui::TextUnformatted(("frame_id " + std::to_string(frame_id)).c_str()); if (ImGui::MenuItem("Loop begin")) { manual_loop_close_modal->set_target(X(frame_id), submaps[frame_id]->frame, submap_poses[frame_id]); } diff --git a/src/glim/viewer/offline_viewer.cpp b/src/glim/viewer/offline_viewer.cpp index 202012ea..03141a0c 100644 --- a/src/glim/viewer/offline_viewer.cpp +++ b/src/glim/viewer/offline_viewer.cpp @@ -36,10 +36,18 @@ void OfflineViewer::main_menu() { bool start_save_map = false; bool start_export_map = false; + std::shared_ptr global_mapping; + if (ImGui::BeginMainMenuBar()) { if (ImGui::BeginMenu("File")) { - if (ImGui::MenuItem("Open Map")) { - start_open_map = true; + if (!async_global_mapping) { // if a previously loaded map does not yet exist + if (ImGui::MenuItem("Open New Map")) { + start_open_map = true; + } + } else { + if (ImGui::MenuItem("Open Additional Map")) { + start_open_map = true; + } } if (ImGui::MenuItem("Close Map")) { @@ -105,7 +113,16 @@ void OfflineViewer::main_menu() { ExtensionModule::export_classes(name); } - progress_modal->open>("open", [this, map_path](guik::ProgressInterface& progress) { return load_map(progress, map_path); }); + // if a map is already loaded, use existing map to load new map into + if (async_global_mapping) { + logger->info("global map already exists, loading new map into existing global map"); + global_mapping = std::dynamic_pointer_cast(async_global_mapping->get_global_mapping()); + } + + progress_modal->open>("open", [this, map_path, &global_mapping](guik::ProgressInterface& progress) { + global_mapping = load_map(progress, map_path, std::move(global_mapping)); + return global_mapping; + }); } } auto open_result = progress_modal->run>("open"); @@ -152,26 +169,29 @@ void OfflineViewer::main_menu() { } } -std::shared_ptr OfflineViewer::load_map(guik::ProgressInterface& progress, const std::string& path) { +std::shared_ptr OfflineViewer::load_map(guik::ProgressInterface& progress, const std::string& path, std::shared_ptr global_mapping) { progress.set_title("Load map"); progress.set_text("Now loading"); progress.set_maximum(1); - glim::GlobalMappingParams params; - params.isam2_relinearize_skip = 1; - params.isam2_relinearize_thresh = 0.0; + if (global_mapping == nullptr) { // if no map is loaded yet initialize new GlobalMapping + glim::GlobalMappingParams params; + params.isam2_relinearize_skip = 1; + params.isam2_relinearize_thresh = 0.0; - const auto result = pfd::message("Confirm", "Do optimization?", pfd::choice::yes_no).result(); - params.enable_optimization = (result == pfd::button::ok) || (result == pfd::button::yes); + const auto result = pfd::message("Confirm", "Do optimization?", pfd::choice::yes_no).result(); + params.enable_optimization = (result == pfd::button::ok) || (result == pfd::button::yes); + + logger->info("enable_optimization={}", params.enable_optimization); + global_mapping.reset(new glim::GlobalMapping(params)); + } - logger->info("enable_optimization={}", params.enable_optimization); - std::shared_ptr global_mapping(new glim::GlobalMapping(params)); if (!global_mapping->load(path)) { logger->error("failed to load {}", path); return nullptr; } - return global_mapping; + return std::move(global_mapping); } bool OfflineViewer::save_map(guik::ProgressInterface& progress, const std::string& path) {