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

Change submap_data_ in pose graph to MapById. (#593) 

* Change submap_data_ in pose graph to MapById.
master
Wolfgang Hess 2017-10-17 13:13:37 +02:00 committed by Christoph Schütte
parent fdda1dd091
commit e5f9815f67
10 changed files with 71 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
cartographer/mapping/id.h
View File

@ -79,40 +79,6 @@ inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
}
template <typename ValueType, typename IdType>
class NestedVectorsById {
public:
// Appends data to a trajectory, creating trajectories as needed.
IdType Append(int trajectory_id, const ValueType& value) {
data_.resize(std::max<size_t>(data_.size(), trajectory_id + 1));
const IdType id{trajectory_id,
static_cast<int>(data_[trajectory_id].size())};
data_[trajectory_id].push_back(value);
return id;
}
const ValueType& at(const IdType& id) const {
return data_.at(id.trajectory_id).at(GetIndex(id));
}
ValueType& at(const IdType& id) {
return data_.at(id.trajectory_id).at(GetIndex(id));
}
int num_trajectories() const { return static_cast<int>(data_.size()); }
int num_indices(int trajectory_id) const {
return static_cast<int>(data_.at(trajectory_id).size());
}
// TODO(whess): Remove once no longer needed.
const std::vector<std::vector<ValueType>> data() const { return data_; }
private:
static int GetIndex(const NodeId& id) { return id.node_index; }
static int GetIndex(const SubmapId& id) { return id.submap_index; }
std::vector<std::vector<ValueType>> data_;
};
template <typename IteratorType>
class Range {
public:
@ -161,13 +127,15 @@ class MapById {
}
explicit ConstIterator(const MapById& map_by_id, const IdType& id)
: current_trajectory_(
map_by_id.trajectories_.find(id.trajectory_id)),
: current_trajectory_(map_by_id.trajectories_.find(id.trajectory_id)),
end_trajectory_(map_by_id.trajectories_.end()) {
CHECK(current_trajectory_ != end_trajectory_);
if (current_trajectory_ != end_trajectory_) {
current_data_ =
current_trajectory_->second.data_.find(MapById::GetIndex(id));
CHECK(current_data_ != current_trajectory_->second.data_.end());
if (current_data_ == current_trajectory_->second.data_.end()) {
current_trajectory_ = end_trajectory_;
}
}
}
IdDataReference operator*() const {
@ -273,10 +241,9 @@ class MapById {
return IdType{trajectory_id, index};
}
// Returns an iterator to the element at 'id' which must exist.
ConstIterator FindChecked(const IdType& id) {
return ConstIterator(*this, id);
}
// Returns an iterator to the element at 'id' or the end iterator if it does
// not exist.
ConstIterator find(const IdType& id) { return ConstIterator(*this, id); }
// Inserts data (which must not exist already) into a trajectory.
void Insert(const IdType& id, const DataType& data) {

10
cartographer/mapping/id_test.cc
View File

@ -127,20 +127,22 @@ TEST(IdTest, InsertIntoMapById) {
EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42));
}
TEST(IdTest, FindCheckedNodeId) {
TEST(IdTest, FindNodeId) {
MapById<NodeId, int> map_by_id;
map_by_id.Append(42, 42);
map_by_id.Append(42, 43);
map_by_id.Append(42, 44);
CHECK_EQ(map_by_id.FindChecked(NodeId{42, 1})->data, 43);
CHECK_EQ(map_by_id.find(NodeId{42, 1})->data, 43);
EXPECT_TRUE(map_by_id.find(NodeId{42, 3}) == map_by_id.end());
}
TEST(IdTest, FindCheckedSubmapId) {
TEST(IdTest, FindSubmapId) {
MapById<SubmapId, int> map_by_id;
map_by_id.Append(42, 42);
map_by_id.Append(42, 43);
map_by_id.Append(42, 44);
CHECK_EQ(map_by_id.FindChecked(SubmapId{42, 1})->data, 43);
CHECK_EQ(map_by_id.find(SubmapId{42, 1})->data, 43);
EXPECT_TRUE(map_by_id.find(SubmapId{42, 3}) == map_by_id.end());
}
} // namespace

11
cartographer/mapping/map_builder.cc
View File

@ -125,20 +125,11 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
std::to_string(num_trajectory_builders()) + " trajectories.";
}
const int num_submaps =
sparse_pose_graph_->num_submaps(submap_id.trajectory_id);
if (submap_id.submap_index < 0 || submap_id.submap_index >= num_submaps) {
return "Requested submap " + std::to_string(submap_id.submap_index) +
" from trajectory " + std::to_string(submap_id.trajectory_id) +
" but there are only " + std::to_string(num_submaps) +
" submaps in this trajectory.";
}
const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id);
if (submap_data.submap == nullptr) {
return "Requested submap " + std::to_string(submap_id.submap_index) +
" from trajectory " + std::to_string(submap_id.trajectory_id) +
" but it has been trimmed.";
" but it does not exist: maybe it has been trimmed.";
}
submap_data.submap->ToResponseProto(submap_data.pose, response);
return "";

