feat(slam): add cartographer_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,153 @@
|
||||
/*
|
||||
* Copyright 2018 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/autogenerate_ground_truth.h"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace ground_truth {
|
||||
namespace {
|
||||
|
||||
std::vector<double> ComputeCoveredDistance(
|
||||
const mapping::proto::Trajectory& trajectory) {
|
||||
std::vector<double> 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 node in the middle of the submap.
|
||||
//
|
||||
// TODO(whess): Should we consider all nodes inserted into the submap and
|
||||
// exclude, e.g. based on large relative linear or angular distance?
|
||||
std::vector<int> ComputeSubmapRepresentativeNode(
|
||||
const mapping::proto::PoseGraph& pose_graph) {
|
||||
std::vector<int> submap_to_node_index;
|
||||
for (const auto& constraint : pose_graph.constraint()) {
|
||||
if (constraint.tag() !=
|
||||
mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
|
||||
continue;
|
||||
}
|
||||
CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
|
||||
CHECK_EQ(constraint.node_id().trajectory_id(), 0);
|
||||
|
||||
const int next_submap_index = static_cast<int>(submap_to_node_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_node_index.push_back(constraint.node_id().node_index());
|
||||
}
|
||||
return submap_to_node_index;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
proto::GroundTruth GenerateGroundTruth(
|
||||
const mapping::proto::PoseGraph& pose_graph,
|
||||
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);
|
||||
|
||||
const std::vector<int> submap_to_node_index =
|
||||
ComputeSubmapRepresentativeNode(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::PoseGraph::Constraint::INTRA_SUBMAP) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// For some submaps at the very end, we have not chosen a representative
|
||||
// node, but those should not be part of loop closure anyway.
|
||||
CHECK_EQ(constraint.submap_id().trajectory_id(), 0);
|
||||
CHECK_EQ(constraint.node_id().trajectory_id(), 0);
|
||||
if (constraint.submap_id().submap_index() >=
|
||||
static_cast<int>(submap_to_node_index.size())) {
|
||||
continue;
|
||||
}
|
||||
const int matched_node = constraint.node_id().node_index();
|
||||
const int representative_node =
|
||||
submap_to_node_index.at(constraint.submap_id().submap_index());
|
||||
|
||||
// Covered distance between the two should not be too small.
|
||||
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;
|
||||
}
|
||||
|
||||
// Compute the transform between the nodes according to the solution and
|
||||
// the constraint.
|
||||
const transform::Rigid3d solution_pose1 =
|
||||
transform::ToRigid3(trajectory.node(representative_node).pose());
|
||||
const transform::Rigid3d solution_pose2 =
|
||||
transform::ToRigid3(trajectory.node(matched_node).pose());
|
||||
const transform::Rigid3d solution =
|
||||
solution_pose1.inverse() * solution_pose2;
|
||||
|
||||
const transform::Rigid3d submap_solution = transform::ToRigid3(
|
||||
trajectory.submap(constraint.submap_id().submap_index()).pose());
|
||||
const transform::Rigid3d submap_solution_to_node_solution =
|
||||
solution_pose1.inverse() * submap_solution;
|
||||
const transform::Rigid3d node_to_submap_constraint =
|
||||
transform::ToRigid3(constraint.relative_pose());
|
||||
const transform::Rigid3d expected =
|
||||
submap_solution_to_node_solution * node_to_submap_constraint;
|
||||
|
||||
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_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.";
|
||||
return ground_truth;
|
||||
}
|
||||
|
||||
} // namespace ground_truth
|
||||
} // namespace cartographer
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* Copyright 2018 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_AUTOGENERATE_GROUND_TRUTH_H_
|
||||
#define CARTOGRAPHER_GROUND_TRUTH_AUTOGENERATE_GROUND_TRUTH_H_
|
||||
|
||||
#include "cartographer/ground_truth/proto/relations.pb.h"
|
||||
#include "cartographer/mapping/proto/pose_graph.pb.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace ground_truth {
|
||||
|
||||
// Generates GroundTruth proto from the given pose graph using the specified
|
||||
// criteria parameters. See
|
||||
// 'https://google-cartographer.readthedocs.io/en/latest/evaluation.html' for
|
||||
// more details.
|
||||
proto::GroundTruth GenerateGroundTruth(
|
||||
const mapping::proto::PoseGraph& pose_graph, double min_covered_distance,
|
||||
double outlier_threshold_meters, double outlier_threshold_radians);
|
||||
|
||||
} // namespace ground_truth
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_GROUND_TRUTH_AUTOGENERATE_GROUND_TRUTH_H
|
||||
@@ -0,0 +1,102 @@
|
||||
/*
|
||||
* 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 <cmath>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/ground_truth/autogenerate_ground_truth.h"
|
||||
#include "cartographer/ground_truth/proto/relations.pb.h"
|
||||
#include "cartographer/io/proto_stream.h"
|
||||
#include "cartographer/io/proto_stream_deserializer.h"
|
||||
#include "cartographer/mapping/proto/pose_graph.pb.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "gflags/gflags.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
DEFINE_string(pose_graph_filename, "",
|
||||
"Proto stream file containing the pose graph used 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_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 {
|
||||
namespace {
|
||||
|
||||
void Run(const std::string& pose_graph_filename,
|
||||
const std::string& output_filename, 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::PoseGraph pose_graph =
|
||||
io::DeserializePoseGraphFromFile(pose_graph_filename);
|
||||
|
||||
LOG(INFO) << "Autogenerating ground truth relations...";
|
||||
const proto::GroundTruth ground_truth =
|
||||
GenerateGroundTruth(pose_graph, min_covered_distance,
|
||||
outlier_threshold_meters, outlier_threshold_radians);
|
||||
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_meters,
|
||||
FLAGS_outlier_threshold_radians);
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* 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/ground_truth/proto/relations.pb.h"
|
||||
#include "cartographer/ground_truth/relations_text_file.h"
|
||||
#include "cartographer/io/proto_stream.h"
|
||||
#include "cartographer/io/proto_stream_deserializer.h"
|
||||
#include "cartographer/mapping/proto/pose_graph.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(
|
||||
pose_graph_filename, "",
|
||||
"Proto stream file containing the pose graph used to assess 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.");
|
||||
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 {
|
||||
namespace {
|
||||
|
||||
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) {
|
||||
const transform::Rigid3d error =
|
||||
(pose1.inverse() * pose2) * expected.inverse();
|
||||
return Error{error.translation().squaredNorm(),
|
||||
common::Pow2(transform::GetAngle(error))};
|
||||
}
|
||||
|
||||
std::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 std::string(out.str());
|
||||
}
|
||||
|
||||
std::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 WriteRelationMetricsToFile(const std::vector<Error>& 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 (int 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,
|
||||
const common::Time time) {
|
||||
const common::Time earliest_time =
|
||||
transform_interpolation_buffer.earliest_time();
|
||||
if (transform_interpolation_buffer.Has(time)) {
|
||||
return transform_interpolation_buffer.Lookup(time);
|
||||
} else if (time < earliest_time) {
|
||||
return transform_interpolation_buffer.Lookup(earliest_time);
|
||||
}
|
||||
return transform_interpolation_buffer.Lookup(
|
||||
transform_interpolation_buffer.latest_time());
|
||||
}
|
||||
|
||||
void Run(const std::string& pose_graph_filename,
|
||||
const std::string& relations_filename,
|
||||
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 =
|
||||
io::DeserializePoseGraphFromFile(pose_graph_filename);
|
||||
|
||||
const transform::TransformInterpolationBuffer transform_interpolation_buffer(
|
||||
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));
|
||||
}
|
||||
|
||||
std::vector<Error> errors;
|
||||
for (const auto& relation : ground_truth.relation()) {
|
||||
const auto pose1 =
|
||||
LookupTransform(transform_interpolation_buffer,
|
||||
common::FromUniversal(relation.timestamp1()));
|
||||
const auto pose2 =
|
||||
LookupTransform(transform_interpolation_buffer,
|
||||
common::FromUniversal(relation.timestamp2()));
|
||||
const transform::Rigid3d expected =
|
||||
transform::ToRigid3(relation.expected());
|
||||
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);
|
||||
}
|
||||
|
||||
} // 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 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.");
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
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_pose_graph_filename, FLAGS_relations_filename,
|
||||
FLAGS_read_text_file_with_unix_timestamps, FLAGS_write_relation_metrics);
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
// 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 = "proto3";
|
||||
|
||||
package cartographer.ground_truth.proto;
|
||||
|
||||
import "cartographer/transform/proto/transform.proto";
|
||||
|
||||
message Relation {
|
||||
int64 timestamp1 = 1;
|
||||
int64 timestamp2 = 2;
|
||||
|
||||
// The 'expected' relative transform of the tracking frame from 'timestamp2'
|
||||
// to 'timestamp1'.
|
||||
transform.proto.Rigid3d expected = 3;
|
||||
double covered_distance = 4;
|
||||
}
|
||||
|
||||
message GroundTruth {
|
||||
repeated Relation relation = 1;
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* 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 std::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 std::string& relations_filename);
|
||||
|
||||
} // namespace ground_truth
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_GROUND_TRUTH_RELATIONS_TEXT_FILE_H_
|
||||
Reference in New Issue
Block a user