From 43a9f2fbe9a36b4041ef88a240760d6fce92430a Mon Sep 17 00:00:00 2001
From: ymd-stella <world.applepie@gmail.com>
Date: Sat, 16 Dec 2023 21:09:31 +0900
Subject: [PATCH] Add interface to frame_publisher for direct access to frame
 information

---
 src/stella_vslam/publish/frame_publisher.cc | 26 +++++++++++++++++++++
 src/stella_vslam/publish/frame_publisher.h  | 10 ++++++++
 2 files changed, 36 insertions(+)

diff --git a/src/stella_vslam/publish/frame_publisher.cc b/src/stella_vslam/publish/frame_publisher.cc
index 0e9e37336..ed081ac67 100644
--- a/src/stella_vslam/publish/frame_publisher.cc
+++ b/src/stella_vslam/publish/frame_publisher.cc
@@ -73,6 +73,32 @@ cv::Mat frame_publisher::draw_frame() {
     return img;
 }
 
+tracker_state_t frame_publisher::get_tracking_state() {
+    std::lock_guard<std::mutex> lock(mtx_);
+    return tracking_state_;
+}
+
+std::vector<cv::KeyPoint> frame_publisher::get_keypoints() {
+    std::lock_guard<std::mutex> lock(mtx_);
+    return curr_keypts_;
+}
+
+bool frame_publisher::get_mapping_is_enabled() {
+    std::lock_guard<std::mutex> lock(mtx_);
+    return mapping_is_enabled_;
+}
+
+std::vector<std::shared_ptr<data::landmark>> frame_publisher::get_landmarks() {
+    std::lock_guard<std::mutex> lock(mtx_);
+    return curr_lms_;
+}
+
+cv::Mat frame_publisher::get_image() {
+    cv::Mat img;
+    img_.copyTo(img);
+    return img;
+}
+
 unsigned int frame_publisher::draw_tracked_points(cv::Mat& img, const std::vector<cv::KeyPoint>& curr_keypts,
                                                   const std::vector<std::shared_ptr<data::landmark>>& curr_lms,
                                                   const bool mapping_is_enabled,
diff --git a/src/stella_vslam/publish/frame_publisher.h b/src/stella_vslam/publish/frame_publisher.h
index 9ab8c103d..429323d6e 100644
--- a/src/stella_vslam/publish/frame_publisher.h
+++ b/src/stella_vslam/publish/frame_publisher.h
@@ -51,6 +51,16 @@ class frame_publisher {
      */
     cv::Mat draw_frame();
 
+    tracker_state_t get_tracking_state();
+
+    std::vector<cv::KeyPoint> get_keypoints();
+
+    bool get_mapping_is_enabled();
+
+    std::vector<std::shared_ptr<data::landmark>> get_landmarks();
+
+    cv::Mat get_image();
+
 protected:
     unsigned int draw_tracked_points(cv::Mat& img, const std::vector<cv::KeyPoint>& curr_keypts,
                                      const std::vector<std::shared_ptr<data::landmark>>& curr_lms,