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

Multi-trajectory SparsePoseGraph::ToProto. (#227) 

Multi-trajectory SparsePoseGraph::ToProto.
Remove obsolete TrajectoryNode::ToProto.
Add GetSubmapStates.
master
Mac Mason 2017-04-19 11:01:03 -04:00 committed by Wolfgang Hess
parent 4f1914b2b4
commit 92d360a8f2
9 changed files with 232 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
cartographer/mapping/sparse_pose_graph.cc
View File

@ -16,8 +16,6 @@
#include "cartographer/mapping/sparse_pose_graph.h"
#include <unordered_map>
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
@ -38,19 +36,46 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
LOG(FATAL) << "Unsupported tag.";
}
std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes) {
std::vector<std::vector<TrajectoryNode>> trajectories;
std::unordered_map<const Submaps*, int> trajectory_ids;
for (const auto& node : trajectory_nodes) {
const auto* trajectory = node.constant_data->trajectory;
if (trajectory_ids.emplace(trajectory, trajectories.size()).second) {
trajectories.push_back({node});
} else {
trajectories[trajectory_ids[trajectory]].push_back(node);
}
std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
const std::vector<const Submaps*>& trajectories) {
std::unordered_map<const Submaps*, int> result;
for (const auto& trajectory : trajectories) {
result.emplace(trajectory, result.size());
}
return result;
}
void GroupTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
std::vector<std::pair<int, int>>* new_indices) {
CHECK_NOTNULL(grouped_nodes)->clear();
CHECK_NOTNULL(new_indices)->clear();
grouped_nodes->resize(trajectory_ids.size());
for (const auto& node : trajectory_nodes) {
const int id = trajectory_ids.at(node.constant_data->trajectory);
new_indices->emplace_back(id, (*grouped_nodes)[id].size());
(*grouped_nodes)[id].push_back(node);
}
}
// TODO(macmason): This function is very nearly a copy of GroupTrajectoryNodes.
// Consider refactoring them to share an implementation.
void GroupSubmapStates(
const std::vector<SparsePoseGraph::SubmapState>& submap_states,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::pair<int, int>>* new_indices) {
CHECK_NOTNULL(new_indices)->clear();
std::vector<int> submap_group_sizes(trajectory_ids.size(), 0);
for (const auto& submap_state : submap_states) {
const int id = trajectory_ids.at(submap_state.trajectory);
new_indices->emplace_back(id, submap_group_sizes[id]);
submap_group_sizes[id]++;
}
return trajectories;
}
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
@ -74,32 +99,62 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
const auto trajectory_nodes = GetTrajectoryNodes();
std::vector<const Submaps*> submap_pointers;
for (const auto& trajectory_node : trajectory_nodes) {
submap_pointers.push_back(trajectory_node.constant_data->trajectory);
}
const auto trajectory_ids = ComputeTrajectoryIds(submap_pointers);
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> grouped_node_indices;
GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes,
&grouped_node_indices);
std::vector<std::pair<int, int>> grouped_submap_indices;
GroupSubmapStates(GetSubmapStates(), trajectory_ids, &grouped_submap_indices);
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
transform::ToProto(constraint.pose.zbar_ij);
constraint_proto->mutable_sqrt_lambda()->Reserve(36);
for (int i = 0; i != 36; ++i) {
constraint_proto->mutable_sqrt_lambda()->Add(0.);
}
Eigen::Map<Eigen::Matrix<double, 6, 6>>(
constraint_proto->mutable_sqrt_lambda()->mutable_data()) =
constraint.pose.sqrt_Lambda_ij;
// TODO(whess): Support multi-trajectory.
constraint_proto->mutable_submap_id()->set_submap_index(constraint.i);
constraint_proto->mutable_scan_id()->set_scan_index(constraint.j);
constraint_proto->mutable_submap_id()->set_trajectory_id(
grouped_submap_indices[constraint.i].first);
constraint_proto->mutable_submap_id()->set_submap_index(
grouped_submap_indices[constraint.i].second);
constraint_proto->mutable_scan_id()->set_trajectory_id(
grouped_node_indices[constraint.j].first);
constraint_proto->mutable_scan_id()->set_scan_index(
grouped_node_indices[constraint.j].second);
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}
// TODO(whess): Support multi-trajectory.
proto::Trajectory* const trajectory = proto.add_trajectory();
*trajectory = mapping::ToProto(GetTrajectoryNodes());
const std::vector<std::vector<const Submaps*>> components =
GetConnectedTrajectories();
CHECK_EQ(components.size(), 1);
CHECK_EQ(components[0].size(), 1);
const Submaps* const submaps = components[0][0];
for (const auto& transform : GetSubmapTransforms(*submaps)) {
*trajectory->add_submap()->mutable_pose() = transform::ToProto(transform);
for (const auto& group : grouped_nodes) {
auto* trajectory_proto = proto.add_trajectory();
for (const auto& node : group) {
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(common::ToUniversal(node.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node.pose * node.constant_data->tracking_to_pose);
}
const Submaps* const submaps = group[0].constant_data->trajectory;
for (const auto& transform : GetSubmapTransforms(*submaps)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}
}
return proto;

55
cartographer/mapping/sparse_pose_graph.h
View File

@ -17,6 +17,9 @@
#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
#include <set>
#include <unordered_map>
#include <utility>
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
@ -33,9 +36,22 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// Splits TrajectoryNodes by ID.
std::vector<std::vector<TrajectoryNode>> SplitTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes);
// Construct a mapping from trajectory (ie Submaps*) to an integer. These
// values are used to track trajectory identity between scans and submaps.
std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
const std::vector<const Submaps*>& trajectories);
// TrajectoryNodes are provided in a flat vector, but serialization requires
// that we group them by trajectory. This groups the elements of
// 'trajectory_nodes' into 'grouped_nodes' (so that (*grouped_nodes)[i]
// contains a complete single trajectory). The re-indexing done is stored in
// 'new_indices', such that 'trajectory_nodes[i]' landed in
// '(*grouped_nodes)[new_indices[i].first][new_indices[i].second]'.
void GroupTrajectoryNodes(
const std::vector<TrajectoryNode>& trajectory_nodes,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::vector<TrajectoryNode>>* grouped_nodes,
std::vector<std::pair<int, int>>* new_indices);
class SparsePoseGraph {
public:
@ -63,6 +79,24 @@ class SparsePoseGraph {
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct SubmapState {
const Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<int> scan_indices;
// Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match
// against this submap. Likewise, all new scans are matched against submaps
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const Submaps* trajectory = nullptr;
};
SparsePoseGraph() {}
virtual ~SparsePoseGraph() {}
@ -95,15 +129,24 @@ class SparsePoseGraph {
// Returns the current optimized trajectory.
virtual std::vector<TrajectoryNode> GetTrajectoryNodes() = 0;
// TODO(macmason, wohe): Consider replacing this with a GroupSubmapStates,
// which would have better separation of concerns.
virtual std::vector<SubmapState> GetSubmapStates() = 0;
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
// Serializes the constraints and the computed trajectory.
//
// TODO(whess): Support multiple trajectories.
// Serializes the constraints and trajectories.
proto::SparsePoseGraph ToProto();
};
// Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be
// grouped by trajectory. The arguments are just as in 'GroupTrajectoryNodes'.
void GroupSubmapStates(
const std::vector<SparsePoseGraph::SubmapState>& submap_states,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::pair<int, int>>* new_indices);
} // namespace mapping
} // namespace cartographer

