From ee0df671193bd59d3c088dc85225ebd60b3ca79a Mon Sep 17 00:00:00 2001 From: lschmid Date: Sun, 23 Jun 2024 20:49:51 -0400 Subject: [PATCH 1/2] add simple ostream formatters for all types --- pose_graph_tools/CMakeLists.txt | 6 +- .../include/pose_graph_tools/bow_queries.h | 2 + .../include/pose_graph_tools/bow_query.h | 2 + .../include/pose_graph_tools/bow_vector.h | 3 + .../include/pose_graph_tools/pose_graph.h | 2 + .../pose_graph_tools/pose_graph_edge.h | 2 + .../pose_graph_tools/pose_graph_node.h | 2 + pose_graph_tools/src/ostream.cpp | 69 +++++++++++++++++++ 8 files changed, 85 insertions(+), 3 deletions(-) create mode 100644 pose_graph_tools/src/ostream.cpp diff --git a/pose_graph_tools/CMakeLists.txt b/pose_graph_tools/CMakeLists.txt index 23343d6..0a4f225 100644 --- a/pose_graph_tools/CMakeLists.txt +++ b/pose_graph_tools/CMakeLists.txt @@ -10,12 +10,12 @@ include(CMakePackageConfigHelpers) find_package(Eigen3 REQUIRED) -add_library(${PROJECT_NAME} INTERFACE) +add_library(${PROJECT_NAME} src/ostream.cpp) target_include_directories( ${PROJECT_NAME} - INTERFACE $ + PUBLIC $ $) -target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen) +target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen) install( TARGETS ${PROJECT_NAME} diff --git a/pose_graph_tools/include/pose_graph_tools/bow_queries.h b/pose_graph_tools/include/pose_graph_tools/bow_queries.h index 5cfa141..6166162 100644 --- a/pose_graph_tools/include/pose_graph_tools/bow_queries.h +++ b/pose_graph_tools/include/pose_graph_tools/bow_queries.h @@ -13,6 +13,8 @@ struct BowQueries { uint32_t destination_robot_id; std::vector queries; + + friend std::ostream& operator<<(std::ostream& os, const BowQueries& queries); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/include/pose_graph_tools/bow_query.h b/pose_graph_tools/include/pose_graph_tools/bow_query.h index a4ddc03..b36c027 100644 --- a/pose_graph_tools/include/pose_graph_tools/bow_query.h +++ b/pose_graph_tools/include/pose_graph_tools/bow_query.h @@ -13,6 +13,8 @@ struct BowQuery { uint32_t robot_id; uint32_t pose_id; BowVector bow_vector; + + friend std::ostream& operator<<(std::ostream& os, const BowQuery& query); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/include/pose_graph_tools/bow_vector.h b/pose_graph_tools/include/pose_graph_tools/bow_vector.h index 48f2704..5ae566c 100644 --- a/pose_graph_tools/include/pose_graph_tools/bow_vector.h +++ b/pose_graph_tools/include/pose_graph_tools/bow_vector.h @@ -11,6 +11,9 @@ struct BowVector { std::vector word_ids; std::vector word_values; + + friend std::ostream& operator<<(std::ostream& os, + const BowVector& bow_vector); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/include/pose_graph_tools/pose_graph.h b/pose_graph_tools/include/pose_graph_tools/pose_graph.h index 43b26fe..6016678 100644 --- a/pose_graph_tools/include/pose_graph_tools/pose_graph.h +++ b/pose_graph_tools/include/pose_graph_tools/pose_graph.h @@ -15,6 +15,8 @@ struct PoseGraph { uint64_t stamp_ns; std::vector nodes; std::vector edges; + + friend std::ostream& operator<<(std::ostream& os, const PoseGraph& graph); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/include/pose_graph_tools/pose_graph_edge.h b/pose_graph_tools/include/pose_graph_tools/pose_graph_edge.h index 30e2f68..f89abff 100644 --- a/pose_graph_tools/include/pose_graph_tools/pose_graph_edge.h +++ b/pose_graph_tools/include/pose_graph_tools/pose_graph_edge.h @@ -32,6 +32,8 @@ struct PoseGraphEdge { Eigen::Affine3d pose; Eigen::Matrix covariance; + + friend std::ostream& operator<<(std::ostream& os, const PoseGraphEdge& edge); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/include/pose_graph_tools/pose_graph_node.h b/pose_graph_tools/include/pose_graph_tools/pose_graph_node.h index 3aa116f..2bc64ea 100644 --- a/pose_graph_tools/include/pose_graph_tools/pose_graph_node.h +++ b/pose_graph_tools/include/pose_graph_tools/pose_graph_node.h @@ -15,6 +15,8 @@ struct PoseGraphNode { int32_t robot_id; uint64_t key; Eigen::Affine3d pose; + + friend std::ostream& operator<<(std::ostream& os, const PoseGraphNode& node); }; } // namespace pose_graph_tools diff --git a/pose_graph_tools/src/ostream.cpp b/pose_graph_tools/src/ostream.cpp new file mode 100644 index 0000000..fadc4df --- /dev/null +++ b/pose_graph_tools/src/ostream.cpp @@ -0,0 +1,69 @@ +#include "pose_graph_tools/all.h" + +namespace pose_graph_tools { + +// TODO(lschmid): If important consider better pretty printing with linebreaks, +// naming the enums etc. + +std::ostream& operator<<(std::ostream& os, const BowVector& bow_vector) { + os << "BowVector{word_ids=["; + for (const auto& word_id : bow_vector.word_ids) { + os << word_id << ", "; + } + os << "], word_values=["; + for (const auto& word_value : bow_vector.word_values) { + os << word_value << ", "; + } + os << "]}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const BowQuery& query) { + os << "BowQuery{robot_id=" << query.robot_id << ", pose_id=" << query.pose_id + << ", bow_vector=" << query.bow_vector << "}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const BowQueries& queries) { + os << "BowQueries{destination_robot_id=" << queries.destination_robot_id + << ", queries=["; + for (const auto& query : queries.queries) { + os << query << ", "; + } + os << "]}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const PoseGraphNode& node) { + os << "PoseGraphNode{stamp_ns=" << node.stamp_ns + << ", robot_id=" << node.robot_id << ", key=" << node.key + << ", translation=[" << node.pose.translation().transpose() + << "], rotation=[" + << Eigen::Quaterniond(node.pose.rotation()).coeffs().transpose() << "]}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const PoseGraphEdge& edge) { + os << "PoseGraphEdge{type=" << edge.type << ", key_from=" << edge.key_from + << ", key_to=" << edge.key_to << ", robot_from=" << edge.robot_from + << ", robot_to=" << edge.robot_to << ", stamp_ns=" << edge.stamp_ns + << ", translation=[" << edge.pose.translation().transpose() + << "], rotation=[" + << Eigen::Quaterniond(edge.pose.rotation()).coeffs().transpose() << "]}"; + return os; +} + +std::ostream& operator<<(std::ostream& os, const PoseGraph& graph) { + os << "PoseGraph{stamp_ns=" << graph.stamp_ns << ", nodes=["; + for (const auto& node : graph.nodes) { + os << node << ", "; + } + os << "], edges=["; + for (const auto& edge : graph.edges) { + os << edge << ", "; + } + os << "]}"; + return os; +} + +} // namespace pose_graph_tools From 99ca7be42b33b37e3cf3fed754a73e83646901e9 Mon Sep 17 00:00:00 2001 From: lschmid Date: Sun, 23 Jun 2024 21:05:18 -0400 Subject: [PATCH 2/2] update CMakeLists --- pose_graph_tools/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/pose_graph_tools/CMakeLists.txt b/pose_graph_tools/CMakeLists.txt index 0a4f225..0e222a8 100644 --- a/pose_graph_tools/CMakeLists.txt +++ b/pose_graph_tools/CMakeLists.txt @@ -16,6 +16,8 @@ target_include_directories( PUBLIC $ $) target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen) +set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) +add_library(pose_graph_tools::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) install( TARGETS ${PROJECT_NAME}