Introduce MatchingResult for LocalTrajectoryBuilder::AddRangeData. (#619)
In preparation for #574. Depends on ~~#618~~ (merged) and ~~#617~~ (merged).master
parent
5363285b8f
commit
eb4415d17d
|
@ -47,14 +47,21 @@ class GlobalTrajectoryBuilder
|
||||||
void AddRangefinderData(const common::Time time,
|
void AddRangefinderData(const common::Time time,
|
||||||
const Eigen::Vector3f& origin,
|
const Eigen::Vector3f& origin,
|
||||||
const sensor::TimedPointCloud& ranges) override {
|
const sensor::TimedPointCloud& ranges) override {
|
||||||
std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
|
||||||
insertion_result = local_trajectory_builder_.AddRangeData(
|
matching_result = local_trajectory_builder_.AddRangeData(
|
||||||
time, sensor::TimedRangeData{origin, ranges, {}});
|
time, sensor::TimedRangeData{origin, ranges, {}});
|
||||||
if (insertion_result == nullptr) {
|
if (matching_result == nullptr) {
|
||||||
|
// The range data has not been fully accumulated yet.
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
|
std::unique_ptr<mapping::NodeId> node_id;
|
||||||
insertion_result->insertion_submaps);
|
if (matching_result->insertion_result != nullptr) {
|
||||||
|
node_id = ::cartographer::common::make_unique<mapping::NodeId>(
|
||||||
|
sparse_pose_graph_->AddScan(
|
||||||
|
matching_result->insertion_result->constant_data, trajectory_id_,
|
||||||
|
matching_result->insertion_result->insertion_submaps));
|
||||||
|
CHECK_EQ(node_id->trajectory_id, trajectory_id_);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AddSensorData(const sensor::ImuData& imu_data) override {
|
void AddSensorData(const sensor::ImuData& imu_data) override {
|
||||||
|
|
|
@ -73,7 +73,7 @@ void LocalTrajectoryBuilder::ScanMatch(
|
||||||
matching_submap->probability_grid(), pose_observation, &summary);
|
matching_submap->probability_grid(), pose_observation, &summary);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
const sensor::TimedRangeData& range_data) {
|
const sensor::TimedRangeData& range_data) {
|
||||||
// Initialize extrapolator now if we do not ever use an IMU.
|
// Initialize extrapolator now if we do not ever use an IMU.
|
||||||
|
@ -126,7 +126,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
const common::Time time, const sensor::RangeData& range_data) {
|
const common::Time time, const sensor::RangeData& range_data) {
|
||||||
// Transforms 'range_data' from the tracking frame into a frame where gravity
|
// Transforms 'range_data' from the tracking frame into a frame where gravity
|
||||||
|
@ -158,8 +158,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
TransformRangeData(gravity_aligned_range_data,
|
TransformRangeData(gravity_aligned_range_data,
|
||||||
transform::Embed3D(pose_estimate_2d.cast<float>()));
|
transform::Embed3D(pose_estimate_2d.cast<float>()));
|
||||||
last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
|
last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
|
||||||
return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
|
std::unique_ptr<InsertionResult> insertion_result =
|
||||||
|
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
|
||||||
pose_estimate, gravity_alignment.rotation());
|
pose_estimate, gravity_alignment.rotation());
|
||||||
|
return common::make_unique<MatchingResult>(
|
||||||
|
MatchingResult{time, pose_estimate, std::move(range_data_in_local),
|
||||||
|
std::move(insertion_result)});
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
||||||
|
|
|
@ -45,6 +45,13 @@ class LocalTrajectoryBuilder {
|
||||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
||||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
||||||
};
|
};
|
||||||
|
struct MatchingResult {
|
||||||
|
common::Time time;
|
||||||
|
transform::Rigid3d local_pose;
|
||||||
|
sensor::RangeData range_data_in_local;
|
||||||
|
// 'nullptr' if dropped by the motion filter.
|
||||||
|
std::unique_ptr<const InsertionResult> insertion_result;
|
||||||
|
};
|
||||||
|
|
||||||
explicit LocalTrajectoryBuilder(
|
explicit LocalTrajectoryBuilder(
|
||||||
const proto::LocalTrajectoryBuilderOptions& options);
|
const proto::LocalTrajectoryBuilderOptions& options);
|
||||||
|
@ -55,14 +62,16 @@ class LocalTrajectoryBuilder {
|
||||||
|
|
||||||
const mapping::PoseEstimate& pose_estimate() const;
|
const mapping::PoseEstimate& pose_estimate() const;
|
||||||
|
|
||||||
// Range data must be approximately horizontal for 2D SLAM.
|
// Returns 'MatchingResult' when range data accumulation completed,
|
||||||
std::unique_ptr<InsertionResult> AddRangeData(
|
// otherwise 'nullptr'. Range data must be approximately horizontal
|
||||||
|
// for 2D SLAM.
|
||||||
|
std::unique_ptr<MatchingResult> AddRangeData(
|
||||||
common::Time, const sensor::TimedRangeData& range_data);
|
common::Time, const sensor::TimedRangeData& range_data);
|
||||||
void AddImuData(const sensor::ImuData& imu_data);
|
void AddImuData(const sensor::ImuData& imu_data);
|
||||||
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
|
||||||
common::Time time, const sensor::RangeData& range_data);
|
common::Time time, const sensor::RangeData& range_data);
|
||||||
sensor::RangeData TransformAndFilterRangeData(
|
sensor::RangeData TransformAndFilterRangeData(
|
||||||
const transform::Rigid3f& gravity_alignment,
|
const transform::Rigid3f& gravity_alignment,
|
||||||
|
|
|
@ -58,7 +58,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
|
||||||
options_.imu_gravity_time_constant(), imu_data);
|
options_.imu_gravity_time_constant(), imu_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
const sensor::TimedRangeData& range_data) {
|
const sensor::TimedRangeData& range_data) {
|
||||||
if (extrapolator_ == nullptr) {
|
if (extrapolator_ == nullptr) {
|
||||||
|
@ -106,7 +106,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
|
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||||
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
|
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
|
||||||
const sensor::RangeData filtered_range_data_in_tracking = {
|
const sensor::RangeData filtered_range_data_in_tracking = {
|
||||||
|
@ -166,10 +166,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
|
||||||
filtered_range_data_in_tracking, pose_estimate.cast<float>());
|
filtered_range_data_in_tracking, pose_estimate.cast<float>());
|
||||||
last_pose_estimate_ = {time, pose_estimate,
|
last_pose_estimate_ = {time, pose_estimate,
|
||||||
filtered_range_data_in_local.returns};
|
filtered_range_data_in_local.returns};
|
||||||
return InsertIntoSubmap(
|
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
|
||||||
time, filtered_range_data_in_local, filtered_range_data_in_tracking,
|
time, filtered_range_data_in_local, filtered_range_data_in_tracking,
|
||||||
high_resolution_point_cloud_in_tracking,
|
high_resolution_point_cloud_in_tracking,
|
||||||
low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
|
low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
|
||||||
|
return common::make_unique<MatchingResult>(MatchingResult{
|
||||||
|
time, pose_estimate, std::move(filtered_range_data_in_local),
|
||||||
|
std::move(insertion_result)});
|
||||||
}
|
}
|
||||||
|
|
||||||
void LocalTrajectoryBuilder::AddOdometerData(
|
void LocalTrajectoryBuilder::AddOdometerData(
|
||||||
|
@ -212,8 +215,8 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
|
||||||
filtered_range_data_in_tracking.returns,
|
filtered_range_data_in_tracking.returns,
|
||||||
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
|
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
|
||||||
options_.rotational_histogram_size());
|
options_.rotational_histogram_size());
|
||||||
return std::unique_ptr<InsertionResult>(new InsertionResult{
|
return common::make_unique<InsertionResult>(
|
||||||
std::make_shared<const mapping::TrajectoryNode::Data>(
|
InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
|
||||||
mapping::TrajectoryNode::Data{
|
mapping::TrajectoryNode::Data{
|
||||||
time,
|
time,
|
||||||
gravity_alignment,
|
gravity_alignment,
|
||||||
|
|
|
@ -44,6 +44,13 @@ class LocalTrajectoryBuilder {
|
||||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
||||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
|
||||||
};
|
};
|
||||||
|
struct MatchingResult {
|
||||||
|
common::Time time;
|
||||||
|
transform::Rigid3d local_pose;
|
||||||
|
sensor::RangeData range_data_in_local;
|
||||||
|
// 'nullptr' if dropped by the motion filter.
|
||||||
|
std::unique_ptr<const InsertionResult> insertion_result;
|
||||||
|
};
|
||||||
|
|
||||||
explicit LocalTrajectoryBuilder(
|
explicit LocalTrajectoryBuilder(
|
||||||
const proto::LocalTrajectoryBuilderOptions& options);
|
const proto::LocalTrajectoryBuilderOptions& options);
|
||||||
|
@ -53,13 +60,15 @@ class LocalTrajectoryBuilder {
|
||||||
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
void AddImuData(const sensor::ImuData& imu_data);
|
void AddImuData(const sensor::ImuData& imu_data);
|
||||||
std::unique_ptr<InsertionResult> AddRangeData(
|
// Returns 'MatchingResult' when range data accumulation completed,
|
||||||
|
// otherwise 'nullptr'.
|
||||||
|
std::unique_ptr<MatchingResult> AddRangeData(
|
||||||
common::Time time, const sensor::TimedRangeData& range_data);
|
common::Time time, const sensor::TimedRangeData& range_data);
|
||||||
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
void AddOdometerData(const sensor::OdometryData& odometry_data);
|
||||||
const mapping::PoseEstimate& pose_estimate() const;
|
const mapping::PoseEstimate& pose_estimate() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
|
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
|
||||||
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
common::Time time, const sensor::RangeData& range_data_in_tracking);
|
||||||
|
|
||||||
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
std::unique_ptr<InsertionResult> InsertIntoSubmap(
|
||||||
|
|
|
@ -251,12 +251,13 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
|
||||||
for (const TrajectoryNode& node : expected_trajectory) {
|
for (const TrajectoryNode& node : expected_trajectory) {
|
||||||
AddLinearOnlyImuObservation(node.time, node.pose);
|
AddLinearOnlyImuObservation(node.time, node.pose);
|
||||||
const auto range_data = GenerateRangeData(node.pose);
|
const auto range_data = GenerateRangeData(node.pose);
|
||||||
if (local_trajectory_builder_->AddRangeData(
|
const std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
|
||||||
node.time,
|
matching_result = local_trajectory_builder_->AddRangeData(
|
||||||
sensor::TimedRangeData{
|
node.time, sensor::TimedRangeData{
|
||||||
range_data.origin, range_data.returns, {}}) != nullptr) {
|
range_data.origin, range_data.returns, {}});
|
||||||
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
|
if (matching_result != nullptr) {
|
||||||
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
|
EXPECT_THAT(matching_result->local_pose,
|
||||||
|
transform::IsNearly(node.pose, 1e-1));
|
||||||
++num_poses;
|
++num_poses;
|
||||||
LOG(INFO) << "num_poses: " << num_poses;
|
LOG(INFO) << "num_poses: " << num_poses;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue