37 Refactor PoseGraphNode for cleaner observer-observable relationship

pose_graph/include/pose_graph/details/pose_graph_event_observer.hpp

@@ -17,24 +17,24 @@
 #include "pose_graph/details/keyframe.h"
 
 namespace pose_graph {
-class PoseGraphEventObserver
-    : public std::enable_shared_from_this<PoseGraphEventObserver> {
+class PoseGraphEventObserver {
  public:
   PoseGraphEventObserver() = default;
   virtual ~PoseGraphEventObserver() = default;
 
   virtual void OnPoseGraphLoaded() = 0;
   virtual void OnPoseGraphSaved() = 0;
-  virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) = 0;
+  virtual void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute,
+                               int sequence_count) = 0;
   virtual void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute,
-                                int count) = 0;
+                                int count, int sequence_count) = 0;
   virtual void OnKeyFrameConnectionFound(
       KeyFrame::Attributes current_kf_attribute,
       KeyFrame::Attributes old_kf_attribute,
       std::vector<cv::Point2f> matched_2d_old_norm,
       std::vector<double> matched_id, cv::Mat& thumb_image) = 0;
   virtual void OnPoseGraphOptimization(
-      std::vector<KeyFrame::Attributes> kf_attributes) = 0;
+      std::vector<KeyFrame::Attributes> kf_attributes, int sequence_count) = 0;
   virtual void OnNewSequentialEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0;
   virtual void OnNewLoopEdge(Eigen::Vector3d p1, Eigen::Vector3d p2) = 0;
 };

pose_graph/src/pose_graph.cpp

@@ -241,7 +241,8 @@ bool PoseGraph::Load() {
       }
     }
     if (event_observer_) {
-      event_observer_->OnKeyFrameLoaded(current_kf_attribute, count);
+      event_observer_->OnKeyFrameLoaded(current_kf_attribute, count,
+                                        GetCurrentSequenceCount());
     }
     count++;
   }
@@ -489,7 +490,8 @@ void PoseGraph::AddKeyFrame(std::shared_ptr<KeyFrame> current_keyframe) {
   }
 
   // Generic callback
