From 34a816abdb41eb0895d9eaf7fc96d1df2820f01c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 07:18:16 +0200 Subject: [PATCH 1/9] return submap ids in InsertionResult instead of pointers --- .../mapping/internal/2d/pose_graph_2d.cc | 22 ++++++++++++++----- .../mapping/internal/2d/pose_graph_2d.h | 4 ++-- .../mapping/internal/3d/pose_graph_3d.cc | 22 ++++++++++++++----- .../mapping/internal/3d/pose_graph_3d.h | 4 ++-- .../internal/global_trajectory_builder.cc | 7 +++--- cartographer/mapping/pose_graph.h | 5 ----- cartographer/mapping/pose_graph_interface.h | 5 +++++ .../mapping/trajectory_builder_interface.h | 2 +- 8 files changed, 45 insertions(+), 26 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 88addd72e5..5077c49a66 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -117,7 +117,7 @@ std::vector PoseGraph2D::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -NodeId PoseGraph2D::AppendNode( +std::pair> PoseGraph2D::AppendNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps, @@ -141,18 +141,28 @@ NodeId PoseGraph2D::AppendNode( data_.submap_data.at(submap_id).submap = insertion_submaps.back(); LOG(INFO) << "Inserted submap " << submap_id << "."; } - return node_id; + std::vector submap_ids; + auto submap_id_iter = + std::prev(data_.submap_data.EndOfTrajectory(trajectory_id), + insertion_submaps.size()); + for (int i = 0; i < static_cast(insertion_submaps.size()); ++i) { + submap_ids.push_back(submap_id_iter->id); + ++submap_id_iter; + } + return {node_id, submap_ids}; } -NodeId PoseGraph2D::AddNode( +std::pair> PoseGraph2D::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); - const NodeId node_id = AppendNode(constant_data, trajectory_id, - insertion_submaps, optimized_pose); + const auto node_submap_ids = AppendNode(constant_data, trajectory_id, + insertion_submaps, optimized_pose); + const NodeId& node_id = node_submap_ids.first; + // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = @@ -161,7 +171,7 @@ NodeId PoseGraph2D::AddNode( return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); - return node_id; + return node_submap_ids; } void PoseGraph2D::AddWorkItem( diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 2fcca2ca86..9547eb9ee3 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -77,7 +77,7 @@ class PoseGraph2D : public PoseGraph { // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - NodeId AddNode( + std::pair> AddNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) @@ -171,7 +171,7 @@ class PoseGraph2D : public PoseGraph { // Appends the new node and submap (if needed) to the internal data // structures. - NodeId AppendNode( + std::pair> AppendNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps, diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 7801be8831..eea0e6eeac 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -105,7 +105,7 @@ std::vector PoseGraph3D::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -NodeId PoseGraph3D::AppendNode( +std::pair> PoseGraph3D::AppendNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps, @@ -129,18 +129,28 @@ NodeId PoseGraph3D::AppendNode( data_.submap_data.at(submap_id).submap = insertion_submaps.back(); LOG(INFO) << "Inserted submap " << submap_id << "."; } - return node_id; + std::vector submap_ids; + auto submap_id_iter = + std::prev(data_.submap_data.EndOfTrajectory(trajectory_id), + insertion_submaps.size()); + for (int i = 0; i < static_cast(insertion_submaps.size()); ++i) { + submap_ids.push_back(submap_id_iter->id); + ++submap_id_iter; + } + return {node_id, submap_ids}; } -NodeId PoseGraph3D::AddNode( +std::pair> PoseGraph3D::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); - const NodeId node_id = AppendNode(constant_data, trajectory_id, - insertion_submaps, optimized_pose); + const auto node_submap_ids = AppendNode(constant_data, trajectory_id, + insertion_submaps, optimized_pose); + const NodeId& node_id = node_submap_ids.first; + // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = @@ -149,7 +159,7 @@ NodeId PoseGraph3D::AddNode( return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); - return node_id; + return node_submap_ids; } void PoseGraph3D::AddWorkItem( diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 0195c84cdf..91c59cefcd 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -75,7 +75,7 @@ class PoseGraph3D : public PoseGraph { // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - NodeId AddNode( + std::pair> AddNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) @@ -174,7 +174,7 @@ class PoseGraph3D : public PoseGraph { EXCLUSIVE_LOCKS_REQUIRED(mutex_); // Appends the new node and submap (if needed) to the internal data stuctures. - NodeId AppendNode( + std::pair> AppendNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps, diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index 3260f05bfe..22e73fee07 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -65,15 +65,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { std::unique_ptr insertion_result; if (matching_result->insertion_result != nullptr) { kLocalSlamInsertionResults->Increment(); - auto node_id = pose_graph_->AddNode( + const auto node_submap_ids = pose_graph_->AddNode( matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->insertion_submaps); + const auto& node_id = node_submap_ids.first; CHECK_EQ(node_id.trajectory_id, trajectory_id_); insertion_result = absl::make_unique(InsertionResult{ node_id, matching_result->insertion_result->constant_data, - std::vector>( - matching_result->insertion_result->insertion_submaps.begin(), - matching_result->insertion_result->insertion_submaps.end())}); + node_submap_ids.second}); } if (local_slam_result_callback_) { local_slam_result_callback_( diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 1dbf52a6fc..0071c264c1 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -112,11 +112,6 @@ class PoseGraph : public PoseGraphInterface { // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() const = 0; - // Returns the current optimized transform and submap itself for the given - // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does - // not exist (anymore). - virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; - proto::PoseGraph ToProto(bool include_unfinished_submaps) const override; // Returns the IMU data. diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index e950401516..ee8f8acc4c 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -95,6 +95,11 @@ class PoseGraphInterface { // Waits for all computations to finish and computes optimized poses. virtual void RunFinalOptimization() = 0; + // Returns the current optimized transform and submap itself for the given + // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does + // not exist (anymore). + virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; + // Returns data for all submaps. virtual MapById GetAllSubmapData() const = 0; diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index 1173d8ee35..377fc92cbe 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -50,7 +50,7 @@ class TrajectoryBuilderInterface { struct InsertionResult { NodeId node_id; std::shared_ptr constant_data; - std::vector> insertion_submaps; + std::vector insertion_submap_ids; }; // A callback which is called after local SLAM processes an accumulated From f0b2f1c52ce033d6b1f42c0cead4e119fca0dfa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 07:30:32 +0200 Subject: [PATCH 2/9] update pose graph stub --- cartographer/cloud/internal/client/pose_graph_stub.cc | 5 +++++ cartographer/cloud/internal/client/pose_graph_stub.h | 1 + 2 files changed, 6 insertions(+) diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index f3f2138f85..90bfc1a1aa 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -51,6 +51,11 @@ PoseGraphStub::GetAllSubmapData() const { LOG(FATAL) << "Not implemented"; } +mapping::PoseGraphInterface::SubmapData PoseGraphStub::GetSubmapData( + const mapping::SubmapId& submap_id) const { + LOG(FATAL) << "Not implemented"; +} + mapping::MapById PoseGraphStub::GetAllSubmapPoses() const { google::protobuf::Empty request; diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index 4401f46859..5ad9802204 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -34,6 +34,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { void RunFinalOptimization() override; mapping::MapById GetAllSubmapData() const override; + SubmapData GetSubmapData(const mapping::SubmapId& submap_id) const override; mapping::MapById GetAllSubmapPoses() const override; transform::Rigid3d GetLocalToGlobalTransform( From 7e6949e555575ee645a3371f33390a201771c04d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 07:45:40 +0200 Subject: [PATCH 3/9] bring back pointers --- cartographer/mapping/internal/global_trajectory_builder.cc | 3 +++ cartographer/mapping/trajectory_builder_interface.h | 1 + 2 files changed, 4 insertions(+) diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index 22e73fee07..c4a61d6cd7 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -72,6 +72,9 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { CHECK_EQ(node_id.trajectory_id, trajectory_id_); insertion_result = absl::make_unique(InsertionResult{ node_id, matching_result->insertion_result->constant_data, + std::vector>( + matching_result->insertion_result->insertion_submaps.begin(), + matching_result->insertion_result->insertion_submaps.end()), node_submap_ids.second}); } if (local_slam_result_callback_) { diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index 377fc92cbe..09aaaeaa9b 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -50,6 +50,7 @@ class TrajectoryBuilderInterface { struct InsertionResult { NodeId node_id; std::shared_ptr constant_data; + std::vector> insertion_submaps; std::vector insertion_submap_ids; }; From b7578314de49a0f4ac2c259211c1c39ea964cfe5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 07:51:34 +0200 Subject: [PATCH 4/9] update mock pose graph --- cartographer/mapping/internal/testing/mock_pose_graph.h | 1 + 1 file changed, 1 insertion(+) diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 675d89f64d..d82e72de38 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -34,6 +34,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface { MOCK_METHOD0(RunFinalOptimization, void()); MOCK_CONST_METHOD0(GetAllSubmapData, mapping::MapById()); + MOCK_CONST_METHOD1(GetSubmapData, SubmapData(const mapping::SubmapId&)); MOCK_CONST_METHOD0(GetAllSubmapPoses, mapping::MapById()); MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int)); From 9348f12c81683f844cac22288b5277a4b1bdc9ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 07:58:16 +0200 Subject: [PATCH 5/9] swap struct order --- cartographer/mapping/internal/global_trajectory_builder.cc | 4 ++-- cartographer/mapping/trajectory_builder_interface.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index c4a61d6cd7..142a89a3b1 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -72,10 +72,10 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { CHECK_EQ(node_id.trajectory_id, trajectory_id_); insertion_result = absl::make_unique(InsertionResult{ node_id, matching_result->insertion_result->constant_data, + node_submap_ids.second, std::vector>( matching_result->insertion_result->insertion_submaps.begin(), - matching_result->insertion_result->insertion_submaps.end()), - node_submap_ids.second}); + matching_result->insertion_result->insertion_submaps.end())}); } if (local_slam_result_callback_) { local_slam_result_callback_( diff --git a/cartographer/mapping/trajectory_builder_interface.h b/cartographer/mapping/trajectory_builder_interface.h index 09aaaeaa9b..d1c4357a50 100644 --- a/cartographer/mapping/trajectory_builder_interface.h +++ b/cartographer/mapping/trajectory_builder_interface.h @@ -50,8 +50,8 @@ class TrajectoryBuilderInterface { struct InsertionResult { NodeId node_id; std::shared_ptr constant_data; - std::vector> insertion_submaps; std::vector insertion_submap_ids; + std::vector> insertion_submaps; }; // A callback which is called after local SLAM processes an accumulated From b719996546d98e8a544984ccd13ab36867a5b26e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 08:05:16 +0200 Subject: [PATCH 6/9] add checks, clarify iterator name --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 7 ++++--- cartographer/mapping/internal/3d/pose_graph_3d.cc | 7 ++++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 5077c49a66..d1c9968045 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -142,12 +142,13 @@ std::pair> PoseGraph2D::AppendNode( LOG(INFO) << "Inserted submap " << submap_id << "."; } std::vector submap_ids; - auto submap_id_iter = + auto submap_data_iter = std::prev(data_.submap_data.EndOfTrajectory(trajectory_id), insertion_submaps.size()); for (int i = 0; i < static_cast(insertion_submaps.size()); ++i) { - submap_ids.push_back(submap_id_iter->id); - ++submap_id_iter; + CHECK_EQ(submap_data_iter->data.submap, insertion_submaps.at(i)); + submap_ids.push_back(submap_data_iter->id); + ++submap_data_iter; } return {node_id, submap_ids}; } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index eea0e6eeac..46833fd0c9 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -130,12 +130,13 @@ std::pair> PoseGraph3D::AppendNode( LOG(INFO) << "Inserted submap " << submap_id << "."; } std::vector submap_ids; - auto submap_id_iter = + auto submap_data_iter = std::prev(data_.submap_data.EndOfTrajectory(trajectory_id), insertion_submaps.size()); for (int i = 0; i < static_cast(insertion_submaps.size()); ++i) { - submap_ids.push_back(submap_id_iter->id); - ++submap_id_iter; + CHECK_EQ(submap_data_iter->data.submap, insertion_submaps.at(i)); + submap_ids.push_back(submap_data_iter->id); + ++submap_data_iter; } return {node_id, submap_ids}; } From 23c0041a51c4a229513b790150d8f87c0de3a54f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Thu, 6 Sep 2018 09:19:28 +0200 Subject: [PATCH 7/9] rerun ci From 37f83d231539277da30b1a3509607a6cac684d58 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev Date: Mon, 5 Nov 2018 13:30:26 +0100 Subject: [PATCH 8/9] Ran clang-format. --- .../io/probability_grid_points_processor.cc | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 07969077bc..1d746f9574 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -36,14 +36,15 @@ void DrawTrajectoriesIntoImage( const std::vector& trajectories, cairo_surface_t* cairo_surface) { for (size_t i = 0; i < trajectories.size(); ++i) { - DrawTrajectory(trajectories[i], GetColor(i), - [&probability_grid, - &offset](const transform::Rigid3d& pose) -> Eigen::Array2i { - return probability_grid.limits().GetCellIndex( - pose.cast().translation().head<2>()) - - offset; - }, - cairo_surface); + DrawTrajectory( + trajectories[i], GetColor(i), + [&probability_grid, + &offset](const transform::Rigid3d& pose) -> Eigen::Array2i { + return probability_grid.limits().GetCellIndex( + pose.cast().translation().head<2>()) - + offset; + }, + cairo_surface); } } From 7816562162352740759ee7ab93b847f6f69cf637 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev Date: Tue, 12 Feb 2019 12:58:36 +0100 Subject: [PATCH 9/9] Ran clang-format. --- cartographer/sensor/compressed_point_cloud.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cartographer/sensor/compressed_point_cloud.cc b/cartographer/sensor/compressed_point_cloud.cc index d840e4702c..f5337e57d2 100644 --- a/cartographer/sensor/compressed_point_cloud.cc +++ b/cartographer/sensor/compressed_point_cloud.cc @@ -61,8 +61,8 @@ RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const { return {current_point_}; } -CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator:: -operator++() { +CompressedPointCloud::ConstIterator& +CompressedPointCloud::ConstIterator::operator++() { --remaining_points_; if (remaining_points_ > 0) { ReadNextPoint();