From be4f9a34c488c4daaf0fd18159f7ea492c0b6c61 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Wed, 22 Nov 2023 11:09:27 +0900 Subject: [PATCH 1/7] return one-sided values in interpolation helper --- include/glim/util/concurrent_vector.hpp | 4 ++++ include/glim/util/interpolation_helper.hpp | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/include/glim/util/concurrent_vector.hpp b/include/glim/util/concurrent_vector.hpp index 7faebb1f..5985e1e6 100644 --- a/include/glim/util/concurrent_vector.hpp +++ b/include/glim/util/concurrent_vector.hpp @@ -134,6 +134,10 @@ class ConcurrentVector { std::vector get_and_clear(int num_max) { std::vector buffer; std::lock_guard lock(mutex); + if (values.empty()) { + return buffer; + } + if (values.size() <= num_max) { buffer.assign(values.begin(), values.end()); values.clear(); diff --git a/include/glim/util/interpolation_helper.hpp b/include/glim/util/interpolation_helper.hpp index 1ca3cbe8..ef9d5f38 100644 --- a/include/glim/util/interpolation_helper.hpp +++ b/include/glim/util/interpolation_helper.hpp @@ -32,10 +32,16 @@ class InterpolationHelper { InterpolationHelperResult find(const double stamp, StampedValue* left_ptr, StampedValue* right_ptr, int* remove_cursor_ptr) const { if (values.empty() || values.back().first < stamp) { + if (!values.empty() && left_ptr) { + *left_ptr = values.back(); + } return InterpolationHelperResult::WAITING; } if (values.front().first > stamp) { + if (right_ptr) { + *right_ptr = values.front(); + } return InterpolationHelperResult::FAILURE; } From 1f42440cbe60d7afa5f0f88fc1599bd4594ed53b Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 27 Nov 2023 13:24:21 +0900 Subject: [PATCH 2/7] recover corrupted graph --- src/glim/backend/global_mapping.cpp | 46 +++++++++++++++++++++++++++-- thirdparty/gtsam_ext | 2 +- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/src/glim/backend/global_mapping.cpp b/src/glim/backend/global_mapping.cpp index 8351fa65..4f648b12 100644 --- a/src/glim/backend/global_mapping.cpp +++ b/src/glim/backend/global_mapping.cpp @@ -455,9 +455,15 @@ void GlobalMapping::save(const std::string& path) { try { gtsam::serializeToBinaryFile(serializable_factors, path + "/graph.bin"); + } catch (boost::archive::archive_exception e) { + spdlog::warn("failed to serialize graph!!"); + spdlog::warn(e.what()); + } + + try { gtsam::serializeToBinaryFile(isam2->calculateEstimate(), path + "/values.bin"); } catch (boost::archive::archive_exception e) { - spdlog::warn("failed to serialize factor graph!!"); + spdlog::warn("failed to serialize values!!"); spdlog::warn(e.what()); } @@ -601,9 +607,23 @@ bool GlobalMapping::load(const std::string& path) { gtsam::NonlinearFactorGraph graph; spdlog::info("deserializing factor graph"); - gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph); + bool needs_recovery = false; + try { + gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph); + } catch (std::exception& e) { + spdlog::warn("failed to deserialize graph!!"); + spdlog::warn(e.what()); + needs_recovery = true; + } spdlog::info("deserializing values"); - gtsam::deserializeFromBinaryFile(path + "/values.bin", values); + try { + gtsam::deserializeFromBinaryFile(path + "/values.bin", values); + } catch (std::exception& e) { + spdlog::warn("failed to deserialize values!!"); + spdlog::warn(e.what()); + needs_recovery = true; + } + spdlog::info("|graph|={} |values|={}", graph.size(), values.size()); spdlog::info("creating matching cost factors"); for (const auto& factor : matching_cost_factors) { @@ -634,6 +654,26 @@ bool GlobalMapping::load(const std::string& path) { } } + spdlog::info("validating the graph"); + for (const auto& submap : submaps) { + if (!values.exists(X(submap->id))) { + spdlog::warn("insert missing pose value for submap {}", submap->id); + values.insert_or_assign(X(submap->id), gtsam::Pose3(submap->T_world_origin.matrix())); + } + + if (!values.exists(V(submap->id * 2)) || !values.exists(V(submap->id * 2 + 1))) { + spdlog::warn("insert missing velocity values for submap {}", submap->id); + values.insert_or_assign(V(submap->id * 2), gtsam::Vector3(0.0, 0.0, 0.0)); + values.insert_or_assign(V(submap->id * 2 + 1), gtsam::Vector3(0.0, 0.0, 0.0)); + } + + if (!values.exists(B(submap->id * 2)) || !values.exists(B(submap->id * 2 + 1))) { + spdlog::warn("insert missing bias values for submap {}", submap->id); + values.insert_or_assign(B(submap->id * 2), gtsam::imuBias::ConstantBias(gtsam::Vector6::Zero())); + values.insert_or_assign(B(submap->id * 2 + 1), gtsam::imuBias::ConstantBias(gtsam::Vector6::Zero())); + } + } + spdlog::info("optimize"); Callbacks::on_smoother_update(*isam2, graph, values); auto result = isam2->update(graph, values); diff --git a/thirdparty/gtsam_ext b/thirdparty/gtsam_ext index 6ef8c232..01d43d4f 160000 --- a/thirdparty/gtsam_ext +++ b/thirdparty/gtsam_ext @@ -1 +1 @@ -Subproject commit 6ef8c232ee6c958adff7a21cdae30ed19e726f80 +Subproject commit 01d43d4fd7221cf670685333aadbc30cefe233c7 From a05dc672c4626ff233a1063ea68d03c375f18903 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 27 Nov 2023 13:51:32 +0900 Subject: [PATCH 3/7] update gtsam_ext --- thirdparty/gtsam_ext | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/gtsam_ext b/thirdparty/gtsam_ext index 01d43d4f..13fb3c76 160000 --- a/thirdparty/gtsam_ext +++ b/thirdparty/gtsam_ext @@ -1 +1 @@ -Subproject commit 01d43d4fd7221cf670685333aadbc30cefe233c7 +Subproject commit 13fb3c76b64530832f52a80d07ad2da4c267b89a From 34686343189b8ccf61d6ee7ab6a43ff599906ebf Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 27 Nov 2023 14:06:33 +0900 Subject: [PATCH 4/7] enable CUDA_MULTIARCH for build test --- docker/focal/Dockerfile | 3 ++- docker/focal_llvm/Dockerfile | 3 ++- docker/jammy/Dockerfile | 3 ++- docker/jammy_llvm/Dockerfile | 3 ++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/docker/focal/Dockerfile b/docker/focal/Dockerfile index 7c166514..8171e943 100644 --- a/docker/focal/Dockerfile +++ b/docker/focal/Dockerfile @@ -2,7 +2,7 @@ FROM koide3/gtsam_docker:focal_cuda11.2 RUN apt-get update \ && apt-get install --no-install-recommends -y \ - libfmt-dev libspdlog-dev libopencv-dev \ + libfmt-dev libspdlog-dev libopencv-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* @@ -29,6 +29,7 @@ RUN make -j$(nproc) WORKDIR /root/glim/build RUN cmake .. \ -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DBUILD_WITH_VIEWER=ON \ -DBUILD_WITH_MARCH_NATIVE=OFF \ -DCMAKE_BUILD_TYPE=Release diff --git a/docker/focal_llvm/Dockerfile b/docker/focal_llvm/Dockerfile index b998dada..28bb4f97 100644 --- a/docker/focal_llvm/Dockerfile +++ b/docker/focal_llvm/Dockerfile @@ -2,7 +2,7 @@ FROM koide3/gtsam_docker:focal_cuda11.2 RUN apt-get update \ && apt-get install --no-install-recommends -y \ - libfmt-dev libspdlog-dev libopencv-dev \ + libfmt-dev libspdlog-dev libopencv-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* @@ -31,6 +31,7 @@ RUN make -j$(nproc) WORKDIR /root/glim/build RUN CC=clang CXX=clang++ cmake .. \ -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DBUILD_WITH_VIEWER=ON \ -DBUILD_WITH_MARCH_NATIVE=OFF \ -DCMAKE_BUILD_TYPE=Release diff --git a/docker/jammy/Dockerfile b/docker/jammy/Dockerfile index 4e87b34a..dadbd944 100644 --- a/docker/jammy/Dockerfile +++ b/docker/jammy/Dockerfile @@ -2,7 +2,7 @@ FROM koide3/gtsam_docker:jammy_cuda11.8 RUN apt-get update \ && apt-get install --no-install-recommends -y \ - libfmt-dev libspdlog-dev libopencv-dev \ + libfmt-dev libspdlog-dev libopencv-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* @@ -29,6 +29,7 @@ RUN make -j$(nproc) WORKDIR /root/glim/build RUN cmake .. \ -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DBUILD_WITH_VIEWER=ON \ -DBUILD_WITH_MARCH_NATIVE=OFF \ -DCMAKE_BUILD_TYPE=Release diff --git a/docker/jammy_llvm/Dockerfile b/docker/jammy_llvm/Dockerfile index 338b1ba3..65af3eba 100644 --- a/docker/jammy_llvm/Dockerfile +++ b/docker/jammy_llvm/Dockerfile @@ -2,7 +2,7 @@ FROM koide3/gtsam_docker:jammy_cuda11.8 RUN apt-get update \ && apt-get install --no-install-recommends -y \ - libfmt-dev libspdlog-dev libopencv-dev \ + libfmt-dev libspdlog-dev libopencv-dev \ && apt-get clean \ && rm -rf /var/lib/apt/lists/* @@ -34,6 +34,7 @@ RUN sed -i '496{s/^/\/\//}' /usr/lib/llvm-14/lib/clang/14.0.0/include/omp.h WORKDIR /root/glim/build RUN CC=clang CXX=clang++ cmake .. \ -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DBUILD_WITH_VIEWER=ON \ -DBUILD_WITH_MARCH_NATIVE=OFF \ -DCMAKE_BUILD_TYPE=Release From 239a75cf2bbc2dcc594dee3d801b005ae04a19b7 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 27 Nov 2023 14:36:21 +0900 Subject: [PATCH 5/7] recovery for endpoint values --- src/glim/backend/global_mapping.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/glim/backend/global_mapping.cpp b/src/glim/backend/global_mapping.cpp index 4f648b12..147929c1 100644 --- a/src/glim/backend/global_mapping.cpp +++ b/src/glim/backend/global_mapping.cpp @@ -603,17 +603,17 @@ bool GlobalMapping::load(const std::string& path) { Callbacks::on_insert_submap(submap); } + bool corrupted_graph = false; gtsam::Values values; gtsam::NonlinearFactorGraph graph; spdlog::info("deserializing factor graph"); - bool needs_recovery = false; try { gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph); } catch (std::exception& e) { spdlog::warn("failed to deserialize graph!!"); spdlog::warn(e.what()); - needs_recovery = true; + corrupted_graph = true; } spdlog::info("deserializing values"); try { @@ -621,7 +621,7 @@ bool GlobalMapping::load(const std::string& path) { } catch (std::exception& e) { spdlog::warn("failed to deserialize values!!"); spdlog::warn(e.what()); - needs_recovery = true; + corrupted_graph = true; } spdlog::info("|graph|={} |values|={}", graph.size(), values.size()); @@ -659,21 +659,37 @@ bool GlobalMapping::load(const std::string& path) { if (!values.exists(X(submap->id))) { spdlog::warn("insert missing pose value for submap {}", submap->id); values.insert_or_assign(X(submap->id), gtsam::Pose3(submap->T_world_origin.matrix())); + corrupted_graph = true; + } + + if (!values.exists(E(submap->id * 2)) || !values.exists(E(submap->id * 2 + 1))) { + spdlog::warn("insert missing endpoint values for submap {}", submap->id); + values.insert_or_assign(E(submap->id * 2), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_L).matrix())); + values.insert_or_assign(E(submap->id * 2 + 1), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_R).matrix())); + corrupted_graph = true; } if (!values.exists(V(submap->id * 2)) || !values.exists(V(submap->id * 2 + 1))) { spdlog::warn("insert missing velocity values for submap {}", submap->id); values.insert_or_assign(V(submap->id * 2), gtsam::Vector3(0.0, 0.0, 0.0)); values.insert_or_assign(V(submap->id * 2 + 1), gtsam::Vector3(0.0, 0.0, 0.0)); + corrupted_graph = true; } if (!values.exists(B(submap->id * 2)) || !values.exists(B(submap->id * 2 + 1))) { spdlog::warn("insert missing bias values for submap {}", submap->id); values.insert_or_assign(B(submap->id * 2), gtsam::imuBias::ConstantBias(gtsam::Vector6::Zero())); values.insert_or_assign(B(submap->id * 2 + 1), gtsam::imuBias::ConstantBias(gtsam::Vector6::Zero())); + corrupted_graph = true; } } + if (corrupted_graph) { + spdlog::warn("disable optimization because the loaded graph is corrupted"); + gtsam_ext::ISAM2Params isam2_params; + isam2.reset(new gtsam_ext::ISAM2ExtDummy(isam2_params)); + } + spdlog::info("optimize"); Callbacks::on_smoother_update(*isam2, graph, values); auto result = isam2->update(graph, values); From c011036797cc611f03f5cf14ffcd39eec495781c Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Mon, 5 Feb 2024 10:38:34 +0900 Subject: [PATCH 6/7] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 4c0b5655..a1dbd5b8 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,11 @@ ![GLIM](docs/assets/logo2.png "GLIM Logo") +## Important Notes + +Releases after v0.1.1 (239a75cf2bbc2dcc594dee3d801b005ae04a19b7, Nov, 27th, 2023) can be subject to the change to a non-permissive license. We have not decided anything on the new license model. Note that v0.1.1 and earlier remain on the MIT license. + +## Introduction + **GLIM** is a versatile and extensible range-based 3D mapping framework. - ***Accuracy:*** The backend of GLIM is based on global matching cost minimization that enables to accurately retain the global consistency of a map. Optionally, GPU acceleration can be used to maximize the mapping speed and quality. From dca827585a63ae7c21b837a5125f29294e346203 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Mon, 5 Feb 2024 10:40:08 +0900 Subject: [PATCH 7/7] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a1dbd5b8..2f92aa20 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ Tested on Ubuntu 20.04 with CUDA 11.6 / Ubuntu 22.04 with CUDA 11.8 / NVIDIA Jet ## License -This package is released under the MIT license. For commercial support, please contact ```k.koide@aist.go.jp```. +For commercial support, please contact ```k.koide@aist.go.jp```. ## Contact [Kenji Koide](https://staff.aist.go.jp/k.koide/), k.koide@aist.go.jp