12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ImuTracker uses conjugate. (#693) 

Normalized quaternions can be inverted with conjugate, which is faster.
The motivation for optimizing ImuTracker::Advance is that
per-point unwarping will call this for every point.
master
gaschler 2017-11-23 13:50:44 +01:00 committed by Wally B. Feed
parent a8bd98680e
commit 147c8166b6
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/imu_tracker.cc
View File

@ -42,7 +42,7 @@ void ImuTracker::Advance(const common::Time time) {
transform::AngleAxisVectorToRotationQuaternion( transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t)); Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized(); orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ = rotation.inverse() * gravity_vector_; gravity_vector_ = rotation.conjugate() * gravity_vector_;
time_ = time; time_ = time;
} }
@ -61,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation(
// Change the 'orientation_' so that it agrees with the current // Change the 'orientation_' so that it agrees with the current
// 'gravity_vector_'. // 'gravity_vector_'.
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ()); gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
orientation_ = (orientation_ * rotation).normalized(); orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_ * gravity_vector_).z(), 0.); CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99); CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);