Skip to content

Commit

Permalink
normalize rotation
Browse files Browse the repository at this point in the history
  • Loading branch information
koide3 committed Nov 22, 2024
1 parent e27b4c6 commit 7864abc
Show file tree
Hide file tree
Showing 3 changed files with 8 additions and 3 deletions.
2 changes: 2 additions & 0 deletions src/glim/odometry/initial_state_estimation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
6 changes: 4 additions & 2 deletions src/glim/odometry/odometry_estimation_ct.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand Down
3 changes: 2 additions & 1 deletion src/glim/odometry/odometry_estimation_imu.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 7864abc

Please sign in to comment.