Merge branch 'master' of github.com:koide3/glim

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.
@@ -34,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 ```[email protected]```.
+For commercial support, please contact ```[email protected]```.
 
 ## Contact
 [Kenji Koide](https://staff.aist.go.jp/k.koide/), [email protected]  

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

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

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

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

include/glim/util/concurrent_vector.hpp

@@ -134,6 +134,10 @@ class ConcurrentVector {
   std::vector<T, Alloc> get_and_clear(int num_max) {
     std::vector<T, Alloc> buffer;
     std::lock_guard<std::mutex> lock(mutex);
+    if (values.empty()) {
+      return buffer;
+    }
+
     if (values.size() <= num_max) {
       buffer.assign(values.begin(), values.end());
       values.clear();

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;
     }
 

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());
   }
 
@@ -597,13 +603,27 @@ 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");
-  gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph);
+  try {
+    gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph);
+  } catch (std::exception& e) {
+    spdlog::warn("failed to deserialize graph!!");
+    spdlog::warn(e.what());
+    corrupted_graph = 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());
+    corrupted_graph = 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,42 @@ 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()));
+      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);