From eb805d257371ad613db3eb388e721f8bed137557 Mon Sep 17 00:00:00 2001
From: koide3 <31344317+koide3@users.noreply.github.com>
Date: Fri, 22 Nov 2024 22:13:52 +0900
Subject: [PATCH] normalize rotation (#118)

---
 src/glim/odometry/initial_state_estimation.cpp | 2 ++
 src/glim/odometry/odometry_estimation_ct.cpp   | 6 ++++--
 src/glim/odometry/odometry_estimation_imu.cpp  | 3 ++-
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/glim/odometry/initial_state_estimation.cpp b/src/glim/odometry/initial_state_estimation.cpp
index ca328170..5524d14f 100644
--- a/src/glim/odometry/initial_state_estimation.cpp
+++ b/src/glim/odometry/initial_state_estimation.cpp
@@ -29,6 +29,8 @@ void NaiveInitialStateEstimation ::set_init_state(const Eigen::Isometry3d& init_
   force_init = true;
   this->init_T_world_imu = init_T_world_imu;
   this->init_v_world_imu = init_v_world_imu;
+
+  this->init_T_world_imu.linear() = Eigen::Quaterniond(this->init_T_world_imu.linear()).normalized().toRotationMatrix();
 }
 
 void NaiveInitialStateEstimation::insert_imu(double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel) {
diff --git a/src/glim/odometry/odometry_estimation_ct.cpp b/src/glim/odometry/odometry_estimation_ct.cpp
index 4c468cbe..d94ec449 100644
--- a/src/glim/odometry/odometry_estimation_ct.cpp
+++ b/src/glim/odometry/odometry_estimation_ct.cpp
@@ -133,8 +133,10 @@ EstimationFrame::ConstPtr OdometryEstimationCT::insert_frame(const PreprocessedF
     const auto& last_frame = frames[last];
     const double last_time_begin = last_frame->stamp + last_frame->frame->times[0];
     const double last_time_end = last_frame->stamp + last_frame->frame->times[last_frame->frame->size() - 1];
-    const gtsam::Pose3 last_T_world_lidar_begin = smoother->calculateEstimate<gtsam::Pose3>(X(last));
-    const gtsam::Pose3 last_T_world_lidar_end = smoother->calculateEstimate<gtsam::Pose3>(Y(last));
+    const auto last_T_world_lidar_begin_ = smoother->calculateEstimate<gtsam::Pose3>(X(last));
+    const auto last_T_world_lidar_end_ = smoother->calculateEstimate<gtsam::Pose3>(Y(last));
+    const auto last_T_world_lidar_begin = gtsam::Pose3(last_T_world_lidar_begin_.rotation().normalized(), last_T_world_lidar_begin_.translation());
+    const auto last_T_world_lidar_end = gtsam::Pose3(last_T_world_lidar_end_.rotation().normalized(), last_T_world_lidar_end_.translation());
 
     // Initial guess
     const double current_time_begin = new_frame->stamp + new_frame->frame->times[0];
diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp
index 783648ac..39660eb7 100644
--- a/src/glim/odometry/odometry_estimation_imu.cpp
+++ b/src/glim/odometry/odometry_estimation_imu.cpp
@@ -217,7 +217,8 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed
   gtsam::FixedLagSmootherKeyTimestampMap new_stamps;
 
   const double last_stamp = frames[last]->stamp;
-  const auto last_T_world_imu = smoother->calculateEstimate<gtsam::Pose3>(X(last));
+  const auto last_T_world_imu_ = smoother->calculateEstimate<gtsam::Pose3>(X(last));
+  const auto last_T_world_imu = gtsam::Pose3(last_T_world_imu_.rotation().normalized(), last_T_world_imu_.translation());
   const auto last_v_world_imu = smoother->calculateEstimate<gtsam::Vector3>(V(last));
   const auto last_imu_bias = smoother->calculateEstimate<gtsam::imuBias::ConstantBias>(B(last));
   const gtsam::NavState last_nav_world_imu(last_T_world_imu, last_v_world_imu);