Merge pull request #16 from SPARK/feature/ostream

Feature/ostream
MIT-SPARK · Jun 24, 2024 · 4983120 · 4983120
2 parents ad959ea + 99ca7be
commit 4983120
Show file tree

Hide file tree

Showing 8 changed files with 87 additions and 3 deletions.
diff --git a/pose_graph_tools/CMakeLists.txt b/pose_graph_tools/CMakeLists.txt
@@ -10,12 +10,14 @@ include(CMakePackageConfigHelpers)
 
 find_package(Eigen3 REQUIRED)
 
-add_library(${PROJECT_NAME} INTERFACE)
+add_library(${PROJECT_NAME} src/ostream.cpp)
 target_include_directories(
   ${PROJECT_NAME}
-  INTERFACE $<INSTALL_INTERFACE:include>
+  PUBLIC $<INSTALL_INTERFACE:include>
             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
-target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen)
+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}

diff --git 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<BowQuery> 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
@@ -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
@@ -11,6 +11,9 @@ struct BowVector {
 
   std::vector<uint32_t> word_ids;
   std::vector<float> 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
@@ -15,6 +15,8 @@ struct PoseGraph {
   uint64_t stamp_ns;
   std::vector<PoseGraphNode> nodes;
   std::vector<PoseGraphEdge> 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
@@ -32,6 +32,8 @@ struct PoseGraphEdge {
 
   Eigen::Affine3d pose;
   Eigen::Matrix<double, 6, 6> 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
@@ -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
@@ -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