[Issue #626] - Frame publisher function to get landmarks and keypoints with lock held (#628)

* Keypoints & Landmark sizes do not match #626 - Add function to fetch both while mtx held

* [#626] Refactor - Change order of function name to intuitively match returned pair order

Author: Skwangles

src/stella_vslam/publish/frame_publisher.cc

@@ -94,6 +94,12 @@ std::string frame_publisher::get_tracking_state() {
     return state_str;
 }
 
+std::pair<std::vector<cv::KeyPoint>, std::vector<std::shared_ptr<data::landmark>>> frame_publisher::get_keypoints_and_landmarks() {
+    std::lock_guard<std::mutex> lock(mtx_);
+
+    return std::make_pair(curr_keypts_, curr_lms_);
+}
+
 std::vector<cv::KeyPoint> frame_publisher::get_keypoints() {
     std::lock_guard<std::mutex> lock(mtx_);
     return curr_keypts_;
@@ -194,6 +200,7 @@ void frame_publisher::update(const std::vector<std::shared_ptr<data::landmark>>&
 
     img.copyTo(img_);
 
+    assert(keypts.size() == curr_lms.size());
     curr_keypts_ = keypts;
     tracking_time_elapsed_ms_ = tracking_time_elapsed_ms;
     extraction_time_elapsed_ms_ = extraction_time_elapsed_ms;

src/stella_vslam/publish/frame_publisher.h

@@ -7,6 +7,7 @@
 #include <mutex>
 #include <vector>
 #include <memory>
+#include <utility>
 
 #include <opencv2/core/mat.hpp>
 #include <opencv2/core/types.hpp>
@@ -61,6 +62,8 @@ class frame_publisher {
 
     std::vector<std::shared_ptr<data::landmark>> get_landmarks();
 
+    std::pair<std::vector<cv::KeyPoint>, std::vector<std::shared_ptr<data::landmark>>> get_keypoints_and_landmarks();
+
     cv::Mat get_image();
 
     double get_tracking_time_elapsed_ms();