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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -36,6 +36,10 @@ CreateKalmanLocalTrajectoryBuilderOptions(
*options.mutable_pose_tracker_options() = *options.mutable_pose_tracker_options() =
kalman_filter::CreatePoseTrackerOptions( kalman_filter::CreatePoseTrackerOptions(
parameter_dictionary->GetDictionary("pose_tracker").get()); 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; return options;
} }

View File

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

View File

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

View File

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

View File

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

View File

@ -27,4 +27,7 @@ message KalmanLocalTrajectoryBuilderOptions {
optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
real_time_correlative_scan_matcher_options = 2; real_time_correlative_scan_matcher_options = 2;
optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3;
optional double odometer_translational_variance = 4;
optional double odometer_rotational_variance = 5;
} }

View File

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

View File

@ -42,27 +42,20 @@ struct Data {
PointCloud ranges; 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) Data(const common::Time time, const Imu& imu)
: type(Type::kImu), time(time), imu(imu) {} : type(Type::kImu), time(time), imu(imu) {}
Data(const common::Time time, const Rangefinder& rangefinder) Data(const common::Time time, const Rangefinder& rangefinder)
: type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {}
Data(const common::Time time, const Odometer& odometer) Data(const common::Time time, const transform::Rigid3d& odometer_pose)
: type(Type::kOdometer), time(time), odometer(odometer) {} : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {}
Type type; Type type;
common::Time time; common::Time time;
Imu imu; Imu imu;
Rangefinder rangefinder; Rangefinder rangefinder;
Odometer odometer; transform::Rigid3d odometer_pose;
}; };
} // namespace sensor } // namespace sensor

View File

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

View File

@ -83,6 +83,9 @@ TRAJECTORY_BUILDER_3D = {
translation_delta_cost_weight = 1e-1, translation_delta_cost_weight = 1e-1,
rotation_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 = { optimizing_local_trajectory_builder = {