42
cartographer/mapping/sparse_pose_graph_test.cc
View File

@ -16,7 +16,9 @@
#include "cartographer/mapping/sparse_pose_graph.h"
#include <algorithm>
#include <deque>
#include <vector>
#include "glog/logging.h"
#include "gmock/gmock.h"
@ -27,6 +29,7 @@ namespace {
class FakeSubmaps : public Submaps {
public:
~FakeSubmaps() override {}
const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; }
int size() const override { LOG(FATAL) << "Not implemented."; }
@ -38,7 +41,7 @@ class FakeSubmaps : public Submaps {
}
};
TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
TEST(SparsePoseGraphTest, TrajectoryFunctions) {
std::vector<TrajectoryNode> trajectory_nodes;
const std::vector<FakeSubmaps> submaps(5);
std::deque<TrajectoryNode::ConstantData> constant_data;
@ -52,15 +55,42 @@ TEST(SparsePoseGraphTest, SplitTrajectoryNodes) {
trajectory_nodes.push_back(node);
}
}
std::vector<std::vector<TrajectoryNode>> split_trajectory_nodes =
SplitTrajectoryNodes(trajectory_nodes);
EXPECT_EQ(split_trajectory_nodes.size(), submaps.size());
std::vector<const Submaps*> submap_pointers;
for (const auto& submap : submaps) {
submap_pointers.push_back(&submap);
}
const auto index_map = ComputeTrajectoryIds(submap_pointers);
ASSERT_EQ(submaps.size(), index_map.size());
for (const auto& kv : index_map) {
const auto pointer_iterator =
std::find(submap_pointers.begin(), submap_pointers.end(), kv.first);
ASSERT_TRUE(pointer_iterator != submap_pointers.end());
const auto pointer_index = pointer_iterator - submap_pointers.begin();
EXPECT_EQ(kv.second, pointer_index);
}
std::vector<std::vector<TrajectoryNode>> grouped_nodes;
std::vector<std::pair<int, int>> new_indices;
GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes,
&new_indices);
ASSERT_EQ(grouped_nodes.size(), submaps.size());
for (size_t i = 0; i < submaps.size(); ++i) {
EXPECT_EQ(split_trajectory_nodes[i].size(), kNumTrajectoryNodes);
for (const auto& node : split_trajectory_nodes[i]) {
EXPECT_EQ(grouped_nodes[i].size(), kNumTrajectoryNodes);
for (const auto& node : grouped_nodes[i]) {
EXPECT_EQ(node.constant_data->trajectory, &submaps[i]);
}
}
ASSERT_EQ(trajectory_nodes.size(), new_indices.size());
for (size_t i = 0; i < new_indices.size(); ++i) {
const auto index_pair = new_indices[i];
EXPECT_EQ(trajectory_nodes[i].constant_data->trajectory,
grouped_nodes[index_pair.first][index_pair.second]
.constant_data->trajectory);
}
}
} // namespace

