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

Use shared_ptr<> for Submap objects. (#347) 

Also remove `finished_submap` and `matching_submaps`. These are now
implicit.

Related to #283.

PAIR=wohe
master
Holger Rapp 2017-06-19 17:23:48 +02:00 committed by Wolfgang Hess
parent c8f02264a4
commit f242b5242a
17 changed files with 97 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/trajectory_builder.h
View File

@ -54,7 +54,7 @@ class TrajectoryBuilder {
}; };
struct SubmapData { struct SubmapData {
const Submap* submap; std::shared_ptr<const Submap> submap;
transform::Rigid3d pose; transform::Rigid3d pose;
}; };

7
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -48,16 +48,11 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
if (insertion_result == nullptr) { if (insertion_result == nullptr) {
return; return;
} }
const Submap* const finished_submap =
insertion_result->insertion_submaps.front()->finished()
? insertion_result->insertion_submaps.front()
: nullptr;
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->time, insertion_result->tracking_to_tracking_2d,
insertion_result->range_data_in_tracking_2d, insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d, trajectory_id_, insertion_result->pose_estimate_2d, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps, std::move(insertion_result->insertion_submaps));
finished_submap);
} }
void GlobalTrajectoryBuilder::AddImuData( void GlobalTrajectoryBuilder::AddImuData(

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -176,8 +176,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
return nullptr; return nullptr;
} }
const Submap* const matching_submap = submaps_.Get(submaps_.matching_index()); std::vector<std::shared_ptr<const Submap>> insertion_submaps;
std::vector<const Submap*> insertion_submaps;
for (int insertion_index : submaps_.insertion_indices()) { for (int insertion_index : submaps_.insertion_indices()) {
insertion_submaps.push_back(submaps_.Get(insertion_index)); insertion_submaps.push_back(submaps_.Get(insertion_index));
} }
@ -186,7 +185,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
transform::Embed3D(pose_estimate_2d.cast<float>()))); transform::Embed3D(pose_estimate_2d.cast<float>())));
return common::make_unique<InsertionResult>(InsertionResult{ return common::make_unique<InsertionResult>(InsertionResult{
time, matching_submap, insertion_submaps, tracking_to_tracking_2d, time, std::move(insertion_submaps), tracking_to_tracking_2d,
range_data_in_tracking_2d, pose_estimate_2d}); range_data_in_tracking_2d, pose_estimate_2d});
} }

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

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

