12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
gaschler 2018-07-02 14:51:48 +02:00 committed by Wally B. Feed
parent 7fcc4576a8
commit 7839f3f216
5 changed files with 35 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping/id.h
View File

@ -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;

14
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -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_(

14
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -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_(

6
cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc
View File

@ -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);

10
cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc
View File

@ -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);