6
cartographer/mapping/sparse_pose_graph.h
View File

@ -99,11 +99,9 @@ class SparsePoseGraph {
// Gets the current trajectory clusters.
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Return the number of submaps for the given 'trajectory_id'.
virtual int num_submaps(int trajectory_id) = 0;
// Returns the current optimized transform and submap itself for the given
// 'submap_id'.
// 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does
// not exist (anymore).
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
// Returns data for all Submaps by trajectory.

3
cartographer/mapping/trajectory_node.cc
View File

@ -44,8 +44,7 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) {
proto.add_rotational_scan_matcher_histogram(
constant_data.rotational_scan_matcher_histogram(i));
}
*proto.mutable_local_pose() =
transform::ToProto(constant_data.local_pose);
*proto.mutable_local_pose() = transform::ToProto(constant_data.local_pose);
return proto;
}

65
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -107,12 +107,9 @@ void SparsePoseGraph::AddScan(
++num_trajectory_nodes_;
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
submap_data_.num_indices(trajectory_id) == 0 ||
submap_data_
.at(mapping::SubmapId{
trajectory_id, submap_data_.num_indices(trajectory_id) - 1})
.submap != insertion_submaps.back()) {
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
const mapping::SubmapId submap_id =
@ -249,16 +246,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
Constraint::INTRA_SUBMAP});
}
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
++trajectory_id) {
for (int submap_index = 0;
submap_index < submap_data_.num_indices(trajectory_id);
++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index};
if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id);
}
for (const auto& submap_id_data : submap_data_) {
if (submap_id_data.data.state == SubmapState::kFinished) {
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id_data.id);
}
}
@ -489,8 +480,7 @@ void SparsePoseGraph::RunOptimization() {
const mapping::NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it =
std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
++node_it) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
@ -544,14 +534,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (trajectory_id >= submap_data_.num_trajectories()) {
return 0;
}
return submap_data_.num_indices(trajectory_id);
}
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
@ -561,20 +543,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
all_submap_data(submap_data_.num_trajectories());
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
++trajectory_id) {
all_submap_data[trajectory_id].reserve(
submap_data_.num_indices(trajectory_id));
for (int submap_index = 0;
submap_index < submap_data_.num_indices(trajectory_id);
++submap_index) {
all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
mapping::SubmapId{trajectory_id, submap_index}));
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
for (const int trajectory_id : submap_data_.trajectory_ids()) {
result.resize(trajectory_id + 1);
for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
}
}
return all_submap_data;
return result;
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@ -597,10 +574,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) {
return {};
}
auto submap = submap_data_.at(submap_id).submap;
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, transform::Embed3D(
@ -666,11 +644,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
}
// Mark the submap with 'submap_id' as trimmed and remove its data.
auto& submap_data = parent_->submap_data_.at(submap_id);
CHECK(submap_data.state == SubmapState::kFinished);
submap_data.state = SubmapState::kTrimmed;
CHECK(submap_data.submap != nullptr);
submap_data.submap.reset();
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(submap_id);
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id);

