pose graph updates (#11)

MIT-SPARK · Apr 25, 2024 · 215e1f5 · 215e1f5
1 parent f238a0f
commit 215e1f5
Show file tree

Hide file tree

Showing 9 changed files with 59 additions and 59 deletions.
diff --git a/include/kimera_multi_lcd/io.h b/include/kimera_multi_lcd/io.h
@@ -6,8 +6,8 @@
 
 #pragma once
 
-#include <pose_graph_tools/BowVector.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
 
 #include <map>
 
@@ -16,29 +16,29 @@
 namespace kimera_multi_lcd {
 
 // Save BoW vectors
-void saveBowVectors(const std::map<PoseId, pose_graph_tools::BowVector>& bow_vectors,
+void saveBowVectors(const std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors,
                     const std::string& filename);
 
 void saveBowVectors(const std::map<PoseId, DBoW2::BowVector>& bow_vectors,
                     const std::string& filename);
 
 // Save VLC Frames
-void saveVLCFrames(const std::map<PoseId, pose_graph_tools::VLCFrameMsg>& vlc_frames,
+void saveVLCFrames(const std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames,
                    const std::string& filename);
 
 void saveVLCFrames(const std::map<PoseId, VLCFrame>& vlc_frames,
                    const std::string& filename);
 
 // Save BoW vectors
 void loadBowVectors(const std::string& filename,
-                    std::map<PoseId, pose_graph_tools::BowVector>& bow_vectors);
+                    std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors);
 
 void loadBowVectors(const std::string& filename,
                     std::map<PoseId, DBoW2::BowVector>& bow_vectors);
 
 // Save VLC Frames
 void loadVLCFrames(const std::string& filename,
-                   std::map<PoseId, pose_graph_tools::VLCFrameMsg>& vlc_frames);
+                   std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames);
 
 void loadVLCFrames(const std::string& filename, std::map<PoseId, VLCFrame>& vlc_frames);
 }  // namespace kimera_multi_lcd
diff --git a/include/kimera_multi_lcd/serializer.h b/include/kimera_multi_lcd/serializer.h
@@ -1,16 +1,16 @@
 #pragma once
-#include <pose_graph_tools/BowVector.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
 
 #include <nlohmann/json.hpp>
 
-namespace pose_graph_tools {
+namespace pose_graph_tools_msgs {
 
-void to_json(nlohmann::json& j, const pose_graph_tools::BowVector& bow_vector);
+void to_json(nlohmann::json& j, const pose_graph_tools_msgs::BowVector& bow_vector);
 
-void from_json(const nlohmann::json& j, pose_graph_tools::BowVector& bow_vector);
+void from_json(const nlohmann::json& j, pose_graph_tools_msgs::BowVector& bow_vector);
 
-void to_json(nlohmann::json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame);
+void to_json(nlohmann::json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame);
 
-void from_json(const nlohmann::json& j, pose_graph_tools::VLCFrameMsg& vlc_frame);
-}  // namespace pose_graph_tools
+void from_json(const nlohmann::json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame);
+}  // namespace pose_graph_tools_msgs
diff --git a/include/kimera_multi_lcd/types.h b/include/kimera_multi_lcd/types.h
@@ -11,7 +11,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <pcl/point_types.h>
 #include <pcl_ros/point_cloud.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
 #include <opencv/cv.hpp>
 #include <opencv2/features2d.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -71,7 +71,7 @@ class VLCFrame {
            const std::vector<gtsam::Vector3>& keypoints_3d,
            const std::vector<gtsam::Vector3>& versors,
            const OrbDescriptor& descriptors_mat);
-  VLCFrame(const pose_graph_tools::VLCFrameMsg& msg);
+  VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg);
   size_t robot_id_;
   size_t pose_id_;
   size_t submap_id_;  // ID of the submap that contains this frame (pose)
