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

Adds a bit of terminology documentation. (#591) 

Changes code to match the new documentation.
master
Wolfgang Hess 2017-10-17 10:07:54 +02:00 committed by GitHub
parent 76ed37768f
commit bcd1be92b1
11 changed files with 81 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/proto/trajectory_node_data.proto
View File

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

2
cartographer/mapping/sparse_pose_graph.cc
View File

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

6
cartographer/mapping/trajectory_node.cc
View File

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

7
cartographer/mapping/trajectory_node.h
View File

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

4
cartographer/mapping/trajectory_node_test.cc
View File

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

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -98,7 +98,7 @@ void SparsePoseGraph::AddScan(
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& 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;
}
}

12
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -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()),

2
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -132,7 +132,7 @@ std::vector<std::pair<Eigen::VectorXf, float>> 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;

14
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -95,7 +95,7 @@ void SparsePoseGraph::AddScan(
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& 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;
}
}

1
docs/source/index.rst
View File

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

50
docs/source/terminology.rst Normal file
View File

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