diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc index ea63a7a..3031d01 100644 --- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc +++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -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 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 ComputeSubmapRepresentativeScan( const mapping::proto::SparsePoseGraph& pose_graph) { std::vector submap_to_scan_index; @@ -85,7 +88,8 @@ std::vector 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 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); } diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc index 641dc0e..2bebfbb 100644 --- a/cartographer/ground_truth/compute_relations_metrics_main.cc +++ b/cartographer/ground_truth/compute_relations_metrics_main.cc @@ -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& 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( + pose_graph.trajectory(0)); + + proto::GroundTruth ground_truth; + if (read_text_file_with_unix_timestamps) { + LOG(INFO) << "Reading relations from '" << relations_filename << "'..."; + 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)); } - const auto transform_interpolation_buffer = - transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto); - - LOG(INFO) << "Reading relations from '" << relations_filename << "'..."; std::vector 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()); + 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); } diff --git a/cartographer/ground_truth/relations_text_file.cc b/cartographer/ground_truth/relations_text_file.cc new file mode 100644 index 0000000..60ccdf8 --- /dev/null +++ b/cartographer/ground_truth/relations_text_file.cc @@ -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 + +#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 diff --git a/cartographer/ground_truth/relations_text_file.h b/cartographer/ground_truth/relations_text_file.h new file mode 100644 index 0000000..1ffe89c --- /dev/null +++ b/cartographer/ground_truth/relations_text_file.h @@ -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 + +#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_ diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 85a07f9..d4863a5 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -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);