diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 91476aa..65525d2 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -16,8 +16,6 @@ #include "cartographer/mapping/sparse_pose_graph.h" -#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" @@ -38,19 +36,46 @@ proto::SparsePoseGraph::Constraint::Tag ToProto( LOG(FATAL) << "Unsupported tag."; } -std::vector> SplitTrajectoryNodes( - const std::vector& trajectory_nodes) { - std::vector> trajectories; - 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) { - trajectories.push_back({node}); - } else { - trajectories[trajectory_ids[trajectory]].push_back(node); - } +std::unordered_map ComputeTrajectoryIds( + const std::vector& trajectories) { + std::unordered_map result; + for (const auto& trajectory : trajectories) { + result.emplace(trajectory, result.size()); + } + return result; +} + +void GroupTrajectoryNodes( + const std::vector& trajectory_nodes, + const std::unordered_map& trajectory_ids, + std::vector>* grouped_nodes, + std::vector>* 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& submap_states, + const std::unordered_map& trajectory_ids, + std::vector>* new_indices) { + CHECK_NOTNULL(new_indices)->clear(); + std::vector 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 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> grouped_nodes; + std::vector> grouped_node_indices; + GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes, + &grouped_node_indices); + + std::vector> 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>( 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> 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; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 22d08f8..2b46988 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -17,6 +17,9 @@ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ +#include +#include +#include #include #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> SplitTrajectoryNodes( - const std::vector& 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 ComputeTrajectoryIds( + const std::vector& 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& trajectory_nodes, + const std::unordered_map& trajectory_ids, + std::vector>* grouped_nodes, + std::vector>* 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 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 GetTrajectoryNodes() = 0; + // TODO(macmason, wohe): Consider replacing this with a GroupSubmapStates, + // which would have better separation of concerns. + virtual std::vector GetSubmapStates() = 0; + // Returns the collection of constraints. virtual std::vector 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& submap_states, + const std::unordered_map& trajectory_ids, + std::vector>* new_indices); + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/sparse_pose_graph_test.cc b/cartographer/mapping/sparse_pose_graph_test.cc index e0fb635..168616a 100644 --- a/cartographer/mapping/sparse_pose_graph_test.cc +++ b/cartographer/mapping/sparse_pose_graph_test.cc @@ -16,7 +16,9 @@ #include "cartographer/mapping/sparse_pose_graph.h" +#include #include +#include #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 trajectory_nodes; const std::vector submaps(5); std::deque constant_data; @@ -52,15 +55,42 @@ TEST(SparsePoseGraphTest, SplitTrajectoryNodes) { trajectory_nodes.push_back(node); } } - std::vector> split_trajectory_nodes = - SplitTrajectoryNodes(trajectory_nodes); - EXPECT_EQ(split_trajectory_nodes.size(), submaps.size()); + + std::vector 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> grouped_nodes; + std::vector> 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 diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc deleted file mode 100644 index c8d6d95..0000000 --- a/cartographer/mapping/trajectory_node.cc +++ /dev/null @@ -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& 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 diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index bf58c3d..b5c51e9 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -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 trajectory_nodes; }; -proto::Trajectory ToProto(const std::vector& nodes); - } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ef35a11..0c538fe 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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(constraint_builder_.GetNumFinishedScans()) == - trajectory_nodes_.size(); - }, - common::FromSeconds(1.))) { + while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { + return static_cast(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, ¬ification]( - 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, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); locker.Await([¬ification]() { 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 SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +std::vector SparsePoseGraph::GetSubmapStates() { + common::MutexLocker locker(&mutex_); + return submap_states_; +} + std::vector SparsePoseGraph::constraints() { return constraints_; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index f67885a..fc0af6e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -96,27 +96,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); + std::vector GetSubmapStates() override EXCLUDES(mutex_); + std::vector 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 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 work_item) REQUIRES(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 050e5a9..13e9b4b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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(constraint_builder_.GetNumFinishedScans()) == - trajectory_nodes_.size(); - }, - common::FromSeconds(1.))) { + while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) { + return static_cast(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, ¬ification]( - 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, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + constraints_.insert(constraints_.end(), result.begin(), result.end()); + common::MutexLocker locker(&mutex_); + notification = true; + }); locker.Await([¬ification]() { 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 SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +std::vector +SparsePoseGraph::GetSubmapStates() { + std::vector 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::constraints() { return constraints_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 7403e23..995b7ab 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -95,9 +95,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); + std::vector GetSubmapStates() override; std::vector constraints() override; private: + // This is 'mapping::SubmapState', but with the 3D versions of 'submap' and + // 'trajectory'. struct SubmapState { const Submap* submap = nullptr;