37
cartographer/mapping/trajectory_node.cc
View File

@ -1,37 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping {
proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes) {
proto::Trajectory trajectory;
for (const auto& node : nodes) {
const auto& data = *node.constant_data;
auto* node_proto = trajectory.add_node();
node_proto->set_timestamp(common::ToUniversal(data.time));
*node_proto->mutable_pose() =
transform::ToProto(node.pose * data.tracking_to_pose);
}
return trajectory;
}
} // namespace mapping
} // namespace cartographer

3
cartographer/mapping/trajectory_node.h
View File

@ -22,7 +22,6 @@
#include "Eigen/Core"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
@ -67,8 +66,6 @@ struct TrajectoryNodes {
std::vector<mapping::TrajectoryNode> trajectory_nodes;
};
proto::Trajectory ToProto(const std::vector<TrajectoryNode>& nodes);
} // namespace mapping
} // namespace cartographer

50
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -294,30 +294,25 @@ void SparsePoseGraph::WaitForAllComputations() {
common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start =
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
},
common::FromSeconds(1.))) {
while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
}, common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
<< "%...";
num_finished_scans_at_start) << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
constraint_builder_.WhenDone([this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
locker.Await([&notification]() { return notification; });
}
@ -351,13 +346,13 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
extrapolation_transforms[trajectory] =
transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
*trajectory).back() *
ExtrapolateSubmapTransforms(
optimized_submap_transforms_, *trajectory)
.back()
.inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@ -396,6 +391,11 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
common::MutexLocker locker(&mutex_);
return submap_states_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}

20
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -96,27 +96,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override;
private:
struct SubmapState {
const mapping::Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<int> scan_indices;
// Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match
// against this submap. Likewise, all new scans are matched against submaps
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const mapping::Submaps* trajectory = nullptr;
};
// Handles a new work item.
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);

60
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -285,30 +285,25 @@ void SparsePoseGraph::WaitForAllComputations() {
common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start =
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
},
common::FromSeconds(1.))) {
while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
}, common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start)
<< "%...";
num_finished_scans_at_start) << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
constraint_builder_.WhenDone([this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
locker.Await([&notification]() { return notification; });
}
@ -341,13 +336,13 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
extrapolation_transforms[trajectory] =
transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
*trajectory).back() *
ExtrapolateSubmapTransforms(
optimized_submap_transforms_, *trajectory)
.back()
.inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
@ -386,6 +381,21 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<mapping::SparsePoseGraph::SubmapState>
SparsePoseGraph::GetSubmapStates() {
std::vector<mapping::SparsePoseGraph::SubmapState> result;
common::MutexLocker locker(&mutex_);
for (const auto& submap_state : submap_states_) {
mapping::SparsePoseGraph::SubmapState entry;
entry.submap = submap_state.submap;
entry.scan_indices = submap_state.scan_indices;
entry.finished = submap_state.finished;
entry.trajectory = submap_state.trajectory;
result.push_back(entry);
}
return result;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -95,9 +95,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override;
std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
EXCLUDES(mutex_);
std::vector<SubmapState> GetSubmapStates() override;
std::vector<Constraint> constraints() override;
private:
// This is 'mapping::SubmapState', but with the 3D versions of 'submap' and
// 'trajectory'.
struct SubmapState {
const Submap* submap = nullptr;