Extract ground truth generation into a library. (#1215)
Extract ground truth generation from `autogenerate_ground_truth_main.cc` into a library function in `ground_truth.h/cc`, so it can be reused outside that binary. Tested with Cartographer ROS. `cartographer_autogenerate_ground_truth -pose_graph_filename=/home/afleck/Downloads/b3-2016-04-05-15-51-36.bag.pbstream -output_filename=/home/afleck/Downloads/ground_truth.pb` still runs as before.master
Arno Fleck
Wally B. Feed
parent
b4594bcdbd
commit
eed07d2f80
|
|
@ -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
|
||||
|
|
@ -19,6 +19,7 @@
|
|||
#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"
|
||||
|
|
@ -27,10 +28,9 @@
|
|||
#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(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 "
|
||||
|
|
@ -46,126 +46,6 @@ 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void Run(const std::string& pose_graph_filename,
|
||||
const std::string& output_filename, const double min_covered_distance,
|
||||
const double outlier_threshold_meters,
|
||||
|
|
|
|||
Loading…
Reference in New Issue