pose_tracker.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_KALMAN_FILTER_POSE_TRACKER_H_
18 #define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
19 
20 #include <deque>
21 #include <memory>
22 
23 #include "Eigen/Cholesky"
24 #include "Eigen/Core"
29 #include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
33 #include "cartographer/sensor/proto/sensor.pb.h"
35 
36 namespace cartographer {
37 namespace kalman_filter {
38 
39 typedef Eigen::Matrix3d Pose2DCovariance;
40 typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
41 
44  PoseCovariance covariance;
45 };
46 
48  const PoseAndCovariance& pose_and_covariance);
49 
50 PoseCovariance BuildPoseCovariance(double translational_variance,
51  double rotational_variance);
52 
53 proto::PoseTrackerOptions CreatePoseTrackerOptions(
54  common::LuaParameterDictionary* parameter_dictionary);
55 
56 // A Kalman filter for a 3D state of position and orientation.
57 // Includes functions to update from IMU and range data matches.
58 class PoseTracker {
59  public:
60  enum {
61  kMapPositionX = 0,
70  kDimension // We terminate loops with this.
71  };
72 
75  using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
77 
78  // Create a new Kalman filter starting at the origin with a standard normal
79  // distribution at 'time'.
80  PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
81  virtual ~PoseTracker();
82 
83  // Sets 'pose' and 'covariance' to the mean and covariance of the belief at
84  // 'time' which must be >= to the current time. Must not be nullptr.
85  void GetPoseEstimateMeanAndCovariance(common::Time time,
87  PoseCovariance* covariance);
88 
89  // Updates from an IMU reading (in the IMU frame).
90  void AddImuLinearAccelerationObservation(
91  common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
92  void AddImuAngularVelocityObservation(
93  common::Time time, const Eigen::Vector3d& imu_angular_velocity);
94 
95  // Updates from a pose estimate in the map frame.
96  void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
97  const PoseCovariance& covariance);
98 
99  // Updates from a pose estimate in the odometer's map-like frame.
100  void AddOdometerPoseObservation(common::Time time,
101  const transform::Rigid3d& pose,
102  const PoseCovariance& covariance);
103 
104  common::Time time() const { return time_; }
105 
106  // Returns the belief at the 'time' which must be >= to the current time.
107  Distribution GetBelief(common::Time time);
108 
109  const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
110 
111  Eigen::Quaterniond gravity_orientation() const {
112  return imu_tracker_.orientation();
113  }
114 
115  private:
116  // Returns the distribution required to initialize the KalmanFilter.
117  static Distribution KalmanFilterInit();
118 
119  // Build a model noise distribution (zero mean) for the given time delta.
120  const Distribution BuildModelNoise(double delta_t) const;
121 
122  // Predict the state forward in time. This is a no-op if 'time' has not moved
123  // forward.
124  void Predict(common::Time time);
125 
126  // Computes a pose combining the given 'state' with the 'imu_tracker_'
127  // orientation.
128  transform::Rigid3d RigidFromState(const PoseTracker::State& state);
129 
130  const proto::PoseTrackerOptions options_;
135 };
136 
137 } // namespace kalman_filter
138 } // namespace cartographer
139 
140 #endif // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
proto::PoseTrackerOptions CreatePoseTrackerOptions(common::LuaParameterDictionary *const parameter_dictionary)
Eigen::Matrix< double, 6, 6 > PoseCovariance
Definition: pose_tracker.h:40
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
Eigen::Matrix3d Pose2DCovariance
Definition: pose_tracker.h:39
PoseCovariance BuildPoseCovariance(const double translational_variance, const double rotational_variance)
Eigen::Matrix< double, kDimension, kDimension > StateCovariance
Definition: pose_tracker.h:75
mapping::OdometryStateTracker odometry_state_tracker_
Definition: pose_tracker.h:134
const proto::PoseTrackerOptions options_
Definition: pose_tracker.h:130
Eigen::Quaterniond gravity_orientation() const
Definition: pose_tracker.h:111
GaussianDistribution< T, N > operator*(const Eigen::Matrix< T, N, M > &lhs, const GaussianDistribution< T, M > &rhs)


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