Add tool to compare to ground truth. (#148)

This computes metrics based on a SLAM result and a relations
file containing ground truth data.
master
Wolfgang Hess 2016-11-30 15:18:26 +01:00 committed by GitHub
parent 51a0ec06a1
commit 0e826377c4
6 changed files with 212 additions and 10 deletions

View File

@ -13,6 +13,7 @@
# limitations under the License. # limitations under the License.
add_subdirectory("common") add_subdirectory("common")
add_subdirectory("ground_truth")
add_subdirectory("io") add_subdirectory("io")
add_subdirectory("kalman_filter") add_subdirectory("kalman_filter")
add_subdirectory("mapping") add_subdirectory("mapping")

View File

@ -26,6 +26,9 @@
namespace cartographer { namespace cartographer {
namespace common { namespace common {
constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds =
(719162ll * 24ll * 60ll * 60ll);
struct UniversalTimeScaleClock { struct UniversalTimeScaleClock {
using rep = int64; using rep = int64;
using period = std::ratio<1, 10000000>; using period = std::ratio<1, 10000000>;

View File

@ -0,0 +1,28 @@
# Copyright 2016 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
google_binary(cartographer_compute_relations_metrics
USES_GFLAGS
USES_GLOG
SRCS
compute_relations_metrics_main.cc
DEPENDS
common_math
common_port
mapping_proto_trajectory
transform_rigid_transform
transform_transform
transform_transform_interpolation_buffer
)

View File

@ -0,0 +1,168 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
DEFINE_string(
trajectory_filename, "",
"Proto containing the trajectory of which to assess the quality.");
DEFINE_string(relations_filename, "",
"Relations file containing the ground truth.");
namespace cartographer {
namespace ground_truth {
namespace {
transform::Rigid3d LookupPose(const transform::TransformInterpolationBuffer&
transform_interpolation_buffer,
const double time) {
constexpr int64 kUtsTicksPerSecond = 10000000;
common::Time common_time =
common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds *
kUtsTicksPerSecond) +
common::FromSeconds(time);
return transform_interpolation_buffer.Lookup(common_time);
}
struct Error {
double translational_squared;
double rotational_squared;
};
Error ComputeError(const transform::Rigid3d& pose1,
const transform::Rigid3d& pose2,
const transform::Rigid3d& expected) {
const transform::Rigid3d error =
(pose1.inverse() * pose2) * expected.inverse();
return Error{error.translation().squaredNorm(),
common::Pow2(transform::GetAngle(error))};
}
string MeanAndStdDevString(const std::vector<double>& values) {
CHECK_GE(values.size(), 2);
const double mean =
std::accumulate(values.begin(), values.end(), 0.) / values.size();
double sum_of_squared_differences = 0.;
for (const double value : values) {
sum_of_squared_differences += common::Pow2(value - mean);
}
const double standard_deviation =
std::sqrt(sum_of_squared_differences / (values.size() - 1));
std::ostringstream out;
out << std::fixed << std::setprecision(5) << mean << " +/- "
<< standard_deviation;
return string(out.str());
}
string StatisticsString(const std::vector<Error>& errors) {
std::vector<double> translational_errors;
std::vector<double> squared_translational_errors;
std::vector<double> rotational_errors_degrees;
std::vector<double> squared_rotational_errors_degrees;
for (const Error& error : errors) {
translational_errors.push_back(std::sqrt(error.translational_squared));
squared_translational_errors.push_back(error.translational_squared);
rotational_errors_degrees.push_back(
common::RadToDeg(std::sqrt(error.rotational_squared)));
squared_rotational_errors_degrees.push_back(
common::Pow2(rotational_errors_degrees.back()));
}
return "Abs translational error " +
MeanAndStdDevString(translational_errors) +
" m\n"
"Sqr translational error " +
MeanAndStdDevString(squared_translational_errors) +
" m^2\n"
"Abs rotational error " +
MeanAndStdDevString(rotational_errors_degrees) +
" deg\n"
"Sqr rotational error " +
MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n";
}
void Run(const string& trajectory_filename, const string& relations_filename) {
LOG(INFO) << "Reading trajectory from '" << trajectory_filename << "'...";
mapping::proto::Trajectory trajectory_proto;
{
std::ifstream trajectory_stream(trajectory_filename.c_str(),
std::ios::binary);
CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream));
}
const auto transform_interpolation_buffer =
transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto);
LOG(INFO) << "Reading relations from '" << relations_filename << "'...";
std::vector<Error> errors;
{
std::ifstream relations_stream(relations_filename.c_str());
double time1, time2, x, y, z, roll, pitch, yaw;
while (relations_stream >> time1 >> time2 >> x >> y >> z >> roll >> pitch >>
yaw) {
const auto pose1 = LookupPose(*transform_interpolation_buffer, time1);
const auto pose2 = LookupPose(*transform_interpolation_buffer, time2);
const transform::Rigid3d expected =
transform::Rigid3d(transform::Rigid3d::Vector(x, y, z),
transform::RollPitchYaw(roll, pitch, yaw));
errors.push_back(ComputeError(pose1, pose2, expected));
}
CHECK(relations_stream.eof());
}
LOG(INFO) << "Result:\n" << StatisticsString(errors);
}
} // namespace
} // namespace cartographer
} // namespace ground_truth
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
FLAGS_logtostderr = true;
google::SetUsageMessage(
"\n\n"
"This program computes the relation based metric described in:\n"
"R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n"
"C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n"
"algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387407, 2009.\n"
"\n"
"Note: Timestamps in the relations_file are interpreted relative to\n"
" the Unix epoch.");
google::ParseCommandLineFlags(&argc, &argv, true);
if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) {
google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics");
return EXIT_FAILURE;
}
::cartographer::ground_truth::Run(FLAGS_trajectory_filename,
FLAGS_relations_filename);
}

View File

@ -28,16 +28,6 @@ namespace transform {
namespace { namespace {
// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
// specification http://wiki.ros.org/urdf/XML/joint.
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
const double yaw) {
const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
return yaw_angle * pitch_angle * roll_angle;
}
Eigen::Vector3d TranslationFromDictionary( Eigen::Vector3d TranslationFromDictionary(
common::LuaParameterDictionary* dictionary) { common::LuaParameterDictionary* dictionary) {
const std::vector<double> translation = dictionary->GetArrayValuesAsDoubles(); const std::vector<double> translation = dictionary->GetArrayValuesAsDoubles();
@ -47,6 +37,14 @@ Eigen::Vector3d TranslationFromDictionary(
} // namespace } // namespace
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
const double yaw) {
const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
return yaw_angle * pitch_angle * roll_angle;
}
transform::Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary) { transform::Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary) {
const Eigen::Vector3d translation = const Eigen::Vector3d translation =
TranslationFromDictionary(dictionary->GetDictionary("translation").get()); TranslationFromDictionary(dictionary->GetDictionary("translation").get());

View File

@ -222,6 +222,10 @@ std::ostream& operator<<(std::ostream& os,
using Rigid3d = Rigid3<double>; using Rigid3d = Rigid3<double>;
using Rigid3f = Rigid3<float>; using Rigid3f = Rigid3<float>;
// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
// specification http://wiki.ros.org/urdf/XML/joint.
Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw);
// Returns an transform::Rigid3d given a 'dictionary' containing 'translation' // Returns an transform::Rigid3d given a 'dictionary' containing 'translation'
// (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw) // (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw)
// or a dictionary with (w, x, y, z) values as a quaternion. // or a dictionary with (w, x, y, z) values as a quaternion.