5
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// The current state of the submap in the background threads. When this
// transitions to kFinished, all scans are tried to match against this submap.
// Likewise, all new scans are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished, kTrimmed };
enum class SubmapState { kActive, kFinished };
struct SubmapData {
std::shared_ptr<const Submap> submap;
@ -205,7 +204,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.
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_
GUARDED_BY(mutex_);
// Data that are currently being shown.

3
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -132,7 +132,8 @@ std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
histograms_at_angles.emplace_back(
node.constant_data->rotational_scan_matcher_histogram,
transform::GetYaw(
node.global_pose * transform::Rigid3d::Rotation(
node.global_pose *
transform::Rigid3d::Rotation(
node.constant_data->gravity_alignment.inverse())));
}
return histograms_at_angles;

68
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -104,12 +104,9 @@ void SparsePoseGraph::AddScan(
++num_trajectory_nodes_;
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
submap_data_.num_indices(trajectory_id) == 0 ||
submap_data_
.at(mapping::SubmapId{
trajectory_id, submap_data_.num_indices(trajectory_id) - 1})
.submap != insertion_submaps.back()) {
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
const mapping::SubmapId submap_id =
@ -185,7 +182,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
submap_data_.at(submap_id).node_ids) {
submap_nodes.push_back(mapping::TrajectoryNode{
trajectory_nodes_.at(submap_node_id).constant_data,
inverse_submap_pose * trajectory_nodes_.at(submap_node_id).global_pose});
inverse_submap_pose *
trajectory_nodes_.at(submap_node_id).global_pose});
}
const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
@ -268,16 +266,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
Constraint::INTRA_SUBMAP});
}
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
++trajectory_id) {
for (int submap_index = 0;
submap_index < submap_data_.num_indices(trajectory_id);
++submap_index) {
const mapping::SubmapId submap_id{trajectory_id, submap_index};
if (submap_data_.at(submap_id).state == SubmapState::kFinished) {
CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id);
}
for (const auto& submap_id_data : submap_data_) {
if (submap_id_data.data.state == SubmapState::kFinished) {
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id_data.id);
}
}
@ -524,8 +516,7 @@ void SparsePoseGraph::RunOptimization() {
const mapping::NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it =
std::next(trajectory_nodes_.FindChecked(last_optimized_node_id));
auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
++node_it) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
@ -572,14 +563,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (trajectory_id >= submap_data_.num_trajectories()) {
return 0;
}
return submap_data_.num_indices(trajectory_id);
}
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
@ -589,20 +572,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
all_submap_data(submap_data_.num_trajectories());
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
++trajectory_id) {
all_submap_data[trajectory_id].reserve(
submap_data_.num_indices(trajectory_id));
for (int submap_index = 0;
submap_index < submap_data_.num_indices(trajectory_id);
++submap_index) {
all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
mapping::SubmapId{trajectory_id, submap_index}));
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>> result;
for (const int trajectory_id : submap_data_.trajectory_ids()) {
result.resize(trajectory_id + 1);
for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) {
result[trajectory_id].resize(submap_id_data.id.submap_index + 1);
result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id);
}
}
return all_submap_data;
return result;
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
@ -624,10 +602,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) {
return {};
}
auto submap = submap_data_.at(submap_id).submap;
auto submap = it->data.submap;
if (optimized_submap_transforms_.Contains(submap_id)) {
// We already have an optimized pose.
return {submap, optimized_submap_transforms_.at(submap_id).pose};
@ -692,11 +671,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
}
// Mark the submap with 'submap_id' as trimmed and remove its data.
auto& submap_data = parent_->submap_data_.at(submap_id);
CHECK(submap_data.state == SubmapState::kFinished);
submap_data.state = SubmapState::kTrimmed;
CHECK(submap_data.submap != nullptr);
submap_data.submap.reset();
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
parent_->submap_data_.Trim(submap_id);
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id);

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// The current state of the submap in the background threads. When this
// transitions to kFinished, all scans are tried to match against this submap.
// Likewise, all new scans are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished, kTrimmed };
enum class SubmapState { kActive, kFinished };
struct SubmapData {
std::shared_ptr<const Submap> submap;
@ -209,7 +208,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.
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_
GUARDED_BY(mutex_);
// Data that are currently being shown.