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