35
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -54,7 +54,7 @@ SparsePoseGraph::~SparsePoseGraph() {
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded( std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const int trajectory_id, const int trajectory_id,
const std::vector<const Submap*>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
@ -98,9 +98,8 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan( void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose, common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const int trajectory_id, const Submap* const matching_submap, const int trajectory_id,
const std::vector<const Submap*>& insertion_submaps, const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const Submap* const finished_submap) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@ -136,9 +135,12 @@ void SparsePoseGraph::AddScan(
options_.global_sampling_ratio()); options_.global_sampling_ratio());
} }
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps, ComputeConstraintsForScan(trajectory_id, insertion_submaps,
finished_submap, pose); newly_finished_submap, pose);
}); });
} }
@ -168,7 +170,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
if (node_id.trajectory_id != submap_id.trajectory_id && if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) { global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap, node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
&trajectory_connectivity_); &trajectory_connectivity_);
} else { } else {
@ -190,7 +192,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.point_cloud_pose; .point_cloud_pose;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap, node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
initial_relative_pose); initial_relative_pose);
} }
@ -216,9 +218,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
} }
void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id, const Submap* matching_submap, const int trajectory_id,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const transform::Rigid2d& pose) { const bool newly_finished_submap, const transform::Rigid2d& pose) {
const std::vector<mapping::SubmapId> submap_ids = const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size()); CHECK_EQ(submap_ids.size(), insertion_submaps.size());
@ -228,7 +230,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id) .at(matching_id.trajectory_id)
.at(matching_id.submap_index) .at(matching_id.submap_index)
.pose * .pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose;
const mapping::NodeId node_id{ const mapping::NodeId node_id{
matching_id.trajectory_id, matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) < static_cast<size_t>(matching_id.trajectory_id) <
@ -241,12 +245,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose); matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const Submap* submap = insertion_submaps[i];
const mapping::SubmapId submap_id = submap_ids[i]; const mapping::SubmapId submap_id = submap_ids[i];
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
pose;
constraints_.push_back(Constraint{submap_id, constraints_.push_back(Constraint{submap_id,
node_id, node_id,
{transform::Embed3D(constraint_transform), {transform::Embed3D(constraint_transform),
@ -268,8 +272,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
} }
} }
if (finished_submap != nullptr) { if (newly_finished_submap) {
CHECK(finished_submap == insertion_submaps.front());
const mapping::SubmapId finished_submap_id = submap_ids.front(); const mapping::SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);

39
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -62,19 +62,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' that
// that will later be optimized. The 'tracking_to_pose' is remembered so // will later be optimized. The 'tracking_to_pose' is remembered so that the
// that the optimized pose can be embedded into 3D. The 'pose' was determined // optimized pose can be embedded into 3D. The 'pose' was determined by scan
// by scan matching against the 'matching_submap' and the scan was inserted // matching against the 'insertion_submaps.front()' and the scan was inserted
// into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a // into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// freshly finished submap. It is also contained in 'insertion_submaps' for // 'true', this submap was inserted into for the last time.
// the last time. void AddScan(
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose, int trajectory_id, const transform::Rigid2d& pose, int trajectory_id,
const Submap* matching_submap, const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
const std::vector<const Submap*>& insertion_submaps, EXCLUDES(mutex_);
const Submap* finished_submap) EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization. // Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, common::Time time,
@ -99,7 +98,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Likewise, all new scans are matched against submaps which are finished. // Likewise, all new scans are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished, kTrimmed }; enum class SubmapState { kActive, kFinished, kTrimmed };
struct SubmapData { struct SubmapData {
const Submap* submap = nullptr; std::shared_ptr<const Submap> submap;
// IDs of the scans that were inserted into this map together with // 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 // constraints for them. They are not to be matched again when this submap
@ -115,15 +114,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded( std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
int trajectory_id, const std::vector<const Submap*>& insertion_submaps) int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(int trajectory_id, void ComputeConstraintsForScan(
const Submap* matching_submap, int trajectory_id,
std::vector<const Submap*> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const Submap* finished_submap, bool newly_finished_submap, const transform::Rigid2d& pose)
const transform::Rigid2d& pose)
REQUIRES(mutex_); REQUIRES(mutex_);
// Computes constraints for a scan and submap pair. // Computes constraints for a scan and submap pair.

14
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

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

7
cartographer/mapping_2d/submaps.cc
View File

@ -166,16 +166,15 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
range_data_inserter_.Insert(range_data, &submap->probability_grid_); range_data_inserter_.Insert(range_data, &submap->probability_grid_);
++submap->num_range_data_; ++submap->num_range_data_;
} }
const Submap* const last_submap = Get(size() - 1); if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
if (last_submap->num_range_data_ == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>()); AddSubmap(range_data.origin.head<2>());
} }
} }
const Submap* Submaps::Get(int index) const { std::shared_ptr<const Submap> Submaps::Get(int index) const {
CHECK_GE(index, 0); CHECK_GE(index, 0);
CHECK_LT(index, size()); CHECK_LT(index, size());
return submaps_[index].get(); return submaps_[index];
} }
int Submaps::size() const { return submaps_.size(); } int Submaps::size() const { return submaps_.size(); }

5
cartographer/mapping_2d/submaps.h
View File

@ -79,7 +79,7 @@ class Submaps {
Submaps(const Submaps&) = delete; Submaps(const Submaps&) = delete;
Submaps& operator=(const Submaps&) = delete; Submaps& operator=(const Submaps&) = delete;
const Submap* Get(int index) const; std::shared_ptr<const Submap> Get(int index) const;
int size() const; int size() const;
// Returns the index of the newest initialized Submap which can be // Returns the index of the newest initialized Submap which can be
@ -98,8 +98,7 @@ class Submaps {
void AddSubmap(const Eigen::Vector2f& origin); void AddSubmap(const Eigen::Vector2f& origin);
const proto::SubmapsOptions options_; const proto::SubmapsOptions options_;
std::vector<std::shared_ptr<Submap>> submaps_;
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_; RangeDataInserter range_data_inserter_;
}; };

8
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -58,16 +58,10 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
if (insertion_result == nullptr) { if (insertion_result == nullptr) {
return; return;
} }
const Submap* const finished_submap =
insertion_result->insertion_submaps.front()->finished()
? insertion_result->insertion_submaps.front()
: nullptr;
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, trajectory_id_, insertion_result->pose_observation, trajectory_id_,
insertion_result->matching_submap, insertion_result->insertion_submaps, std::move(insertion_result->insertion_submaps));
finished_submap);
} }
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

