diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc index c14a379..3fa6120 100644 --- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc +++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -61,31 +61,31 @@ std::vector ComputeCoveredDistance( return covered_distance; } -// We pick the representative scan in the middle of the submap. +// We pick the representative node in the middle of the submap. // -// TODO(whess): Should we consider all scans inserted into the submap and +// TODO(whess): Should we consider all nodes inserted into the submap and // exclude, e.g. based on large relative linear or angular distance? -std::vector ComputeSubmapRepresentativeScan( +std::vector ComputeSubmapRepresentativeNode( const mapping::proto::SparsePoseGraph& pose_graph) { - std::vector submap_to_scan_index; + std::vector submap_to_node_index; for (const auto& constraint : pose_graph.constraint()) { if (constraint.tag() != mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) { continue; } CHECK_EQ(constraint.submap_id().trajectory_id(), 0); - CHECK_EQ(constraint.scan_id().trajectory_id(), 0); + CHECK_EQ(constraint.node_id().trajectory_id(), 0); - const int next_submap_index = static_cast(submap_to_scan_index.size()); + const int next_submap_index = static_cast(submap_to_node_index.size()); const int submap_index = constraint.submap_id().submap_index(); if (submap_index <= next_submap_index) { continue; } CHECK_EQ(submap_index, next_submap_index + 1); - submap_to_scan_index.push_back(constraint.scan_id().scan_index()); + submap_to_node_index.push_back(constraint.node_id().node_index()); } - return submap_to_scan_index; + return submap_to_node_index; } proto::GroundTruth GenerateGroundTruth( @@ -96,8 +96,8 @@ proto::GroundTruth GenerateGroundTruth( const std::vector covered_distance = ComputeCoveredDistance(trajectory); - const std::vector submap_to_scan_index = - ComputeSubmapRepresentativeScan(pose_graph); + const std::vector submap_to_node_index = + ComputeSubmapRepresentativeNode(pose_graph); int num_outliers = 0; proto::GroundTruth ground_truth; @@ -109,41 +109,41 @@ proto::GroundTruth GenerateGroundTruth( } // For some submaps at the very end, we have not chosen a representative - // scan, but those should not be part of loop closure anyway. + // node, but those should not be part of loop closure anyway. CHECK_EQ(constraint.submap_id().trajectory_id(), 0); - CHECK_EQ(constraint.scan_id().trajectory_id(), 0); + CHECK_EQ(constraint.node_id().trajectory_id(), 0); if (constraint.submap_id().submap_index() >= - static_cast(submap_to_scan_index.size())) { + static_cast(submap_to_node_index.size())) { continue; } - const int matched_scan = constraint.scan_id().scan_index(); - const int representative_scan = - submap_to_scan_index.at(constraint.submap_id().submap_index()); + const int matched_node = constraint.node_id().node_index(); + const int representative_node = + submap_to_node_index.at(constraint.submap_id().submap_index()); // Covered distance between the two should not be too small. - if (std::abs(covered_distance.at(matched_scan) - - covered_distance.at(representative_scan)) < + if (std::abs(covered_distance.at(matched_node) - + covered_distance.at(representative_node)) < min_covered_distance) { continue; } - // Compute the transform between the scans according to the solution and + // Compute the transform between the nodes according to the solution and // the constraint. const transform::Rigid3d solution_pose1 = - transform::ToRigid3(trajectory.node(representative_scan).pose()); + transform::ToRigid3(trajectory.node(representative_node).pose()); const transform::Rigid3d solution_pose2 = - transform::ToRigid3(trajectory.node(matched_scan).pose()); + transform::ToRigid3(trajectory.node(matched_node).pose()); const transform::Rigid3d solution = solution_pose1.inverse() * solution_pose2; const transform::Rigid3d submap_solution = transform::ToRigid3( trajectory.submap(constraint.submap_id().submap_index()).pose()); - const transform::Rigid3d submap_solution_to_scan_solution = + const transform::Rigid3d submap_solution_to_node_solution = solution_pose1.inverse() * submap_solution; - const transform::Rigid3d scan_to_submap_constraint = + const transform::Rigid3d node_to_submap_constraint = transform::ToRigid3(constraint.relative_pose()); const transform::Rigid3d expected = - submap_solution_to_scan_solution * scan_to_submap_constraint; + submap_solution_to_node_solution * node_to_submap_constraint; const transform::Rigid3d error = solution * expected.inverse(); @@ -154,8 +154,8 @@ proto::GroundTruth GenerateGroundTruth( } auto* const new_relation = ground_truth.add_relation(); new_relation->set_timestamp1( - trajectory.node(representative_scan).timestamp()); - new_relation->set_timestamp2(trajectory.node(matched_scan).timestamp()); + trajectory.node(representative_node).timestamp()); + new_relation->set_timestamp2(trajectory.node(matched_node).timestamp()); *new_relation->mutable_expected() = transform::ToProto(expected); } LOG(INFO) << "Generated " << ground_truth.relation_size() diff --git a/cartographer/mapping/proto/sparse_pose_graph.proto b/cartographer/mapping/proto/sparse_pose_graph.proto index c2ac704..04e68b0 100644 --- a/cartographer/mapping/proto/sparse_pose_graph.proto +++ b/cartographer/mapping/proto/sparse_pose_graph.proto @@ -24,9 +24,9 @@ message SubmapId { optional int32 submap_index = 2; // Submap index in the given trajectory. } -message ScanId { +message NodeId { optional int32 trajectory_id = 1; - optional int32 scan_index = 2; // Scan index in the given trajectory. + optional int32 node_index = 2; // Node index in the given trajectory. } message SparsePoseGraph { @@ -40,7 +40,7 @@ message SparsePoseGraph { } optional SubmapId submap_id = 1; // Submap ID. - optional ScanId scan_id = 2; // Scan ID. + optional NodeId node_id = 2; // Scan ID. // Pose of the scan relative to submap, i.e. taking data from the scan frame // into the submap frame. optional transform.proto.Rigid3d relative_pose = 3; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index a4eea84..17361cd 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -120,9 +120,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { submap_id.submap_index); const NodeId node_id = node_id_remapping.at(constraint.node_id); - constraint_proto->mutable_scan_id()->set_trajectory_id( + constraint_proto->mutable_node_id()->set_trajectory_id( node_id.trajectory_id); - constraint_proto->mutable_scan_id()->set_scan_index(node_id.node_index); + constraint_proto->mutable_node_id()->set_node_index(node_id.node_index); constraint_proto->set_tag(mapping::ToProto(constraint.tag)); }