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

Remove unused 'imu_gravity_variance' option. (#363)

master
Wolfgang Hess 2017-06-26 13:23:58 +02:00 committed by GitHub
parent 9464b38320
commit 2df8bcde61
8 changed files with 21 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
cartographer/kalman_filter/pose_tracker.cc
View File

@ -111,17 +111,6 @@ PoseTracker::State ModelFunction(const PoseTracker::State& state,
} // namespace
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance) {
GaussianDistribution<double, 6> distribution(
Eigen::Matrix<double, 6, 1>::Zero(), pose_and_covariance.covariance);
Eigen::Matrix<double, 6, 6> linear_transform;
linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), transform.rotation().matrix();
return {transform * pose_and_covariance.pose,
(linear_transform * distribution).GetCovariance()};
}
proto::PoseTrackerOptions CreatePoseTrackerOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::PoseTrackerOptions options;
@ -133,8 +122,6 @@ proto::PoseTrackerOptions CreatePoseTrackerOptions(
parameter_dictionary->GetDouble("velocity_model_variance"));
options.set_imu_gravity_time_constant(
parameter_dictionary->GetDouble("imu_gravity_time_constant"));
options.set_imu_gravity_variance(
parameter_dictionary->GetDouble("imu_gravity_variance"));
options.set_num_odometry_states(
parameter_dictionary->GetNonNegativeInt("num_odometry_states"));
CHECK_GT(options.num_odometry_states(), 0);
@ -165,20 +152,8 @@ PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) {
return kalman_filter_.GetBelief();
}
void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time,
transform::Rigid3d* pose,
PoseCovariance* covariance) {
const Distribution belief = GetBelief(time);
*pose = RigidFromState(belief.GetMean());
static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance.");
static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance.");
static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance.");
static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance.");
*covariance = belief.GetCovariance().block<6, 6>(0, 0);
covariance->block<2, 2>(3, 3) +=
options_.imu_gravity_variance() * Eigen::Matrix2d::Identity();
transform::Rigid3d PoseTracker::GetPoseEstimateMean(const common::Time time) {
return RigidFromState(GetBelief(time).GetMean());
}
const PoseTracker::Distribution PoseTracker::BuildModelNoise(

17
cartographer/kalman_filter/pose_tracker.h
View File

@ -36,17 +36,8 @@
namespace cartographer {
namespace kalman_filter {
typedef Eigen::Matrix3d Pose2DCovariance;
typedef Eigen::Matrix<double, 6, 6> PoseCovariance;
struct PoseAndCovariance {
transform::Rigid3d pose;
PoseCovariance covariance;
};
PoseAndCovariance operator*(const transform::Rigid3d& transform,
const PoseAndCovariance& pose_and_covariance);
PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance);
@ -80,11 +71,9 @@ class PoseTracker {
PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);
virtual ~PoseTracker();
// Sets 'pose' and 'covariance' to the mean and covariance of the belief at
// 'time' which must be >= to the current time. Must not be nullptr.
void GetPoseEstimateMeanAndCovariance(common::Time time,
transform::Rigid3d* pose,
PoseCovariance* covariance);
// 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(

42
cartographer/kalman_filter/pose_tracker_test.cc
View File

@ -44,7 +44,6 @@ class PoseTrackerTest : public ::testing::Test {
position_model_variance = 1e-8,
velocity_model_variance = 1e-8,
imu_gravity_time_constant = 100.,
imu_gravity_variance = 1e-9,
num_odometry_states = 1,
}
)text");
@ -59,9 +58,7 @@ class PoseTrackerTest : public ::testing::Test {
TEST_F(PoseTrackerTest, SaveAndRestore) {
std::vector<Rigid3d> poses(3);
std::vector<PoseCovariance> covariances(3);
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500),
&poses[0], &covariances[0]);
poses[0] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(1500));
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9));
@ -72,18 +69,15 @@ TEST_F(PoseTrackerTest, SaveAndRestore) {
pose_tracker_->AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500),
&poses[1], &covariances[1]);
poses[1] = pose_tracker_->GetPoseEstimateMean(common::FromUniversal(3500));
copy_of_pose_tracker.AddImuLinearAccelerationObservation(
common::FromUniversal(3000), observation);
copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance(
common::FromUniversal(3500), &poses[2], &covariances[2]);
poses[2] =
copy_of_pose_tracker.GetPoseEstimateMean(common::FromUniversal(3500));
EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6)));
EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all());
EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6));
EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all());
}
TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
@ -96,9 +90,7 @@ TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
}
{
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
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"
@ -113,9 +105,7 @@ TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) {
time += std::chrono::milliseconds(5);
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
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()));
@ -133,9 +123,7 @@ TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
}
{
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
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"
@ -153,9 +141,7 @@ TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) {
time += std::chrono::milliseconds(5);
Rigid3d pose;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance);
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()));
@ -174,9 +160,7 @@ TEST_F(PoseTrackerTest, AddPoseObservation) {
}
{
Rigid3d actual;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3));
}
@ -193,9 +177,7 @@ TEST_F(PoseTrackerTest, AddPoseObservation) {
time += std::chrono::milliseconds(15);
Rigid3d actual;
PoseCovariance covariance;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance);
const Rigid3d actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(expected, 1e-3));
}
@ -221,13 +203,11 @@ TEST_F(PoseTrackerTest, AddOdometerPoseObservation) {
odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2)));
Rigid3d actual;
PoseCovariance unused_covariance;
for (const Rigid3d& pose : odometer_track) {
time += std::chrono::seconds(1);
pose_tracker_->AddOdometerPoseObservation(
time, pose, kOdometerVariance * PoseCovariance::Identity());
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual,
&unused_covariance);
actual = pose_tracker_->GetPoseEstimateMean(time);
EXPECT_THAT(actual, IsNearly(pose, 1e-2));
}
// Sanity check that the test has signal:

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

