From f5c88ff32e94d3dbff9a5cb31e5c6ca45b877b52 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 4 Apr 2018 17:18:31 +0200 Subject: [PATCH] Tool for comparing pure localization to offline optimization (#803) Adds a tool to measure the difference between a trajectory from a pbstream and one given by tf messages in a bag file, and a script to evaluate real-time pure localization poses compared to a globally optimized mapping poses. --- .../cartographer_ros/CMakeLists.txt | 11 ++ .../dev/trajectory_comparison_main.cc | 140 ++++++++++++++++++ .../backpack_2d_localization_evaluation.lua | 27 ++++ ...pare_localization_to_offline_trajectory.sh | 74 +++++++++ 4 files changed, 252 insertions(+) create mode 100644 cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc create mode 100644 cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua create mode 100755 cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index bd567bd..27ea478 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -102,6 +102,17 @@ install(TARGETS cartographer_pbstream_map_publisher RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +google_binary(cartographer_dev_trajectory_comparison + SRCS + dev/trajectory_comparison_main.cc +) + +install(TARGETS cartographer_dev_trajectory_comparison + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # TODO(cschuet): Add support for shared library case. if (${BUILD_GRPC}) google_binary(cartographer_grpc_node diff --git a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc b/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc new file mode 100644 index 0000000..102c9e5 --- /dev/null +++ b/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc @@ -0,0 +1,140 @@ +/* + * 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 +#include +#include +#include +#include + +#include "cartographer/common/math.h" +#include "cartographer/io/proto_stream.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" +#include "cartographer/transform/transform_interpolation_buffer.h" +#include "cartographer_ros/msg_conversion.h" +#include "cartographer_ros/time_conversion.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ros/ros.h" +#include "ros/time.h" +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "tf2_eigen/tf2_eigen.h" +#include "tf2_msgs/TFMessage.h" + +DEFINE_string(bag_filename, "", + "Bags to process, must be in the same order as the trajectories " + "in 'pose_graph_filename'."); +DEFINE_string(pbstream_filename, "", + "Proto stream file containing the pose graph."); + +namespace cartographer_ros { +namespace { + +double FractionSmallerThan(const std::vector& v, double x) { + return static_cast(std::count_if( + v.begin(), v.end(), [=](double value) { return value < x; })) / + v.size(); +} + +std::string QuantilesToString(std::vector* v) { + if (v->empty()) return "(empty vector)"; + std::sort(v->begin(), v->end()); + std::stringstream result; + const int kNumQuantiles = 10; + for (int i = 0; i < kNumQuantiles; ++i) { + auto value = v->at(v->size() * i / kNumQuantiles); + auto percentage = 100 * i / kNumQuantiles; + result << percentage << "%: " << value << "\n"; + } + result << "100%: " << v->back() << "\n"; + return result.str(); +} + +void Run(const std::string& pbstream_filename, + const std::string& bag_filename) { + cartographer::io::ProtoStreamReader reader(pbstream_filename); + cartographer::mapping::proto::PoseGraph pose_graph_proto; + CHECK(reader.ReadProto(&pose_graph_proto)); + const cartographer::mapping::proto::Trajectory& last_trajectory_proto = + *pose_graph_proto.mutable_trajectory()->rbegin(); + const cartographer::transform::TransformInterpolationBuffer + transform_interpolation_buffer(last_trajectory_proto); + + rosbag::Bag bag; + bag.open(bag_filename, rosbag::bagmode::Read); + rosbag::View view(bag); + std::vector deviation_translation, deviation_rotation; + const double signal_maximum = std::numeric_limits::max(); + for (const rosbag::MessageInstance& message : view) { + if (!message.isType()) { + continue; + } + auto tf_message = message.instantiate(); + for (const auto& transform : tf_message->transforms) { + if (transform.header.frame_id != "map" || + transform.child_frame_id != "base_link") { + continue; + } + const cartographer::common::Time transform_time = + FromRos(message.getTime()); + if (!transform_interpolation_buffer.Has(transform_time)) { + deviation_translation.push_back(signal_maximum); + deviation_rotation.push_back(signal_maximum); + continue; + } + auto optimized_transform = + transform_interpolation_buffer.Lookup(transform_time); + auto published_transform = ToRigid3d(transform); + deviation_translation.push_back((published_transform.translation() - + optimized_transform.translation()) + .norm()); + deviation_rotation.push_back( + published_transform.rotation().angularDistance( + optimized_transform.rotation())); + } + } + bag.close(); + LOG(INFO) << "Distribution of translation difference:\n" + << QuantilesToString(&deviation_translation); + LOG(INFO) << "Distribution of rotation difference:\n" + << QuantilesToString(&deviation_rotation); + LOG(INFO) << "Fraction of translation difference smaller than 1m: " + << FractionSmallerThan(deviation_translation, 1); + LOG(INFO) << "Fraction of translation difference smaller than 0.1m: " + << FractionSmallerThan(deviation_translation, 0.1); + LOG(INFO) << "Fraction of translation difference smaller than 0.05m: " + << FractionSmallerThan(deviation_translation, 0.05); + LOG(INFO) << "Fraction of translation difference smaller than 0.01m: " + << FractionSmallerThan(deviation_translation, 0.01); +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + FLAGS_alsologtostderr = true; + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "This compares a trajectory from a bag file against the " + "last trajectory in a pbstream file.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; + CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing."; + ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_bag_filename); +} diff --git a/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua b/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua new file mode 100644 index 0000000..09afbe6 --- /dev/null +++ b/cartographer_ros/configuration_files/backpack_2d_localization_evaluation.lua @@ -0,0 +1,27 @@ +-- 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 "backpack_2d_localization.lua" + +-- output map to base_link for evaluation +options.provide_odom_frame = false +POSE_GRAPH.optimization_problem.log_solver_summary = true + +-- fast localization +MAP_BUILDER.num_background_threads = 12 +POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio +POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio +POSE_GRAPH.max_num_final_iterations = 1 + +return options diff --git a/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh b/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh new file mode 100755 index 0000000..53822c5 --- /dev/null +++ b/cartographer_ros/scripts/dev/compare_localization_to_offline_trajectory.sh @@ -0,0 +1,74 @@ +#!/bin/bash + +# 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. + +set -o errexit +set -o verbose + +MAPFILE="$1" +BAGFILE="$2" +if [ "$#" -ne 2 ]; then + echo "\n\nUsage: $0 \n" + exit 1; +fi + +LAUNCHSCRIPT=' + + + + +' +echo $LAUNCHSCRIPT | roslaunch - + +LAUNCHSCRIPT=' + + + + + + + + + + +' +echo $LAUNCHSCRIPT | roslaunch - + +rosrun cartographer_ros cartographer_dev_trajectory_comparison \ + -bag_filename $BAGFILE.tf-result.bag \ + -pbstream_filename $BAGFILE.pbstream