From 7839f3f21608909ae1415df5770af15fae2f7961 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 2 Jul 2018 14:51:48 +0200 Subject: [PATCH] Avoid uninitialized NodeId, SubmapId (#1223) Previously, NodeId and SubmapId could be (partially) uninitialized, for instance like this: ``` NodeId node_id; SubmapId submap{0}; // uninitialized submap_index ``` This introduces constructors to prevent this. --- cartographer/mapping/id.h | 6 ++++++ cartographer/mapping/internal/2d/pose_graph_2d.cc | 14 ++++++++++---- cartographer/mapping/internal/3d/pose_graph_3d.cc | 14 ++++++++++---- .../constraints/constraint_builder_2d_test.cc | 6 +++--- .../constraints/constraint_builder_3d_test.cc | 10 ++++++---- 5 files changed, 35 insertions(+), 15 deletions(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 504e3d0..6cf427a 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -55,6 +55,9 @@ common::Time GetTime(const T& t) { // Uniquely identifies a trajectory node using a combination of a unique // trajectory ID and a zero-based index of the node inside that trajectory. struct NodeId { + NodeId(int trajectory_id, int node_index) + : trajectory_id(trajectory_id), node_index(node_index) {} + int trajectory_id; int node_index; @@ -83,6 +86,9 @@ inline std::ostream& operator<<(std::ostream& os, const NodeId& v) { // Uniquely identifies a submap using a combination of a unique trajectory ID // and a zero-based index of the submap inside that trajectory. struct SubmapId { + SubmapId(int trajectory_id, int submap_index) + : trajectory_id(trajectory_id), submap_index(submap_index) {} + int trajectory_id; int submap_index; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 84faa89..4a94279 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -385,10 +385,16 @@ void PoseGraph2D::HandleWorkQueue( const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { - trajectory_id_to_last_optimized_node_id[trajectory_id] = - std::prev(node_data.EndOfTrajectory(trajectory_id))->id; - trajectory_id_to_last_optimized_submap_id[trajectory_id] = - std::prev(submap_data.EndOfTrajectory(trajectory_id))->id; + if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + continue; + } + trajectory_id_to_last_optimized_node_id.emplace( + trajectory_id, + std::prev(node_data.EndOfTrajectory(trajectory_id))->id); + trajectory_id_to_last_optimized_submap_id.emplace( + trajectory_id, + std::prev(submap_data.EndOfTrajectory(trajectory_id))->id); } } global_slam_optimization_callback_( diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 4fb1649..27b482d 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -402,10 +402,16 @@ void PoseGraph3D::HandleWorkQueue( const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { - trajectory_id_to_last_optimized_node_id[trajectory_id] = - std::prev(node_data.EndOfTrajectory(trajectory_id))->id; - trajectory_id_to_last_optimized_submap_id[trajectory_id] = - std::prev(submap_data.EndOfTrajectory(trajectory_id))->id; + if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + continue; + } + trajectory_id_to_last_optimized_node_id.emplace( + trajectory_id, + std::prev(node_data.EndOfTrajectory(trajectory_id))->id); + trajectory_id_to_last_optimized_submap_id.emplace( + trajectory_id, + std::prev(submap_data.EndOfTrajectory(trajectory_id))->id); } } global_slam_optimization_callback_( diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index 6534dae..8913d84 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -79,12 +79,12 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) { for (int i = 0; i < 2; ++i) { EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); for (int j = 0; j < 2; ++j) { - constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{}, + constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{0, 0}, &node_data, transform::Rigid2d::Identity()); } - constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap, NodeId{}, - &node_data); + constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap, + NodeId{0, 0}, &node_data); constraint_builder_->NotifyEndOfNode(); thread_pool_.WaitUntilIdle(); EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes); diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc index 01793a9..6da38c8 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc @@ -87,12 +87,14 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) { EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); for (int j = 0; j < 2; ++j) { constraint_builder_->MaybeAddConstraint( - submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes, - transform::Rigid3d::Identity(), transform::Rigid3d::Identity()); + submap_id, &submap, NodeId{0, 0}, node.constant_data.get(), + submap_nodes, transform::Rigid3d::Identity(), + transform::Rigid3d::Identity()); } constraint_builder_->MaybeAddGlobalConstraint( - submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes, - Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity()); + submap_id, &submap, NodeId{0, 0}, node.constant_data.get(), + submap_nodes, Eigen::Quaterniond::Identity(), + Eigen::Quaterniond::Identity()); constraint_builder_->NotifyEndOfNode(); thread_pool_.WaitUntilIdle(); EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);