diff --git a/cartographer/kalman_filter/CMakeLists.txt b/cartographer/kalman_filter/CMakeLists.txt index 7ae17c4..546b8fd 100644 --- a/cartographer/kalman_filter/CMakeLists.txt +++ b/cartographer/kalman_filter/CMakeLists.txt @@ -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 ) diff --git a/cartographer/kalman_filter/pose_tracker.cc b/cartographer/kalman_filter/pose_tracker.cc index 36ff048..40381db 100644 --- a/cartographer/kalman_filter/pose_tracker.cc +++ b/cartographer/kalman_filter/pose_tracker.cc @@ -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::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 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(); } diff --git a/cartographer/kalman_filter/pose_tracker.h b/cartographer/kalman_filter/pose_tracker.h index 19871e4..9fc958e 100644 --- a/cartographer/kalman_filter/pose_tracker.h +++ b/cartographer/kalman_filter/pose_tracker.h @@ -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 diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index 6989025..78a7688 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/cartographer/mapping/CMakeLists.txt @@ -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 diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc new file mode 100644 index 0000000..a6df73e --- /dev/null +++ b/cartographer/mapping/imu_tracker.cc @@ -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 +#include + +#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::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 diff --git a/cartographer/mapping/imu_tracker.h b/cartographer/mapping/imu_tracker.h new file mode 100644 index 0000000..2e0aa6a --- /dev/null +++ b/cartographer/mapping/imu_tracker.h @@ -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_ diff --git a/cartographer/kalman_filter/odometry_state_tracker.cc b/cartographer/mapping/odometry_state_tracker.cc similarity index 93% rename from cartographer/kalman_filter/odometry_state_tracker.cc rename to cartographer/mapping/odometry_state_tracker.cc index 2ffc557..a1ee4cf 100644 --- a/cartographer/kalman_filter/odometry_state_tracker.cc +++ b/cartographer/mapping/odometry_state_tracker.cc @@ -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 diff --git a/cartographer/kalman_filter/odometry_state_tracker.h b/cartographer/mapping/odometry_state_tracker.h similarity index 84% rename from cartographer/kalman_filter/odometry_state_tracker.h rename to cartographer/mapping/odometry_state_tracker.h index f02ca5a..5634692 100644 --- a/cartographer/kalman_filter/odometry_state_tracker.h +++ b/cartographer/mapping/odometry_state_tracker.h @@ -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 @@ -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_