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

Removes odometer covariance from public API. (#137)

master
Damon Kohler 2016-11-21 12:34:52 +01:00 committed by GitHub
parent 5136a3a81e
commit a39bbff70c
24 changed files with 87 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cartographer/kalman_filter/pose_tracker.cc
View File

@ -356,5 +356,20 @@ PoseCovariance PoseCovarianceFromProtoMatrix(
return covariance;
}
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

3
cartographer/kalman_filter/pose_tracker.h
View File

@ -54,6 +54,9 @@ Pose2DCovariance Project2D(const PoseCovariance& embedded_covariance);
PoseCovariance Embed3D(const Pose2DCovariance& embedded_covariance,
double position_variance, double orientation_variance);
PoseCovariance BuildPoseCovariance(double translational_variance,
double rotational_variance);
// Deserializes the 'proto_matrix' into a PoseCovariance.
PoseCovariance PoseCovarianceFromProtoMatrix(
const sensor::proto::Matrix& proto_matrix);

4
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -94,8 +94,8 @@ void CollatedTrajectoryBuilder::HandleCollatedSensorData(
return;
case sensor::Data::Type::kOdometer:
wrapped_trajectory_builder_->AddOdometerData(
data->time, data->odometer.pose, data->odometer.covariance);
wrapped_trajectory_builder_->AddOdometerData(data->time,
data->odometer_pose);
return;
}
LOG(FATAL);

5
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -56,9 +56,8 @@ class GlobalTrajectoryBuilderInterface {
virtual void AddImuData(common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) = 0;
virtual void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) = 0;
virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0;
};
} // namespace mapping

6
cartographer/mapping/trajectory_builder.h
View File

@ -100,11 +100,9 @@ class TrajectoryBuilder {
}
void AddOdometerData(const string& sensor_id, common::Time time,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
const transform::Rigid3d& odometer_pose) {
AddSensorData(sensor_id,
common::make_unique<sensor::Data>(
time, sensor::Data::Odometer{pose, covariance}));
common::make_unique<sensor::Data>(time, odometer_pose));
}
};

7
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -56,10 +56,9 @@ void GlobalTrajectoryBuilder::AddImuData(
angular_velocity);
}
void GlobalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
local_trajectory_builder_.AddOdometerData(time, pose, covariance);
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
local_trajectory_builder_.AddOdometerData(time, pose);
}
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

5
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -44,9 +44,8 @@ class GlobalTrajectoryBuilder
const sensor::PointCloud& ranges) override;
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) override;
void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
private:
const proto::LocalTrajectoryBuilderOptions options_;

16
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -59,6 +59,10 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
*options.mutable_submaps_options() = CreateSubmapsOptions(
parameter_dictionary->GetDictionary("submaps").get());
options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data"));
options.set_odometer_translational_variance(
parameter_dictionary->GetDouble("odometer_translational_variance"));
options.set_odometer_rotational_variance(
parameter_dictionary->GetDouble("odometer_rotational_variance"));
return options;
}
@ -264,15 +268,17 @@ void LocalTrajectoryBuilder::AddImuData(
<< common::RadToDeg(kMaxInclination);
}
void LocalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
void LocalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
if (pose_tracker_ == nullptr) {
// Until we've initialized the UKF with our first IMU message, we cannot
// process odometry poses.
// process odometer poses.
LOG_EVERY_N(INFO, 100) << "PoseTracker not yet initialized.";
} else {
pose_tracker_->AddOdometerPoseObservation(time, pose, covariance);
pose_tracker_->AddOdometerPoseObservation(
time, pose, kalman_filter::BuildPoseCovariance(
options_.odometer_translational_variance(),
options_.odometer_rotational_variance()));
}
}

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -67,8 +67,7 @@ class LocalTrajectoryBuilder {
common::Time, const sensor::LaserFan& laser_fan);
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
const Submaps* submaps() const;

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

@ -56,4 +56,7 @@ message LocalTrajectoryBuilderOptions {
// True if IMU data should be expected and used.
optional bool use_imu_data = 12;
optional double odometer_translational_variance = 17;
optional double odometer_rotational_variance = 18;
}

7
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -59,10 +59,9 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index);
}
void GlobalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
local_trajectory_builder_->AddOdometerData(time, pose, covariance);
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
local_trajectory_builder_->AddOdometerData(time, pose);
}
const GlobalTrajectoryBuilder::PoseEstimate&

5
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -42,9 +42,8 @@ class GlobalTrajectoryBuilder
const Eigen::Vector3d& angular_velocity) override;
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const PoseEstimate& pose_estimate() const override;
private:

10
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -189,15 +189,19 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedLaserFan(
}
void KalmanLocalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
const common::Time time, const transform::Rigid3d& pose) {
if (!pose_tracker_) {
pose_tracker_.reset(new kalman_filter::PoseTracker(
options_.kalman_local_trajectory_builder_options()
.pose_tracker_options(),
kalman_filter::PoseTracker::ModelFunction::k3D, time));
}
pose_tracker_->AddOdometerPoseObservation(time, pose, covariance);
pose_tracker_->AddOdometerPoseObservation(
time, pose, kalman_filter::BuildPoseCovariance(
options_.kalman_local_trajectory_builder_options()
.odometer_translational_variance(),
options_.kalman_local_trajectory_builder_options()
.odometer_rotational_variance()));
}
const KalmanLocalTrajectoryBuilder::PoseEstimate&