@@ -81,7 +81,7 @@ class VLCFrame {
   OrbDescriptor descriptors_mat_;
   gtsam::Pose3 T_submap_pose_;  // 3D pose in submap frame
   void initializeDescriptorsVector();
-  void toROSMessage(pose_graph_tools::VLCFrameMsg* msg) const;
+  void toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const;
   // void pruneInvalidKeypoints();
 };  // class VLCFrame
 

diff --git a/include/kimera_multi_lcd/utils.h b/include/kimera_multi_lcd/utils.h
@@ -7,33 +7,33 @@
 #pragma once
 
 #include <DBoW2/DBoW2.h>
-#include <pose_graph_tools/BowQuery.h>
-#include <pose_graph_tools/BowVector.h>
-#include <pose_graph_tools/PoseGraph.h>
-#include <pose_graph_tools/PoseGraphEdge.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
-#include <pose_graph_tools/VLCFrameQuery.h>
+#include <pose_graph_tools_msgs/BowQuery.h>
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/PoseGraph.h>
+#include <pose_graph_tools_msgs/PoseGraphEdge.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/VLCFrameQuery.h>
 
 #include <map>
 
 #include "kimera_multi_lcd/types.h"
 
 namespace kimera_multi_lcd {
-void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector* msg);
+void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg);
 
-void BowVectorFromMsg(const pose_graph_tools::BowVector& msg,
+void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg,
                       DBoW2::BowVector* bow_vec);
 
-void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools::VLCFrameMsg* msg);
-void VLCFrameFromMsg(const pose_graph_tools::VLCFrameMsg& msg, VLCFrame* frame);
+void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg);
+void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame);
 
-void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg);
-void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge);
+void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg);
+void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge);
 
 // Compute the payload size in a BowQuery message
-size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg);
+size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg);
 
 // Compute the payload size of a VLC frame
-size_t computeVLCFramePayloadBytes(const pose_graph_tools::VLCFrameMsg& msg);
+size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg);
 
 }  // namespace kimera_multi_lcd
diff --git a/package.xml b/package.xml
@@ -23,7 +23,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>image_transport</depend>
-  <depend>pose_graph_tools</depend>
+  <depend>pose_graph_tools_msgs</depend>
   <depend>pcl_ros</depend>
   <depend>rosbag</depend>
   <depend>cv_bridge</depend>

