imu_integration.h
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 
17 #ifndef CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_
18 #define CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_
19 
20 #include <algorithm>
21 #include <deque>
22 
23 #include "Eigen/Core"
24 #include "Eigen/Geometry"
27 #include "glog/logging.h"
28 
29 namespace cartographer {
30 namespace mapping_3d {
31 
32 struct ImuData {
34  Eigen::Vector3d linear_acceleration;
35  Eigen::Vector3d angular_velocity;
36 };
37 
38 template <typename T>
40  Eigen::Matrix<T, 3, 1> delta_velocity;
41  Eigen::Quaternion<T> delta_rotation;
42 };
43 
44 // Returns velocity delta in map frame.
46  const std::deque<ImuData>& imu_data, const common::Time start_time,
47  const common::Time end_time, std::deque<ImuData>::const_iterator* it);
48 
49 template <typename T>
51  const std::deque<ImuData>& imu_data,
52  const Eigen::Transform<T, 3, Eigen::Affine>&
53  linear_acceleration_calibration,
54  const Eigen::Transform<T, 3, Eigen::Affine>& angular_velocity_calibration,
55  const common::Time start_time, const common::Time end_time,
56  std::deque<ImuData>::const_iterator* it) {
57  CHECK_LE(start_time, end_time);
58  CHECK(*it != imu_data.cend());
59  CHECK_LE((*it)->time, start_time);
60  if ((*it) + 1 != imu_data.cend()) {
61  CHECK_GT(((*it) + 1)->time, start_time);
62  }
63 
64  common::Time current_time = start_time;
65 
66  IntegrateImuResult<T> result = {Eigen::Matrix<T, 3, 1>::Zero(),
67  Eigen::Quaterniond::Identity().cast<T>()};
68  while (current_time < end_time) {
69  common::Time next_imu_data = common::Time::max();
70  if ((*it) + 1 != imu_data.cend()) {
71  next_imu_data = ((*it) + 1)->time;
72  }
73  common::Time next_time = std::min(next_imu_data, end_time);
74  const T delta_t(common::ToSeconds(next_time - current_time));
75 
76  const Eigen::Matrix<T, 3, 1> delta_angle =
77  (angular_velocity_calibration * (*it)->angular_velocity.cast<T>()) *
78  delta_t;
79  result.delta_rotation *=
81  result.delta_velocity +=
82  result.delta_rotation * ((linear_acceleration_calibration *
83  (*it)->linear_acceleration.cast<T>()) *
84  delta_t);
85  current_time = next_time;
86  if (current_time == next_imu_data) {
87  ++(*it);
88  }
89  }
90  return result;
91 }
92 
93 } // namespace mapping_3d
94 } // namespace cartographer
95 
96 #endif // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_
Eigen::Quaternion< T > AngleAxisVectorToRotationQuaternion(const Eigen::Matrix< T, 3, 1 > &angle_axis)
Definition: transform.h:86
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
double ToSeconds(const Duration duration)
Definition: time.cc:29
IntegrateImuResult< double > IntegrateImu(const std::deque< ImuData > &imu_data, const common::Time start_time, const common::Time end_time, std::deque< ImuData >::const_iterator *it)


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