-  event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes());
+  event_observer_->OnKeyFrameAdded(current_keyframe->getAttributes(),
+                                   GetCurrentSequenceCount());
 }
 
 void PoseGraph::AddKeyFrame(std::shared_ptr<KeyFrame> current_keyframe,
@@ -1008,7 +1010,8 @@ void PoseGraph::StartOptimizationThread() {
       Optimize4DoF();
       auto kf_attributes = GetKeyFrameAttributes();
       if (event_observer_) {
-        event_observer_->OnPoseGraphOptimization(kf_attributes);
+        event_observer_->OnPoseGraphOptimization(kf_attributes,
+                                                 GetCurrentSequenceCount());
       }
       std::this_thread::sleep_for(std::chrono::milliseconds(2000));
     }

pose_graph_ros/CMakeLists.txt

@@ -22,6 +22,7 @@ catkin_package()
 
 add_executable(pose_graph_node
     src/pose_graph_node.cpp
+    src/pose_graph_event_observer_impl.cpp
     src/utility/CameraPoseVisualization.cpp
     )
 

pose_graph_ros/include/pose_graph_ros/pose_graph_node.hpp

@@ -16,19 +16,18 @@
 
 #include <nav_msgs/Odometry.h>
 #include <nav_msgs/Path.h>
-#include <opencv2/opencv.hpp>
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/PointCloud.h>
 
 #include "camodocal/camera_models/CameraFactory.h"
 #include "pose_graph/pose_graph.hpp"
-#include "pose_graph/details/pose_graph_event_observer.hpp"
+
 #include "pose_graph_ros/utility/CameraPoseVisualization.h"
 
 namespace pose_graph {
-class PoseGraphNode : public PoseGraphEventObserver {
+class PoseGraphNode {
  public:
   ~PoseGraphNode();
 
@@ -40,24 +39,8 @@ class PoseGraphNode : public PoseGraphEventObserver {
   void StartCommandAndProcessingThreads();
   void Process();
   void Command();
-  void Publish();
   void NewSequence();
 
-  // PoseGraphEventObserver callbacks
-  void OnPoseGraphLoaded() final;
-  void OnPoseGraphSaved() final;
-  void OnKeyFrameAdded(KeyFrame::Attributes kf_attribute) final;
-  void OnKeyFrameLoaded(KeyFrame::Attributes kf_attribute, int count) final;
-  void OnKeyFrameConnectionFound(KeyFrame::Attributes current_kf_attribute,
-                                 KeyFrame::Attributes old_kf_attribute,
-                                 std::vector<cv::Point2f> matched_2d_old_norm,
-                                 std::vector<double> matched_id,
-                                 cv::Mat& thumb_image) final;
-  void OnPoseGraphOptimization(
-      std::vector<KeyFrame::Attributes> kf_attributes) final;
-  void OnNewSequentialEdge(Vector3d p1, Vector3d p2) final;
-  void OnNewLoopEdge(Vector3d p1, Vector3d p2) final;
-
   // ros callbacks
   void ImuForwardCallback(const nav_msgs::OdometryConstPtr& forward_msg);
   void VioCallback(const nav_msgs::OdometryConstPtr& pose_msg);
@@ -69,35 +52,19 @@ class PoseGraphNode : public PoseGraphEventObserver {
 
  private:
   ros::NodeHandle nh_{"~"};
+
+  // pose graph
   PoseGraphConfig config_;
   PoseGraph pose_graph_ = PoseGraph();
-  std::unique_ptr<CameraPoseVisualization> camera_pose_vis_;
-  bool loop_closure_;
-  camodocal::CameraPtr camera_;  // Note: internally it uses a shared pointer.
-  bool visualise_imu_forward_;
-  std::queue<sensor_msgs::ImageConstPtr> image_buffer_;
-  std::queue<sensor_msgs::PointCloudConstPtr> point_buffer_;
-  std::queue<nav_msgs::Odometry::ConstPtr> pose_buffer_;
-  std::queue<Eigen::Vector3d> odometry_buffer_;
-  std::mutex buffer_mutex_;
-  std::mutex process_mutex_;
-  Eigen::Vector3d last_t_ = {-100, -100, -100};
-  std::string vins_result_path_;
-  std::string image_topic_;
-  nav_msgs::Path path_[10];
-  nav_msgs::Path base_path_;
-  nav_msgs::Path no_loop_path_;
-  std::unique_ptr<CameraPoseVisualization> posegraph_visualization_;
+  class PoseGraphEventObserverImpl; // forward declaration
+  std::shared_ptr<PoseGraphEventObserverImpl> pose_graph_event_observer_;
 
   // ros parameters
   std::string config_file_path_;
-  int visualization_shift_x_ = 0;
-  int visualization_shift_y_ = 0;
   int skip_cnt_threshold_ = 0;
   int skip_cnt_ = 0;
   double skip_distance_ = 0.0;
   static const int skip_first_cnt_threshold = 10;
-  const bool save_loop_path = true;
   int skip_first_cnt_ = 0;
   int frame_index_ = 0;
   int sequence_index_ = 1;
@@ -106,6 +73,11 @@ class PoseGraphNode : public PoseGraphEventObserver {
   std::thread measurement_thread_;
   std::thread keyboard_command_thread_;
 
+  // ros publishers
+  ros::Publisher pub_camera_pose_visual_;
+  ros::Publisher pub_key_odometrys_;
+  ros::Publisher pub_vio_path_;
+
   // ros subscribers
   ros::Subscriber sub_imu_forward_;
   ros::Subscriber sub_vio_;
@@ -115,16 +87,20 @@ class PoseGraphNode : public PoseGraphEventObserver {
   ros::Subscriber sub_point_;
   ros::Subscriber sub_relo_relative_pose_;
 
-  // ros publishers
-  ros::Publisher pub_match_points_;
-  ros::Publisher pub_camera_pose_visual_;
-  ros::Publisher pub_key_odometrys_;
-  ros::Publisher pub_vio_path_;
-  ros::Publisher pub_pg_path_;
-  ros::Publisher pub_base_path_;
-  ros::Publisher pub_pose_graph_;
-  ros::Publisher pub_path_[10];
-  ros::Publisher pub_match_img_;
+  // others
+  std::unique_ptr<CameraPoseVisualization> camera_pose_vis_;
+  bool loop_closure_;
+  camodocal::CameraPtr camera_;  // Note: internally it uses a shared pointer.
+  bool visualise_imu_forward_;
+  std::queue<sensor_msgs::ImageConstPtr> image_buffer_;
+  std::queue<sensor_msgs::PointCloudConstPtr> point_buffer_;
+  std::queue<nav_msgs::Odometry::ConstPtr> pose_buffer_;
+  std::queue<Eigen::Vector3d> odometry_buffer_;
+  std::mutex buffer_mutex_;
+  std::mutex process_mutex_;
+  Eigen::Vector3d last_t_ = {-100, -100, -100};
+  std::string image_topic_;
+  nav_msgs::Path no_loop_path_;
 };
 }  // namespace pose_graph