diff --git a/src/io.cpp b/src/io.cpp
@@ -15,7 +15,7 @@ namespace kimera_multi_lcd {
 
 using nlohmann::json;
 
-void saveBowVectors(const std::map<PoseId, pose_graph_tools::BowVector>& bow_vectors,
+void saveBowVectors(const std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors,
                     const std::string& filename) {
   std::ofstream outfile(filename);
   json record;
@@ -30,15 +30,15 @@ void saveBowVectors(const std::map<PoseId, pose_graph_tools::BowVector>& bow_vec
 
 void saveBowVectors(const std::map<PoseId, DBoW2::BowVector>& bow_vectors,
                     const std::string& filename) {
-  std::map<PoseId, pose_graph_tools::BowVector> pg_bow_vectors;
+  std::map<PoseId, pose_graph_tools_msgs::BowVector> pg_bow_vectors;
   for (const auto& id_bow : bow_vectors) {
-    pg_bow_vectors[id_bow.first] = pose_graph_tools::BowVector();
+    pg_bow_vectors[id_bow.first] = pose_graph_tools_msgs::BowVector();
     BowVectorToMsg(id_bow.second, &pg_bow_vectors[id_bow.first]);
   }
   saveBowVectors(pg_bow_vectors, filename);
 }
 
-void saveVLCFrames(const std::map<PoseId, pose_graph_tools::VLCFrameMsg>& vlc_frames,
+void saveVLCFrames(const std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames,
                    const std::string& filename) {
   std::ofstream outfile(filename);
   json record;
@@ -53,17 +53,17 @@ void saveVLCFrames(const std::map<PoseId, pose_graph_tools::VLCFrameMsg>& vlc_fr
 
 void saveVLCFrames(const std::map<PoseId, VLCFrame>& vlc_frames,
                    const std::string& filename) {
-  std::map<PoseId, pose_graph_tools::VLCFrameMsg> pg_vlc_frames;
+  std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg> pg_vlc_frames;
   for (const auto& id_frame : vlc_frames) {
-    pg_vlc_frames[id_frame.first] = pose_graph_tools::VLCFrameMsg();
+    pg_vlc_frames[id_frame.first] = pose_graph_tools_msgs::VLCFrameMsg();
     VLCFrameToMsg(id_frame.second, &pg_vlc_frames[id_frame.first]);
   }
   saveVLCFrames(pg_vlc_frames, filename);
 }
 
 // Save BoW vectors
 void loadBowVectors(const std::string& filename,
-                    std::map<PoseId, pose_graph_tools::BowVector>& bow_vectors) {
+                    std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors) {
   std::stringstream ss;
   std::ifstream infile(filename);
   if (!infile) {
@@ -80,7 +80,7 @@ void loadBowVectors(const std::string& filename,
 
 void loadBowVectors(const std::string& filename,
                     std::map<PoseId, DBoW2::BowVector>& bow_vectors) {
-  std::map<PoseId, pose_graph_tools::BowVector> pg_bow_vectors;
+  std::map<PoseId, pose_graph_tools_msgs::BowVector> pg_bow_vectors;
   loadBowVectors(filename, pg_bow_vectors);
 
   for (const auto& id_bow : pg_bow_vectors) {
@@ -91,7 +91,7 @@ void loadBowVectors(const std::string& filename,
 
 // Save VLC Frames
 void loadVLCFrames(const std::string& filename,
-                   std::map<PoseId, pose_graph_tools::VLCFrameMsg>& vlc_frames) {
+                   std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames) {
   std::stringstream ss;
   std::ifstream infile(filename);
   if (!infile) {
@@ -108,7 +108,7 @@ void loadVLCFrames(const std::string& filename,
 
 void loadVLCFrames(const std::string& filename,
                    std::map<PoseId, VLCFrame>& vlc_frames) {
-  std::map<PoseId, pose_graph_tools::VLCFrameMsg> pg_vlc_frames;
+  std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg> pg_vlc_frames;
   loadVLCFrames(filename, pg_vlc_frames);
   for (const auto& id_frame : pg_vlc_frames) {
     vlc_frames[id_frame.first] = VLCFrame();

diff --git a/src/serializer.cpp b/src/serializer.cpp
@@ -37,18 +37,18 @@ void from_json(const json& j, pcl::PointCloud<pcl::PointXYZ>& points) {
 
 }  // namespace pcl
 
-namespace pose_graph_tools {
+namespace pose_graph_tools_msgs {
 
-void to_json(json& j, const pose_graph_tools::BowVector& bow_vector) {
+void to_json(json& j, const pose_graph_tools_msgs::BowVector& bow_vector) {
   j = json{{"word_ids", bow_vector.word_ids}, {"word_values", bow_vector.word_values}};
 }
 
-void from_json(const json& j, pose_graph_tools::BowVector& bow_vector) {
+void from_json(const json& j, pose_graph_tools_msgs::BowVector& bow_vector) {
   j.at("word_ids").get_to(bow_vector.word_ids);
   j.at("word_values").get_to(bow_vector.word_values);
 }
 
-void to_json(json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame) {
+void to_json(json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) {
   pcl::PointCloud<pcl::PointXYZ> versors;
   pcl::fromROSMsg(vlc_frame.versors, versors);
 
@@ -73,7 +73,7 @@ void to_json(json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame) {
              {"qw", vlc_frame.T_submap_pose.orientation.w}}}};
 }
 
-void from_json(const json& j, pose_graph_tools::VLCFrameMsg& vlc_frame) {
+void from_json(const json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) {
   j.at("robot_id").get_to(vlc_frame.robot_id);
   j.at("pose_id").get_to(vlc_frame.pose_id);
   j.at("submap_id").get_to(vlc_frame.submap_id);
@@ -99,4 +99,4 @@ void from_json(const json& j, pose_graph_tools::VLCFrameMsg& vlc_frame) {
   j.at("T_submap_pose").at("qz").get_to(T_submap_R.z);
   j.at("T_submap_pose").at("qw").get_to(T_submap_R.w);
 }
-}  // namespace pose_graph_tools
+}  // namespace pose_graph_tools_msgs
diff --git a/src/types.cpp b/src/types.cpp
@@ -26,7 +26,7 @@ VLCFrame::VLCFrame(const RobotId& robot_id,
   initializeDescriptorsVector();
 }
 
-VLCFrame::VLCFrame(const pose_graph_tools::VLCFrameMsg& msg)
+VLCFrame::VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg)
     : robot_id_(msg.robot_id), pose_id_(msg.pose_id), submap_id_(msg.submap_id) {
   T_submap_pose_ = gtsam::Pose3(gtsam::Rot3(msg.T_submap_pose.orientation.w,
                                             msg.T_submap_pose.orientation.x,
@@ -72,7 +72,7 @@ VLCFrame::VLCFrame(const pose_graph_tools::VLCFrameMsg& msg)
   }
 }
 
-void VLCFrame::toROSMessage(pose_graph_tools::VLCFrameMsg* msg) const {
+void VLCFrame::toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const {
   msg->robot_id = robot_id_;
   msg->pose_id = pose_id_;
 

diff --git a/src/utils.cpp b/src/utils.cpp
@@ -10,7 +10,7 @@
 
 namespace kimera_multi_lcd {
 
-void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector* msg) {
+void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg) {
   msg->word_ids.clear();
   msg->word_values.clear();
   for (auto it = bow_vec.begin(); it != bow_vec.end(); ++it) {
@@ -19,7 +19,7 @@ void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector
   }
 }
 
-void BowVectorFromMsg(const pose_graph_tools::BowVector& msg,
+void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg,
                       DBoW2::BowVector* bow_vec) {
   assert(msg.word_ids.size() == msg.word_values.size());
   bow_vec->clear();
@@ -28,22 +28,22 @@ void BowVectorFromMsg(const pose_graph_tools::BowVector& msg,
   }
 }
 
-void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools::VLCFrameMsg* msg) {
+void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg) {
   frame.toROSMessage(msg);
 }
 
-void VLCFrameFromMsg(const pose_graph_tools::VLCFrameMsg& msg, VLCFrame* frame) {
+void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame) {
   *frame = VLCFrame(msg);
 }
 
-void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg) {
+void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg) {
   // Yulun: this function currently does not assign covariance!
 
   msg->robot_from = edge.vertex_src_.first;
   msg->key_from = edge.vertex_src_.second;
   msg->robot_to = edge.vertex_dst_.first;
   msg->key_to = edge.vertex_dst_.second;
-  msg->type = pose_graph_tools::PoseGraphEdge::LOOPCLOSE;
+  msg->type = pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE;
 
   gtsam::Pose3 pose = edge.T_src_dst_;
   gtsam::Quaternion quat = pose.rotation().toQuaternion();
@@ -59,7 +59,7 @@ void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg) {
   msg->pose.position.z = position.z();
 }
 
-void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge) {
+void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge) {
   edge->vertex_src_ = std::make_pair(msg.robot_from, msg.key_from);
   edge->vertex_dst_ = std::make_pair(msg.robot_to, msg.key_to);
 
@@ -74,7 +74,7 @@ void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge) {
   edge->T_src_dst_ = T_src_dst;
 }
 
-size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg) {
+size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg) {
   size_t bytes = 0;
   bytes += sizeof(msg.robot_id);
   bytes += sizeof(msg.pose_id);
@@ -83,7 +83,7 @@ size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg) {
   return bytes;
 }
 
-size_t computeVLCFramePayloadBytes(const pose_graph_tools::VLCFrameMsg& msg) {
+size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg) {
   size_t bytes = 0;
   bytes += sizeof(msg.robot_id);
   bytes += sizeof(msg.pose_id);