From 4917dd65ce3a6e59be2064f72db2303bc25cae45 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 25 Oct 2016 16:56:26 +0200 Subject: [PATCH] Add serialization of the sparse pose graph. (#97) --- cartographer/mapping/CMakeLists.txt | 3 + cartographer/mapping/proto/CMakeLists.txt | 8 ++ .../mapping/proto/sparse_pose_graph.proto | 54 +++++++++++++ cartographer/mapping/sparse_pose_graph.cc | 80 ++++++++++++++++++- cartographer/mapping/sparse_pose_graph.h | 8 +- cartographer/proto/trajectory.proto | 7 ++ cartographer/sensor/ordered_multi_queue.h | 3 +- 7 files changed, 159 insertions(+), 4 deletions(-) create mode 100644 cartographer/mapping/proto/sparse_pose_graph.proto diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index ab8fa17..a05d067 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/cartographer/mapping/CMakeLists.txt @@ -116,13 +116,16 @@ google_library(mapping_sparse_pose_graph sparse_pose_graph.h DEPENDS common_lua_parameter_dictionary + kalman_filter_pose_tracker mapping_proto_scan_matching_progress + mapping_proto_sparse_pose_graph mapping_proto_sparse_pose_graph_options mapping_sparse_pose_graph_constraint_builder mapping_sparse_pose_graph_optimization_problem_options mapping_submaps mapping_trajectory_node transform_rigid_transform + transform_transform ) google_library(mapping_submaps diff --git a/cartographer/mapping/proto/CMakeLists.txt b/cartographer/mapping/proto/CMakeLists.txt index 43c99c0..77da2d0 100644 --- a/cartographer/mapping/proto/CMakeLists.txt +++ b/cartographer/mapping/proto/CMakeLists.txt @@ -26,6 +26,14 @@ google_proto_library(mapping_proto_scan_matching_progress scan_matching_progress.proto ) +google_proto_library(mapping_proto_sparse_pose_graph + SRCS + sparse_pose_graph.proto + DEPENDS + proto_trajectory + transform_proto_transform +) + google_proto_library(mapping_proto_sparse_pose_graph_options SRCS sparse_pose_graph_options.proto diff --git a/cartographer/mapping/proto/sparse_pose_graph.proto b/cartographer/mapping/proto/sparse_pose_graph.proto new file mode 100644 index 0000000..e646b8b --- /dev/null +++ b/cartographer/mapping/proto/sparse_pose_graph.proto @@ -0,0 +1,54 @@ +// 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. + +syntax = "proto2"; + +package cartographer.mapping.proto; + +import "cartographer/transform/proto/transform.proto"; +import "cartographer/proto/trajectory.proto"; + +message SparsePoseGraph { + message Constraint { + message SubmapId { + optional int32 trajectory_id = 1; + optional int32 submap_index = 2; // Submap index in the given trajectory. + } + + message ScanId { + optional int32 trajectory_id = 1; + optional int32 scan_index = 2; // Scan index in the given trajectory. + } + + // Differentiates between intra-submap (where the scan was inserted into the + // submap) and inter-submap constraints (where the scan was not inserted + // into the submap). + enum Tag { + INTRA_SUBMAP = 0; + INTER_SUBMAP = 1; + } + + optional SubmapId submap_id = 1; // Submap ID. + optional ScanId scan_id = 2; // Scan ID. + // Pose of the scan relative to submap, i.e. taking data from the scan frame + // into the submap frame. + optional transform.proto.Rigid3d relative_pose = 3; + // 6x6 square root inverse of the covariance matrix. + repeated double sqrt_Lambda = 4; // NOLINT + optional Tag tag = 5; + } + + repeated Constraint constraint = 2; + repeated .cartographer.proto.Trajectory trajectory = 4; +} diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index a9bd631..e9534d8 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -18,17 +18,41 @@ #include +#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" +#include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping { +proto::SparsePoseGraph::Constraint::Tag ToProto( + const SparsePoseGraph::Constraint2D::Tag& tag) { + switch (tag) { + case SparsePoseGraph::Constraint2D::Tag::INTRA_SUBMAP: + return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; + case SparsePoseGraph::Constraint2D::Tag::INTER_SUBMAP: + return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; + } + LOG(FATAL) << "Unsupported tag."; +} + +proto::SparsePoseGraph::Constraint::Tag ToProto( + const SparsePoseGraph::Constraint3D::Tag& tag) { + switch (tag) { + case SparsePoseGraph::Constraint3D::Tag::INTRA_SUBMAP: + return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; + case SparsePoseGraph::Constraint3D::Tag::INTER_SUBMAP: + return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; + } + LOG(FATAL) << "Unsupported tag."; +} + std::vector> SplitTrajectoryNodes( const std::vector& trajectory_nodes) { std::vector> trajectories; - std::unordered_map trajectory_ids; + std::unordered_map trajectory_ids; for (const auto& node : trajectory_nodes) { const auto* trajectory = node.constant_data->trajectory; if (trajectory_ids.emplace(trajectory, trajectories.size()).second) { @@ -49,7 +73,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( sparse_pose_graph::CreateConstraintBuilderOptions( parameter_dictionary->GetDictionary("constraint_builder").get()); *options.mutable_optimization_problem_options() = - mapping::sparse_pose_graph::CreateOptimizationProblemOptions( + sparse_pose_graph::CreateOptimizationProblemOptions( parameter_dictionary->GetDictionary("optimization_problem").get()); options.set_max_num_final_iterations( parameter_dictionary->GetNonNegativeInt("max_num_final_iterations")); @@ -59,5 +83,57 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( return options; } +proto::SparsePoseGraph SparsePoseGraph::ToProto() { + proto::SparsePoseGraph proto; + for (const auto& constraint : constraints_2d()) { + auto* const constraint_proto = proto.add_constraint(); + *constraint_proto->mutable_relative_pose() = + transform::ToProto(transform::Embed3D(constraint.pose.zbar_ij)); + for (int i = 0; i != 36; ++i) { + constraint_proto->mutable_sqrt_lambda()->Add(0.); + } + constexpr double kFakePositionCovariance = 1.; + constexpr double kFakeOrientationCovariance = 1.; + Eigen::Map>( + constraint_proto->mutable_sqrt_lambda()->mutable_data()) = + kalman_filter::Embed3D(constraint.pose.sqrt_Lambda_ij, + kFakePositionCovariance, + kFakeOrientationCovariance); + // 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->set_tag(mapping::ToProto(constraint.tag)); + } + for (const auto& constraint : constraints_3d()) { + auto* const constraint_proto = proto.add_constraint(); + *constraint_proto->mutable_relative_pose() = + transform::ToProto(constraint.pose.zbar_ij); + for (int i = 0; i != 36; ++i) { + constraint_proto->mutable_sqrt_lambda()->Add(0.); + } + Eigen::Map>( + 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->set_tag(mapping::ToProto(constraint.tag)); + } + + // TODO(whess): Support multi-trajectory. + ::cartographer::proto::Trajectory* const trajectory = proto.add_trajectory(); + *trajectory = mapping::ToProto(GetTrajectoryNodes()); + const std::vector> 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); + } + + return proto; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 5740d61..bf001a6 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -21,6 +21,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/proto/scan_matching_progress.pb.h" +#include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" @@ -110,7 +111,7 @@ class SparsePoseGraph { // continuous, non-loop-closed frame) into the global map frame (i.e. the // discontinuous, loop-closed frame). virtual transform::Rigid3d GetLocalToGlobalTransform( - const mapping::Submaps& submaps) = 0; + const Submaps& submaps) = 0; // Returns the current optimized trajectory. virtual std::vector GetTrajectoryNodes() = 0; @@ -120,6 +121,11 @@ class SparsePoseGraph { // Returns the collection of 3D constraints. virtual std::vector constraints_3d() = 0; + + // Serializes the constraints and the computed trajectory. + // + // TODO(whess): Support multiple trajectories. + proto::SparsePoseGraph ToProto(); }; } // namespace mapping diff --git a/cartographer/proto/trajectory.proto b/cartographer/proto/trajectory.proto index 3dce959..11fe086 100644 --- a/cartographer/proto/trajectory.proto +++ b/cartographer/proto/trajectory.proto @@ -28,6 +28,13 @@ message Trajectory { optional transform.proto.Rigid3d pose = 5; } + message Submap { + optional transform.proto.Rigid3d pose = 1; + } + // Time-ordered sequence of Nodes. repeated Node node = 1; + + // Submaps associated with the trajectory. + repeated Submap submap = 2; } diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h index 74d5247..fc4e313 100644 --- a/cartographer/sensor/ordered_multi_queue.h +++ b/cartographer/sensor/ordered_multi_queue.h @@ -167,7 +167,8 @@ class OrderedMultiQueue { void CannotMakeProgress() { for (auto& entry : queues_) { if (entry.second.queue.Size() > kMaxQueueSize) { - LOG_EVERY_N(WARNING, 60) << "Queues waiting for data: " << EmptyQueuesDebugString(); + LOG_EVERY_N(WARNING, 60) << "Queues waiting for data: " + << EmptyQueuesDebugString(); return; } }