8
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -140,7 +140,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
pose_tracker_->GetPoseEstimateMeanAndCovariance( pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &pose_prediction, &unused_covariance_prediction); time, &pose_prediction, &unused_covariance_prediction);
const Submap* const matching_submap = std::shared_ptr<const Submap> matching_submap =
submaps_->Get(submaps_->matching_index()); submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose = transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction; matching_submap->local_pose().inverse() * pose_prediction;
@ -219,9 +219,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
} }
const Submap* const matching_submap = std::vector<std::shared_ptr<const Submap>> insertion_submaps;
submaps_->Get(submaps_->matching_index());
std::vector<const Submap*> insertion_submaps;
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
@ -231,7 +229,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
pose_tracker_->gravity_orientation()); pose_tracker_->gravity_orientation());
return std::unique_ptr<InsertionResult>( return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation, new InsertionResult{time, range_data_in_tracking, pose_observation,
matching_submap, insertion_submaps}); std::move(insertion_submaps)});
} }
} // namespace mapping_3d } // namespace mapping_3d

3
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -37,8 +37,7 @@ class LocalTrajectoryBuilderInterface {
common::Time time; common::Time time;
sensor::RangeData range_data_in_tracking; sensor::RangeData range_data_in_tracking;
transform::Rigid3d pose_observation; transform::Rigid3d pose_observation;
const Submap* matching_submap; std::vector<std::shared_ptr<const Submap>> insertion_submaps;
std::vector<const Submap*> insertion_submaps;
}; };
virtual ~LocalTrajectoryBuilderInterface() {} virtual ~LocalTrajectoryBuilderInterface() {}

8
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -236,7 +236,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
} }
ceres::Problem problem; ceres::Problem problem;
const Submap* const matching_submap = std::shared_ptr<const Submap> matching_submap =
submaps_->Get(submaps_->matching_index()); submaps_->Get(submaps_->matching_index());
// We transform the states in 'batches_' in place to be in the submap frame as // We transform the states in 'batches_' in place to be in the submap frame as
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
@ -409,9 +409,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_observation)) { if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr; return nullptr;
} }
const Submap* const matching_submap = std::vector<std::shared_ptr<const Submap>> insertion_submaps;
submaps_->Get(submaps_->matching_index());
std::vector<const Submap*> insertion_submaps;
for (int insertion_index : submaps_->insertion_indices()) { for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index)); insertion_submaps.push_back(submaps_->Get(insertion_index));
} }
@ -425,7 +423,7 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
return std::unique_ptr<InsertionResult>( return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation, new InsertionResult{time, range_data_in_tracking, pose_observation,
matching_submap, insertion_submaps}); std::move(insertion_submaps)});
} }
OptimizingLocalTrajectoryBuilder::State OptimizingLocalTrajectoryBuilder::State