6
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -51,10 +51,8 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;

4
cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc
View File

@ -36,6 +36,10 @@ CreateKalmanLocalTrajectoryBuilderOptions(
*options.mutable_pose_tracker_options() =
kalman_filter::CreatePoseTrackerOptions(
parameter_dictionary->GetDictionary("pose_tracker").get());
options.set_odometer_translational_variance(
parameter_dictionary->GetDouble("odometer_translational_variance"));
options.set_odometer_rotational_variance(
parameter_dictionary->GetDouble("odometer_rotational_variance"));
return options;
}

3
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -111,6 +111,9 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
imu_gravity_variance = 1e-4,
num_odometry_states = 1,
},
odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7,
},
optimizing_local_trajectory_builder = {
high_resolution_grid_weight = 5.,

5
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -56,9 +56,8 @@ class LocalTrajectoryBuilderInterface {
virtual std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
virtual void AddOdometerData(
common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) = 0;
virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0;
// Register a 'trajectory_node_index' from the SparsePoseGraph corresponding
// to the latest inserted laser scan. This is used to remember which

3
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -126,8 +126,7 @@ void OptimizingLocalTrajectoryBuilder::AddImuData(
}
void OptimizingLocalTrajectoryBuilder::AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
const common::Time time, const transform::Rigid3d& pose) {
odometer_data_.push_back(OdometerData{time, pose});
RemoveObsoleteSensorData();
}

6
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -55,10 +55,8 @@ class OptimizingLocalTrajectoryBuilder
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(
const common::Time time, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) override;
void AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) override;
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;

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

@ -27,4 +27,7 @@ message KalmanLocalTrajectoryBuilderOptions {
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 odometer_translational_variance = 4;
optional double odometer_rotational_variance = 5;
}

12
cartographer/sensor/collator_test.cc
View File

@ -31,13 +31,13 @@ namespace {
TEST(Collator, Ordering) {
const std::array<string, 4> kSensorId = {
{"horizontal_laser", "vertical_laser", "imu", "odometry"}};
Data zero(common::FromUniversal(0), sensor::Data::Rangefinder{});
Data first(common::FromUniversal(100), sensor::Data::Rangefinder{});
Data second(common::FromUniversal(200), sensor::Data::Rangefinder{});
Data zero(common::FromUniversal(0), Data::Rangefinder{});
Data first(common::FromUniversal(100), Data::Rangefinder{});
Data second(common::FromUniversal(200), Data::Rangefinder{});
Data third(common::FromUniversal(300), Data::Imu{});
Data fourth(common::FromUniversal(400), sensor::Data::Rangefinder{});
Data fifth(common::FromUniversal(500), sensor::Data::Rangefinder{});
Data sixth(common::FromUniversal(600), Data::Odometer{});
Data fourth(common::FromUniversal(400), Data::Rangefinder{});
Data fifth(common::FromUniversal(500), Data::Rangefinder{});
Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity());
std::vector<std::pair<string, Data>> received;
Collator collator;

13
cartographer/sensor/data.h
View File

@ -42,27 +42,20 @@ struct Data {
PointCloud ranges;
};
struct Odometer {
transform::Rigid3d pose;
// TODO(damonkohler): Remove this in favor of using the configurable
// constant variance directly.
kalman_filter::PoseCovariance covariance;
};
Data(const common::Time time, const Imu& imu)
: type(Type::kImu), time(time), imu(imu) {}
Data(const common::Time time, const Rangefinder& rangefinder)
: type(Type::kRangefinder), time(time), rangefinder(rangefinder) {}
Data(const common::Time time, const Odometer& odometer)
: type(Type::kOdometer), time(time), odometer(odometer) {}
Data(const common::Time time, const transform::Rigid3d& odometer_pose)
: type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {}
Type type;
common::Time time;
Imu imu;
Rangefinder rangefinder;
Odometer odometer;
transform::Rigid3d odometer_pose;
};
} // namespace sensor

2
configuration_files/trajectory_builder_2d.lua
View File

@ -20,6 +20,8 @@ TRAJECTORY_BUILDER_2D = {
laser_max_z = 2.,
laser_missing_echo_ray_length = 5.,
laser_voxel_filter_size = 0.025,
odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7,
use_online_correlative_scan_matching = false,
adaptive_voxel_filter = {

3
configuration_files/trajectory_builder_3d.lua
View File

@ -83,6 +83,9 @@ TRAJECTORY_BUILDER_3D = {
translation_delta_cost_weight = 1e-1,
rotation_delta_cost_weight = 1e-1,
},
odometer_translational_variance = 1e-7,
odometer_rotational_variance = 1e-7,
},
optimizing_local_trajectory_builder = {