normalize rotation (#118)

koide3 · Nov 22, 2024 · eb805d2 · eb805d2
1 parent e27b4c6
commit eb805d2
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git 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
@@ -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
@@ -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);