diff --git a/cartographer/ground_truth/autogenerate_ground_truth_main.cc b/cartographer/ground_truth/autogenerate_ground_truth_main.cc index 620dba1..fd60b40 100644 --- a/cartographer/ground_truth/autogenerate_ground_truth_main.cc +++ b/cartographer/ground_truth/autogenerate_ground_truth_main.cc @@ -121,9 +121,10 @@ proto::GroundTruth GenerateGroundTruth( submap_to_node_index.at(constraint.submap_id().submap_index()); // Covered distance between the two should not be too small. - if (std::abs(covered_distance.at(matched_node) - - covered_distance.at(representative_node)) < - min_covered_distance) { + double covered_distance_in_constraint = + std::abs(covered_distance.at(matched_node) - + covered_distance.at(representative_node)); + if (covered_distance_in_constraint < min_covered_distance) { continue; } @@ -157,6 +158,7 @@ proto::GroundTruth GenerateGroundTruth( trajectory.node(representative_node).timestamp()); new_relation->set_timestamp2(trajectory.node(matched_node).timestamp()); *new_relation->mutable_expected() = transform::ToProto(expected); + new_relation->set_covered_distance(covered_distance_in_constraint); } LOG(INFO) << "Generated " << ground_truth.relation_size() << " relations and ignored " << num_outliers << " outliers."; diff --git a/cartographer/ground_truth/compute_relations_metrics_main.cc b/cartographer/ground_truth/compute_relations_metrics_main.cc index 3ad0f1d..fd520d1 100644 --- a/cartographer/ground_truth/compute_relations_metrics_main.cc +++ b/cartographer/ground_truth/compute_relations_metrics_main.cc @@ -43,6 +43,9 @@ DEFINE_string(relations_filename, "", 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."); +DEFINE_bool(write_relation_metrics, false, + "Enable exporting relation metrics as comma-separated values to " + "[pose_graph_filename].relation_metrics.csv"); namespace cartographer { namespace ground_truth { @@ -109,6 +112,46 @@ std::string StatisticsString(const std::vector& errors) { MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n"; } +void WriteRelationMetricsToFile(const std::vector& errors, + const proto::GroundTruth& ground_truth, + const std::string& relation_metrics_filename) { + std::ofstream relation_errors_file; + std::string log_file_path; + LOG(INFO) << "Writing relation metrics to '" + relation_metrics_filename + + "'..."; + relation_errors_file.open(relation_metrics_filename); + relation_errors_file + << "translational_error,squared_translational_error,rotational_" + "errors_degree,squared_rotational_errors_degree," + "expected_translation_x,expected_translation_y,expected_" + "translation_z,expected_rotation_w,expected_rotation_x," + "expected_rotation_y,expected_rotation_z,covered_distance\n"; + for (size_t relation_index = 0; relation_index < ground_truth.relation_size(); + ++relation_index) { + const Error& error = errors[relation_index]; + const proto::Relation& relation = ground_truth.relation(relation_index); + double translational_error = std::sqrt(error.translational_squared); + double squared_translational_error = error.translational_squared; + double rotational_errors_degree = + common::RadToDeg(std::sqrt(error.rotational_squared)); + double squared_rotational_errors_degree = + common::Pow2(rotational_errors_degree); + relation_errors_file << translational_error << "," + << squared_translational_error << "," + << rotational_errors_degree << "," + << squared_rotational_errors_degree << "," + << relation.expected().translation().x() << "," + << relation.expected().translation().y() << "," + << relation.expected().translation().z() << "," + << relation.expected().rotation().w() << "," + << relation.expected().rotation().x() << "," + << relation.expected().rotation().y() << "," + << relation.expected().rotation().z() << "," + << relation.covered_distance() << "\n"; + } + relation_errors_file.close(); +} + transform::Rigid3d LookupTransform( const transform::TransformInterpolationBuffer& transform_interpolation_buffer, @@ -126,7 +169,8 @@ transform::Rigid3d LookupTransform( void Run(const std::string& pose_graph_filename, const std::string& relations_filename, - const bool read_text_file_with_unix_timestamps) { + const bool read_text_file_with_unix_timestamps, + const bool write_relation_metrics) { LOG(INFO) << "Reading pose graph from '" << pose_graph_filename << "'..."; mapping::proto::PoseGraph pose_graph; { @@ -162,6 +206,12 @@ void Run(const std::string& pose_graph_filename, errors.push_back(ComputeError(pose1, pose2, expected)); } + const std::string relation_metrics_filename = + pose_graph_filename + ".relation_metrics.csv"; + if (write_relation_metrics) { + WriteRelationMetricsToFile(errors, ground_truth, relation_metrics_filename); + } + LOG(INFO) << "Result:\n" << StatisticsString(errors); } @@ -184,7 +234,8 @@ int main(int argc, char** argv) { google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics"); return EXIT_FAILURE; } - ::cartographer::ground_truth::Run(FLAGS_pose_graph_filename, - FLAGS_relations_filename, - FLAGS_read_text_file_with_unix_timestamps); + + ::cartographer::ground_truth::Run( + FLAGS_pose_graph_filename, FLAGS_relations_filename, + FLAGS_read_text_file_with_unix_timestamps, FLAGS_write_relation_metrics); } diff --git a/cartographer/ground_truth/proto/relations.proto b/cartographer/ground_truth/proto/relations.proto index e0a40b3..fec2f43 100644 --- a/cartographer/ground_truth/proto/relations.proto +++ b/cartographer/ground_truth/proto/relations.proto @@ -25,6 +25,7 @@ message Relation { // The 'expected' relative transform of the tracking frame from 'timestamp2' // to 'timestamp1'. transform.proto.Rigid3d expected = 3; + double covered_distance = 4; } message GroundTruth {