12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move the ImuTracker and OdometryStateTracker to mapping. (#50) 

Both are useful without a UKF.
master
Wolfgang Hess 2016-10-13 10:51:12 +02:00 committed by GitHub
parent 8f64860b5d
commit cc7cc6f72b
8 changed files with 181 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cartographer/kalman_filter/CMakeLists.txt
View File

@ -20,16 +20,6 @@ google_library(kalman_filter_gaussian_distribution
gaussian_distribution.h
)
google_library(kalman_filter_odometry_state_tracker
SRCS
odometry_state_tracker.cc
HDRS
odometry_state_tracker.h
DEPENDS
common_time
transform_rigid_transform
)
google_library(kalman_filter_pose_tracker
USES_EIGEN
USES_GLOG
@ -43,9 +33,10 @@ google_library(kalman_filter_pose_tracker
common_port
common_time
kalman_filter_gaussian_distribution
kalman_filter_odometry_state_tracker
kalman_filter_proto_pose_tracker_options
kalman_filter_unscented_kalman_filter
mapping_imu_tracker
mapping_odometry_state_tracker
sensor_proto_sensor
transform_transform
)

65
cartographer/kalman_filter/pose_tracker.cc
View File

@ -123,56 +123,6 @@ PoseTracker::State ModelFunction2D(const PoseTracker::State& state,
} // namespace
ImuTracker::ImuTracker(const proto::PoseTrackerOptions& options,
const common::Time time)
: options_(options),
time_(time),
last_linear_acceleration_time_(common::Time::min()),
orientation_(Eigen::Quaterniond::Identity()),
gravity_direction_(Eigen::Vector3d::UnitZ()),
imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
void ImuTracker::Predict(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_direction_ = rotation.inverse() * gravity_direction_;
time_ = time;
}
void ImuTracker::AddImuLinearAccelerationObservation(
const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
Predict(time);
// Update the 'gravity_direction_' with an exponential moving average using
// the 'imu_gravity_time_constant'.
const double delta_t =
last_linear_acceleration_time_ > common::Time::min()
? common::ToSeconds(time - last_linear_acceleration_time_)
: std::numeric_limits<double>::infinity();
last_linear_acceleration_time_ = time;
const double alpha =
1. - std::exp(-delta_t / options_.imu_gravity_time_constant());
gravity_direction_ =
(1. - alpha) * gravity_direction_ + alpha * imu_linear_acceleration;
// Change the 'orientation_' so that it agrees with the current
// 'gravity_direction_'.
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
gravity_direction_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_.inverse() * Eigen::Vector3d::UnitZ())
.dot(gravity_direction_),
0.);
}
void ImuTracker::AddImuAngularVelocityObservation(
const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
Predict(time);
imu_angular_velocity_ = imu_angular_velocity;
}
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance) {
GaussianDistribution<double, 6> distribution(
@ -219,7 +169,7 @@ PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
model_function_(model_function),
time_(time),
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
imu_tracker_(options, time),
imu_tracker_(options.imu_gravity_time_constant(), time),
odometry_state_tracker_(options.num_odometry_states()) {}
PoseTracker::~PoseTracker() {}
@ -270,7 +220,7 @@ const PoseTracker::Distribution PoseTracker::BuildModelNoise(
}
void PoseTracker::Predict(const common::Time time) {
imu_tracker_.Predict(time);
imu_tracker_.Advance(time);
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
if (delta_t == 0.) {
@ -293,14 +243,15 @@ void PoseTracker::Predict(const common::Time time) {
void PoseTracker::AddImuLinearAccelerationObservation(
const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) {
imu_tracker_.AddImuLinearAccelerationObservation(time,
imu_linear_acceleration);
imu_tracker_.Advance(time);
imu_tracker_.AddImuLinearAccelerationObservation(imu_linear_acceleration);
Predict(time);
}
void PoseTracker::AddImuAngularVelocityObservation(
const common::Time time, const Eigen::Vector3d& imu_angular_velocity) {
imu_tracker_.AddImuAngularVelocityObservation(time, imu_angular_velocity);
imu_tracker_.Advance(time);
imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity);
Predict(time);
}
@ -349,8 +300,8 @@ void PoseTracker::AddOdometerPoseObservation(
{time, odometer_pose, RigidFromState(belief.GetMean())});
}
const OdometryStateTracker::OdometryStates& PoseTracker::odometry_states()
const {
const mapping::OdometryStateTracker::OdometryStates&
PoseTracker::odometry_states() const {
return odometry_state_tracker_.odometry_states();
}

38
cartographer/kalman_filter/pose_tracker.h
View File

@ -26,9 +26,10 @@
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "cartographer/kalman_filter/odometry_state_tracker.h"
#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/odometry_state_tracker.h"
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/transform.h"
@ -60,35 +61,6 @@ PoseCovariance PoseCovarianceFromProtoMatrix(
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* parameter_dictionary);
// Keeps track of the orientation using angular velocities and linear
// accelerations from an IMU. Because averaged linear acceleration (assuming
// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
// though yaw does.
class ImuTracker {
public:
ImuTracker(const proto::PoseTrackerOptions& options, common::Time time);
// Updates the orientation to reflect the given 'time'.
void Predict(common::Time time);
// Updates from an IMU reading (in the IMU frame).
void AddImuLinearAccelerationObservation(
common::Time time, const Eigen::Vector3d& imu_linear_acceleration);
void AddImuAngularVelocityObservation(
common::Time time, const Eigen::Vector3d& imu_angular_velocity);
// Query the current orientation estimate.
Eigen::Quaterniond orientation() { return orientation_; }
private:
const proto::PoseTrackerOptions options_;
common::Time time_;
common::Time last_linear_acceleration_time_;
Eigen::Quaterniond orientation_;
Eigen::Vector3d gravity_direction_;
Eigen::Vector3d imu_angular_velocity_;
};
// A Kalman filter for a 3D state of position and orientation.
// Includes functions to update from IMU and laser scan matches.
class PoseTracker {
@ -145,7 +117,7 @@ class PoseTracker {
// Returns the belief at the 'time' which must be >= to the current time.
Distribution GetBelief(common::Time time);
const OdometryStateTracker::OdometryStates& odometry_states() const;
const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
private:
// Returns the distribution required to initialize the KalmanFilter.
@ -166,8 +138,8 @@ class PoseTracker {
const ModelFunction model_function_;
common::Time time_;
KalmanFilter kalman_filter_;
ImuTracker imu_tracker_;
OdometryStateTracker odometry_state_tracker_;
mapping::ImuTracker imu_tracker_;
mapping::OdometryStateTracker odometry_state_tracker_;
};
} // namespace kalman_filter

23
cartographer/mapping/CMakeLists.txt
View File

@ -28,6 +28,19 @@ google_library(mapping_global_trajectory_builder_interface
sensor_point_cloud
)
google_library(mapping_imu_tracker
USES_EIGEN
USES_GLOG
SRCS
imu_tracker.cc
HDRS
imu_tracker.h
DEPENDS
common_math
common_time
transform_transform
)
google_library(mapping_map_builder
USES_EIGEN
USES_GLOG
@ -57,6 +70,16 @@ google_library(mapping_map_builder
transform_transform
)
google_library(mapping_odometry_state_tracker
SRCS
odometry_state_tracker.cc
HDRS
odometry_state_tracker.h
DEPENDS
common_time
transform_rigid_transform
)
google_library(mapping_probability_values
USES_GLOG
SRCS

76
cartographer/mapping/imu_tracker.cc Normal file
View File

@ -0,0 +1,76 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/imu_tracker.h"
#include <cmath>
#include <limits>
#include "cartographer/common/math.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
ImuTracker::ImuTracker(const double imu_gravity_time_constant,
const common::Time time)
: imu_gravity_time_constant_(imu_gravity_time_constant),
time_(time),
last_linear_acceleration_time_(common::Time::min()),
orientation_(Eigen::Quaterniond::Identity()),
gravity_vector_(Eigen::Vector3d::UnitZ()),
imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
void ImuTracker::Advance(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ = rotation.inverse() * gravity_vector_;
time_ = time;
}
void ImuTracker::AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration) {
// Update the 'gravity_vector_' with an exponential moving average using the
// 'imu_gravity_time_constant'.
const double delta_t =
last_linear_acceleration_time_ > common::Time::min()
? common::ToSeconds(time_ - last_linear_acceleration_time_)
: std::numeric_limits<double>::infinity();
last_linear_acceleration_time_ = time_;
const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
gravity_vector_ =
(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
// Change the 'orientation_' so that it agrees with the current
// 'gravity_vector_'.
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}
void ImuTracker::AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity) {
imu_angular_velocity_ = imu_angular_velocity;
}
} // namespace mapping
} // namespace cartographer

58
cartographer/mapping/imu_tracker.h Normal file
View File

@ -0,0 +1,58 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_
#define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_
#include "Eigen/Geometry"
#include "cartographer/common/time.h"
namespace cartographer {
namespace mapping {
// Keeps track of the orientation using angular velocities and linear
// accelerations from an IMU. Because averaged linear acceleration (assuming
// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
// though yaw does.
class ImuTracker {
public:
ImuTracker(double imu_gravity_time_constant, common::Time time);
// Advances to the given 'time' and updates the orientation to reflect this.
void Advance(common::Time time);
// Updates from an IMU reading (in the IMU frame).
void AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration);
void AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity);
// Query the current orientation estimate.
Eigen::Quaterniond orientation() { return orientation_; }
private:
const double imu_gravity_time_constant_;
common::Time time_;
common::Time last_linear_acceleration_time_;
Eigen::Quaterniond orientation_;
Eigen::Vector3d gravity_vector_;
Eigen::Vector3d imu_angular_velocity_;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_

6
cartographer/kalman_filter/odometry_state_tracker.cc → cartographer/mapping/odometry_state_tracker.cc
View File

@ -14,12 +14,12 @@
* limitations under the License.
*/
#include "cartographer/kalman_filter/odometry_state_tracker.h"
#include "cartographer/mapping/odometry_state_tracker.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace kalman_filter {
namespace mapping {
OdometryState::OdometryState(const common::Time time,
const transform::Rigid3d& odometer_pose,
@ -50,5 +50,5 @@ const OdometryState& OdometryStateTracker::newest() const {
return odometry_states_.back();
}
} // namespace kalman_filter
} // namespace mapping
} // namespace cartographer

12
cartographer/kalman_filter/odometry_state_tracker.h → cartographer/mapping/odometry_state_tracker.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
#define CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
#ifndef CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_
#define CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_
#include <deque>
@ -23,7 +23,7 @@
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace kalman_filter {
namespace mapping {
struct OdometryState {
OdometryState(common::Time time, const transform::Rigid3d& odometer_pose,
@ -52,7 +52,7 @@ class OdometryStateTracker {
// Returns true if no elements are present in the odometry queue.
bool empty() const;
// Retrieves the most recent OdometryState or an empty one if non yet present.
// Retrieves the most recent OdometryState. Must not be called when empty.
const OdometryState& newest() const;
private:
@ -60,7 +60,7 @@ class OdometryStateTracker {
size_t window_size_;
};
} // namespace kalman_filter
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_
#endif // CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_