Skip to content

Commit

Permalink
Multiple Map Loading (#146)
Browse files Browse the repository at this point in the history 
* load additional map into GlobalMapping

* global_mapping: use native rekeying function, avoid LinearContainerFactor in additional graph

+ proper clang-format

* global_mapping: add a between factor between existing and loaded graph

create extra factor between both graphs to avoid indeterminant system
exception

* global_mapping: get state of existing graph, do rekeying of new graph based on this.

* global_mapping: change factor between graphs to betweenfactor

* offline_viewer + global_mapping: remove last temporary factor next optimisation

* deferring optimization when an additional map is loaded

* offline_viewer: remove outdated message of temporary factor

---------

Co-authored-by: k.koide <[email protected]>
  • Loading branch information
@remco-r @koide3
remco-r and koide3 authored Feb 14, 2025
1 parent d1c23be commit d8d79bb
Show file tree
Hide file tree
Showing 6 changed files with 127 additions and 41 deletions.
5 changes: 5 additions & 0 deletions include/glim/mapping/async_global_mapping.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,11 @@ class AsyncGlobalMapping {

std::vector<Eigen::Vector4d> export_points();

std::shared_ptr<glim::GlobalMappingBase> get_global_mapping() {
std::lock_guard<std::mutex> lock(global_mapping_mutex);
return global_mapping;
}

private:
void run();

Expand Down
2 changes: 1 addition & 1 deletion include/glim/viewer/offline_viewer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ class OfflineViewer : public InteractiveViewer {

void main_menu();

std::shared_ptr<GlobalMapping> load_map(guik::ProgressInterface& progress, const std::string& path);
std::shared_ptr<GlobalMapping> load_map(guik::ProgressInterface& progress, const std::string& path, std::shared_ptr<GlobalMapping> global_mapping);
bool save_map(guik::ProgressInterface& progress, const std::string& path);
bool export_map(guik::ProgressInterface& progress, const std::string& path);

Expand Down
115 changes: 87 additions & 28 deletions src/glim/mapping/global_mapping.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include <glim/mapping/global_mapping.hpp>

#include <map>
#include <unordered_set>
#include <spdlog/spdlog.h>
#include <boost/filesystem.hpp>
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<Eigen::Vector3i, gtsam_points::Vector3iHash> existing_factors;
for (const auto& factor : isam2->getFactorsUnsafe()) {
if (factor == nullptr) {
continue;
}
if (factor->keys().size() != 2) {
continue;
}
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;

Expand All @@ -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;
Expand All @@ -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<gtsam_points::GaussianVoxelMapGPU>(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!!");
Expand All @@ -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<gtsam_points::GaussianVoxelMapCPU>(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());
Expand All @@ -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());
Expand All @@ -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<gtsam::Key, gtsam::Key> 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<gtsam::LinearContainerFactor>(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) {
Expand Down Expand Up @@ -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");

Expand Down
1 change: 1 addition & 0 deletions src/glim/mapping/sub_map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions src/glim/viewer/interactive_viewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
}
Expand Down
44 changes: 32 additions & 12 deletions src/glim/viewer/offline_viewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,18 @@ void OfflineViewer::main_menu() {
bool start_save_map = false;
bool start_export_map = false;

std::shared_ptr<GlobalMapping> 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")) {
Expand Down Expand Up @@ -105,7 +113,16 @@ void OfflineViewer::main_menu() {
ExtensionModule::export_classes(name);
}

progress_modal->open<std::shared_ptr<GlobalMapping>>("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<GlobalMapping>(async_global_mapping->get_global_mapping());
}

progress_modal->open<std::shared_ptr<GlobalMapping>>("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<std::shared_ptr<GlobalMapping>>("open");
Expand Down Expand Up @@ -152,26 +169,29 @@ void OfflineViewer::main_menu() {
}
}

std::shared_ptr<GlobalMapping> OfflineViewer::load_map(guik::ProgressInterface& progress, const std::string& path) {
std::shared_ptr<glim::GlobalMapping> OfflineViewer::load_map(guik::ProgressInterface& progress, const std::string& path, std::shared_ptr<GlobalMapping> 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<glim::GlobalMapping> 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) {
Expand Down

0 comments on commit d8d79bb

Please sign in to comment.