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

Make use of Submap in 2D similar to 3D. (#343) 

Use mapping_2d::Submap instead of mapping::Submap in 2D.
Use `finished()` instead of `finished_probability_grid()` in 2D.

PAIR=wohe
master
Holger Rapp 2017-06-19 14:43:10 +02:00 committed by GitHub
parent 8d94fe85e9
commit b944b19159
14 changed files with 79 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer/mapping/submaps.h
View File

@ -67,13 +67,6 @@ class Submap {
// Number of RangeData inserted.
size_t num_range_data() const { return num_range_data_; }
// The 'finished_probability_grid' when this submap is finished and will not
// change anymore. Otherwise, this is nullptr and the next call to
// InsertRangeData() will change the submap.
const mapping_2d::ProbabilityGrid* finished_probability_grid() const {
return finished_probability_grid_;
}
// Fills data into the 'response'.
virtual void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
@ -83,9 +76,8 @@ class Submap {
const transform::Rigid3d local_pose_;
protected:
// TODO(hrapp): All of this should be private.
// TODO(hrapp): This should be private.
int num_range_data_ = 0;
const mapping_2d::ProbabilityGrid* finished_probability_grid_ = nullptr;
};
// Submaps is a sequence of maps to which scans are matched and into which scans

18
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -45,13 +45,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalRangeData(
time, sensor::RangeData{origin, ranges, {}});
if (insertion_result != nullptr) {
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps);
if (insertion_result == nullptr) {
return;
}
const Submap* const finished_submap =
insertion_result->insertion_submaps.front()->finished()
? insertion_result->insertion_submaps.front()
: nullptr;
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps,
finished_submap);
}
void GlobalTrajectoryBuilder::AddImuData(

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -176,9 +176,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
return nullptr;
}
const mapping::Submap* const matching_submap =
submaps_.Get(submaps_.matching_index());
std::vector<const mapping::Submap*> insertion_submaps;
const Submap* const matching_submap = submaps_.Get(submaps_.matching_index());
std::vector<const Submap*> insertion_submaps;
for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index));
}

4
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -41,8 +41,8 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
common::Time time;
const mapping::Submap* matching_submap;
std::vector<const mapping::Submap*> insertion_submaps;
const Submap* matching_submap;
std::vector<const Submap*> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d;
sensor::RangeData range_data_in_tracking_2d;
transform::Rigid2d pose_estimate_2d;

