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

Rename ScanId to NodeId in the serialization. (#394) 

This is consistent with the naming in the C++ structs.
master
Wolfgang Hess 2017-07-07 13:58:23 +02:00 committed by GitHub
parent 00e3d33e43
commit 94a848cd46
3 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/ground_truth/autogenerate_ground_truth_main.cc
View File

@ -61,31 +61,31 @@ std::vector<double> ComputeCoveredDistance(
return covered_distance; 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? // exclude, e.g. based on large relative linear or angular distance?
std::vector<int> ComputeSubmapRepresentativeScan( std::vector<int> ComputeSubmapRepresentativeNode(
const mapping::proto::SparsePoseGraph& pose_graph) { const mapping::proto::SparsePoseGraph& pose_graph) {
std::vector<int> submap_to_scan_index; std::vector<int> submap_to_node_index;
for (const auto& constraint : pose_graph.constraint()) { for (const auto& constraint : pose_graph.constraint()) {
if (constraint.tag() != if (constraint.tag() !=
mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) { mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
continue; continue;
} }
CHECK_EQ(constraint.submap_id().trajectory_id(), 0); 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<int>(submap_to_scan_index.size()); const int next_submap_index = static_cast<int>(submap_to_node_index.size());
const int submap_index = constraint.submap_id().submap_index(); const int submap_index = constraint.submap_id().submap_index();
if (submap_index <= next_submap_index) { if (submap_index <= next_submap_index) {
continue; continue;
} }
CHECK_EQ(submap_index, next_submap_index + 1); 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( proto::GroundTruth GenerateGroundTruth(
@ -96,8 +96,8 @@ proto::GroundTruth GenerateGroundTruth(
const std::vector<double> covered_distance = const std::vector<double> covered_distance =
ComputeCoveredDistance(trajectory); ComputeCoveredDistance(trajectory);
const std::vector<int> submap_to_scan_index = const std::vector<int> submap_to_node_index =
ComputeSubmapRepresentativeScan(pose_graph); ComputeSubmapRepresentativeNode(pose_graph);
int num_outliers = 0; int num_outliers = 0;
proto::GroundTruth ground_truth; proto::GroundTruth ground_truth;
@ -109,41 +109,41 @@ proto::GroundTruth GenerateGroundTruth(
} }
// For some submaps at the very end, we have not chosen a representative // 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.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() >= if (constraint.submap_id().submap_index() >=
static_cast<int>(submap_to_scan_index.size())) { static_cast<int>(submap_to_node_index.size())) {
continue; continue;
} }
const int matched_scan = constraint.scan_id().scan_index(); const int matched_node = constraint.node_id().node_index();
const int representative_scan = const int representative_node =
submap_to_scan_index.at(constraint.submap_id().submap_index()); submap_to_node_index.at(constraint.submap_id().submap_index());
// Covered distance between the two should not be too small. // Covered distance between the two should not be too small.
if (std::abs(covered_distance.at(matched_scan) - if (std::abs(covered_distance.at(matched_node) -
covered_distance.at(representative_scan)) < covered_distance.at(representative_node)) <
min_covered_distance) { min_covered_distance) {
continue; 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. // the constraint.
const transform::Rigid3d solution_pose1 = const transform::Rigid3d solution_pose1 =
transform::ToRigid3(trajectory.node(representative_scan).pose()); transform::ToRigid3(trajectory.node(representative_node).pose());
const transform::Rigid3d solution_pose2 = const transform::Rigid3d solution_pose2 =
transform::ToRigid3(trajectory.node(matched_scan).pose()); transform::ToRigid3(trajectory.node(matched_node).pose());
const transform::Rigid3d solution = const transform::Rigid3d solution =
solution_pose1.inverse() * solution_pose2; solution_pose1.inverse() * solution_pose2;
const transform::Rigid3d submap_solution = transform::ToRigid3( const transform::Rigid3d submap_solution = transform::ToRigid3(
trajectory.submap(constraint.submap_id().submap_index()).pose()); 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; solution_pose1.inverse() * submap_solution;
const transform::Rigid3d scan_to_submap_constraint = const transform::Rigid3d node_to_submap_constraint =
transform::ToRigid3(constraint.relative_pose()); transform::ToRigid3(constraint.relative_pose());
const transform::Rigid3d expected = 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(); const transform::Rigid3d error = solution * expected.inverse();
@ -154,8 +154,8 @@ proto::GroundTruth GenerateGroundTruth(
} }
auto* const new_relation = ground_truth.add_relation(); auto* const new_relation = ground_truth.add_relation();
new_relation->set_timestamp1( new_relation->set_timestamp1(
trajectory.node(representative_scan).timestamp()); trajectory.node(representative_node).timestamp());
new_relation->set_timestamp2(trajectory.node(matched_scan).timestamp()); new_relation->set_timestamp2(trajectory.node(matched_node).timestamp());
*new_relation->mutable_expected() = transform::ToProto(expected); *new_relation->mutable_expected() = transform::ToProto(expected);
} }
LOG(INFO) << "Generated " << ground_truth.relation_size() LOG(INFO) << "Generated " << ground_truth.relation_size()

6
cartographer/mapping/proto/sparse_pose_graph.proto
View File

@ -24,9 +24,9 @@ message SubmapId {
optional int32 submap_index = 2; // Submap index in the given trajectory. optional int32 submap_index = 2; // Submap index in the given trajectory.
} }
message ScanId { message NodeId {
optional int32 trajectory_id = 1; 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 { message SparsePoseGraph {
@ -40,7 +40,7 @@ message SparsePoseGraph {
} }
optional SubmapId submap_id = 1; // Submap ID. 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 // Pose of the scan relative to submap, i.e. taking data from the scan frame
// into the submap frame. // into the submap frame.
optional transform.proto.Rigid3d relative_pose = 3; optional transform.proto.Rigid3d relative_pose = 3;

4
cartographer/mapping/sparse_pose_graph.cc
View File

@ -120,9 +120,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
submap_id.submap_index); submap_id.submap_index);
const NodeId node_id = node_id_remapping.at(constraint.node_id); 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); 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)); constraint_proto->set_tag(mapping::ToProto(constraint.tag));
} }