Add ground truth proto support for trajectory evaluation. (#349)
This adds support of GroundTruth protos in addition to text files. Fixes the ground truth generation code to skip outliers. Improves the ground truth generation to use expected relative poses instead of the solution. Writes constraints including the roll and pitch from the node for 2D SLAM, so that constraints correctly capture the expected transform between the 3D poses of scan and submap in 2D SLAM. Related to #239. Related to #310.master
Wolfgang Hess
GitHub
parent
58bb70f53a
commit
0fbdf6d0ec
|
|
@ -32,9 +32,12 @@ DEFINE_string(output_filename, "", "File to write the ground truth proto to.");
|
|||
DEFINE_double(min_covered_distance, 100.,
|
||||
"Minimum covered distance in meters before a loop closure is "
|
||||
"considered a candidate for autogenerated ground truth.");
|
||||
DEFINE_double(outlier_threshold, 0.15,
|
||||
DEFINE_double(outlier_threshold_meters, 0.15,
|
||||
"Distance in meters beyond which constraints are considered "
|
||||
"outliers.");
|
||||
DEFINE_double(outlier_threshold_radians, 0.02,
|
||||
"Distance in radians beyond which constraints are considered "
|
||||
"outliers.");
|
||||
|
||||
namespace cartographer {
|
||||
namespace ground_truth {
|
||||
|
|
@ -59,7 +62,7 @@ std::vector<double> ComputeCoveredDistance(
|
|||
// We pick the representative scan in the middle of the submap.
|
||||
//
|
||||
// TODO(whess): Should we consider all scans inserted into the submap and
|
||||
// excluded, 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(
|
||||
const mapping::proto::SparsePoseGraph& pose_graph) {
|
||||
std::vector<int> submap_to_scan_index;
|
||||
|
|
@ -85,7 +88,8 @@ std::vector<int> ComputeSubmapRepresentativeScan(
|
|||
|
||||
proto::GroundTruth GenerateGroundTruth(
|
||||
const mapping::proto::SparsePoseGraph& pose_graph,
|
||||
const double min_covered_distance, const double outlier_threshold) {
|
||||
const double min_covered_distance, const double outlier_threshold_meters,
|
||||
const double outlier_threshold_radians) {
|
||||
const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0);
|
||||
const std::vector<double> covered_distance =
|
||||
ComputeCoveredDistance(trajectory);
|
||||
|
|
@ -123,27 +127,34 @@ proto::GroundTruth GenerateGroundTruth(
|
|||
|
||||
// Compute the transform between the scans according to the solution and
|
||||
// the constraint.
|
||||
const auto solution_pose1 =
|
||||
const transform::Rigid3d solution_pose1 =
|
||||
transform::ToRigid3(trajectory.node(representative_scan).pose());
|
||||
const auto solution_pose2 =
|
||||
const transform::Rigid3d solution_pose2 =
|
||||
transform::ToRigid3(trajectory.node(matched_scan).pose());
|
||||
const auto solution = solution_pose1.inverse() * solution_pose2;
|
||||
const transform::Rigid3d solution =
|
||||
solution_pose1.inverse() * solution_pose2;
|
||||
|
||||
const auto scan_to_submap_constraint =
|
||||
transform::ToRigid3(constraint.relative_pose());
|
||||
const auto submap_solution = transform::ToRigid3(
|
||||
const transform::Rigid3d submap_solution = transform::ToRigid3(
|
||||
trajectory.submap(constraint.submap_id().submap_index()).pose());
|
||||
const auto error =
|
||||
submap_solution * scan_to_submap_constraint * solution_pose2.inverse();
|
||||
const transform::Rigid3d submap_solution_to_scan_solution =
|
||||
solution_pose1.inverse() * submap_solution;
|
||||
const transform::Rigid3d scan_to_submap_constraint =
|
||||
transform::ToRigid3(constraint.relative_pose());
|
||||
const transform::Rigid3d expected =
|
||||
submap_solution_to_scan_solution * scan_to_submap_constraint;
|
||||
|
||||
if (error.translation().norm() > outlier_threshold) {
|
||||
const transform::Rigid3d error = solution * expected.inverse();
|
||||
|
||||
if (error.translation().norm() > outlier_threshold_meters ||
|
||||
transform::GetAngle(error) > outlier_threshold_radians) {
|
||||
++num_outliers;
|
||||
continue;
|
||||
}
|
||||
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());
|
||||
*new_relation->mutable_expected() = transform::ToProto(solution);
|
||||
*new_relation->mutable_expected() = transform::ToProto(expected);
|
||||
}
|
||||
LOG(INFO) << "Generated " << ground_truth.relation_size()
|
||||
<< " relations and ignored " << num_outliers << " outliers.";
|
||||
|
|
@ -151,7 +162,9 @@ proto::GroundTruth GenerateGroundTruth(
|
|||
}
|
||||
|
||||
void Run(const string& pose_graph_filename, const string& output_filename,
|
||||
const double min_covered_distance, const double outlier_threshold) {
|
||||
const double min_covered_distance,
|
||||
const double outlier_threshold_meters,
|
||||
const double outlier_threshold_radians) {
|
||||
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
||||
mapping::proto::SparsePoseGraph pose_graph;
|
||||
{
|
||||
|
|
@ -162,7 +175,8 @@ void Run(const string& pose_graph_filename, const string& output_filename,
|
|||
}
|
||||
LOG(INFO) << "Autogenerating ground truth relations...";
|
||||
const proto::GroundTruth ground_truth =
|
||||
GenerateGroundTruth(pose_graph, min_covered_distance, outlier_threshold);
|
||||
GenerateGroundTruth(pose_graph, min_covered_distance,
|
||||
outlier_threshold_meters, outlier_threshold_radians);
|
||||
LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '"
|
||||
<< output_filename << "'.";
|
||||
{
|
||||
|
|
@ -202,5 +216,6 @@ int main(int argc, char** argv) {
|
|||
}
|
||||
::cartographer::ground_truth::Run(
|
||||
FLAGS_pose_graph_filename, FLAGS_output_filename,
|
||||
FLAGS_min_covered_distance, FLAGS_outlier_threshold);
|
||||
FLAGS_min_covered_distance, FLAGS_outlier_threshold_meters,
|
||||
FLAGS_outlier_threshold_radians);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,7 +25,9 @@
|
|||
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||
#include "cartographer/ground_truth/proto/relations.pb.h"
|
||||
#include "cartographer/ground_truth/relations_text_file.h"
|
||||
#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "cartographer/transform/transform_interpolation_buffer.h"
|
||||
|
|
@ -33,31 +35,28 @@
|
|||
#include "glog/logging.h"
|
||||
|
||||
DEFINE_string(
|
||||
trajectory_filename, "",
|
||||
"Proto containing the trajectory of which to assess the quality.");
|
||||
pose_graph_filename, "",
|
||||
"File with the pose graph proto from which to assess the quality.");
|
||||
DEFINE_string(relations_filename, "",
|
||||
"Relations file containing the ground truth.");
|
||||
DEFINE_bool(read_text_file_with_unix_timestamps, false,
|
||||
"Enable support for the relations text files as in the paper. "
|
||||
"Default is to read from a GroundTruth proto file.");
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
// TODO(whess): This gives different results for the translational error if
|
||||
// 'pose1' and 'pose2' are swapped and 'expected' is inverted. Consider a
|
||||
// different way to compute translational error. Maybe just look at the
|
||||
// absolute difference in translation norms of each relative transform as a
|
||||
// lower bound of the translational error.
|
||||
Error ComputeError(const transform::Rigid3d& pose1,
|
||||
const transform::Rigid3d& pose2,
|
||||
const transform::Rigid3d& expected) {
|
||||
|
|
@ -109,33 +108,40 @@ string StatisticsString(const std::vector<Error>& errors) {
|
|||
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;
|
||||
void Run(const string& pose_graph_filename, const string& relations_filename,
|
||||
const bool read_text_file_with_unix_timestamps) {
|
||||
LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'...";
|
||||
mapping::proto::SparsePoseGraph pose_graph;
|
||||
{
|
||||
std::ifstream trajectory_stream(trajectory_filename.c_str(),
|
||||
std::ios::binary);
|
||||
CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream));
|
||||
std::ifstream stream(pose_graph_filename.c_str());
|
||||
CHECK(pose_graph.ParseFromIstream(&stream));
|
||||
CHECK_EQ(pose_graph.trajectory_size(), 1)
|
||||
<< "Only pose graphs containing a single trajectory are supported.";
|
||||
}
|
||||
|
||||
const auto transform_interpolation_buffer =
|
||||
transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto);
|
||||
transform::TransformInterpolationBuffer::FromTrajectory(
|
||||
pose_graph.trajectory(0));
|
||||
|
||||
proto::GroundTruth ground_truth;
|
||||
if (read_text_file_with_unix_timestamps) {
|
||||
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));
|
||||
ground_truth = ReadRelationsTextFile(relations_filename);
|
||||
} else {
|
||||
LOG(INFO) << "Reading ground truth from '" << relations_filename << "'...";
|
||||
std::ifstream ground_truth_stream(relations_filename.c_str(),
|
||||
std::ios::binary);
|
||||
CHECK(ground_truth.ParseFromIstream(&ground_truth_stream));
|
||||
}
|
||||
CHECK(relations_stream.eof());
|
||||
|
||||
std::vector<Error> errors;
|
||||
for (const auto& relation : ground_truth.relation()) {
|
||||
const auto pose1 = transform_interpolation_buffer->Lookup(
|
||||
common::FromUniversal(relation.timestamp1()));
|
||||
const auto pose2 = transform_interpolation_buffer->Lookup(
|
||||
common::FromUniversal(relation.timestamp2()));
|
||||
const transform::Rigid3d expected =
|
||||
transform::ToRigid3(relation.expected());
|
||||
errors.push_back(ComputeError(pose1, pose2, expected));
|
||||
}
|
||||
|
||||
LOG(INFO) << "Result:\n" << StatisticsString(errors);
|
||||
|
|
@ -153,16 +159,14 @@ int main(int argc, char** argv) {
|
|||
"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. 387–407, 2009.\n"
|
||||
"\n"
|
||||
"Note: Timestamps in the relations_file are interpreted relative to\n"
|
||||
" the Unix epoch.");
|
||||
"algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.");
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) {
|
||||
if (FLAGS_pose_graph_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);
|
||||
::cartographer::ground_truth::Run(FLAGS_pose_graph_filename,
|
||||
FLAGS_relations_filename,
|
||||
FLAGS_read_text_file_with_unix_timestamps);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,61 @@
|
|||
/*
|
||||
* 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 "cartographer/ground_truth/relations_text_file.h"
|
||||
|
||||
#include <fstream>
|
||||
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace ground_truth {
|
||||
|
||||
namespace {
|
||||
|
||||
common::Time UnixToCommonTime(double unix_time) {
|
||||
constexpr int64 kUtsTicksPerSecond = 10000000;
|
||||
return common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds *
|
||||
kUtsTicksPerSecond) +
|
||||
common::FromSeconds(unix_time);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
proto::GroundTruth ReadRelationsTextFile(const string& relations_filename) {
|
||||
proto::GroundTruth ground_truth;
|
||||
std::ifstream relations_stream(relations_filename.c_str());
|
||||
double unix_time_1, unix_time_2, x, y, z, roll, pitch, yaw;
|
||||
while (relations_stream >> unix_time_1 >> unix_time_2 >> x >> y >> z >>
|
||||
roll >> pitch >> yaw) {
|
||||
const common::Time common_time_1 = UnixToCommonTime(unix_time_1);
|
||||
const common::Time common_time_2 = UnixToCommonTime(unix_time_2);
|
||||
const transform::Rigid3d expected =
|
||||
transform::Rigid3d(transform::Rigid3d::Vector(x, y, z),
|
||||
transform::RollPitchYaw(roll, pitch, yaw));
|
||||
auto* const new_relation = ground_truth.add_relation();
|
||||
new_relation->set_timestamp1(common::ToUniversal(common_time_1));
|
||||
new_relation->set_timestamp2(common::ToUniversal(common_time_2));
|
||||
*new_relation->mutable_expected() = transform::ToProto(expected);
|
||||
}
|
||||
CHECK(relations_stream.eof());
|
||||
return ground_truth;
|
||||
}
|
||||
|
||||
} // namespace ground_truth
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
|
||||
#define CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/ground_truth/proto/relations.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace ground_truth {
|
||||
|
||||
// Reads a text file and converts it to a GroundTruth proto. Each line contains:
|
||||
// time1 time2 x y z roll pitch yaw
|
||||
// using Unix epoch timestamps.
|
||||
//
|
||||
// This is the format used in the relations files provided for:
|
||||
// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
|
||||
// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
|
||||
// Robots, vol. 27, no. 4, pp. 387–407, 2009.
|
||||
proto::GroundTruth ReadRelationsTextFile(const string& relations_filename);
|
||||
|
||||
} // namespace ground_truth
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
|
||||
|
|
@ -98,8 +98,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
|||
|
||||
for (const auto& constraint : constraints()) {
|
||||
auto* const constraint_proto = proto.add_constraint();
|
||||
*constraint_proto->mutable_relative_pose() =
|
||||
transform::ToProto(constraint.pose.zbar_ij);
|
||||
const auto& node = all_trajectory_nodes.at(constraint.node_id.trajectory_id)
|
||||
.at(constraint.node_id.node_index);
|
||||
*constraint_proto->mutable_relative_pose() = transform::ToProto(
|
||||
constraint.pose.zbar_ij * node.constant_data->tracking_to_pose);
|
||||
constraint_proto->set_translation_weight(
|
||||
constraint.pose.translation_weight);
|
||||
constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
|
||||
|
|
|
|||
Loading…
Reference in New Issue