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, software11 * 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 and14 * limitations under the License.15 */16 17 #include "cartographer/mapping/imu_tracker.h"18 19 #include20 #include 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 the52 // '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 ::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 current62 // '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 mapping76 } // namespace cartographer