IMU的输入为imu_linear_acceleration 和 imu_angular_velocity 线加速和角速度。最终作为属性输出的是方位四元数。
Eigen::Quaterniond orientation() const { return orientation_; }
1 /* 2 * Copyright 2016 The Cartographer Authors 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "cartographer/mapping/imu_tracker.h" 18 19 #include <cmath> 20 #include <limits> 21 22 #include "cartographer/common/math.h" 23 #include "cartographer/transform/transform.h" 24 #include "glog/logging.h" 25 26 namespace cartographer { 27 namespace mapping { 28 29 ImuTracker::ImuTracker(const double imu_gravity_time_constant, 30 const common::Time time) 31 : imu_gravity_time_constant_(imu_gravity_time_constant), 32 time_(time), 33 last_linear_acceleration_time_(common::Time::min()), 34 orientation_(Eigen::Quaterniond::Identity()), 35 gravity_vector_(Eigen::Vector3d::UnitZ()), 36 imu_angular_velocity_(Eigen::Vector3d::Zero()) {} 37 38 void ImuTracker::Advance(const common::Time time) { 39 CHECK_LE(time_, time); 40 const double delta_t = common::ToSeconds(time - time_); 41 const Eigen::Quaterniond rotation = 42 transform::AngleAxisVectorToRotationQuaternion( 43 Eigen::Vector3d(imu_angular_velocity_ * delta_t)); 44 orientation_ = (orientation_ * rotation).normalized(); 45 gravity_vector_ = rotation.conjugate() * gravity_vector_; 46 time_ = time; 47 } 48 49 void ImuTracker::AddImuLinearAccelerationObservation( 50 const Eigen::Vector3d& imu_linear_acceleration) { 51 // Update the 'gravity_vector_' with an exponential moving average using the 52 // 'imu_gravity_time_constant'. 53 const double delta_t = 54 last_linear_acceleration_time_ > common::Time::min() 55 ? common::ToSeconds(time_ - last_linear_acceleration_time_) 56 : std::numeric_limits<double>::infinity(); 57 last_linear_acceleration_time_ = time_; 58 const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_); 59 gravity_vector_ = 60 (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; 61 // Change the 'orientation_' so that it agrees with the current 62 // 'gravity_vector_'. 63 const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( 64 gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); 65 orientation_ = (orientation_ * rotation).normalized(); 66 CHECK_GT((orientation_ * gravity_vector_).z(), 0.); 67 CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99); 68 } 69 70 void ImuTracker::AddImuAngularVelocityObservation( 71 const Eigen::Vector3d& imu_angular_velocity) { 72 imu_angular_velocity_ = imu_angular_velocity; 73 } 74 75 } // namespace mapping 76 } // namespace cartographer