Wip/output keyframes

example/run_euroc_slam.cc

@@ -23,11 +23,12 @@
 #include <opencv2/core/mat.hpp>
 #include <opencv2/core/types.hpp>
 #include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
 #include <spdlog/spdlog.h>
 #include <popl.hpp>
-
 #include <ghc/filesystem.hpp>
 namespace fs = ghc::filesystem;
+#include <nlohmann/json.hpp>
 
 #ifdef USE_STACK_TRACE_LOGGER
 #include <backward.hpp>
@@ -45,6 +46,7 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
                    const bool wait_loop_ba,
                    const bool auto_term,
                    const std::string& eval_log_dir,
+                   const std::string& keyframe_image_dir,
                    const std::string& map_db_path,
                    const bool equal_hist) {
     const euroc_sequence sequence(sequence_dir_path);
@@ -141,11 +143,13 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
     slam->shutdown();
 
     if (!eval_log_dir.empty()) {
+        const auto dir_path = fs::path(eval_log_dir);
+        fs::create_directories(dir_path);
         // output the trajectories for evaluation
-        slam->save_frame_trajectory(eval_log_dir + "/frame_trajectory.txt", "TUM");
-        slam->save_keyframe_trajectory(eval_log_dir + "/keyframe_trajectory.txt", "TUM");
+        slam->save_frame_trajectory(dir_path / "frame_trajectory.txt", "TUM");
+        slam->save_keyframe_trajectory(dir_path / "keyframe_trajectory.txt", "TUM");
         // output the tracking times for evaluation
-        std::ofstream ofs(eval_log_dir + "/track_times.txt", std::ios::out);
+        std::ofstream ofs(dir_path / "track_times.txt", std::ios::out);
         if (ofs.is_open()) {
             for (const auto track_time : track_times) {
                 ofs << track_time << std::endl;
@@ -154,6 +158,42 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
         }
     }
 
+    if (!keyframe_image_dir.empty()) {
+        const auto dir_path = fs::path(keyframe_image_dir);
+        fs::create_directories(dir_path / "images");
+
+        auto image_infos = slam->get_keyframe_images();
+        nlohmann::json transforms;
+        for (const auto& image_info : image_infos) {
+            const auto filepath = fs::path("images") / (std::to_string(std::get<1>(image_info)) + std::string(".png"));
+            cv::Mat image = std::get<0>(image_info);
+            if (image.channels() < 3) {
+                cv::cvtColor(image, image, cv::COLOR_GRAY2BGR);
+            }
+            cv::imwrite(dir_path / filepath, image);
+            nlohmann::json frame;
+            frame["file_path"] = filepath;
+            Eigen::Matrix4d rot_to_opengl;
+            rot_to_opengl << 1, 0, 0, 0,
+                0, -1, 0, 0,
+                0, 0, -1, 0,
+                0, 0, 0, 1;
+            Eigen::Matrix4d t = rot_to_opengl * std::get<2>(image_info) * rot_to_opengl;
+            frame["transform_matrix"] = {
+                {t(0, 0), t(0, 1), t(0, 2), t(0, 3)},
+                {t(1, 0), t(1, 1), t(1, 2), t(1, 3)},
+                {t(2, 0), t(2, 1), t(2, 2), t(2, 3)},
+                {t(3, 0), t(3, 1), t(3, 2), t(3, 3)},
+            };
+            transforms["frames"].push_back(frame);
+        }
+        std::ofstream ofs(dir_path / "transforms.json", std::ios::out);
+        if (ofs.is_open()) {
+            ofs << transforms.dump(4) << std::endl;
+            ofs.close();
+        }
+    }
+
     if (!map_db_path.empty()) {
         // output the map database
         slam->save_map_database(map_db_path);
@@ -173,6 +213,7 @@ void stereo_tracking(const std::shared_ptr<stella_vslam::system>& slam,
                      const bool wait_loop_ba,
                      const bool auto_term,
                      const std::string& eval_log_dir,
+                     const std::string& keyframe_image_dir,
                      const std::string& map_db_path,
                      const bool equal_hist) {
     const euroc_sequence sequence(sequence_dir_path);
@@ -323,6 +364,7 @@ int main(int argc, char* argv[]) {
     auto auto_term = op.add<popl::Switch>("", "auto-term", "automatically terminate the viewer");
     auto log_level = op.add<popl::Value<std::string>>("", "log-level", "log level", "info");
     auto eval_log_dir = op.add<popl::Value<std::string>>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", "");
+    auto keyframe_image_dir = op.add<popl::Value<std::string>>("", "keyframe-image-dir", "save keyframe images to this path", "");
     auto map_db_path_in = op.add<popl::Value<std::string>>("i", "map-db-in", "load a map from this path", "");
     auto map_db_path_out = op.add<popl::Value<std::string>>("o", "map-db-out", "store a map database at this path after slam", "");
     auto disable_mapping = op.add<popl::Switch>("", "disable-mapping", "disable mapping");
@@ -408,6 +450,7 @@ int main(int argc, char* argv[]) {
                       wait_loop_ba->is_set(),
                       auto_term->is_set(),
                       eval_log_dir->value(),
+                      keyframe_image_dir->value(),
                       map_db_path_out->value(),
                       equal_hist->is_set());
     }
@@ -420,6 +463,7 @@ int main(int argc, char* argv[]) {
                         wait_loop_ba->is_set(),
                         auto_term->is_set(),
                         eval_log_dir->value(),
+                        keyframe_image_dir->value(),
                         map_db_path_out->value(),
                         equal_hist->is_set());
     }

example/run_video_slam.cc

@@ -18,11 +18,12 @@
 #include <opencv2/core/types.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/videoio.hpp>
+#include <opencv2/imgproc.hpp>
 #include <spdlog/spdlog.h>
 #include <popl.hpp>
-
 #include <ghc/filesystem.hpp>
 namespace fs = ghc::filesystem;
+#include <nlohmann/json.hpp>
 
 #ifdef USE_STACK_TRACE_LOGGER
 #include <backward.hpp>
@@ -42,6 +43,7 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
                    const bool wait_loop_ba,
                    const bool auto_term,
                    const std::string& eval_log_dir,
+                   const std::string& keyframe_image_dir,
                    const std::string& map_db_path,
                    const double start_timestamp) {
     // load the mask image
@@ -87,7 +89,7 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
 
             if (!frame.empty() && (num_frame % frame_skip == 0)) {
                 // input the current frame and estimate the camera pose
-                slam->feed_monocular_frame(frame, timestamp, mask);
+                slam->feed_monocular_frame(frame.clone(), timestamp, mask);
             }
 
             const auto tp_2 = std::chrono::steady_clock::now();
@@ -144,11 +146,13 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
     slam->shutdown();
 
     if (!eval_log_dir.empty()) {
+        const auto dir_path = fs::path(eval_log_dir);
+        fs::create_directories(dir_path);
         // output the trajectories for evaluation
-        slam->save_frame_trajectory(eval_log_dir + "/frame_trajectory.txt", "TUM");
-        slam->save_keyframe_trajectory(eval_log_dir + "/keyframe_trajectory.txt", "TUM");
+        slam->save_frame_trajectory(dir_path / "frame_trajectory.txt", "TUM");
+        slam->save_keyframe_trajectory(dir_path / "keyframe_trajectory.txt", "TUM");
         // output the tracking times for evaluation
-        std::ofstream ofs(eval_log_dir + "/track_times.txt", std::ios::out);
+        std::ofstream ofs(dir_path / "track_times.txt", std::ios::out);
         if (ofs.is_open()) {
             for (const auto track_time : track_times) {
                 ofs << track_time << std::endl;
@@ -157,6 +161,42 @@ void mono_tracking(const std::shared_ptr<stella_vslam::system>& slam,
         }
     }
 
+    if (!keyframe_image_dir.empty()) {
+        const auto dir_path = fs::path(keyframe_image_dir);
+        fs::create_directories(dir_path / "images");
+
+        auto image_infos = slam->get_keyframe_images();
+        nlohmann::json transforms;
+        for (const auto& image_info : image_infos) {
+            const auto filepath = fs::path("images") / (std::to_string(std::get<1>(image_info)) + std::string(".png"));
+            cv::Mat image = std::get<0>(image_info);
+            if (image.channels() < 3) {
+                cv::cvtColor(image, image, cv::COLOR_GRAY2BGR);
+            }
+            cv::imwrite(dir_path / filepath, image);
+            nlohmann::json frame;
+            frame["file_path"] = filepath;
+            Eigen::Matrix4d rot_to_opengl;
+            rot_to_opengl << 1, 0, 0, 0,
+                0, -1, 0, 0,
+                0, 0, -1, 0,
+                0, 0, 0, 1;
+            Eigen::Matrix4d t = rot_to_opengl * std::get<2>(image_info) * rot_to_opengl;
+            frame["transform_matrix"] = {
+                {t(0, 0), t(0, 1), t(0, 2), t(0, 3)},
+                {t(1, 0), t(1, 1), t(1, 2), t(1, 3)},
+                {t(2, 0), t(2, 1), t(2, 2), t(2, 3)},
+                {t(3, 0), t(3, 1), t(3, 2), t(3, 3)},
+            };
+            transforms["frames"].push_back(frame);
+        }
+        std::ofstream ofs(dir_path / "transforms.json", std::ios::out);
+        if (ofs.is_open()) {
+            ofs << transforms.dump(4) << std::endl;
+            ofs.close();
+        }
+    }
+
     if (!map_db_path.empty()) {
         // output the map database
         slam->save_map_database(map_db_path);
@@ -187,6 +227,7 @@ int main(int argc, char* argv[]) {
     auto auto_term = op.add<popl::Switch>("", "auto-term", "automatically terminate the viewer");
     auto log_level = op.add<popl::Value<std::string>>("", "log-level", "log level", "info");
     auto eval_log_dir = op.add<popl::Value<std::string>>("", "eval-log-dir", "store trajectory and tracking times at this path (Specify the directory where it exists.)", "");
+    auto keyframe_image_dir = op.add<popl::Value<std::string>>("", "keyframe-image-dir", "save keyframe images to this path", "");
     auto map_db_path_in = op.add<popl::Value<std::string>>("i", "map-db-in", "load a map from this path", "");
     auto map_db_path_out = op.add<popl::Value<std::string>>("o", "map-db-out", "store a map database at this path after slam", "");
     auto disable_mapping = op.add<popl::Switch>("", "disable-mapping", "disable mapping");
@@ -288,6 +329,7 @@ int main(int argc, char* argv[]) {
                       wait_loop_ba->is_set(),
                       auto_term->is_set(),
                       eval_log_dir->value(),
+                      keyframe_image_dir->value(),
                       map_db_path_out->value(),
                       timestamp);
     }

src/stella_vslam/data/frame.cc

@@ -21,6 +21,13 @@ frame::frame(const double timestamp, camera::base* camera, feature::orb_params*
       // Initialize association with 3D points
       landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {}
 
+frame::frame(const cv::Mat& image, const double timestamp, camera::base* camera, feature::orb_params* orb_params,
+             const frame_observation frm_obs, const std::unordered_map<unsigned int, marker2d>& markers_2d)
+    : id_(next_id_++), image_(image), timestamp_(timestamp), camera_(camera), orb_params_(orb_params), frm_obs_(frm_obs),
+      markers_2d_(markers_2d),
+      // Initialize association with 3D points
+      landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.num_keypts_, nullptr)) {}
+
 void frame::set_pose_cw(const Mat44_t& pose_cw) {
     pose_is_valid_ = true;
     pose_cw_ = pose_cw;

src/stella_vslam/data/frame.h

@@ -53,6 +53,18 @@ class frame {
     frame(const double timestamp, camera::base* camera, feature::orb_params* orb_params,
           const frame_observation frm_obs, const std::unordered_map<unsigned int, marker2d>& markers_2d);
 
+    /**
+     * Constructor for monocular frame
+     * @param image
+     * @param timestamp
+     * @param camera
+     * @param orb_params
+     * @param frm_obs
+     * @param markers_2d
+     */
+    frame(const cv::Mat& image, const double timestamp, camera::base* camera, feature::orb_params* orb_params,
+          const frame_observation frm_obs, const std::unordered_map<unsigned int, marker2d>& markers_2d);
+
     /**
      * Set camera pose and refresh rotation and translation
      * @param pose_cw
@@ -168,6 +180,9 @@ class frame {
     //! current frame ID
     unsigned int id_;
 
+    //! image
+    cv::Mat image_;
+
     //! next frame ID
     static std::atomic<unsigned int> next_id_;
 

src/stella_vslam/data/map_database.cc

@@ -49,6 +49,24 @@ std::shared_ptr<keyframe> map_database::get_keyframe(unsigned int id) const {
     return keyframes_.at(id);
 }
 
+void map_database::add_image(unsigned int keyfrm_id, const cv::Mat& image) {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+    images_[keyfrm_id] = image;
+}
+
+void map_database::erase_image(unsigned int keyfrm_id) {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+    images_.erase(keyfrm_id);
+}
+
+cv::Mat map_database::get_image(unsigned int keyfrm_id) const {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+    if (!images_.count(keyfrm_id)) {
+        return cv::Mat();
+    }
+    return images_.at(keyfrm_id);
+}
+
 void map_database::add_landmark(std::shared_ptr<landmark>& lm) {
     std::lock_guard<std::mutex> lock(mtx_map_access_);
     landmarks_[lm->id_] = lm;

src/stella_vslam/data/map_database.h

@@ -59,6 +59,25 @@ class map_database {
      */
     std::shared_ptr<keyframe> get_keyframe(unsigned int id) const;
 
+    /**
+     * Add image to the database
+     * @param keyfrm_id
+     * @param image
+     */
+    void add_image(unsigned int keyfrm_id, const cv::Mat& image);
+
+    /**
+     * Erase image from the database
+     * @param keyfrm_id
+     */
+    void erase_image(unsigned int keyfrm_id);
+
+    /**
+     * Get image from the database
+     * @param keyfrm_id
+     */
+    cv::Mat get_image(unsigned int keyfrm_id) const;
+
     /**
      * Add landmark to the database
      * @param lm
@@ -335,6 +354,8 @@ class map_database {
     std::unordered_map<unsigned int, std::shared_ptr<landmark>> landmarks_;
     //! IDs and markers
     std::unordered_map<unsigned int, std::shared_ptr<marker>> markers_;
+    //! IDs and images
+    std::unordered_map<unsigned int, cv::Mat> images_;
 
     //! spanning roots
     std::vector<std::shared_ptr<keyframe>> spanning_roots_;

src/stella_vslam/module/initializer.cc

@@ -200,15 +200,17 @@ bool initializer::create_map_for_monocular(data::bow_vocabulary* bow_vocab, data
     init_keyfrm->graph_node_->set_spanning_root(init_keyfrm);
     curr_keyfrm->graph_node_->set_spanning_root(init_keyfrm);
     map_db_->add_spanning_root(init_keyfrm);
+    if (!init_frm_.image_.empty()) {
+        map_db_->add_image(init_keyfrm->id_, init_frm_.image_);
+    }
+    if (!curr_frm.image_.empty()) {
+        map_db_->add_image(curr_keyfrm->id_, curr_frm.image_);
+    }
 
     // compute BoW representations
     init_keyfrm->compute_bow(bow_vocab);
     curr_keyfrm->compute_bow(bow_vocab);
 
-    // add the keyframes to the map DB
-    map_db_->add_keyframe(init_keyfrm);
-    map_db_->add_keyframe(curr_keyfrm);
-
     // update the frame statistics
     init_frm_.ref_keyfrm_ = init_keyfrm;
     curr_frm.ref_keyfrm_ = curr_keyfrm;
@@ -332,13 +334,13 @@ bool initializer::create_map_for_stereo(data::bow_vocabulary* bow_vocab, data::f
     auto curr_keyfrm = data::keyframe::make_keyframe(map_db_->next_keyframe_id_++, curr_frm);
     curr_keyfrm->graph_node_->set_spanning_root(curr_keyfrm);
     map_db_->add_spanning_root(curr_keyfrm);
+    if (!curr_frm.image_.empty()) {
+        map_db_->add_image(curr_keyfrm->id_, curr_frm.image_);
+    }
 
     // compute BoW representation
     curr_keyfrm->compute_bow(bow_vocab);
 
-    // add to the map DB
-    map_db_->add_keyframe(curr_keyfrm);
-
     // update the frame statistics
     curr_frm.ref_keyfrm_ = curr_keyfrm;
     map_db_->update_frame_statistics(curr_frm, false);

src/stella_vslam/module/keyframe_inserter.cc

@@ -107,6 +107,9 @@ std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map
                                                                        data::frame& curr_frm) {
     auto keyfrm = data::keyframe::make_keyframe(map_db->next_keyframe_id_++, curr_frm);
     keyfrm->update_landmarks();
+    if (!curr_frm.image_.empty()) {
+        map_db->add_image(keyfrm->id_, curr_frm.image_);
+    }
 
     for (const auto& id_mkr2d : keyfrm->markers_2d_) {
         auto marker = map_db->get_marker(id_mkr2d.first);