@ -25,7 +25,6 @@ message PoseTrackerOptions {
// Time constant for the orientation moving average based on observed gravity
// via linear acceleration.
optional double imu_gravity_time_constant = 4;
optional double imu_gravity_variance = 5;
// Maximum number of previous odometry states to keep.
optional int32 num_odometry_states = 6;

21
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -58,11 +58,6 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration);
pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity);
transform::Rigid3d pose_estimate;
kalman_filter::PoseCovariance unused_covariance_estimate;
pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate,
&unused_covariance_estimate);
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
@ -74,10 +69,8 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
return nullptr;
}
transform::Rigid3d pose_prediction;
kalman_filter::PoseCovariance unused_covariance_prediction;
pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &pose_prediction, &unused_covariance_prediction);
const transform::Rigid3d pose_prediction =
pose_tracker_->GetPoseEstimateMean(time);
if (num_accumulated_ == 0) {
first_pose_prediction_ = pose_prediction.cast<float>();
accumulated_range_data_ =
@ -131,10 +124,8 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
return nullptr;
}
transform::Rigid3d pose_prediction;
kalman_filter::PoseCovariance unused_covariance_prediction;
pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &pose_prediction, &unused_covariance_prediction);
const transform::Rigid3d pose_prediction =
pose_tracker_->GetPoseEstimateMean(time);
std::shared_ptr<const Submap> matching_submap =
active_submaps_.submaps().front();
@ -174,9 +165,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
.scan_matcher_variance() *
kalman_filter::PoseCovariance::Identity());
kalman_filter::PoseCovariance unused_covariance_estimate;
pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &scan_matcher_pose_estimate_, &unused_covariance_estimate);
scan_matcher_pose_estimate_ = pose_tracker_->GetPoseEstimateMean(time);
last_pose_estimate_ = {
time, scan_matcher_pose_estimate_,

1
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -107,7 +107,6 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
position_model_variance = 0.000654766,
velocity_model_variance = 0.053926,
imu_gravity_time_constant = 1.,
imu_gravity_variance = 1e-4,
num_odometry_states = 1,
},

1
configuration_files/trajectory_builder_3d.lua
View File

@ -71,7 +71,6 @@ TRAJECTORY_BUILDER_3D = {
velocity_model_variance = 0.53926,
-- This disables gravity alignment in local SLAM.
imu_gravity_time_constant = 1e9,
imu_gravity_variance = 0,
num_odometry_states = 1,
},

3
docs/source/configuration.rst
View File

@ -49,9 +49,6 @@ double imu_gravity_time_constant
Time constant for the orientation moving average based on observed gravity
via linear acceleration.
double imu_gravity_variance
Not yet documented.
int32 num_odometry_states
Maximum number of previous odometry states to keep.