33
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -54,7 +54,7 @@ SparsePoseGraph::~SparsePoseGraph() {
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded( std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const int trajectory_id, const int trajectory_id,
const std::vector<const Submap*>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
@ -95,12 +95,9 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan( void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking, common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose, const int trajectory_id, const transform::Rigid3d& pose, const int trajectory_id,
const Submap* const matching_submap, const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const std::vector<const Submap*>& insertion_submaps,
const Submap* const finished_submap) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose); GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append( trajectory_nodes_.Append(
trajectory_id, trajectory_id,
@ -133,9 +130,12 @@ void SparsePoseGraph::AddScan(
options_.global_sampling_ratio()); options_.global_sampling_ratio());
} }
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps, ComputeConstraintsForScan(trajectory_id, insertion_submaps,
finished_submap, pose); newly_finished_submap, pose);
}); });
} }
@ -197,7 +197,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// yaw component will be handled by the matching procedure in the // yaw component will be handled by the matching procedure in the
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored. // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap, node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
submap_nodes, initial_relative_pose.rotation(), submap_nodes, initial_relative_pose.rotation(),
&trajectory_connectivity_); &trajectory_connectivity_);
@ -210,7 +210,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
if (node_id.trajectory_id == submap_id.trajectory_id || if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) { scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap, node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
submap_nodes, initial_relative_pose); submap_nodes, initial_relative_pose);
} }
@ -235,18 +235,19 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
} }
void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id, const Submap* matching_submap, const int trajectory_id,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const transform::Rigid3d& pose) { const bool newly_finished_submap, const transform::Rigid3d& pose) {
const std::vector<mapping::SubmapId> submap_ids = const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front(); const mapping::SubmapId matching_id = submap_ids.front();
const transform::Rigid3d optimized_pose = const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data() optimization_problem_.submap_data()
.at(matching_id.trajectory_id) .at(matching_id.trajectory_id)
.at(matching_id.submap_index) .at(matching_id.submap_index)
.pose * .pose *
matching_submap->local_pose().inverse() * pose; insertion_submaps.front()->local_pose().inverse() * pose;
const mapping::NodeId node_id{ const mapping::NodeId node_id{
matching_id.trajectory_id, matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) < static_cast<size_t>(matching_id.trajectory_id) <
@ -259,12 +260,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose); scan_data->time, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const Submap* submap = insertion_submaps[i];
const mapping::SubmapId submap_id = submap_ids[i]; const mapping::SubmapId submap_id = submap_ids[i];
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose; insertion_submaps[i]->local_pose().inverse() * pose;
constraints_.push_back( constraints_.push_back(
Constraint{submap_id, Constraint{submap_id,
node_id, node_id,
@ -286,8 +286,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
} }
} }
if (finished_submap != nullptr) { if (newly_finished_submap) {
CHECK(finished_submap == insertion_submaps.front());
const mapping::SubmapId finished_submap_id = submap_ids.front(); const mapping::SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);

33
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -21,6 +21,7 @@
#include <functional> #include <functional>
#include <limits> #include <limits>
#include <map> #include <map>
#include <memory>
#include <unordered_map> #include <unordered_map>
#include <vector> #include <vector>
@ -62,16 +63,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose' // 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 // that will later be optimized. The 'pose' was determined by scan matching
// against the 'matching_submap' and the scan was inserted into the // against 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true',
// finished submap. It is also contained in 'insertion_submaps' for the last // this submap was inserted into for the last time.
// time. void AddScan(
void AddScan(common::Time time, common::Time time, const sensor::RangeData& range_data_in_tracking,
const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, int trajectory_id,
const transform::Rigid3d& pose, int trajectory_id, const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
const Submap* matching_submap, 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. // Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, common::Time time, void AddImuData(int trajectory_id, common::Time time,
@ -98,7 +97,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Likewise, all new scans are matched against submaps which are finished. // Likewise, all new scans are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished, kTrimmed }; enum class SubmapState { kActive, kFinished, kTrimmed };
struct SubmapData { struct SubmapData {
const Submap* submap = nullptr; std::shared_ptr<const Submap> submap;
// IDs of the scans that were inserted into this map together with // 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 // constraints for them. They are not to be matched again when this submap
@ -114,15 +113,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded( std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
int trajectory_id, const std::vector<const Submap*>& insertion_submaps) int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(int trajectory_id, void ComputeConstraintsForScan(
const Submap* matching_submap, int trajectory_id,
std::vector<const Submap*> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const Submap* finished_submap, bool newly_finished_submap, const transform::Rigid3d& pose)
const transform::Rigid3d& pose)
REQUIRES(mutex_); REQUIRES(mutex_);
// Computes constraints for a scan and submap pair. // Computes constraints for a scan and submap pair.

7
cartographer/mapping_3d/submaps.cc
View File

@ -371,10 +371,10 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
AddSubmap(transform::Rigid3d::Identity()); AddSubmap(transform::Rigid3d::Identity());
} }
const Submap* Submaps::Get(int index) const { std::shared_ptr<const Submap> Submaps::Get(int index) const {
CHECK_GE(index, 0); CHECK_GE(index, 0);
CHECK_LT(index, size()); CHECK_LT(index, size());
return submaps_[index].get(); return submaps_[index];
} }
int Submaps::size() const { return submaps_.size(); } int Submaps::size() const { return submaps_.size(); }
@ -407,8 +407,7 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
&submap->low_resolution_hybrid_grid_); &submap->low_resolution_hybrid_grid_);
++submap->num_range_data_; ++submap->num_range_data_;
} }
const Submap* const last_submap = Get(size() - 1); if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
if (last_submap->num_range_data_ == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(), AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment)); gravity_alignment));
} }

7
cartographer/mapping_3d/submaps.h
View File

@ -92,7 +92,7 @@ class Submaps {
Submaps(const Submaps&) = delete; Submaps(const Submaps&) = delete;
Submaps& operator=(const Submaps&) = delete; Submaps& operator=(const Submaps&) = delete;
const Submap* Get(int index) const; std::shared_ptr<const Submap> Get(int index) const;
int size() const; int size() const;
// Returns the index of the newest initialized Submap which can be // Returns the index of the newest initialized Submap which can be
@ -113,10 +113,7 @@ class Submaps {
void AddSubmap(const transform::Rigid3d& local_pose); void AddSubmap(const transform::Rigid3d& local_pose);
const proto::SubmapsOptions options_; const proto::SubmapsOptions options_;
std::vector<std::shared_ptr<Submap>> submaps_;
// 'submaps_' contains pointers, so that resizing the vector does not
// invalidate handed out Submap* pointers.
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_; RangeDataInserter range_data_inserter_;
}; };