00001 /* 00002 * Copyright 2016 The Cartographer Authors 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ 00018 #define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ 00019 00020 #include "Eigen/Geometry" 00021 #include "cartographer/common/time.h" 00022 00023 namespace cartographer { 00024 namespace mapping { 00025 00026 // Keeps track of the orientation using angular velocities and linear 00027 // accelerations from an IMU. Because averaged linear acceleration (assuming 00028 // slow movement) is a direct measurement of gravity, roll/pitch does not drift, 00029 // though yaw does. 00030 class ImuTracker { 00031 public: 00032 ImuTracker(double imu_gravity_time_constant, common::Time time); 00033 00034 // Advances to the given 'time' and updates the orientation to reflect this. 00035 void Advance(common::Time time); 00036 00037 // Updates from an IMU reading (in the IMU frame). 00038 void AddImuLinearAccelerationObservation( 00039 const Eigen::Vector3d& imu_linear_acceleration); 00040 void AddImuAngularVelocityObservation( 00041 const Eigen::Vector3d& imu_angular_velocity); 00042 00043 // Query the current time. 00044 common::Time time() const { return time_; } 00045 00046 // Query the current orientation estimate. 00047 Eigen::Quaterniond orientation() const { return orientation_; } 00048 00049 private: 00050 const double imu_gravity_time_constant_; 00051 common::Time time_; 00052 common::Time last_linear_acceleration_time_; 00053 Eigen::Quaterniond orientation_; 00054 Eigen::Vector3d gravity_vector_; 00055 Eigen::Vector3d imu_angular_velocity_; 00056 }; 00057 00058 } // namespace mapping 00059 } // namespace cartographer 00060 00061 #endif // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_