From 7c03467a78f578d9a5d276cb9cd8a512778b896f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 19 Oct 2017 14:50:58 +0200 Subject: [PATCH] Change GetTrajectoryNodes() to MapById. (#603) --- cartographer/mapping/map_builder.cc | 49 +++++++++---------- cartographer/mapping/proto/trajectory.proto | 5 +- cartographer/mapping/sparse_pose_graph.cc | 37 +++++--------- cartographer/mapping/sparse_pose_graph.h | 5 +- cartographer/mapping_2d/sparse_pose_graph.cc | 13 +---- cartographer/mapping_2d/sparse_pose_graph.h | 4 +- .../mapping_2d/sparse_pose_graph_test.cc | 38 +++++++++----- cartographer/mapping_3d/sparse_pose_graph.cc | 13 +---- cartographer/mapping_3d/sparse_pose_graph.h | 4 +- 9 files changed, 76 insertions(+), 92 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index a9e31b7..a50a295 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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(trajectory_nodes.size()); - ++trajectory_id) { - for (int node_index = 0; - node_index != - static_cast(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 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()) { diff --git a/cartographer/mapping/proto/trajectory.proto b/cartographer/mapping/proto/trajectory.proto index f3d597f..2f72a47 100644 --- a/cartographer/mapping/proto/trajectory.proto +++ b/cartographer/mapping/proto/trajectory.proto @@ -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. diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index a324332..f89948a 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -77,28 +77,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { return trajectory_protos.at(trajectory_id); }; - std::map 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(trajectory_id), - static_cast(old_node_index)}] = - NodeId{static_cast(trajectory_id), - static_cast(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)); } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 2a05b1f..5919c95 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -105,8 +105,7 @@ class SparsePoseGraph { virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0; // Returns data for all submaps. - virtual mapping::MapById - GetAllSubmapData() = 0; + virtual MapById 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> GetTrajectoryNodes() = 0; + virtual MapById GetTrajectoryNodes() = 0; // Serializes the constraints and trajectories. proto::SparsePoseGraph ToProto(); diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index e435ff1..581414f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -490,19 +490,10 @@ void SparsePoseGraph::RunOptimization() { optimized_submap_transforms_ = submap_data; } -std::vector> +mapping::MapById SparsePoseGraph::GetTrajectoryNodes() { - // TODO(cschuet): Rewrite downstream code and switch to MapById. common::MutexLocker locker(&mutex_); - std::vector> 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::constraints() { diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index a108abc..db3fa75 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; - std::vector> GetTrajectoryNodes() - override EXCLUDES(mutex_); + mapping::MapById + GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index d572c62..5d5f3eb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -173,6 +173,11 @@ class SparsePoseGraphTest : public ::testing::Test { MoveRelativeWithNoise(movement, transform::Rigid2d::Identity()); } + template + std::vector ToVectorInt(const Range& range) { + return std::vector(range.begin(), range.end()); + } + sensor::PointCloud point_cloud_; std::unique_ptr 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{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{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{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{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()), diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 1a4fa46..793d0af 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -531,19 +531,10 @@ void SparsePoseGraph::RunOptimization() { } } -std::vector> +mapping::MapById SparsePoseGraph::GetTrajectoryNodes() { - // TODO(cschuet): Rewrite downstream code and switch to MapById. common::MutexLocker locker(&mutex_); - std::vector> 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::constraints() { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 17f37ac..204dd79 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -98,8 +98,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; - std::vector> GetTrajectoryNodes() - override EXCLUDES(mutex_); + mapping::MapById + GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: