autogenerate_ground_truth_main.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <cmath>
18 #include <fstream>
19 #include <string>
20 
22 #include "cartographer/ground_truth/proto/relations.pb.h"
23 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
25 #include "gflags/gflags.h"
26 #include "glog/logging.h"
27 
29  pose_graph_filename, "",
30  "File with the pose graph proto from which to generate ground truth data.");
31 DEFINE_string(output_filename, "", "File to write the ground truth proto to.");
32 DEFINE_double(min_covered_distance, 100.,
33  "Minimum covered distance in meters before a loop closure is "
34  "considered a candidate for autogenerated ground truth.");
35 DEFINE_double(outlier_threshold, 0.15,
36  "Distance in meters beyond which constraints are considered "
37  "outliers.");
38 
39 namespace cartographer {
40 namespace ground_truth {
41 namespace {
42 
43 std::vector<double> ComputeCoveredDistance(
44  const mapping::proto::Trajectory& trajectory) {
45  std::vector<double> covered_distance;
46  covered_distance.push_back(0.);
47  CHECK_GT(trajectory.node_size(), 0)
48  << "Trajectory does not contain any nodes.";
49  for (int i = 1; i < trajectory.node_size(); ++i) {
50  const auto last_pose = transform::ToRigid3(trajectory.node(i - 1).pose());
51  const auto this_pose = transform::ToRigid3(trajectory.node(i).pose());
52  covered_distance.push_back(
53  covered_distance.back() +
54  (last_pose.inverse() * this_pose).translation().norm());
55  }
56  return covered_distance;
57 }
58 
59 // We pick the representative scan in the middle of the submap.
60 //
61 // TODO(whess): Should we consider all scans inserted into the submap and
62 // excluded, e.g. based on large relative linear or angular distance?
63 std::vector<int> ComputeSubmapRepresentativeScan(
64  const mapping::proto::SparsePoseGraph& pose_graph) {
65  std::vector<int> submap_to_scan_index;
66  for (const auto& constraint : pose_graph.constraint()) {
67  if (constraint.tag() !=
68  mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
69  continue;
70  }
71  CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
72  CHECK_EQ(constraint.scan_id().trajectory_id(), 0);
73 
74  const int next_submap_index = static_cast<int>(submap_to_scan_index.size());
75  const int submap_index = constraint.submap_id().submap_index();
76  if (submap_index <= next_submap_index) {
77  continue;
78  }
79 
80  CHECK_EQ(submap_index, next_submap_index + 1);
81  submap_to_scan_index.push_back(constraint.scan_id().scan_index());
82  }
83  return submap_to_scan_index;
84 }
85 
86 proto::GroundTruth GenerateGroundTruth(
87  const mapping::proto::SparsePoseGraph& pose_graph,
88  const double min_covered_distance, const double outlier_threshold) {
89  const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0);
90  const std::vector<double> covered_distance =
91  ComputeCoveredDistance(trajectory);
92 
93  const std::vector<int> submap_to_scan_index =
94  ComputeSubmapRepresentativeScan(pose_graph);
95 
96  int num_outliers = 0;
97  proto::GroundTruth ground_truth;
98  for (const auto& constraint : pose_graph.constraint()) {
99  // We're only interested in loop closure constraints.
100  if (constraint.tag() ==
101  mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) {
102  continue;
103  }
104 
105  // For some submaps at the very end, we have not chosen a representative
106  // scan, but those should not be part of loop closure anyway.
107  CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
108  CHECK_EQ(constraint.scan_id().trajectory_id(), 0);
109  if (constraint.submap_id().submap_index() >=
110  static_cast<int>(submap_to_scan_index.size())) {
111  continue;
112  }
113  const int matched_scan = constraint.scan_id().scan_index();
114  const int representative_scan =
115  submap_to_scan_index.at(constraint.submap_id().submap_index());
116 
117  // Covered distance between the two should not be too small.
118  if (std::abs(covered_distance.at(matched_scan) -
119  covered_distance.at(representative_scan)) <
120  min_covered_distance) {
121  continue;
122  }
123 
124  // Compute the transform between the scans according to the solution and
125  // the constraint.
126  const auto solution_pose1 =
127  transform::ToRigid3(trajectory.node(representative_scan).pose());
128  const auto solution_pose2 =
129  transform::ToRigid3(trajectory.node(matched_scan).pose());
130  const auto solution = solution_pose1.inverse() * solution_pose2;
131 
132  const auto scan_to_submap_constraint =
133  transform::ToRigid3(constraint.relative_pose());
134  const auto submap_solution = transform::ToRigid3(
135  trajectory.submap(constraint.submap_id().submap_index()).pose());
136  const auto error =
137  submap_solution * scan_to_submap_constraint * solution_pose2.inverse();
138 
139  if (error.translation().norm() > outlier_threshold) {
140  ++num_outliers;
141  }
142  auto* const new_relation = ground_truth.add_relation();
143  new_relation->set_timestamp1(
144  trajectory.node(representative_scan).timestamp());
145  new_relation->set_timestamp2(trajectory.node(matched_scan).timestamp());
146  *new_relation->mutable_expected() = transform::ToProto(solution);
147  }
148  LOG(INFO) << "Generated " << ground_truth.relation_size()
149  << " relations and ignored " << num_outliers << " outliers.";
150  return ground_truth;
151 }
152 
153 void Run(const string& pose_graph_filename, const string& output_filename,
154  const double min_covered_distance, const double outlier_threshold) {
155  LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
156  mapping::proto::SparsePoseGraph pose_graph;
157  {
158  std::ifstream stream(pose_graph_filename.c_str());
159  CHECK(pose_graph.ParseFromIstream(&stream));
160  CHECK_EQ(pose_graph.trajectory_size(), 1)
161  << "Only pose graphs containing a single trajectory are supported.";
162  }
163  LOG(INFO) << "Autogenerating ground truth relations...";
164  const proto::GroundTruth ground_truth =
165  GenerateGroundTruth(pose_graph, min_covered_distance, outlier_threshold);
166  LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '"
167  << output_filename << "'.";
168  {
169  std::ofstream output_stream(output_filename,
170  std::ios_base::out | std::ios_base::binary);
171  CHECK(ground_truth.SerializeToOstream(&output_stream))
172  << "Could not serialize ground truth data.";
173  output_stream.close();
174  CHECK(output_stream) << "Could not write ground truth data.";
175  }
176 }
177 
178 } // namespace
179 } // namespace ground_truth
180 } // namespace cartographer
181 
182 int main(int argc, char** argv) {
183  google::InitGoogleLogging(argv[0]);
184  FLAGS_logtostderr = true;
185  google::SetUsageMessage(
186  "\n\n"
187  "This program semi-automatically generates ground truth data from a\n"
188  "pose graph proto.\n"
189  "\n"
190  "The input should contain a single trajectory and should have been\n"
191  "manually assessed to be correctly loop closed. Small local distortions\n"
192  "are acceptable if they are tiny compared to the errors we want to\n"
193  "assess using the generated ground truth data.\n"
194  "\n"
195  "All loop closure constraints separated by long covered distance are\n"
196  "included in the output. Outliers are removed.\n");
197  google::ParseCommandLineFlags(&argc, &argv, true);
198 
199  if (FLAGS_pose_graph_filename.empty() || FLAGS_output_filename.empty()) {
200  google::ShowUsageWithFlagsRestrict(argv[0], "autogenerate_ground_truth");
201  return EXIT_FAILURE;
202  }
203  ::cartographer::ground_truth::Run(
204  FLAGS_pose_graph_filename, FLAGS_output_filename,
205  FLAGS_min_covered_distance, FLAGS_outlier_threshold);
206 }
DEFINE_string(pose_graph_filename,"","File with the pose graph proto from which to generate ground truth data.")
transform::Rigid3d ToRigid3(const proto::Rigid3d &rigid)
Definition: transform.cc:67
transform::Rigid3d pose
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:44
DEFINE_double(min_covered_distance, 100.,"Minimum covered distance in meters before a loop closure is ""considered a candidate for autogenerated ground truth.")
int main(int argc, char **argv)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:57