From 94acf99d8954d22a7426fa8e018a52aa4e76697e Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Wed, 30 Oct 2024 12:16:09 +0900
Subject: [PATCH] check X0 prior

---
 src/glim/mapping/global_mapping.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/glim/mapping/global_mapping.cpp b/src/glim/mapping/global_mapping.cpp
index 5672223a..96b91d22 100644
--- a/src/glim/mapping/global_mapping.cpp
+++ b/src/glim/mapping/global_mapping.cpp
@@ -814,6 +814,7 @@ std::pair<gtsam::NonlinearFactorGraph, gtsam::Values> GlobalMapping::recover_gra
   logger->info("enable_imu={}", enable_imu);
 
   logger->info("creating connectivity map");
+  bool prior_exists = false;
   std::unordered_map<gtsam::Key, std::set<gtsam::Key>> connectivity_map;
   for (const auto& factor : graph) {
     if (!factor) {
@@ -825,6 +826,15 @@ std::pair<gtsam::NonlinearFactorGraph, gtsam::Values> GlobalMapping::recover_gra
         connectivity_map[key].insert(key2);
       }
     }
+
+    if (factor->keys().size() == 1 && factor->keys()[0] == X(0)) {
+      prior_exists |= boost::dynamic_pointer_cast<gtsam_points::LinearDampingFactor>(factor) != nullptr;
+    }
+  }
+
+  if (!prior_exists) {
+    logger->warn("X0 prior is missing");
+    new_factors->emplace_shared<gtsam_points::LinearDampingFactor>(X(0), 6, params.init_pose_damping_scale);
   }
 
   logger->info("fixing missing values and factors");