00001 /* 00002 * Copyright 2017 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_POSE_EXTRAPOLATOR_H_ 00018 #define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_ 00019 00020 #include <deque> 00021 #include <memory> 00022 00023 #include "cartographer/common/time.h" 00024 #include "cartographer/mapping/imu_tracker.h" 00025 #include "cartographer/sensor/imu_data.h" 00026 #include "cartographer/sensor/odometry_data.h" 00027 #include "cartographer/transform/rigid_transform.h" 00028 00029 namespace cartographer { 00030 namespace mapping { 00031 00032 // Keep poses for a certain duration to estimate linear and angular velocity. 00033 // Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if 00034 // available to improve the extrapolation. 00035 class PoseExtrapolator { 00036 public: 00037 explicit PoseExtrapolator(common::Duration pose_queue_duration, 00038 double imu_gravity_time_constant); 00039 00040 PoseExtrapolator(const PoseExtrapolator&) = delete; 00041 PoseExtrapolator& operator=(const PoseExtrapolator&) = delete; 00042 00043 static std::unique_ptr<PoseExtrapolator> InitializeWithImu( 00044 common::Duration pose_queue_duration, double imu_gravity_time_constant, 00045 const sensor::ImuData& imu_data); 00046 00047 // Returns the time of the last added pose or Time::min() if no pose was added 00048 // yet. 00049 common::Time GetLastPoseTime() const; 00050 common::Time GetLastExtrapolatedTime() const; 00051 00052 void AddPose(common::Time time, const transform::Rigid3d& pose); 00053 void AddImuData(const sensor::ImuData& imu_data); 00054 void AddOdometryData(const sensor::OdometryData& odometry_data); 00055 transform::Rigid3d ExtrapolatePose(common::Time time); 00056 00057 // Returns the current gravity alignment estimate as a rotation from 00058 // the tracking frame into a gravity aligned frame. 00059 Eigen::Quaterniond EstimateGravityOrientation(common::Time time); 00060 00061 private: 00062 void UpdateVelocitiesFromPoses(); 00063 void TrimImuData(); 00064 void TrimOdometryData(); 00065 void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const; 00066 Eigen::Quaterniond ExtrapolateRotation(common::Time time, 00067 ImuTracker* imu_tracker) const; 00068 Eigen::Vector3d ExtrapolateTranslation(common::Time time); 00069 00070 const common::Duration pose_queue_duration_; 00071 struct TimedPose { 00072 common::Time time; 00073 transform::Rigid3d pose; 00074 }; 00075 std::deque<TimedPose> timed_pose_queue_; 00076 Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero(); 00077 Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero(); 00078 00079 const double gravity_time_constant_; 00080 std::deque<sensor::ImuData> imu_data_; 00081 std::unique_ptr<ImuTracker> imu_tracker_; 00082 std::unique_ptr<ImuTracker> odometry_imu_tracker_; 00083 std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; 00084 TimedPose cached_extrapolated_pose_; 00085 00086 std::deque<sensor::OdometryData> odometry_data_; 00087 Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); 00088 Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero(); 00089 }; 00090 00091 } // namespace mapping 00092 } // namespace cartographer 00093 00094 #endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_