diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 828d0ae..440839f 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -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); } diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 38a5915..54b27f3 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -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. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 4b51c63..ffc9c8f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index c406179..717c4cc 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -101,7 +101,7 @@ void SparsePoseGraph::AddScan( std::make_shared( 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) {