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

Remove UKF related code and configuration. (#378)

master
Wolfgang Hess 2017-06-30 13:19:13 +02:00 committed by Damon Kohler
parent 17a22edebc
commit 54bd81a78b
23 changed files with 107 additions and 1387 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
cartographer/kalman_filter/gaussian_distribution.h
View File

@ -1,60 +0,0 @@
/*
* 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_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
#define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace cartographer {
namespace kalman_filter {
template <typename T, int N>
class GaussianDistribution {
public:
GaussianDistribution(const Eigen::Matrix<T, N, 1>& mean,
const Eigen::Matrix<T, N, N>& covariance)
: mean_(mean), covariance_(covariance) {}
const Eigen::Matrix<T, N, 1>& GetMean() const { return mean_; }
const Eigen::Matrix<T, N, N>& GetCovariance() const { return covariance_; }
private:
Eigen::Matrix<T, N, 1> mean_;
Eigen::Matrix<T, N, N> covariance_;
};
template <typename T, int N>
GaussianDistribution<T, N> operator+(const GaussianDistribution<T, N>& lhs,
const GaussianDistribution<T, N>& rhs) {
return GaussianDistribution<T, N>(lhs.GetMean() + rhs.GetMean(),
lhs.GetCovariance() + rhs.GetCovariance());
}
template <typename T, int N, int M>
GaussianDistribution<T, N> operator*(const Eigen::Matrix<T, N, M>& lhs,
const GaussianDistribution<T, M>& rhs) {
return GaussianDistribution<T, N>(
lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose());
}
} // namespace kalman_filter
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_

37
cartographer/kalman_filter/gaussian_distribution_test.cc
View File

@ -1,37 +0,0 @@
/*
* 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/kalman_filter/gaussian_distribution.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace kalman_filter {
namespace {
TEST(GaussianDistributionTest, testConstructor) {
Eigen::Matrix2d covariance;
covariance << 1., 2., 3., 4.;
GaussianDistribution<double, 2> distribution(Eigen::Vector2d(0., 1.),
covariance);
EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);
EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9);
EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);
}
} // namespace
} // namespace kalman_filter
} // namespace cartographer

291
cartographer/kalman_filter/pose_tracker.cc
View File

@ -1,291 +0,0 @@
/*
* 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/kalman_filter/pose_tracker.h"
#include <cmath>
#include <limits>
#include <utility>
#include "Eigen/Geometry"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "cartographer/kalman_filter/unscented_kalman_filter.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
namespace cartographer {
namespace kalman_filter {
namespace {
PoseTracker::State AddDelta(const PoseTracker::State& state,
const PoseTracker::State& delta) {
PoseTracker::State new_state = state + delta;
const Eigen::Quaterniond orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
state[PoseTracker::kMapOrientationY],
state[PoseTracker::kMapOrientationZ]));
const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX],
delta[PoseTracker::kMapOrientationY],
delta[PoseTracker::kMapOrientationZ]);
CHECK_LT(rotation_vector.norm(), M_PI / 2.)
<< "Sigma point is far from the mean, recovered delta may be incorrect.";
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(rotation_vector);
const Eigen::Vector3d new_orientation =
transform::RotationQuaternionToAngleAxisVector(orientation * rotation);
new_state[PoseTracker::kMapOrientationX] = new_orientation.x();
new_state[PoseTracker::kMapOrientationY] = new_orientation.y();
new_state[PoseTracker::kMapOrientationZ] = new_orientation.z();
return new_state;
}
PoseTracker::State ComputeDelta(const PoseTracker::State& origin,
const PoseTracker::State& target) {
PoseTracker::State delta = target - origin;
const Eigen::Quaterniond origin_orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(origin[PoseTracker::kMapOrientationX],
origin[PoseTracker::kMapOrientationY],
origin[PoseTracker::kMapOrientationZ]));
const Eigen::Quaterniond target_orientation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(target[PoseTracker::kMapOrientationX],
target[PoseTracker::kMapOrientationY],
target[PoseTracker::kMapOrientationZ]));
const Eigen::Vector3d rotation =
transform::RotationQuaternionToAngleAxisVector(
origin_orientation.inverse() * target_orientation);
delta[PoseTracker::kMapOrientationX] = rotation.x();
delta[PoseTracker::kMapOrientationY] = rotation.y();
delta[PoseTracker::kMapOrientationZ] = rotation.z();
return delta;
}
// Build a model matrix for the given time delta.
PoseTracker::State ModelFunction(const PoseTracker::State& state,
const double delta_t) {
CHECK_GT(delta_t, 0.);
PoseTracker::State new_state;
new_state[PoseTracker::kMapPositionX] =
state[PoseTracker::kMapPositionX] +
delta_t * state[PoseTracker::kMapVelocityX];
new_state[PoseTracker::kMapPositionY] =
state[PoseTracker::kMapPositionY] +
delta_t * state[PoseTracker::kMapVelocityY];
new_state[PoseTracker::kMapPositionZ] =
state[PoseTracker::kMapPositionZ] +
delta_t * state[PoseTracker::kMapVelocityZ];
new_state[PoseTracker::kMapOrientationX] =
state[PoseTracker::kMapOrientationX];
new_state[PoseTracker::kMapOrientationY] =
state[PoseTracker::kMapOrientationY];
new_state[PoseTracker::kMapOrientationZ] =
state[PoseTracker::kMapOrientationZ];
new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];
new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY];
new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ];
return new_state;
}
} // namespace
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::PoseTrackerOptions options;
options.set_position_model_variance(
parameter_dictionary->GetDouble("position_model_variance"));
options.set_orientation_model_variance(
parameter_dictionary->GetDouble("orientation_model_variance"));
options.set_velocity_model_variance(
parameter_dictionary->GetDouble("velocity_model_variance"));
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
return options;
}
PoseTracker::Distribution PoseTracker::KalmanFilterInit() {
State initial_state = State::Zero();
// We are certain about the complete state at the beginning. We define the
// initial pose to be at the origin and axis aligned. Additionally, we claim
// that we are not moving.
StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity();
return Distribution(initial_state, initial_covariance);
}
PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options,
const common::Time time)
: options_(options),
time_(time),
kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta),
imu_tracker_(options.imu_gravity_time_constant(), time),
odometry_state_tracker_(options.num_odometry_states()) {}
PoseTracker::~PoseTracker() {}
PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
Predict(time);
return kalman_filter_.GetBelief();
}
transform::Rigid3d PoseTracker::GetPoseEstimateMean(const common::Time time) {
return RigidFromState(GetBelief(time).GetMean());
}
const PoseTracker::Distribution PoseTracker::BuildModelNoise(
const double delta_t) const {
// Position is constant, but orientation changes.
StateCovariance model_noise = StateCovariance::Zero();
model_noise.diagonal() <<
// Position in map.
options_.position_model_variance() * delta_t,
options_.position_model_variance() * delta_t,
options_.position_model_variance() * delta_t,
// Orientation in map.
options_.orientation_model_variance() * delta_t,
options_.orientation_model_variance() * delta_t,
options_.orientation_model_variance() * delta_t,
// Linear velocities in map.
options_.velocity_model_variance() * delta_t,
options_.velocity_model_variance() * delta_t,
options_.velocity_model_variance() * delta_t;
return Distribution(State::Zero(), model_noise);
}
void PoseTracker::Predict(const common::Time time) {
imu_tracker_.Advance(time);
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
if (delta_t == 0.) {
return;
}
kalman_filter_.Predict(
[this, delta_t](const State& state) -> State {
return ModelFunction(state, delta_t);
},
BuildModelNoise(delta_t));
time_ = time;
}
void PoseTracker::AddImuLinearAccelerationObservation(
const common::Time time, const Eigen::Vector3d& 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_.Advance(time);
imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity);
Predict(time);
}
void PoseTracker::AddPoseObservation(const common::Time time,
const transform::Rigid3d& pose,
const PoseCovariance& covariance) {
Predict(time);
// Noise covariance is taken directly from the input values.
const GaussianDistribution<double, 6> delta(
Eigen::Matrix<double, 6, 1>::Zero(), covariance);
kalman_filter_.Observe<6>(
[this, &pose](const State& state) -> Eigen::Matrix<double, 6, 1> {
const transform::Rigid3d state_pose = RigidFromState(state);
const Eigen::Vector3d delta_orientation =
transform::RotationQuaternionToAngleAxisVector(
pose.rotation().inverse() * state_pose.rotation());
const Eigen::Vector3d delta_translation =
state_pose.translation() - pose.translation();
Eigen::Matrix<double, 6, 1> return_value;
return_value << delta_translation, delta_orientation;
return return_value;
},
delta);
}
// Updates from the odometer are in the odometer's map-like frame, called the
// 'odometry' frame. The odometer_pose converts data from the map frame
// into the odometry frame.
void PoseTracker::AddOdometerPoseObservation(
const common::Time time, const transform::Rigid3d& odometer_pose,
const PoseCovariance& covariance) {
if (!odometry_state_tracker_.empty()) {
const auto& previous_odometry_state = odometry_state_tracker_.newest();
const transform::Rigid3d delta =
previous_odometry_state.odometer_pose.inverse() * odometer_pose;
const transform::Rigid3d new_pose =
previous_odometry_state.state_pose * delta;
AddPoseObservation(time, new_pose, covariance);
}
const Distribution belief = GetBelief(time);
odometry_state_tracker_.AddOdometryState(
{time, odometer_pose, RigidFromState(belief.GetMean())});
}
const mapping::OdometryStateTracker::OdometryStates&
PoseTracker::odometry_states() const {
return odometry_state_tracker_.odometry_states();
}
transform::Rigid3d PoseTracker::RigidFromState(
const PoseTracker::State& state) {
return transform::Rigid3d(
Eigen::Vector3d(state[PoseTracker::kMapPositionX],
state[PoseTracker::kMapPositionY],
state[PoseTracker::kMapPositionZ]),
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(state[PoseTracker::kMapOrientationX],
state[PoseTracker::kMapOrientationY],
state[PoseTracker::kMapOrientationZ])) *
imu_tracker_.orientation());
}
PoseCovariance BuildPoseCovariance(const double translational_variance,
const double rotational_variance) {
const Eigen::Matrix3d translational =
Eigen::Matrix3d::Identity() * translational_variance;
const Eigen::Matrix3d rotational =
Eigen::Matrix3d::Identity() * rotational_variance;
// clang-format off
PoseCovariance covariance;
covariance <<
translational, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotational;
// clang-format on
return covariance;
}
} // namespace kalman_filter
} // namespace cartographer

129
cartographer/kalman_filter/pose_tracker.h
View File

@ -1,129 +0,0 @@
/*
* 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_KALMAN_FILTER_POSE_TRACKER_H_
#define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_
#include <deque>
#include <memory>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/gaussian_distribution.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"
namespace cartographer {
namespace kalman_filter {
typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance);
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* parameter_dictionary);
// A Kalman filter for a 3D state of position and orientation.
// Includes functions to update from IMU and range data matches.
class PoseTracker {
public:
enum {
kMapPositionX = 0,
kMapPositionY,
kMapPositionZ,
kMapOrientationX,
kMapOrientationY,
kMapOrientationZ,
kMapVelocityX,
kMapVelocityY,
kMapVelocityZ,
kDimension // We terminate loops with this.
};
using KalmanFilter = UnscentedKalmanFilter<double, kDimension>;
using State = KalmanFilter::StateType;
using StateCovariance = Eigen::Matrix<double, kDimension, kDimension>;
using Distribution = GaussianDistribution<double, kDimension>;
// Create a new Kalman filter starting at the origin with a standard normal
// distribution at 'time'.
PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
virtual ~PoseTracker();
// Returns the pose of the mean of the belief at 'time' which must be >= to
// the current time.
transform::Rigid3d GetPoseEstimateMean(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);
// Updates from a pose estimate in the map frame.
void AddPoseObservation(common::Time time, const transform::Rigid3d& pose,
const PoseCovariance& covariance);
// Updates from a pose estimate in the odometer's map-like frame.
void AddOdometerPoseObservation(common::Time time,
const transform::Rigid3d& pose,
const PoseCovariance& covariance);
common::Time time() const { return time_; }
// Returns the belief at the 'time' which must be >= to the current time.
Distribution GetBelief(common::Time time);
const mapping::OdometryStateTracker::OdometryStates& odometry_states() const;
Eigen::Quaterniond gravity_orientation() const {
return imu_tracker_.orientation();
}
private:
// Returns the distribution required to initialize the KalmanFilter.
static Distribution KalmanFilterInit();
// Build a model noise distribution (zero mean) for the given time delta.
const Distribution BuildModelNoise(double delta_t) const;
// Predict the state forward in time. This is a no-op if 'time' has not moved
// forward.
void Predict(common::Time time);
// Computes a pose combining the given 'state' with the 'imu_tracker_'
// orientation.
transform::Rigid3d RigidFromState(const PoseTracker::State& state);
const proto::PoseTrackerOptions options_;
common::Time time_;
KalmanFilter kalman_filter_;
mapping::ImuTracker imu_tracker_;
mapping::OdometryStateTracker odometry_state_tracker_;
};
} // namespace kalman_filter
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_

219
cartographer/kalman_filter/pose_tracker_test.cc
View File

@ -1,219 +0,0 @@
/*
* 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/kalman_filter/pose_tracker.h"
#include <random>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/rigid_transform_test_helpers.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace kalman_filter {
namespace {
constexpr double kOdometerVariance = 1e-12;
using transform::IsNearly;
using transform::Rigid3d;
using ::testing::Not;
class PoseTrackerTest : public ::testing::Test {
protected:
PoseTrackerTest() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
orientation_model_variance = 1e-8,
position_model_variance = 1e-8,
velocity_model_variance = 1e-8,
imu_gravity_time_constant = 100.,
num_odometry_states = 1,
}
)text");
const proto::PoseTrackerOptions options =
CreatePoseTrackerOptions(parameter_dictionary.get());
pose_tracker_ =
common::make_unique<PoseTracker>(options, common::FromUniversal(1000));
}
std::unique_ptr<PoseTracker> pose_tracker_;
};
TEST_F(PoseTrackerTest, SaveAndRestore) {
std::vector<Rigid3d> poses(3);
poses[0] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(1500));
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
PoseTracker copy_of_pose_tracker = *pose_tracker_;
const Eigen::Vector3d observation(2, 0, 8);
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
poses[1] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(3500));
copy_of_pose_tracker.AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
poses[2] =
copy_of_pose_tracker.GetPoseEstimateMean(common::FromUniversal(3500));
EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
}
TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::seconds(5);
pose_tracker_->AddImuLinearAccelerationObservation(
time, Eigen::Vector3d(0., 0., 10.));
}
{
const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
for (int i = 0; i < 300; ++i) {
time += std::chrono::seconds(5);
pose_tracker_->AddImuLinearAccelerationObservation(
time, Eigen::Vector3d(0., 10., 0.));
}
time += std::chrono::milliseconds(5);
const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond(
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddImuAngularVelocityObservation(time,
Eigen::Vector3d::Zero());
}
{
const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity();
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
const double target_radians = M_PI / 2.;
const double num_observations = 300.;
const double angular_velocity = target_radians / (num_observations * 5e-3);
for (int i = 0; i < num_observations; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddImuAngularVelocityObservation(
time, Eigen::Vector3d(angular_velocity, 0., 0.));
}
time += std::chrono::milliseconds(5);
const Rigid3d pose = pose_tracker_->GetPoseEstimateMean(time);
const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation());
const Eigen::Quaterniond expected = Eigen::Quaterniond(
Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX()));
EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n"
<< actual.coeffs();
}
TEST_F(PoseTrackerTest, AddPoseObservation) {
auto time = common::FromUniversal(1000);
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(5);
pose_tracker_->AddPoseObservation(
time, Rigid3d::Identity(),
Eigen::Matrix<double, 6, 6>::Identity() * 1e-6);
}
{
const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
}
const Rigid3d expected =
Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(
M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized()));
for (int i = 0; i < 300; ++i) {
time += std::chrono::milliseconds(15);
pose_tracker_->AddPoseObservation(
time, expected, Eigen::Matrix<double, 6, 6>::Identity() * 1e-9);
}
time += std::chrono::milliseconds(15);
const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(expected, 1e-3));
}
TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
common::Time time = common::FromUniversal(0);
std::vector<Rigid3d> odometer_track;
odometer_track.push_back(Rigid3d::Identity());
odometer_track.push_back(
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(
Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) *
Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ())));
odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
Rigid3d actual;
for (const Rigid3d& pose : odometer_track) {
time += std::chrono::seconds(1);
pose_tracker_->AddOdometerPoseObservation(
time, pose, kOdometerVariance * PoseCovariance::Identity());
actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(pose, 1e-2));
}
// Sanity check that the test has signal:
EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2)));
}
} // namespace
} // namespace kalman_filter
} // namespace cartographer

31
cartographer/kalman_filter/proto/pose_tracker_options.proto
View File

@ -1,31 +0,0 @@
// 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.
syntax = "proto2";
package cartographer.kalman_filter.proto;
message PoseTrackerOptions {
// Model variances depend linearly on time.
optional double position_model_variance = 1;
optional double orientation_model_variance = 2;
optional double velocity_model_variance = 3;
// Time constant for the orientation moving average based on observed gravity
// via linear acceleration.
optional double imu_gravity_time_constant = 4;
// Maximum number of previous odometry states to keep.
optional int32 num_odometry_states = 6;
}

291
cartographer/kalman_filter/unscented_kalman_filter.h
View File

@ -1,291 +0,0 @@
/*
* 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_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
#define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_
#include <algorithm>
#include <cmath>
#include <functional>
#include <vector>
#include "Eigen/Cholesky"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "glog/logging.h"
namespace cartographer {
namespace kalman_filter {
template <typename FloatType>
constexpr FloatType sqr(FloatType a) {
return a * a;
}
template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> OuterProduct(
const Eigen::Matrix<FloatType, N, 1>& v) {
return v * v.transpose();
}
// Checks if 'A' is a symmetric matrix.
template <typename FloatType, int N>
void CheckSymmetric(const Eigen::Matrix<FloatType, N, N>& A) {
// This should be pretty much Eigen::Matrix<>::Zero() if the matrix is
// symmetric.
const FloatType norm = (A - A.transpose()).norm();
CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5)
<< "Symmetry check failed with norm: '" << norm << "' from matrix:\n"
<< A;
}
// Returns the matrix square root of a symmetric positive semidefinite matrix.
template <typename FloatType, int N>
Eigen::Matrix<FloatType, N, N> MatrixSqrt(
const Eigen::Matrix<FloatType, N, N>& A) {
CheckSymmetric(A);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FloatType, N, N>>
adjoint_eigen_solver((A + A.transpose()) / 2.);
const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();
CHECK_GT(eigenvalues.minCoeff(), -1e-5)
<< "MatrixSqrt failed with negative eigenvalues: "
<< eigenvalues.transpose();
return adjoint_eigen_solver.eigenvectors() *
adjoint_eigen_solver.eigenvalues()
.cwiseMax(Eigen::Matrix<FloatType, N, 1>::Zero())
.cwiseSqrt()
.asDiagonal() *
adjoint_eigen_solver.eigenvectors().transpose();
}
// Implementation of a Kalman filter. We follow the nomenclature from
// Thrun, S. et al., Probabilistic Robotics, 2006.
//
// Extended to handle non-additive noise/sensors inspired by Kraft, E., A
// Quaternion-based Unscented Kalman Filter for Orientation Tracking.
template <typename FloatType, int N>
class UnscentedKalmanFilter {
public:
using StateType = Eigen::Matrix<FloatType, N, 1>;
using StateCovarianceType = Eigen::Matrix<FloatType, N, N>;
explicit UnscentedKalmanFilter(
const GaussianDistribution<FloatType, N>& initial_belief,
std::function<StateType(const StateType& state, const StateType& delta)>
add_delta = [](const StateType& state,
const StateType& delta) { return state + delta; },
std::function<StateType(const StateType& origin, const StateType& target)>
compute_delta =
[](const StateType& origin, const StateType& target) {
return target - origin;
})
: belief_(initial_belief),
add_delta_(add_delta),
compute_delta_(compute_delta) {}
// Does the control/prediction step for the filter. The control must be
// implicitly added by the function g which also does the state transition.
// 'epsilon' is the additive combination of control and model noise.
void Predict(std::function<StateType(const StateType&)> g,
const GaussianDistribution<FloatType, N>& epsilon) {
CheckSymmetric(epsilon.GetCovariance());
// Get the state mean and matrix root of its covariance.
const StateType& mu = belief_.GetMean();
const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
std::vector<StateType> Y;
Y.reserve(2 * N + 1);
Y.emplace_back(g(mu));
const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
for (int i = 0; i < N; ++i) {
// Order does not matter here as all have the same weights in the
// summation later on anyways.
Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i))));
Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i))));
}
const StateType new_mu = ComputeMean(Y);
StateCovarianceType new_sigma =
kCovWeight0 * OuterProduct<FloatType, N>(compute_delta_(new_mu, Y[0]));
for (int i = 0; i < N; ++i) {
new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
compute_delta_(new_mu, Y[2 * i + 1]));
new_sigma += kCovWeightI * OuterProduct<FloatType, N>(
compute_delta_(new_mu, Y[2 * i + 2]));
}
CheckSymmetric(new_sigma);
belief_ = GaussianDistribution<FloatType, N>(new_mu, new_sigma) + epsilon;
}
// The observation step of the Kalman filter. 'h' transfers the state
// into an observation that should be zero, i.e., the sensor readings should
// be included in this function already. 'delta' is the measurement noise and
// must have zero mean.
template <int K>
void Observe(
std::function<Eigen::Matrix<FloatType, K, 1>(const StateType&)> h,
const GaussianDistribution<FloatType, K>& delta) {
CheckSymmetric(delta.GetCovariance());
// We expect zero mean delta.
CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9);
// Get the state mean and matrix root of its covariance.
const StateType& mu = belief_.GetMean();
const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());
// As in Kraft's paper, we compute W containing the zero-mean sigma points,
// since this is all we need.
std::vector<StateType> W;
W.reserve(2 * N + 1);
W.emplace_back(StateType::Zero());
std::vector<Eigen::Matrix<FloatType, K, 1>> Z;
Z.reserve(2 * N + 1);
Z.emplace_back(h(mu));
Eigen::Matrix<FloatType, K, 1> z_hat = kMeanWeight0 * Z[0];
const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda);
for (int i = 0; i < N; ++i) {
// Order does not matter here as all have the same weights in the
// summation later on anyways.
W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i));
Z.emplace_back(h(add_delta_(mu, W.back())));
W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i));
Z.emplace_back(h(add_delta_(mu, W.back())));
z_hat += kMeanWeightI * Z[2 * i + 1];
z_hat += kMeanWeightI * Z[2 * i + 2];
}
Eigen::Matrix<FloatType, K, K> S =
kCovWeight0 * OuterProduct<FloatType, K>(Z[0] - z_hat);
for (int i = 0; i < N; ++i) {
S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 1] - z_hat);
S += kCovWeightI * OuterProduct<FloatType, K>(Z[2 * i + 2] - z_hat);
}
CheckSymmetric(S);
S += delta.GetCovariance();
Eigen::Matrix<FloatType, N, K> sigma_bar_xz =
kCovWeight0 * W[0] * (Z[0] - z_hat).transpose();
for (int i = 0; i < N; ++i) {
sigma_bar_xz +=
kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose();
sigma_bar_xz +=
kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose();
}
const Eigen::Matrix<FloatType, N, K> kalman_gain =
sigma_bar_xz * S.inverse();
const StateCovarianceType new_sigma =
belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose();
CheckSymmetric(new_sigma);
belief_ = GaussianDistribution<FloatType, N>(
add_delta_(mu, kalman_gain * -z_hat), new_sigma);
}
const GaussianDistribution<FloatType, N>& GetBelief() const {
return belief_;
}
private:
StateType ComputeWeightedError(const StateType& mean_estimate,
const std::vector<StateType>& states) {
StateType weighted_error =
kMeanWeight0 * compute_delta_(mean_estimate, states[0]);
for (int i = 1; i != 2 * N + 1; ++i) {
weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]);
}
return weighted_error;
}
// Algorithm for computing the mean of non-additive states taken from Kraft's
// Section 3.4, adapted to our implementation.
StateType ComputeMean(const std::vector<StateType>& states) {
CHECK_EQ(states.size(), 2 * N + 1);
StateType current_estimate = states[0];
StateType weighted_error = ComputeWeightedError(current_estimate, states);
int iterations = 0;
while (weighted_error.norm() > 1e-9) {
double step_size = 1.;
while (true) {
const StateType next_estimate =
add_delta_(current_estimate, step_size * weighted_error);
const StateType next_error =
ComputeWeightedError(next_estimate, states);
if (next_error.norm() < weighted_error.norm()) {
current_estimate = next_estimate;
weighted_error = next_error;
break;
}
step_size *= 0.5;
CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed.";
}
++iterations;
CHECK_LT(iterations, 20) << "Too many iterations.";
}
return current_estimate;
}
// According to Wikipedia these are the normal values. Thrun does not
// mention those.
constexpr static FloatType kAlpha = 1e-3;
constexpr static FloatType kKappa = 0.;
constexpr static FloatType kBeta = 2.;
constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;
constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);
constexpr static FloatType kCovWeight0 =
kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);
constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));
constexpr static FloatType kCovWeightI = kMeanWeightI;
GaussianDistribution<FloatType, N> belief_;
const std::function<StateType(const StateType& state, const StateType& delta)>
add_delta_;
const std::function<StateType(const StateType& origin,
const StateType& target)>
compute_delta_;
};
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kAlpha;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kKappa;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kBeta;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kLambda;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeight0;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeight0;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kMeanWeightI;
template <typename FloatType, int N>
constexpr FloatType UnscentedKalmanFilter<FloatType, N>::kCovWeightI;
} // namespace kalman_filter
} // namespace cartographer
#endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_

74
cartographer/kalman_filter/unscented_kalman_filter_test.cc
View File

@ -1,74 +0,0 @@
/*
* 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/kalman_filter/unscented_kalman_filter.h"
#include "cartographer/kalman_filter/gaussian_distribution.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace kalman_filter {
namespace {
Eigen::Matrix<double, 1, 1> Scalar(double value) {
return value * Eigen::Matrix<double, 1, 1>::Identity();
}
// A simple model that copies the first to the second state variable.
Eigen::Matrix<double, 2, 1> g(const Eigen::Matrix<double, 2, 1>& state) {
Eigen::Matrix<double, 2, 1> new_state;
new_state << state[0], state[0];
return new_state;
}
// A simple observation of the first state variable.
Eigen::Matrix<double, 1, 1> h(const Eigen::Matrix<double, 2, 1>& state) {
return Scalar(state[0]) - Scalar(5.);
}
TEST(KalmanFilterTest, testConstructor) {
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9);
}
TEST(KalmanFilterTest, testPredict) {
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity()));
filter.Predict(
g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
Eigen::Matrix2d::Identity() * 1e-9));
EXPECT_NEAR(filter.GetBelief().GetMean()[0], 42., 1e-2);
EXPECT_NEAR(filter.GetBelief().GetMean()[1], 42., 1e-2);
}
TEST(KalmanFilterTest, testObserve) {
UnscentedKalmanFilter<double, 2> filter(GaussianDistribution<double, 2>(
Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity()));
for (int i = 0; i < 500; ++i) {
filter.Predict(
g, GaussianDistribution<double, 2>(Eigen::Vector2d(0., 0.),
Eigen::Matrix2d::Identity() * 1e-9));
filter.Observe<1>(
h, GaussianDistribution<double, 1>(Scalar(0.), Scalar(1e-2)));
}
EXPECT_NEAR(filter.GetBelief().GetMean()[0], 5, 1e-2);
EXPECT_NEAR(filter.GetBelief().GetMean()[1], 5, 1e-2);
}
} // namespace
} // namespace kalman_filter
} // namespace cartographer

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -150,6 +150,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
if (last_scan_match_time_ > common::Time::min() &&
time > last_scan_match_time_) {
const double delta_t = common::ToSeconds(time - last_scan_match_time_);
// This adds the observed difference in velocity that would have reduced the
// error to zero.
velocity_estimate_ += (pose_estimate_.translation().head<2>() -
model_prediction.translation().head<2>()) /
delta_t;

7
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
View File

@ -36,14 +36,13 @@ message LocalTrajectoryBuilderOptions {
// cropping.
optional float voxel_filter_size = 3;
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
optional bool use_online_correlative_scan_matching = 5;
// Voxel filter used to compute a sparser point cloud for matching.
optional sensor.proto.AdaptiveVoxelFilterOptions
adaptive_voxel_filter_options = 6;
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
optional bool use_online_correlative_scan_matching = 5;
optional scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 7;
optional scan_matching.proto.CeresScanMatcherOptions

49
cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc
View File

@ -1,49 +0,0 @@
/*
* 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_3d/kalman_local_trajectory_builder_options.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
namespace cartographer {
namespace mapping_3d {
proto::KalmanLocalTrajectoryBuilderOptions
CreateKalmanLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::KalmanLocalTrajectoryBuilderOptions options;
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_real_time_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_pose_tracker_options() =
kalman_filter::CreatePoseTrackerOptions(
parameter_dictionary->GetDictionary("pose_tracker").get());
options.set_scan_matcher_variance(
parameter_dictionary->GetDouble("scan_matcher_variance"));
options.set_odometer_translational_variance(
parameter_dictionary->GetDouble("odometer_translational_variance"));
options.set_odometer_rotational_variance(
parameter_dictionary->GetDouble("odometer_rotational_variance"));
return options;
}
} // namespace mapping_3d
} // namespace cartographer

33
cartographer/mapping_3d/kalman_local_trajectory_builder_options.h
View File

@ -1,33 +0,0 @@
/*
* 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_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h"
namespace cartographer {
namespace mapping_3d {
proto::KalmanLocalTrajectoryBuilderOptions
CreateKalmanLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace mapping_3d
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_

32
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -18,7 +18,6 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
@ -35,13 +34,10 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
motion_filter_(options.motion_filter_options()),
real_time_correlative_scan_matcher_(
common::make_unique<scan_matching::RealTimeCorrelativeScanMatcher>(
options_.kalman_local_trajectory_builder_options()
.real_time_correlative_scan_matcher_options())),
options_.real_time_correlative_scan_matcher_options())),
ceres_scan_matcher_(common::make_unique<scan_matching::CeresScanMatcher>(
options_.ceres_scan_matcher_options())),
odometry_state_tracker_(options_.kalman_local_trajectory_builder_options()
.pose_tracker_options()
.num_odometry_states()),
odometry_state_tracker_(options_.num_odometry_states()),
accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {}
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
@ -49,27 +45,26 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
void LocalTrajectoryBuilder::AddImuData(
const common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
if (!imu_tracker_) {
const bool initial_imu_data = (imu_tracker_ == nullptr);
if (initial_imu_data) {
imu_tracker_ = common::make_unique<mapping::ImuTracker>(
options_.kalman_local_trajectory_builder_options()
.pose_tracker_options()
.imu_gravity_time_constant(),
time);
// Use the accelerometer to gravity align the first pose.
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
options_.imu_gravity_time_constant(), time);
}
Predict(time);
imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration);
imu_tracker_->AddImuAngularVelocityObservation(angular_velocity);
if (initial_imu_data) {
// This uses the first accelerometer measurement to approximately align the
// first pose to gravity.
pose_estimate_ = transform::Rigid3d::Rotation(imu_tracker_->orientation());
}
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
LocalTrajectoryBuilder::AddRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
if (!imu_tracker_) {
if (imu_tracker_ == nullptr) {
LOG(INFO) << "ImuTracker not yet initialized.";
return nullptr;
}
@ -160,8 +155,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking =
adaptive_voxel_filter.Filter(filtered_range_data.returns);
if (options_.kalman_local_trajectory_builder_options()
.use_online_correlative_scan_matching()) {
if (options_.use_online_correlative_scan_matching()) {
// We take a copy since we use 'intial_ceres_pose' as an output argument.
const transform::Rigid3d initial_pose = initial_ceres_pose;
real_time_correlative_scan_matcher_->Match(
@ -200,6 +194,8 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
if (last_scan_match_time_ > common::Time::min() &&
time > last_scan_match_time_) {
const double delta_t = common::ToSeconds(time - last_scan_match_time_);
// This adds the observed difference in velocity that would have reduced the
// error to zero.
velocity_estimate_ +=
(pose_estimate_.translation() - model_prediction.translation()) /
delta_t;

3
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -20,8 +20,9 @@
#include <memory>
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/global_trajectory_builder_interface.h"
#include "cartographer/mapping/imu_tracker.h"
#include "cartographer/mapping/odometry_state_tracker.h"
#include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"

18
cartographer/mapping_3d/local_trajectory_builder_options.cc
View File

@ -16,7 +16,7 @@
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
#include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h"
#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h"
#include "cartographer/mapping_3d/motion_filter.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/submaps.h"
@ -45,17 +45,25 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
parameter_dictionary
->GetDictionary("low_resolution_adaptive_voxel_filter")
.get());
options.set_use_online_correlative_scan_matching(
parameter_dictionary->GetBool("use_online_correlative_scan_matching"));
*options.mutable_real_time_correlative_scan_matcher_options() =
mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
parameter_dictionary
->GetDictionary("real_time_correlative_scan_matcher")
.get());
*options.mutable_ceres_scan_matcher_options() =
scan_matching::CreateCeresScanMatcherOptions(
parameter_dictionary->GetDictionary("ceres_scan_matcher").get());
*options.mutable_motion_filter_options() = CreateMotionFilterOptions(
parameter_dictionary->GetDictionary("motion_filter").get());
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
*options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions(
parameter_dictionary->GetDictionary("submaps").get());
*options.mutable_kalman_local_trajectory_builder_options() =
CreateKalmanLocalTrajectoryBuilderOptions(
parameter_dictionary->GetDictionary("kalman_local_trajectory_builder")
.get());
return options;
}

34
cartographer/mapping_3d/local_trajectory_builder_test.cc
View File

@ -64,6 +64,14 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
max_range = 50.,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.2,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1.,
},
ceres_scan_matcher = {
occupied_space_weight_0 = 5.,
occupied_space_weight_1 = 20.,
@ -76,11 +84,16 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
num_threads = 1,
},
},
motion_filter = {
max_time_seconds = 0.2,
max_distance_meters = 0.02,
max_angle_radians = 0.001,
},
imu_gravity_time_constant = 1.,
num_odometry_states = 1,
submaps = {
high_resolution = 0.2,
high_resolution_max_range = 50.,
@ -92,27 +105,6 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
num_free_space_voxels = 0,
},
},
kalman_local_trajectory_builder = {
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.2,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1.,
},
pose_tracker = {
orientation_model_variance = 5e-4,
position_model_variance = 0.000654766,
velocity_model_variance = 0.053926,
imu_gravity_time_constant = 1.,
num_odometry_states = 1,
},
scan_matcher_variance = 1e-6,
odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7,
},
}
)text");
return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get());

34
cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto
View File

@ -1,34 +0,0 @@
// 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.
syntax = "proto2";
import "cartographer/kalman_filter/proto/pose_tracker_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
package cartographer.mapping_3d.proto;
message KalmanLocalTrajectoryBuilderOptions {
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
optional bool use_online_correlative_scan_matching = 1;
optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 2;
optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3;
optional double scan_matcher_variance = 6;
optional double odometer_translational_variance = 4;
optional double odometer_rotational_variance = 5;
}

22
cartographer/mapping_3d/proto/local_trajectory_builder_options.proto
View File

@ -16,7 +16,7 @@ syntax = "proto2";
import "cartographer/mapping_3d/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
import "cartographer/mapping_3d/proto/submaps_options.proto";
import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
@ -41,11 +41,25 @@ message LocalTrajectoryBuilderOptions {
optional sensor.proto.AdaptiveVoxelFilterOptions
low_resolution_adaptive_voxel_filter_options = 12;
// Whether to solve the online scan matching first using the correlative scan
// matcher to generate a good starting point for Ceres.
optional bool use_online_correlative_scan_matching = 13;
optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 14;
optional scan_matching.proto.CeresScanMatcherOptions
ceres_scan_matcher_options = 6;
optional MotionFilterOptions motion_filter_options = 7;
optional SubmapsOptions submaps_options = 8;
optional KalmanLocalTrajectoryBuilderOptions
kalman_local_trajectory_builder_options = 10;
// Time constant in seconds for the orientation moving average based on
// observed gravity via the IMU. It should be chosen so that the error
// 1. from acceleration measurements not due to gravity (which gets worse when
// the constant is reduced) and
// 2. from integration of angular velocities (which gets worse when the
// constant is increased) is balanced.
optional double imu_gravity_time_constant = 15;
// Maximum number of previous odometry states to keep.
optional int32 num_odometry_states = 16;
optional SubmapsOptions submaps_options = 8;
}

10
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -29,7 +29,6 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
#include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
#include "cartographer/transform/transform.h"
@ -72,8 +71,7 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap,
[=]() EXCLUDES(mutex_) {
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
@ -96,8 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, submap,
[=]() EXCLUDES(mutex_) {
submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
ComputeConstraint(
submap_id, submap, node_id, true, /* match_full_submap */
trajectory_connectivity, compressed_point_cloud,
@ -232,8 +229,7 @@ void ConstraintBuilder::ComputeConstraint(
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(
pose_estimate, pose_estimate,
ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
{{&high_resolution_point_cloud,
submap_scan_matcher->high_resolution_hybrid_grid},
{&low_resolution_point_cloud,

3
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -121,8 +121,7 @@ class ConstraintBuilder {
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const Submap* submap, std::function<void()> work_item)
REQUIRES(mutex_);
const Submap* submap, std::function<void()> work_item) REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(

2
configuration_files/trajectory_builder_2d.lua
View File

@ -21,13 +21,13 @@ TRAJECTORY_BUILDER_2D = {
missing_data_ray_length = 5.,
voxel_filter_size = 0.025,
use_online_correlative_scan_matching = false,
adaptive_voxel_filter = {
max_length = 0.5,
min_num_points = 200,
max_range = 50.,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.1,
angular_search_window = math.rad(20.),

33
configuration_files/trajectory_builder_3d.lua
View File

@ -32,6 +32,14 @@ TRAJECTORY_BUILDER_3D = {
max_range = MAX_3D_RANGE,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.15,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
},
ceres_scan_matcher = {
occupied_space_weight_0 = 1.,
occupied_space_weight_1 = 6.,
@ -51,6 +59,9 @@ TRAJECTORY_BUILDER_3D = {
max_angle_radians = 0.004,
},
imu_gravity_time_constant = 10.,
num_odometry_states = 1,
submaps = {
high_resolution = 0.10,
high_resolution_max_range = 20.,
@ -62,26 +73,4 @@ TRAJECTORY_BUILDER_3D = {
num_free_space_voxels = 2,
},
},
kalman_local_trajectory_builder = {
pose_tracker = {
orientation_model_variance = 5e-3,
position_model_variance = 0.00654766,
velocity_model_variance = 0.53926,
imu_gravity_time_constant = 10.,
num_odometry_states = 1,
},
use_online_correlative_scan_matching = false,
real_time_correlative_scan_matcher = {
linear_search_window = 0.15,
angular_search_window = math.rad(1.),
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
},
scan_matcher_variance = 2.34e-9,
odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7,
},
}

70
docs/source/configuration.rst
View File

@ -33,26 +33,6 @@ int32 num_threads
Not yet documented.
cartographer.kalman_filter.proto.PoseTrackerOptions
===================================================
double position_model_variance
Model variances depend linearly on time.
double orientation_model_variance
Not yet documented.
double velocity_model_variance
Not yet documented.
double imu_gravity_time_constant
Time constant for the orientation moving average based on observed gravity
via linear acceleration.
int32 num_odometry_states
Maximum number of previous odometry states to keep.
cartographer.mapping.proto.MapBuilderOptions
============================================
@ -206,13 +186,13 @@ float voxel_filter_size
Voxel filter that gets applied to the range data immediately after
cropping.
cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
Voxel filter used to compute a sparser point cloud for matching.
bool use_online_correlative_scan_matching
Whether to solve the online scan matching first using the correlative scan
matcher to generate a good starting point for Ceres.
cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
Voxel filter used to compute a sparser point cloud for matching.
cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
Not yet documented.
@ -324,29 +304,6 @@ double rotation_delta_cost_weight
Not yet documented.
cartographer.mapping_3d.proto.KalmanLocalTrajectoryBuilderOptions
=================================================================
bool use_online_correlative_scan_matching
Whether to solve the online scan matching first using the correlative scan
matcher to generate a good starting point for Ceres.
cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
Not yet documented.
cartographer.kalman_filter.proto.PoseTrackerOptions pose_tracker_options
Not yet documented.
double scan_matcher_variance
Not yet documented.
double odometer_translational_variance
Not yet documented.
double odometer_rotational_variance
Not yet documented.
cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions
===========================================================
@ -370,16 +327,31 @@ cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_vo
cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options
Not yet documented.
bool use_online_correlative_scan_matching
Whether to solve the online scan matching first using the correlative scan
matcher to generate a good starting point for Ceres.
cartographer.mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options
Not yet documented.
cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options
Not yet documented.
cartographer.mapping_3d.proto.MotionFilterOptions motion_filter_options
Not yet documented.
cartographer.mapping_3d.proto.SubmapsOptions submaps_options
Not yet documented.
double imu_gravity_time_constant
Time constant in seconds for the orientation moving average based on
observed gravity via the IMU. It should be chosen so that the error
1. from acceleration measurements not due to gravity (which gets worse when
the constant is reduced) and
2. from integration of angular velocities (which gets worse when the
constant is increased) is balanced.
cartographer.mapping_3d.proto.KalmanLocalTrajectoryBuilderOptions kalman_local_trajectory_builder_options
int32 num_odometry_states
Maximum number of previous odometry states to keep.
cartographer.mapping_3d.proto.SubmapsOptions submaps_options
Not yet documented.