From 54f7f4f969fdbecd197fb01ced3a354cb1726536 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 6 Aug 2018 17:34:40 +0200 Subject: [PATCH] Skip constraint search against unfinished submaps. (#1362) The uplink server only receives the grid content of a submap after that submap is finished for efficiency. Therefore, constraint searches against that submap need to be skipped. Also add checks to avoid this in the future. FIXES=#1360 --- cartographer/mapping/internal/2d/pose_graph_2d.cc | 5 +++++ cartographer/mapping/internal/3d/pose_graph_3d.cc | 5 +++++ .../mapping/internal/constraints/constraint_builder_2d.cc | 2 ++ .../mapping/internal/constraints/constraint_builder_2d.h | 2 +- .../mapping/internal/constraints/constraint_builder_3d.cc | 1 + .../mapping/internal/constraints/constraint_builder_3d.h | 4 ++-- cartographer/mapping/map_builder_test.cc | 2 +- 7 files changed, 17 insertions(+), 4 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 2b958b1..5518c27 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -242,6 +242,11 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id, { common::MutexLocker locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); + if (!data_.submap_data.at(submap_id).submap->finished()) { + // Uplink server only receives grids when they are finished, so skip + // constraint search before that. + return; + } const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time last_connection_time = diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 4594971..d29562f 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -256,6 +256,11 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, { common::MutexLocker locker(&mutex_); CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); + if (!data_.submap_data.at(submap_id).submap->finished()) { + // Uplink server only receives grids when they are finished, so skip + // constraint search before that. + return; + } for (const NodeId& submap_node_id : data_.submap_data.at(submap_id).node_ids) { diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index a15a4ec..c27e3f5 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -158,6 +158,7 @@ void ConstraintBuilder2D::WhenDone( const ConstraintBuilder2D::SubmapScanMatcher* ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, const Grid2D* const grid) { + CHECK(grid); if (submap_scan_matchers_.count(submap_id) != 0) { return &submap_scan_matchers_.at(submap_id); } @@ -183,6 +184,7 @@ void ConstraintBuilder2D::ComputeConstraint( const transform::Rigid2d& initial_relative_pose, const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) { + CHECK(submap_scan_matcher.fast_correlative_scan_matcher); const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index aeb3f49..9d2520d 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -107,7 +107,7 @@ class ConstraintBuilder2D { private: struct SubmapScanMatcher { - const Grid2D* grid; + const Grid2D* grid = nullptr; std::unique_ptr fast_correlative_scan_matcher; std::weak_ptr creation_task_handle; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index c4035e1..6aef0b0 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -199,6 +199,7 @@ void ConstraintBuilder3D::ComputeConstraint( const transform::Rigid3d& global_submap_pose, const SubmapScanMatcher& submap_scan_matcher, std::unique_ptr* constraint) { + CHECK(submap_scan_matcher.fast_correlative_scan_matcher); // The 'constraint_transform' (submap i <- node j) is computed from: // - a 'high_resolution_point_cloud' in node j and // - the initial guess 'initial_pose' (submap i <- node j). diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index 8b81131..7bb7368 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -116,8 +116,8 @@ class ConstraintBuilder3D { private: struct SubmapScanMatcher { - const HybridGrid* high_resolution_hybrid_grid; - const HybridGrid* low_resolution_hybrid_grid; + const HybridGrid* high_resolution_hybrid_grid = nullptr; + const HybridGrid* low_resolution_hybrid_grid = nullptr; std::unique_ptr fast_correlative_scan_matcher; std::weak_ptr creation_task_handle; diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index a3b79f8..c079775 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -443,7 +443,7 @@ TEST_P(MapBuilderTestByGridType, LocalizationOnFrozenTrajectory2D) { ++num_cross_trajectory_constraints; } } - EXPECT_GT(num_cross_trajectory_constraints, 3); + EXPECT_GE(num_cross_trajectory_constraints, 3); // TODO(gaschler): Subscribe global slam callback, verify that all nodes are // optimized. EXPECT_THAT(constraints, ::testing::Contains(::testing::Field(