diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 61b5cba..0b66ef7 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -36,23 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto( LOG(FATAL) << "Unsupported tag."; } -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); - } -} - proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::SparsePoseGraphOptions options; @@ -75,11 +58,6 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; - std::vector> grouped_nodes; - std::vector> grouped_node_indices; - GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes, - &grouped_node_indices); - for (const auto& constraint : constraints()) { auto* const constraint_proto = proto.add_constraint(); *constraint_proto->mutable_relative_pose() = @@ -105,19 +83,22 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { constraint_proto->set_tag(mapping::ToProto(constraint.tag)); } - for (const auto& group : grouped_nodes) { + for (const auto& trajectory_nodes : GetTrajectoryNodes()) { auto* trajectory_proto = proto.add_trajectory(); - for (const auto& node : group) { + for (const auto& node : trajectory_nodes) { 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 trajectory = group[0].constant_data->trajectory; - for (const auto& transform : GetSubmapTransforms(trajectory)) { - *trajectory_proto->add_submap()->mutable_pose() = - transform::ToProto(transform); + if (!trajectory_nodes.empty()) { + const Submaps* const trajectory = + trajectory_nodes[0].constant_data->trajectory; + for (const auto& transform : GetSubmapTransforms(trajectory)) { + *trajectory_proto->add_submap()->mutable_pose() = + transform::ToProto(transform); + } } } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index c1c9fa0..4194e98 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -36,18 +36,6 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); -// 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: // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse @@ -93,18 +81,14 @@ class SparsePoseGraph { virtual transform::Rigid3d GetLocalToGlobalTransform( const Submaps* submaps) = 0; - // Returns the current optimized trajectory. - virtual std::vector GetTrajectoryNodes() = 0; + // Returns the current optimized trajectories. + virtual std::vector> GetTrajectoryNodes() = 0; // Serializes the constraints and trajectories. proto::SparsePoseGraph ToProto(); // Returns the collection of constraints. virtual std::vector constraints() = 0; - - protected: - // Returns the mapping from Submaps* to trajectory IDs. - virtual const std::unordered_map& trajectory_ids() = 0; }; } // namespace mapping diff --git a/cartographer/mapping/sparse_pose_graph_test.cc b/cartographer/mapping/sparse_pose_graph_test.cc deleted file mode 100644 index c427edb..0000000 --- a/cartographer/mapping/sparse_pose_graph_test.cc +++ /dev/null @@ -1,87 +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/sparse_pose_graph.h" - -#include -#include -#include - -#include "glog/logging.h" -#include "gmock/gmock.h" - -namespace cartographer { -namespace mapping { -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."; } - - void SubmapToProto(int, const transform::Rigid3d&, - proto::SubmapQuery::Response*) const override { - LOG(FATAL) << "Not implemented."; - } -}; - -TEST(SparsePoseGraphTest, TrajectoryFunctions) { - std::vector trajectory_nodes; - const std::vector submaps(5); - std::deque constant_data; - constexpr int kNumTrajectoryNodes = 10; - for (int j = 0; j < kNumTrajectoryNodes; ++j) { - for (size_t i = 0; i < submaps.size(); ++i) { - constant_data.push_back({}); - constant_data.back().trajectory = &submaps[i]; - TrajectoryNode node; - node.constant_data = &constant_data.back(); - trajectory_nodes.push_back(node); - } - } - - const std::unordered_map index_map = {{&submaps[0], 0}, - {&submaps[1], 1}, - {&submaps[2], 2}, - {&submaps[3], 3}, - {&submaps[4], 4}}; - 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(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 -} // namespace mapping -} // namespace cartographer diff --git a/cartographer/mapping_2d/map_limits.h b/cartographer/mapping_2d/map_limits.h index fa751ba..bb33973 100644 --- a/cartographer/mapping_2d/map_limits.h +++ b/cartographer/mapping_2d/map_limits.h @@ -84,51 +84,6 @@ class MapLimits { .all(); } - // Computes MapLimits that contain the origin, and all rays (both returns and - // misses) in the 'trajectory'. - static MapLimits ComputeMapLimits( - const double resolution, - const std::vector& trajectory_nodes) { - Eigen::AlignedBox2f bounding_box = ComputeMapBoundingBox(trajectory_nodes); - // Add some padding to ensure all rays are still contained in the map after - // discretization. - const float kPadding = 3.f * resolution; - bounding_box.min() -= kPadding * Eigen::Vector2f::Ones(); - bounding_box.max() += kPadding * Eigen::Vector2f::Ones(); - const Eigen::Vector2d pixel_sizes = - bounding_box.sizes().cast() / resolution; - return MapLimits(resolution, bounding_box.max().cast(), - CellLimits(common::RoundToInt(pixel_sizes.y()), - common::RoundToInt(pixel_sizes.x()))); - } - - static Eigen::AlignedBox2f ComputeMapBoundingBox( - const std::vector& trajectory_nodes) { - Eigen::AlignedBox2f bounding_box(Eigen::Vector2f::Zero()); - for (const auto& node : trajectory_nodes) { - const auto& data = *node.constant_data; - if (!data.range_data_3d.returns.empty()) { - const sensor::RangeData range_data = sensor::TransformRangeData( - Decompress(data.range_data_3d), node.pose.cast()); - bounding_box.extend(range_data.origin.head<2>()); - for (const Eigen::Vector3f& hit : range_data.returns) { - bounding_box.extend(hit.head<2>()); - } - } else { - const sensor::RangeData range_data = sensor::TransformRangeData( - data.range_data_2d, node.pose.cast()); - bounding_box.extend(range_data.origin.head<2>()); - for (const Eigen::Vector3f& hit : range_data.returns) { - bounding_box.extend(hit.head<2>()); - } - for (const Eigen::Vector3f& miss : range_data.misses) { - bounding_box.extend(miss.head<2>()); - } - } - } - return bounding_box; - } - private: double resolution_; Eigen::Vector2d max_; diff --git a/cartographer/mapping_2d/map_limits_test.cc b/cartographer/mapping_2d/map_limits_test.cc index cb529a2..350c585 100644 --- a/cartographer/mapping_2d/map_limits_test.cc +++ b/cartographer/mapping_2d/map_limits_test.cc @@ -62,29 +62,6 @@ TEST(MapLimitsTest, ConstructAndGet) { EXPECT_EQ(42., limits.resolution()); } -TEST(MapLimitsTest, ComputeMapLimits) { - const mapping::TrajectoryNode::ConstantData constant_data{ - common::FromUniversal(52), - sensor::RangeData{ - Eigen::Vector3f::Zero(), - {Eigen::Vector3f(-30.f, 1.f, 0.f), Eigen::Vector3f(50.f, -10.f, 0.f)}, - {}}, - Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), nullptr, - transform::Rigid3d::Identity()}; - const mapping::TrajectoryNode trajectory_node{&constant_data, - transform::Rigid3d::Identity()}; - constexpr double kResolution = 0.05; - const MapLimits limits = - MapLimits::ComputeMapLimits(kResolution, {trajectory_node}); - constexpr float kPaddingAwareTolerance = 5 * kResolution; - EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance); - EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance); - EXPECT_LT(200, limits.cell_limits().num_x_cells); - EXPECT_LT(1600, limits.cell_limits().num_y_cells); - EXPECT_GT(400, limits.cell_limits().num_x_cells); - EXPECT_GT(2000, limits.cell_limits().num_y_cells); -} - } // namespace } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c5a96da..1c45224 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -424,9 +424,16 @@ void SparsePoseGraph::RunOptimization() { } } -std::vector SparsePoseGraph::GetTrajectoryNodes() { +std::vector> +SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + std::vector> result( + trajectory_ids_.size()); + for (const auto& node : trajectory_nodes_) { + result.at(trajectory_ids_.at(node.constant_data->trajectory)) + .push_back(node); + } + return result; } std::vector SparsePoseGraph::constraints() { @@ -434,12 +441,6 @@ std::vector SparsePoseGraph::constraints() { return constraints_; } -const std::unordered_map& -SparsePoseGraph::trajectory_ids() { - common::MutexLocker locker(&mutex_); - return trajectory_ids_; -} - transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps* const submaps) { const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index aa4f75c..8a08fae 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -86,13 +86,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; - std::vector GetTrajectoryNodes() override - EXCLUDES(mutex_); - std::vector constraints() override EXCLUDES(mutex_); - - protected: - const std::unordered_map& trajectory_ids() + std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); + std::vector constraints() override EXCLUDES(mutex_); private: struct SubmapState { diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 3cb9d2f..ea47234 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -189,12 +189,13 @@ TEST_F(SparsePoseGraphTest, NoMovement) { MoveRelative(transform::Rigid2d::Identity()); sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); - EXPECT_THAT(nodes.size(), ::testing::Eq(3)); - EXPECT_THAT(nodes[0].pose, + ASSERT_THAT(nodes.size(), ::testing::Eq(1)); + EXPECT_THAT(nodes[0].size(), ::testing::Eq(3)); + EXPECT_THAT(nodes[0][0].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes[1].pose, + EXPECT_THAT(nodes[0][1].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes[2].pose, + EXPECT_THAT(nodes[0][2].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } @@ -208,8 +209,10 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) { } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(nodes.size(), ::testing::Eq(1)); for (int i = 0; i != 4; ++i) { - EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2)) + EXPECT_THAT(poses[i], + IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) << i; } } @@ -224,8 +227,10 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(nodes.size(), ::testing::Eq(1)); for (int i = 0; i != 5; ++i) { - EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[i].pose), 1e-2)) + EXPECT_THAT(poses[i], + IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) << i; } } @@ -247,12 +252,13 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) { } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); + ASSERT_THAT(nodes.size(), ::testing::Eq(1)); 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.front().pose.inverse() * nodes.back().pose; + nodes[0].front().pose.inverse() * nodes[0].back().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 29e4549..c221097 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -415,9 +415,16 @@ void SparsePoseGraph::RunOptimization() { } } -std::vector SparsePoseGraph::GetTrajectoryNodes() { +std::vector> +SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + std::vector> result( + trajectory_ids_.size()); + for (const auto& node : trajectory_nodes_) { + result.at(trajectory_ids_.at(node.constant_data->trajectory)) + .push_back(node); + } + return result; } std::vector SparsePoseGraph::constraints() { @@ -425,12 +432,6 @@ std::vector SparsePoseGraph::constraints() { return constraints_; } -const std::unordered_map& -SparsePoseGraph::trajectory_ids() { - common::MutexLocker locker(&mutex_); - return trajectory_ids_; -} - transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps* const submaps) { const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index a097f0c..b327126 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -73,8 +73,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector& insertion_submaps) EXCLUDES(mutex_); - // The index into the vector of trajectory nodes as used with - // GetTrajectoryNodes() for the next node added with AddScan() is returned. + // The index into the flat vector of trajectory nodes used internally for the + // next node added with AddScan() is returned. int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. @@ -88,13 +88,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; - std::vector GetTrajectoryNodes() override - EXCLUDES(mutex_); - std::vector constraints() override EXCLUDES(mutex_); - - protected: - const std::unordered_map& trajectory_ids() + std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); + std::vector constraints() override EXCLUDES(mutex_); private: struct SubmapState {