compute_relations_metrics_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 <algorithm>
18 #include <cmath>
19 #include <fstream>
20 #include <iomanip>
21 #include <iostream>
22 #include <sstream>
23 #include <string>
24 #include <vector>
25 
28 #include "cartographer/mapping/proto/trajectory.pb.h"
32 #include "gflags/gflags.h"
33 #include "glog/logging.h"
34 
36  trajectory_filename, "",
37  "Proto containing the trajectory of which to assess the quality.");
38 DEFINE_string(relations_filename, "",
39  "Relations file containing the ground truth.");
40 
41 namespace cartographer {
42 namespace ground_truth {
43 namespace {
44 
45 transform::Rigid3d LookupPose(const transform::TransformInterpolationBuffer&
46  transform_interpolation_buffer,
47  const double time) {
48  constexpr int64 kUtsTicksPerSecond = 10000000;
49  common::Time common_time =
51  kUtsTicksPerSecond) +
52  common::FromSeconds(time);
53  return transform_interpolation_buffer.Lookup(common_time);
54 }
55 
56 struct Error {
59 };
60 
61 Error ComputeError(const transform::Rigid3d& pose1,
62  const transform::Rigid3d& pose2,
63  const transform::Rigid3d& expected) {
64  const transform::Rigid3d error =
65  (pose1.inverse() * pose2) * expected.inverse();
66  return Error{error.translation().squaredNorm(),
68 }
69 
70 string MeanAndStdDevString(const std::vector<double>& values) {
71  CHECK_GE(values.size(), 2);
72  const double mean =
73  std::accumulate(values.begin(), values.end(), 0.) / values.size();
74  double sum_of_squared_differences = 0.;
75  for (const double value : values) {
76  sum_of_squared_differences += common::Pow2(value - mean);
77  }
78  const double standard_deviation =
79  std::sqrt(sum_of_squared_differences / (values.size() - 1));
80  std::ostringstream out;
81  out << std::fixed << std::setprecision(5) << mean << " +/- "
82  << standard_deviation;
83  return string(out.str());
84 }
85 
86 string StatisticsString(const std::vector<Error>& errors) {
87  std::vector<double> translational_errors;
88  std::vector<double> squared_translational_errors;
89  std::vector<double> rotational_errors_degrees;
90  std::vector<double> squared_rotational_errors_degrees;
91  for (const Error& error : errors) {
92  translational_errors.push_back(std::sqrt(error.translational_squared));
93  squared_translational_errors.push_back(error.translational_squared);
94  rotational_errors_degrees.push_back(
95  common::RadToDeg(std::sqrt(error.rotational_squared)));
96  squared_rotational_errors_degrees.push_back(
97  common::Pow2(rotational_errors_degrees.back()));
98  }
99  return "Abs translational error " +
100  MeanAndStdDevString(translational_errors) +
101  " m\n"
102  "Sqr translational error " +
103  MeanAndStdDevString(squared_translational_errors) +
104  " m^2\n"
105  "Abs rotational error " +
106  MeanAndStdDevString(rotational_errors_degrees) +
107  " deg\n"
108  "Sqr rotational error " +
109  MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
110 }
111 
112 void Run(const string& trajectory_filename, const string& relations_filename) {
113  LOG(INFO) << "Reading trajectory from '" << trajectory_filename << "'...";
114  mapping::proto::Trajectory trajectory_proto;
115  {
116  std::ifstream trajectory_stream(trajectory_filename.c_str(),
117  std::ios::binary);
118  CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream));
119  }
120 
121  const auto transform_interpolation_buffer =
123 
124  LOG(INFO) << "Reading relations from '" << relations_filename << "'...";
125  std::vector<Error> errors;
126  {
127  std::ifstream relations_stream(relations_filename.c_str());
128  double time1, time2, x, y, z, roll, pitch, yaw;
129  while (relations_stream >> time1 >> time2 >> x >> y >> z >> roll >> pitch >>
130  yaw) {
131  const auto pose1 = LookupPose(*transform_interpolation_buffer, time1);
132  const auto pose2 = LookupPose(*transform_interpolation_buffer, time2);
133  const transform::Rigid3d expected =
135  transform::RollPitchYaw(roll, pitch, yaw));
136  errors.push_back(ComputeError(pose1, pose2, expected));
137  }
138  CHECK(relations_stream.eof());
139  }
140 
141  LOG(INFO) << "Result:\n" << StatisticsString(errors);
142 }
143 
144 } // namespace
145 } // namespace ground_truth
146 } // namespace cartographer
147 
148 int main(int argc, char** argv) {
149  google::InitGoogleLogging(argv[0]);
150  FLAGS_logtostderr = true;
151  google::SetUsageMessage(
152  "\n\n"
153  "This program computes the relation based metric described in:\n"
154  "R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n"
155  "C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n"
156  "algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.\n"
157  "\n"
158  "Note: Timestamps in the relations_file are interpreted relative to\n"
159  " the Unix epoch.");
160  google::ParseCommandLineFlags(&argc, &argv, true);
161 
162  if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) {
163  google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics");
164  return EXIT_FAILURE;
165  }
166  ::cartographer::ground_truth::Run(FLAGS_trajectory_filename,
167  FLAGS_relations_filename);
168 }
static std::unique_ptr< TransformInterpolationBuffer > FromTrajectory(const mapping::proto::Trajectory &trajectory)
double translational_squared
constexpr T Pow2(T a)
Definition: math.h:50
Rigid3< double > Rigid3d
constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds
Definition: time.h:29
constexpr double RadToDeg(double rad)
Definition: math.h:58
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
Duration FromSeconds(const double seconds)
Definition: time.cc:24
double rotational_squared
int main(int argc, char **argv)
DEFINE_string(trajectory_filename,"","Proto containing the trajectory of which to assess the quality.")
const Vector & translation() const
int64_t int64
Definition: port.h:31
FloatType GetAngle(const Rigid3< FloatType > &transform)
Definition: transform.h:34
float value
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, const double yaw)
Eigen::Matrix< double, 3, 1 > Vector


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:38