21
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -53,7 +53,7 @@ SparsePoseGraph::~SparsePoseGraph() {
}
void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& insertion_submaps) {
const std::vector<const Submap*>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
const int trajectory_id = first_submap_id.trajectory_id;
@ -91,8 +91,9 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const int trajectory_id, const mapping::Submap* const matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps) {
const int trajectory_id, const Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps,
const Submap* const finished_submap) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@ -115,10 +116,6 @@ void SparsePoseGraph::AddScan(
submap_ids_.emplace(insertion_submaps.back(), submap_id);
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
const mapping::Submap* const finished_submap =
insertion_submaps.front()->finished_probability_grid() != nullptr
? insertion_submaps.front()
: nullptr;
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
@ -188,8 +185,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
}
}
void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::Submap* submap) {
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const auto submap_id = GetSubmapId(submap);
const auto& submap_data = submap_data_.at(submap_id);
@ -209,9 +205,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
}
void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose) {
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid2d& pose) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
const transform::Rigid2d optimized_pose =
@ -231,7 +226,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {
for (const Submap* submap : insertion_submaps) {
const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);

32
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -66,13 +66,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// that will later be optimized. The 'tracking_to_pose' is remembered so
// that the optimized pose can be embedded into 3D. The 'pose' was determined
// by scan matching against the 'matching_submap' and the scan was inserted
// into the 'insertion_submaps'.
// into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a
// freshly finished submap. It is also contained in 'insertion_submaps' for
// the last time.
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose, int trajectory_id,
const mapping::Submap* matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps)
EXCLUDES(mutex_);
const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps,
const Submap* finished_submap) EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time,
@ -97,7 +99,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Likewise, all new scans are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished, kTrimmed };
struct SubmapData {
const mapping::Submap* submap = nullptr;
const Submap* submap = nullptr;
// IDs of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
@ -110,8 +112,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Handles a new work item.
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
REQUIRES(mutex_) {
mapping::SubmapId GetSubmapId(const Submap* submap) const REQUIRES(mutex_) {
const auto iterator = submap_ids_.find(submap);
CHECK(iterator != submap_ids_.end());
return iterator->second;
@ -120,14 +121,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& insertion_submaps)
REQUIRES(mutex_);
const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
void ComputeConstraintsForScan(const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap,
const transform::Rigid2d& pose)
REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
@ -135,8 +135,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished.
void ComputeConstraintsForOldScans(const mapping::Submap* submap)
REQUIRES(mutex_);
void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
// Registers the callback to run the optimization once all constraints have
// been computed, that will also do all work that queue up in 'scan_queue_'.
@ -185,8 +184,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
GUARDED_BY(mutex_);
std::map<const Submap*, mapping::SubmapId> submap_ids_ GUARDED_BY(mutex_);
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
GUARDED_BY(mutex_);

12
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -38,7 +38,7 @@ namespace cartographer {
namespace mapping_2d {
namespace sparse_pose_graph {
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
return transform::Project2D(submap.local_pose());
}
@ -60,7 +60,7 @@ ConstraintBuilder::~ConstraintBuilder() {
}
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
@ -85,7 +85,7 @@ void ConstraintBuilder::MaybeAddConstraint(
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
common::MutexLocker locker(&mutex_);
@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
@ -159,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
}
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud,

11
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -35,7 +35,7 @@
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/sensor/point_cloud.h"
@ -47,7 +47,7 @@ namespace sparse_pose_graph {
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
// of the Submap.
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
transform::Rigid2d ComputeSubmapPose(const Submap& submap);
// Asynchronously computes constraints.
//
@ -78,8 +78,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const mapping::Submap* submap,
const mapping::NodeId& node_id,
const Submap* submap, const mapping::NodeId& node_id,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
@ -92,7 +91,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
mapping::TrajectoryConnectivity* trajectory_connectivity);
@ -138,7 +137,7 @@ class ConstraintBuilder {
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud,

15
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -149,9 +149,9 @@ class SparsePoseGraphTest : public ::testing::Test {
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
const mapping::Submap* const matching_submap =
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
std::vector<const mapping::Submap*> insertion_submaps;
std::vector<const Submap*> insertion_submaps;
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
@ -161,9 +161,14 @@ class SparsePoseGraphTest : public ::testing::Test {
constexpr int kTrajectoryId = 0;
submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(
common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
const Submap* const finished_submap = insertion_submaps.front()->finished()
? insertion_submaps.front()
: nullptr;
sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), range_data,
pose_estimate, kTrajectoryId, matching_submap,
insertion_submaps, finished_submap);
}
void MoveRelative(const transform::Rigid2d& movement) {

6
cartographer/mapping_2d/submaps.cc
View File

@ -162,7 +162,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid_ == nullptr);
CHECK(!submap->finished_);
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
++submap->num_range_data_;
}
@ -184,10 +184,10 @@ void Submaps::FinishSubmap(int index) {
// Crop the finished Submap before inserting a new Submap to reduce peak
// memory usage a bit.
Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid_ == nullptr);
CHECK(!submap->finished_);
submap->probability_grid_ =
ComputeCroppedProbabilityGrid(submap->probability_grid_);
submap->finished_probability_grid_ = &submap->probability_grid_;
submap->finished_ = true;
if (options_.output_debug_images()) {
// Output the Submap that won't be changed from now on.
WriteDebugImage("submap" + std::to_string(index) + ".webp",

2
cartographer/mapping_2d/submaps.h
View File

@ -46,6 +46,7 @@ class Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
const bool finished() const { return finished_; }
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
@ -56,6 +57,7 @@ class Submap : public mapping::Submap {
friend class Submaps;
ProbabilityGrid probability_grid_;
bool finished_ = false;
};
// A container of Submaps.

8
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -55,15 +55,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
const sensor::PointCloud& ranges) {
auto insertion_result =
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
if (insertion_result == nullptr) {
return;
}
const Submap* const finished_submap =
insertion_result->insertion_submaps.front()->finished()
? insertion_result->insertion_submaps.front()
: nullptr;
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps);
insertion_result->matching_submap, insertion_result->insertion_submaps,
finished_submap);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -90,7 +90,8 @@ void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose, const int trajectory_id,
const Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps) {
const std::vector<const Submap*>& insertion_submaps,
const Submap* const finished_submap) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
@ -113,9 +114,6 @@ void SparsePoseGraph::AddScan(
submap_ids_.emplace(insertion_submaps.back(), submap_id);
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
const Submap* const finished_submap = insertion_submaps.front()->finished()
? insertion_submaps.front()
: nullptr;
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -63,13 +63,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
// that will later be optimized. The 'pose' was determined by scan matching
// against the 'matching_submap' and the scan was inserted into the
// 'insertion_submaps'.
// 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly
// finished submap. It is also contained in 'insertion_submaps' for the last
// time.
void AddScan(common::Time time,
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose, int trajectory_id,
const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
const std::vector<const Submap*>& insertion_submaps,
const Submap* finished_submap) EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time,