From 89a7296bfa37c3eb893c73a08ba43ad4568ec50d Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Tue, 29 Oct 2019 15:35:42 +0100
Subject: [PATCH] fix gravity initialization for calibration

---
 src/calibration/cam_imu_calib.cpp | 9 +++++----
 thirdparty/basalt-headers         | 2 +-
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp
index 94b5262..cd35822 100644
--- a/src/calibration/cam_imu_calib.cpp
+++ b/src/calibration/cam_imu_calib.cpp
@@ -445,16 +445,17 @@ void CamImuCalib::initOptimization() {
     const auto cp_it = calib_init_poses.find(tcid);
 
     if (cp_it != calib_init_poses.end()) {
-      calib_opt->addPoseMeasurement(
-          timestamp_ns,
-          cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse());
+      Sophus::SE3d T_a_i =
+          cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse();
+
+      calib_opt->addPoseMeasurement(timestamp_ns, T_a_i);
 
       if (!g_initialized) {
         for (size_t i = 0;
              i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
           const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
           if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
-            g_a_init = cp_it->second.T_a_c.so3() * ad.data;
+            g_a_init = T_a_i.so3() * ad.data;
             g_initialized = true;
             std::cout << "g_a initialized with " << g_a_init.transpose()
                       << std::endl;
diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers
index 7847c46..fa1a74d 160000
--- a/thirdparty/basalt-headers
+++ b/thirdparty/basalt-headers
@@ -1 +1 @@
-Subproject commit 7847c4699607eaca370427ae4e2e840c5b502526
+Subproject commit fa1a74dfa8f8400b8e1b260c6f6b4c9ce59ed776