From bcd1be92b1871fcc170bd81895bfb7a18f6d01c9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 17 Oct 2017 10:07:54 +0200 Subject: [PATCH] Adds a bit of terminology documentation. (#591) Changes code to match the new documentation. --- .../mapping/proto/trajectory_node_data.proto | 2 +- cartographer/mapping/sparse_pose_graph.cc | 2 +- cartographer/mapping/trajectory_node.cc | 6 +-- cartographer/mapping/trajectory_node.h | 7 +-- cartographer/mapping/trajectory_node_test.cc | 4 +- cartographer/mapping_2d/sparse_pose_graph.cc | 10 ++-- .../mapping_2d/sparse_pose_graph_test.cc | 12 ++--- .../fast_correlative_scan_matcher.cc | 2 +- cartographer/mapping_3d/sparse_pose_graph.cc | 14 +++--- docs/source/index.rst | 1 + docs/source/terminology.rst | 50 +++++++++++++++++++ 11 files changed, 81 insertions(+), 29 deletions(-) create mode 100644 docs/source/terminology.rst diff --git a/cartographer/mapping/proto/trajectory_node_data.proto b/cartographer/mapping/proto/trajectory_node_data.proto index 533cc87..e5dcffd 100644 --- a/cartographer/mapping/proto/trajectory_node_data.proto +++ b/cartographer/mapping/proto/trajectory_node_data.proto @@ -28,5 +28,5 @@ message TrajectoryNodeData { optional sensor.proto.CompressedPointCloud high_resolution_point_cloud = 4; optional sensor.proto.CompressedPointCloud low_resolution_point_cloud = 5; repeated float rotational_scan_matcher_histogram = 6; - optional transform.proto.Rigid3d initial_pose = 7; + optional transform.proto.Rigid3d local_pose = 7; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 4224034..5dbda25 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -87,7 +87,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { 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_proto->mutable_pose() = transform::ToProto(node.global_pose); } } diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc index 850a1ab..2b4553b 100644 --- a/cartographer/mapping/trajectory_node.cc +++ b/cartographer/mapping/trajectory_node.cc @@ -44,8 +44,8 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) { proto.add_rotational_scan_matcher_histogram( constant_data.rotational_scan_matcher_histogram(i)); } - *proto.mutable_initial_pose() = - transform::ToProto(constant_data.initial_pose); + *proto.mutable_local_pose() = + transform::ToProto(constant_data.local_pose); return proto; } @@ -66,7 +66,7 @@ TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) { sensor::CompressedPointCloud(proto.low_resolution_point_cloud()) .Decompress(), rotational_scan_matcher_histogram, - transform::ToRigid3(proto.initial_pose())}; + transform::ToRigid3(proto.local_pose())}; } } // namespace mapping diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 0687bbe..49d3937 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -46,8 +46,8 @@ struct TrajectoryNode { sensor::PointCloud low_resolution_point_cloud; Eigen::VectorXf rotational_scan_matcher_histogram; - // The initial unoptimized node pose. - transform::Rigid3d initial_pose; + // The node pose in the local SLAM frame. + transform::Rigid3d local_pose; }; common::Time time() const { return constant_data->time; } @@ -57,7 +57,8 @@ struct TrajectoryNode { // node is being trimmed, it must survive until all use finishes. std::shared_ptr constant_data; - transform::Rigid3d pose; + // The node pose in the global SLAM frame. + transform::Rigid3d global_pose; }; proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data); diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc index 658daa4..b2ee936 100644 --- a/cartographer/mapping/trajectory_node_test.cc +++ b/cartographer/mapping/trajectory_node_test.cc @@ -51,8 +51,8 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { actual.low_resolution_point_cloud); EXPECT_EQ(expected.rotational_scan_matcher_histogram, actual.rotational_scan_matcher_histogram); - EXPECT_THAT(actual.initial_pose, - transform::IsNearly(expected.initial_pose, 1e-9)); + EXPECT_THAT(actual.local_pose, + transform::IsNearly(expected.local_pose, 1e-9)); } } // namespace diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 52c6677..b36093b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -98,7 +98,7 @@ void SparsePoseGraph::AddScan( const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose); + GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); @@ -222,7 +222,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::SubmapId matching_id = submap_ids.front(); const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid2d pose = transform::Project2D( - constant_data->initial_pose * + constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data().at(matching_id).pose * @@ -429,7 +429,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, constant_data->gravity_alignment.inverse()); optimization_problem_.AddTrajectoryNode( node_id.trajectory_id, constant_data->time, - transform::Project2D(constant_data->initial_pose * + transform::Project2D(constant_data->local_pose * gravity_alignment_inverse), transform::Project2D(pose * gravity_alignment_inverse), constant_data->gravity_alignment); @@ -476,7 +476,7 @@ void SparsePoseGraph::RunOptimization() { for (; node_it != trajectory_end; ++node_it) { const mapping::NodeId node_id = node_it->id; auto& node = trajectory_nodes_.at(node_id); - node.pose = + node.global_pose = transform::Embed3D(node_it->data.pose) * transform::Rigid3d::Rotation(node.constant_data->gravity_alignment); } @@ -491,7 +491,7 @@ void SparsePoseGraph::RunOptimization() { for (int node_index = last_optimized_node_index + 1; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; - auto& node_pose = trajectory_nodes_.at(node_id).pose; + auto& node_pose = trajectory_nodes_.at(node_id).global_pose; node_pose = old_global_to_new_global * node_pose; } } diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index cbb39cc..d572c62 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -194,11 +194,11 @@ TEST_F(SparsePoseGraphTest, NoMovement) { const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(nodes.size(), ::testing::Eq(1u)); EXPECT_THAT(nodes[0].size(), ::testing::Eq(3u)); - EXPECT_THAT(nodes[0][0].pose, + EXPECT_THAT(nodes[0][0].global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes[0][1].pose, + EXPECT_THAT(nodes[0][1].global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); - EXPECT_THAT(nodes[0][2].pose, + EXPECT_THAT(nodes[0][2].global_pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } @@ -215,7 +215,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) { ASSERT_THAT(nodes.size(), ::testing::Eq(1u)); for (int i = 0; i != 4; ++i) { EXPECT_THAT(poses[i], - IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) + IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2)) << i; } } @@ -233,7 +233,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { ASSERT_THAT(nodes.size(), ::testing::Eq(1u)); for (int i = 0; i != 5; ++i) { EXPECT_THAT(poses[i], - IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) + IsNearly(transform::Project2D(nodes[0][i].global_pose), 1e-2)) << i; } } @@ -261,7 +261,7 @@ TEST_F(SparsePoseGraphTest, OverlappingScans) { transform::Rigid2d movement_before = poses.front().inverse() * poses.back(); transform::Rigid2d error_before = movement_before.inverse() * true_movement; transform::Rigid3d optimized_movement = - nodes[0].front().pose.inverse() * nodes[0].back().pose; + nodes[0].front().global_pose.inverse() * nodes[0].back().global_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/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 2fd9987..fef87b6 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -132,7 +132,7 @@ std::vector> HistogramsAtAnglesFromNodes( histograms_at_angles.emplace_back( node.constant_data->rotational_scan_matcher_histogram, transform::GetYaw( - node.pose * transform::Rigid3d::Rotation( + node.global_pose * transform::Rigid3d::Rotation( node.constant_data->gravity_alignment.inverse()))); } return histograms_at_angles; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f4849e6..0b64592 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -95,7 +95,7 @@ void SparsePoseGraph::AddScan( const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose); + GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); @@ -185,7 +185,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, submap_data_.at(submap_id).node_ids) { submap_nodes.push_back(mapping::TrajectoryNode{ trajectory_nodes_.at(submap_node_id).constant_data, - inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose}); + inverse_submap_pose * trajectory_nodes_.at(submap_node_id).global_pose}); } const common::Time scan_time = GetLatestScanTime(node_id, submap_id); @@ -246,7 +246,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; - const transform::Rigid3d& pose = constant_data->initial_pose; + const transform::Rigid3d& pose = constant_data->local_pose; const transform::Rigid3d optimized_pose = optimization_problem_.submap_data().at(matching_id).pose * insertion_submaps.front()->local_pose().inverse() * pose; @@ -445,7 +445,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_.AddTrajectoryNode(node_id.trajectory_id, constant_data->time, - constant_data->initial_pose, pose); + constant_data->local_pose, pose); }); } @@ -475,7 +475,7 @@ void SparsePoseGraph::LogResidualHistograms() { for (const Constraint& constraint : constraints_) { if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { const cartographer::transform::Rigid3d optimized_node_to_map = - trajectory_nodes_.at(constraint.node_id).pose; + trajectory_nodes_.at(constraint.node_id).global_pose; const cartographer::transform::Rigid3d node_to_submap_constraint = constraint.pose.zbar_ij; const cartographer::transform::Rigid3d optimized_submap_to_map = @@ -515,7 +515,7 @@ void SparsePoseGraph::RunOptimization() { for (; node_it != trajectory_end; ++node_it) { const mapping::NodeId node_id = node_it->id; auto& node = trajectory_nodes_.at(node_id); - node.pose = node_it->data.pose; + node.global_pose = node_it->data.pose; } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = @@ -528,7 +528,7 @@ void SparsePoseGraph::RunOptimization() { for (int node_index = last_optimized_node_index + 1; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; - auto& node_pose = trajectory_nodes_.at(node_id).pose; + auto& node_pose = trajectory_nodes_.at(node_id).global_pose; node_pose = old_global_to_new_global * node_pose; } } diff --git a/docs/source/index.rst b/docs/source/index.rst index 83d064e..f29d3e3 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -21,6 +21,7 @@ Cartographer :hidden: configuration + terminology `Cartographer`_ is a system that provides real-time simultaneous localization and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor diff --git a/docs/source/terminology.rst b/docs/source/terminology.rst new file mode 100644 index 0000000..f2e3ba6 --- /dev/null +++ b/docs/source/terminology.rst @@ -0,0 +1,50 @@ +.. Copyright 2017 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. + +=========== +Terminology +=========== + +This documents a few common patterns that exist in the Cartographer codebase. + +Frames +====== + +global (map) frame + This is the frame in which global SLAM results are expressed. It is the fixed + map frame including all loop closure and optimization results. The transform + between this frame and any other frame can jump when new optimization results + are available. Its z-axis points upwards, i.e. the gravitational acceleration + vector points in the -z direction, i.e. the gravitational component measured + by an accelerometer is in the +z direction. + +local (map) frame + This is the frame in which local SLAM results are expressed. It is the fixed + map frame excluding loop closures and the pose graph optimization. For a given + point in time, the transform between this and the global map frame may change, + but the transform between this and all other frames does not change. + +tracking frame + The frame in which sensor data is expressed. + + + +Transforms +========== + +local_pose + Transforms data from the tracking frame to the local frame. + +global_pose + Transforms data from the tracking frame to the global frame.