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 {
const Submap* submap;
std::shared_ptr<const Submap> submap;
transform::Rigid3d pose;
};

7
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -48,16 +48,11 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
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);
std::move(insertion_result->insertion_submaps));
}
void GlobalTrajectoryBuilder::AddImuData(

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

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

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -41,8 +41,7 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
common::Time time;
const Submap* matching_submap;
std::vector<const Submap*> insertion_submaps;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d;
sensor::RangeData range_data_in_tracking_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(
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());
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
@ -98,9 +98,8 @@ std::vector<mapping::SubmapId> 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 Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps,
const Submap* const finished_submap) {
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@ -136,9 +135,12 @@ void SparsePoseGraph::AddScan(
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_) {
ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps,
finished_submap, pose);
ComputeConstraintsForScan(trajectory_id, insertion_submaps,
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 &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
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_connectivity_);
} else {
@ -190,7 +192,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.point_cloud_pose;
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,
initial_relative_pose);
}
@ -216,9 +218,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
}
void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const transform::Rigid2d& pose) {
const int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap, const transform::Rigid2d& pose) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
@ -228,7 +230,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose;
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
@ -241,12 +245,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const Submap* submap = insertion_submaps[i];
const mapping::SubmapId submap_id = submap_ids[i];
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
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,
node_id,
{transform::Embed3D(constraint_transform),
@ -268,8 +272,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
if (finished_submap != nullptr) {
CHECK(finished_submap == insertion_submaps.front());
if (newly_finished_submap) {
const mapping::SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
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& operator=(const SparsePoseGraph&) = delete;
// Adds a new 'range_data_in_pose' observation at 'time', and a 'pose'
// 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'. 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 Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps,
const Submap* finished_submap) EXCLUDES(mutex_);
// Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' 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 'insertion_submaps.front()' and the scan was inserted
// into the 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
// 'true', this submap was inserted into 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 std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
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.
enum class SubmapState { kActive, kFinished, kTrimmed };
struct SubmapData {
const Submap* submap = nullptr;
std::shared_ptr<const Submap> submap;
// 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
@ -115,15 +114,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
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_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(int trajectory_id,
const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap,
const transform::Rigid2d& pose)
void ComputeConstraintsForScan(
int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap, const transform::Rigid2d& pose)
REQUIRES(mutex_);
// 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(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
std::vector<const Submap*> insertion_submaps;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
@ -162,13 +160,9 @@ class SparsePoseGraphTest : public ::testing::Test {
submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
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);
sparse_pose_graph_->AddScan(
common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
pose_estimate, kTrajectoryId, std::move(insertion_submaps));
}
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_);
++submap->num_range_data_;
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data_ == options_.num_range_data()) {
if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
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_LT(index, size());
return submaps_[index].get();
return submaps_[index];
}
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& operator=(const Submaps&) = delete;
const Submap* Get(int index) const;
std::shared_ptr<const Submap> Get(int index) const;
int size() const;
// Returns the index of the newest initialized Submap which can be
@ -98,8 +98,7 @@ class Submaps {
void AddSubmap(const Eigen::Vector2f& origin);
const proto::SubmapsOptions options_;
std::vector<std::unique_ptr<Submap>> submaps_;
std::vector<std::shared_ptr<Submap>> submaps_;
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) {
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,
finished_submap);
std::move(insertion_result->insertion_submaps));
}
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(
time, &pose_prediction, &unused_covariance_prediction);
const Submap* const matching_submap =
std::shared_ptr<const Submap> matching_submap =
submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction;
@ -219,9 +219,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
std::vector<const Submap*> insertion_submaps;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
@ -231,7 +229,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
pose_tracker_->gravity_orientation());
return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation,
matching_submap, insertion_submaps});
std::move(insertion_submaps)});
}
} // namespace mapping_3d

3
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

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

33
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -54,7 +54,7 @@ SparsePoseGraph::~SparsePoseGraph() {
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
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());
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
@ -95,12 +95,9 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
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 Submap* const finished_submap) {
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
trajectory_id,
@ -133,9 +130,12 @@ void SparsePoseGraph::AddScan(
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_) {
ComputeConstraintsForScan(trajectory_id, matching_submap, insertion_submaps,
finished_submap, pose);
ComputeConstraintsForScan(trajectory_id, insertion_submaps,
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
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
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,
submap_nodes, initial_relative_pose.rotation(),
&trajectory_connectivity_);
@ -210,7 +210,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
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,
submap_nodes, initial_relative_pose);
}
@ -235,18 +235,19 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
}
void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const transform::Rigid3d& pose) {
const int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap, const transform::Rigid3d& pose) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data()
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose().inverse() * pose;
insertion_submaps.front()->local_pose().inverse() * pose;
const mapping::NodeId node_id{
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,
scan_data->time, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const Submap* submap = insertion_submaps[i];
const mapping::SubmapId submap_id = submap_ids[i];
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose;
insertion_submaps[i]->local_pose().inverse() * pose;
constraints_.push_back(
Constraint{submap_id,
node_id,
@ -286,8 +286,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
if (finished_submap != nullptr) {
CHECK(finished_submap == insertion_submaps.front());
if (newly_finished_submap) {
const mapping::SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive);

33
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -21,6 +21,7 @@
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <unordered_map>
#include <vector>
@ -62,16 +63,14 @@ 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'. 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,
const Submap* finished_submap) EXCLUDES(mutex_);
// against 'insertion_submaps.front()' and the scan was inserted into the
// 'insertion_submaps'. If 'insertion_submaps.front().finished()' is 'true',
// this submap was inserted into for the last time.
void AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
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.
enum class SubmapState { kActive, kFinished, kTrimmed };
struct SubmapData {
const Submap* submap = nullptr;
std::shared_ptr<const Submap> submap;
// 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
@ -114,15 +113,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
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_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(int trajectory_id,
const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap,
const transform::Rigid3d& pose)
void ComputeConstraintsForScan(
int trajectory_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap, const transform::Rigid3d& pose)
REQUIRES(mutex_);
// 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());
}
const Submap* Submaps::Get(int index) const {
std::shared_ptr<const Submap> Submaps::Get(int index) const {
CHECK_GE(index, 0);
CHECK_LT(index, size());
return submaps_[index].get();
return submaps_[index];
}
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->num_range_data_;
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data_ == options_.num_range_data()) {
if (submaps_.back()->num_range_data_ == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment));
}

7
cartographer/mapping_3d/submaps.h
View File

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