imu_tracker.cc
Go to the documentation of this file.
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 
18 
19 #include <cmath>
20 #include <limits>
21 
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 
39  CHECK_LE(time_, time);
40  const double delta_t = common::ToSeconds(time - time_);
41  const Eigen::Quaterniond rotation =
43  Eigen::Vector3d(imu_angular_velocity_ * delta_t));
44  orientation_ = (orientation_ * rotation).normalized();
45  gravity_vector_ = rotation.inverse() * gravity_vector_;
46  time_ = time;
47 }
48 
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()
56  : std::numeric_limits<double>::infinity();
58  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
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_.inverse() * 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 
71  const Eigen::Vector3d& imu_angular_velocity) {
72  imu_angular_velocity_ = imu_angular_velocity;
73 }
74 
75 } // namespace mapping
76 } // namespace cartographer
Eigen::Vector3d imu_angular_velocity_
Definition: imu_tracker.h:52
Eigen::Quaternion< T > AngleAxisVectorToRotationQuaternion(const Eigen::Matrix< T, 3, 1 > &angle_axis)
Definition: transform.h:86
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
common::Time last_linear_acceleration_time_
Definition: imu_tracker.h:49
ImuTracker(double imu_gravity_time_constant, common::Time time)
Definition: imu_tracker.cc:29
void AddImuLinearAccelerationObservation(const Eigen::Vector3d &imu_linear_acceleration)
Definition: imu_tracker.cc:49
void AddImuAngularVelocityObservation(const Eigen::Vector3d &imu_angular_velocity)
Definition: imu_tracker.cc:70
double ToSeconds(const Duration duration)
Definition: time.cc:29
Eigen::Quaterniond orientation_
Definition: imu_tracker.h:50
void Advance(common::Time time)
Definition: imu_tracker.cc:38


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58