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

Introduce MatchingResult for LocalTrajectoryBuilder::AddRangeData. (#619) 

In preparation for #574. Depends on ~~#618~~ (merged) and ~~#617~~ (merged).
master
Juraj Oršulić 2017-11-10 14:49:41 +01:00 committed by Wally B. Feed
parent 5363285b8f
commit eb4415d17d
6 changed files with 67 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer/mapping/global_trajectory_builder.h
View File

@ -47,14 +47,21 @@ class GlobalTrajectoryBuilder
void AddRangefinderData(const common::Time time,
const Eigen::Vector3f& origin,
const sensor::TimedPointCloud& ranges) override {
std::unique_ptr<typename LocalTrajectoryBuilder::InsertionResult>
insertion_result = local_trajectory_builder_.AddRangeData(
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_.AddRangeData(
time, sensor::TimedRangeData{origin, ranges, {}});
if (insertion_result == nullptr) {
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
}
sparse_pose_graph_->AddScan(insertion_result->constant_data, trajectory_id_,
insertion_result->insertion_submaps);
std::unique_ptr<mapping::NodeId> node_id;
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 {

12
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -73,7 +73,7 @@ void LocalTrajectoryBuilder::ScanMatch(
matching_submap->probability_grid(), pose_observation, &summary);
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddRangeData(const common::Time time,
const sensor::TimedRangeData& range_data) {
// Initialize extrapolator now if we do not ever use an IMU.
@ -126,7 +126,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
return nullptr;
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::RangeData& range_data) {
// Transforms 'range_data' from the tracking frame into a frame where gravity
@ -158,8 +158,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
TransformRangeData(gravity_aligned_range_data,
transform::Embed3D(pose_estimate_2d.cast<float>()));
last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
pose_estimate, gravity_alignment.rotation());
std::unique_ptr<InsertionResult> insertion_result =
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
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>

15
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -45,6 +45,13 @@ class LocalTrajectoryBuilder {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
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(
const proto::LocalTrajectoryBuilderOptions& options);
@ -55,14 +62,16 @@ class LocalTrajectoryBuilder {
const mapping::PoseEstimate& pose_estimate() const;
// Range data must be approximately horizontal for 2D SLAM.
std::unique_ptr<InsertionResult> AddRangeData(
// Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. Range data must be approximately horizontal
// for 2D SLAM.
std::unique_ptr<MatchingResult> AddRangeData(
common::Time, const sensor::TimedRangeData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometerData(const sensor::OdometryData& odometry_data);
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& range_data);
sensor::RangeData TransformAndFilterRangeData(
const transform::Rigid3f& gravity_alignment,

31
cartographer/mapping_3d/local_trajectory_builder.cc
View File

@ -58,7 +58,7 @@ void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& 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,
const sensor::TimedRangeData& range_data) {
if (extrapolator_ == nullptr) {
@ -106,7 +106,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
return nullptr;
}
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
LocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::RangeData& 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>());
last_pose_estimate_ = {time, pose_estimate,
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,
high_resolution_point_cloud_in_tracking,
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(
@ -212,17 +215,17 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
filtered_range_data_in_tracking.returns,
transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
options_.rotational_histogram_size());
return std::unique_ptr<InsertionResult>(new InsertionResult{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
gravity_alignment,
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking,
rotational_scan_matcher_histogram,
pose_estimate}),
std::move(insertion_submaps)});
return common::make_unique<InsertionResult>(
InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time,
gravity_alignment,
{}, // 'filtered_point_cloud' is only used in 2D.
high_resolution_point_cloud_in_tracking,
low_resolution_point_cloud_in_tracking,
rotational_scan_matcher_histogram,
pose_estimate}),
std::move(insertion_submaps)});
}
} // namespace mapping_3d

13
cartographer/mapping_3d/local_trajectory_builder.h
View File

@ -44,6 +44,13 @@ class LocalTrajectoryBuilder {
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
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(
const proto::LocalTrajectoryBuilderOptions& options);
@ -53,13 +60,15 @@ class LocalTrajectoryBuilder {
LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete;
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);
void AddOdometerData(const sensor::OdometryData& odometry_data);
const mapping::PoseEstimate& pose_estimate() const;
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& range_data_in_tracking);
std::unique_ptr<InsertionResult> InsertIntoSubmap(

13
cartographer/mapping_3d/local_trajectory_builder_test.cc
View File

@ -251,12 +251,13 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
for (const TrajectoryNode& node : expected_trajectory) {
AddLinearOnlyImuObservation(node.time, node.pose);
const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangeData(
node.time,
sensor::TimedRangeData{
range_data.origin, range_data.returns, {}}) != nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
const std::unique_ptr<LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
node.time, sensor::TimedRangeData{
range_data.origin, range_data.returns, {}});
if (matching_result != nullptr) {
EXPECT_THAT(matching_result->local_pose,
transform::IsNearly(node.pose, 1e-1));
++num_poses;
LOG(INFO) << "num_poses: " << num_poses;
}