diff --git a/CMakeLists.txt b/CMakeLists.txt index 0adc2af..2b42f83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,11 @@ configure_file( ${PROJECT_SOURCE_DIR}/cartographer/common/config.h.cmake ${PROJECT_BINARY_DIR}/cartographer/common/config.h) +google_binary(cartographer_autogenerate_ground_truth + SRCS + cartographer/ground_truth/autogenerate_ground_truth_main.cc +) + google_binary(cartographer_compute_relations_metrics SRCS cartographer/ground_truth/compute_relations_metrics_main.cc diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc new file mode 100644 index 0000000..ea63a7a --- /dev/null +++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -0,0 +1,206 @@ +/* + * 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 +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/ground_truth/proto/relations.pb.h" +#include "cartographer/mapping/proto/sparse_pose_graph.pb.h" +#include "cartographer/transform/transform.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +DEFINE_string( + pose_graph_filename, "", + "File with the pose graph proto from which to generate ground truth data."); +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, + "Distance in meters beyond which constraints are considered " + "outliers."); + +namespace cartographer { +namespace ground_truth { +namespace { + +std::vector ComputeCoveredDistance( + const mapping::proto::Trajectory& trajectory) { + std::vector covered_distance; + covered_distance.push_back(0.); + CHECK_GT(trajectory.node_size(), 0) + << "Trajectory does not contain any nodes."; + for (int i = 1; i < trajectory.node_size(); ++i) { + const auto last_pose = transform::ToRigid3(trajectory.node(i - 1).pose()); + const auto this_pose = transform::ToRigid3(trajectory.node(i).pose()); + covered_distance.push_back( + covered_distance.back() + + (last_pose.inverse() * this_pose).translation().norm()); + } + return covered_distance; +} + +// 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? +std::vector ComputeSubmapRepresentativeScan( + const mapping::proto::SparsePoseGraph& pose_graph) { + std::vector submap_to_scan_index; + for (const auto& constraint : pose_graph.constraint()) { + if (constraint.tag() != + mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + CHECK_EQ(constraint.submap_id().trajectory_id(), 0); + CHECK_EQ(constraint.scan_id().trajectory_id(), 0); + + const int next_submap_index = static_cast(submap_to_scan_index.size()); + const int submap_index = constraint.submap_id().submap_index(); + if (submap_index <= next_submap_index) { + continue; + } + + CHECK_EQ(submap_index, next_submap_index + 1); + submap_to_scan_index.push_back(constraint.scan_id().scan_index()); + } + return submap_to_scan_index; +} + +proto::GroundTruth GenerateGroundTruth( + const mapping::proto::SparsePoseGraph& pose_graph, + const double min_covered_distance, const double outlier_threshold) { + const mapping::proto::Trajectory& trajectory = pose_graph.trajectory(0); + const std::vector covered_distance = + ComputeCoveredDistance(trajectory); + + const std::vector submap_to_scan_index = + ComputeSubmapRepresentativeScan(pose_graph); + + int num_outliers = 0; + proto::GroundTruth ground_truth; + for (const auto& constraint : pose_graph.constraint()) { + // We're only interested in loop closure constraints. + if (constraint.tag() == + mapping::proto::SparsePoseGraph::Constraint::INTRA_SUBMAP) { + continue; + } + + // For some submaps at the very end, we have not chosen a representative + // scan, but those should not be part of loop closure anyway. + CHECK_EQ(constraint.submap_id().trajectory_id(), 0); + CHECK_EQ(constraint.scan_id().trajectory_id(), 0); + if (constraint.submap_id().submap_index() >= + static_cast(submap_to_scan_index.size())) { + continue; + } + const int matched_scan = constraint.scan_id().scan_index(); + const int representative_scan = + submap_to_scan_index.at(constraint.submap_id().submap_index()); + + // Covered distance between the two should not be too small. + if (std::abs(covered_distance.at(matched_scan) - + covered_distance.at(representative_scan)) < + min_covered_distance) { + continue; + } + + // Compute the transform between the scans according to the solution and + // the constraint. + const auto solution_pose1 = + transform::ToRigid3(trajectory.node(representative_scan).pose()); + const auto solution_pose2 = + transform::ToRigid3(trajectory.node(matched_scan).pose()); + const auto solution = solution_pose1.inverse() * solution_pose2; + + const auto scan_to_submap_constraint = + transform::ToRigid3(constraint.relative_pose()); + const auto submap_solution = transform::ToRigid3( + trajectory.submap(constraint.submap_id().submap_index()).pose()); + const auto error = + submap_solution * scan_to_submap_constraint * solution_pose2.inverse(); + + if (error.translation().norm() > outlier_threshold) { + ++num_outliers; + } + 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); + } + LOG(INFO) << "Generated " << ground_truth.relation_size() + << " relations and ignored " << num_outliers << " outliers."; + return ground_truth; +} + +void Run(const string& pose_graph_filename, const string& output_filename, + const double min_covered_distance, const double outlier_threshold) { + LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; + mapping::proto::SparsePoseGraph pose_graph; + { + 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."; + } + LOG(INFO) << "Autogenerating ground truth relations..."; + const proto::GroundTruth ground_truth = + GenerateGroundTruth(pose_graph, min_covered_distance, outlier_threshold); + LOG(INFO) << "Writing " << ground_truth.relation_size() << " relations to '" + << output_filename << "'."; + { + std::ofstream output_stream(output_filename, + std::ios_base::out | std::ios_base::binary); + CHECK(ground_truth.SerializeToOstream(&output_stream)) + << "Could not serialize ground truth data."; + output_stream.close(); + CHECK(output_stream) << "Could not write ground truth data."; + } +} + +} // namespace +} // namespace ground_truth +} // namespace cartographer + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + google::SetUsageMessage( + "\n\n" + "This program semi-automatically generates ground truth data from a\n" + "pose graph proto.\n" + "\n" + "The input should contain a single trajectory and should have been\n" + "manually assessed to be correctly loop closed. Small local distortions\n" + "are acceptable if they are tiny compared to the errors we want to\n" + "assess using the generated ground truth data.\n" + "\n" + "All loop closure constraints separated by long covered distance are\n" + "included in the output. Outliers are removed.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + + if (FLAGS_pose_graph_filename.empty() || FLAGS_output_filename.empty()) { + google::ShowUsageWithFlagsRestrict(argv[0], "autogenerate_ground_truth"); + return EXIT_FAILURE; + } + ::cartographer::ground_truth::Run( + FLAGS_pose_graph_filename, FLAGS_output_filename, + FLAGS_min_covered_distance, FLAGS_outlier_threshold); +} diff --git a/cartographer/ground_truth/proto/relations.proto b/cartographer/ground_truth/proto/relations.proto new file mode 100644 index 0000000..42c4e5a --- /dev/null +++ b/cartographer/ground_truth/proto/relations.proto @@ -0,0 +1,32 @@ +// 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. + +syntax = "proto2"; + +package cartographer.ground_truth.proto; + +import "cartographer/transform/proto/transform.proto"; + +message Relation { + optional int64 timestamp1 = 1; + optional int64 timestamp2 = 2; + + // The 'expected' relative transform of the tracking frame from 'timestamp2' + // to 'timestamp1'. + optional transform.proto.Rigid3d expected = 3; +} + +message GroundTruth { + repeated Relation relation = 1; +}