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

Change GetTrajectoryNodes() to MapById. (#603)

master
Christoph Schütte 2017-10-19 14:50:58 +02:00 committed by GitHub
parent 49ead6055c
commit 7c03467a78
9 changed files with 76 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
cartographer/mapping/map_builder.cc
View File

@ -153,25 +153,16 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
}
// Next we serialize all node data.
{
const auto trajectory_nodes = sparse_pose_graph_->GetTrajectoryNodes();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(trajectory_nodes.size());
++trajectory_id) {
for (int node_index = 0;
node_index !=
static_cast<int>(trajectory_nodes[trajectory_id].size());
++node_index) {
proto::SerializedData proto;
auto* const node_proto = proto.mutable_node();
// TODO(whess): Handle trimmed data.
node_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
node_proto->mutable_node_id()->set_node_index(node_index);
*node_proto->mutable_node_data() =
ToProto(*trajectory_nodes[trajectory_id][node_index].constant_data);
// TODO(whess): Only enable optionally? Resulting pbstream files will be
// a lot larger now.
writer->WriteProto(proto);
}
for (const auto& node_id_data : sparse_pose_graph_->GetTrajectoryNodes()) {
proto::SerializedData proto;
auto* const node_proto = proto.mutable_node();
node_proto->mutable_node_id()->set_trajectory_id(
node_id_data.id.trajectory_id);
node_proto->mutable_node_id()->set_node_index(
node_id_data.id.node_index);
*node_proto->mutable_node_data() =
ToProto(*node_id_data.data.constant_data);
writer->WriteProto(proto);
}
// TODO(whess): Serialize additional sensor data: IMU, odometry.
}
@ -206,18 +197,26 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
}
}
MapById<NodeId, transform::Rigid3d> node_poses;
for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
for (const proto::Trajectory::Node& node_proto :
trajectory_proto.node()) {
node_poses.Insert(NodeId{trajectory_proto.trajectory_id(),
node_proto.node_index()},
transform::ToRigid3(node_proto.pose()));
}
}
for (;;) {
proto::SerializedData proto;
if (!reader->ReadProto(&proto)) {
break;
}
if (proto.has_node()) {
const auto& pose_graph_node =
pose_graph.trajectory(proto.node().node_id().trajectory_id())
.node(proto.node().node_id().node_index());
const transform::Rigid3d pose =
transform::ToRigid3(pose_graph_node.pose());
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, pose,
const transform::Rigid3d node_pose =
node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
proto.node().node_id().node_index()});
sparse_pose_graph_->AddNodeFromProto(map_trajectory_id, node_pose,
proto.node());
}
if (proto.has_submap()) {

5
cartographer/mapping/proto/trajectory.proto
View File

@ -21,8 +21,11 @@ import "cartographer/transform/proto/transform.proto";
option java_outer_classname = "TrajectoryOuterClass";
message Trajectory {
// NEXT_ID: 7
// NEXT_ID: 8
message Node {
// Index of this node within its trajectory.
optional int32 node_index = 7;
optional int64 timestamp = 1;
// Transform from tracking to global map frame.

37
cartographer/mapping/sparse_pose_graph.cc
View File

@ -77,28 +77,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
return trajectory_protos.at(trajectory_id);
};
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
const auto all_trajectory_nodes = GetTrajectoryNodes();
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
++trajectory_id) {
auto* const trajectory_proto = trajectory(trajectory_id);
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
for (size_t old_node_index = 0;
old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
const auto& node = single_trajectory_nodes[old_node_index];
if (node.constant_data != nullptr) {
node_id_remapping[NodeId{static_cast<int>(trajectory_id),
static_cast<int>(old_node_index)}] =
NodeId{static_cast<int>(trajectory_id),
static_cast<int>(trajectory_proto->node_size())};
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(
common::ToUniversal(node.constant_data->time));
*node_proto->mutable_pose() = transform::ToProto(node.global_pose);
}
}
for (const auto& node_id_data : GetTrajectoryNodes()) {
CHECK(node_id_data.data.constant_data != nullptr);
auto* const node_proto =
trajectory(node_id_data.id.trajectory_id)->add_node();
node_proto->set_node_index(node_id_data.id.node_index);
node_proto->set_timestamp(
common::ToUniversal(node_id_data.data.constant_data->time));
*node_proto->mutable_pose() =
transform::ToProto(node_id_data.data.global_pose);
}
for (const auto& submap_id_data : GetAllSubmapData()) {
@ -123,10 +110,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint_proto->mutable_submap_id()->set_submap_index(
constraint.submap_id.submap_index);
const NodeId node_id = node_id_remapping.at(constraint.node_id);
constraint_proto->mutable_node_id()->set_trajectory_id(
node_id.trajectory_id);
constraint_proto->mutable_node_id()->set_node_index(node_id.node_index);
constraint.node_id.trajectory_id);
constraint_proto->mutable_node_id()->set_node_index(
constraint.node_id.node_index);
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}

5
cartographer/mapping/sparse_pose_graph.h
View File

@ -105,8 +105,7 @@ class SparsePoseGraph {
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
// Returns data for all submaps.
virtual mapping::MapById<mapping::SubmapId, SubmapData>
GetAllSubmapData() = 0;
virtual MapById<SubmapId, SubmapData> GetAllSubmapData() = 0;
// Returns the transform converting data in the local map frame (i.e. the
// continuous, non-loop-closed frame) into the global map frame (i.e. the
@ -114,7 +113,7 @@ class SparsePoseGraph {
virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
// Returns the current optimized trajectories.
virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() = 0;
// Serializes the constraints and trajectories.
proto::SparsePoseGraph ToProto();

13
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -490,19 +490,10 @@ void SparsePoseGraph::RunOptimization() {
optimized_submap_transforms_ = submap_data;
}
std::vector<std::vector<mapping::TrajectoryNode>>
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() {
// TODO(cschuet): Rewrite downstream code and switch to MapById.
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
nodes.resize(trajectory_id + 1);
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
nodes.at(trajectory_id).resize(node.id.node_index + 1);
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
}
}
return nodes;
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
private:

38
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -173,6 +173,11 @@ class SparsePoseGraphTest : public ::testing::Test {
MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
}
template <typename Range>
std::vector<int> ToVectorInt(const Range& range) {
return std::vector<int>(range.begin(), range.end());
}
sensor::PointCloud point_cloud_;
std::unique_ptr<ActiveSubmaps> active_submaps_;
common::ThreadPool thread_pool_;
@ -183,7 +188,7 @@ class SparsePoseGraphTest : public ::testing::Test {
TEST_F(SparsePoseGraphTest, EmptyMap) {
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
EXPECT_THAT(nodes.size(), ::testing::Eq(0u));
EXPECT_TRUE(nodes.empty());
}
TEST_F(SparsePoseGraphTest, NoMovement) {
@ -192,13 +197,14 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
MoveRelative(transform::Rigid2d::Identity());
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
EXPECT_THAT(nodes[0].size(), ::testing::Eq(3u));
EXPECT_THAT(nodes[0][0].global_pose,
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
EXPECT_THAT(nodes.at(mapping::NodeId{0, 0}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes[0][1].global_pose,
EXPECT_THAT(nodes.at(mapping::NodeId{0, 1}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes[0][2].global_pose,
EXPECT_THAT(nodes.at(mapping::NodeId{0, 2}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
}
@ -212,10 +218,13 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 4; ++i) {
EXPECT_THAT(poses[i],
IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
IsNearly(transform::Project2D(
nodes.at(mapping::NodeId{0, i}).global_pose),
1e-2))
<< i;
}
}
@ -230,10 +239,13 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 5; ++i) {
EXPECT_THAT(poses[i],
IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2))
IsNearly(transform::Project2D(
nodes.at(mapping::NodeId{0, i}).global_pose),
1e-2))
<< i;
}
}
@ -255,13 +267,15 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) {
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(nodes.size(), ::testing::Eq(1u));
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
transform::Rigid2d true_movement =
ground_truth.front().inverse() * ground_truth.back();
transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
transform::Rigid2d error_before = movement_before.inverse() * true_movement;
transform::Rigid3d optimized_movement =
nodes[0].front().global_pose.inverse() * nodes[0].back().global_pose;
nodes.BeginOfTrajectory(0)->data.global_pose.inverse() *
std::prev(nodes.EndOfTrajectory(0))->data.global_pose;
transform::Rigid2d optimized_error =
transform::Project2D(optimized_movement).inverse() * true_movement;
EXPECT_THAT(std::abs(optimized_error.normalized_angle()),

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -531,19 +531,10 @@ void SparsePoseGraph::RunOptimization() {
}
}
std::vector<std::vector<mapping::TrajectoryNode>>
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() {
// TODO(cschuet): Rewrite downstream code and switch to MapById.
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> nodes;
for (const int trajectory_id : trajectory_nodes_.trajectory_ids()) {
nodes.resize(trajectory_id + 1);
for (const auto& node : trajectory_nodes_.trajectory(trajectory_id)) {
nodes.at(trajectory_id).resize(node.id.node_index + 1);
nodes.at(trajectory_id).at(node.id.node_index) = node.data;
}
}
return nodes;
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
private: