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

Drop trajectory_id from TrajectoryNode. (#330) 

PAIR=wohe
master
Holger Rapp 2017-06-12 14:28:16 +02:00 committed by GitHub
parent 1a376c07b2
commit f68da37c6e
4 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/sparse_pose_graph.cc
View File

@ -81,7 +81,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}
for (const auto& trajectory_nodes : GetTrajectoryNodes()) {
const auto all_trajectory_nodes = GetTrajectoryNodes();
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
++trajectory_id) {
const auto& trajectory_nodes = all_trajectory_nodes[trajectory_id];
auto* trajectory_proto = proto.add_trajectory();
for (const auto& node : trajectory_nodes) {
auto* node_proto = trajectory_proto->add_node();
@ -91,8 +94,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
}
if (!trajectory_nodes.empty()) {
for (const auto& transform : GetSubmapTransforms(
trajectory_nodes[0].constant_data->trajectory_id)) {
for (const auto& transform : GetSubmapTransforms(trajectory_id)) {
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(transform);
}

3
cartographer/mapping/trajectory_node.h
View File

@ -40,9 +40,6 @@ struct TrajectoryNode {
// Range data in 'pose' frame. Only used in the 3D case.
sensor::CompressedRangeData range_data_3d;
// Trajectory this node belongs to.
int trajectory_id;
// Transform from the 3D 'tracking' frame to the 'pose' frame of the range
// data, which contains roll, pitch and height for 2D. In 3D this is always
// identity.

3
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan(
mapping::TrajectoryNode::Data{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose}),
tracking_to_pose}),
optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
@ -228,7 +228,6 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.size())
: 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {

3
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -101,7 +101,7 @@ void SparsePoseGraph::AddScan(
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id,
sensor::Compress(range_data_in_tracking),
transform::Rigid3d::Identity()}),
optimized_pose});
++num_trajectory_nodes_;
@ -245,7 +245,6 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.size())
: 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) {