Markers and visualization (#622)

* Marker optimization ; DB save/load ; init from multiple observations

Loading/saving marker info, 2D and 3D, to sqlite3

keep_fixed_ flag in marker: previously, the marker positions were not
optimized, the position was estimated once based on a single observation
and kept fixed. The default is now to include the position in the
optimization, which could still be turned off by setting the flag to
true (would currently need to be done in the code)

initialized_before_ flag in marker and
"required_keyframes_for_marker_initialization" setting: if a single
observation of the marker is used to estimate its 3D position, a bad
estimate can still throw off the optimization procedure, causing the
markers to mess up the map building instead of helping. The setting
allows the initialization to wait until a number of keyframes have seen
the marker. Only when this happens, the flag is set, and the marker will
be included in the optimization. The default is to wait for 3 frames,
but can be controlled using a config file setting.

* Aruco nano support

This patch add support for using the aruco nano header file
(https://sourceforge.net/projects/aruco/files/ArucoNano-v5/) to
detect markers. The user can enable the support using a CMake
config setting, and then use "aruconano" as marker model.

* Modifications for marker visualisation

This draws the 2D detected markers, together with their IDs,
on the observed image. Because this needs the original coordinates
(not the undistorted ones), they are stored in the marker2d
class now as well (not saved/loaded to database though). To
allow a publisher (eg the socket_publisher) to share the 3D marker
info, a method has been added to retrieve them.

* Clearing markers as well in 'clear()'

* Refactoring and some minor fixes

- Delete references from markers when erasing keyframes
- required_keyframes_for_marker_initialization can now be set individusually in KeyframeInseter and Initialiser

---------

Co-authored-by: Jori Liesenborgs <[email protected]>
Co-authored-by: ymd-stella <[email protected]>

CMakeLists.txt

@@ -141,6 +141,11 @@ else()
     message(STATUS "aruco: disabled")
 endif()
 
+option(USE_ARUCO_NANO "Compile with aruco nano support" OFF)
+if (USE_ARUCO_NANO)
+    add_definitions(-DUSE_ARUCO_NANO)
+endif()
+
 # spdlog
 find_package(spdlog QUIET)
 if(spdlog_FOUND)

src/stella_vslam/config.cc

@@ -1,5 +1,8 @@
 #include "stella_vslam/config.h"
 #include "stella_vslam/marker_model/aruco.h"
+#ifdef USE_ARUCO_NANO
+#include "stella_vslam/marker_model/aruconano.h"
+#endif // USE_ARUCO_NANO
 #include "stella_vslam/util/string.h"
 #include "stella_vslam/util/yaml.h"
 
@@ -33,6 +36,15 @@ config::config(const YAML::Node& yaml_node, const std::string& config_file_path)
                 marker_model_yaml_node["marker_size"].as<double>(),
                 marker_model_yaml_node["max_markers"].as<double>());
         }
+
+#ifdef USE_ARUCO_NANO
+        else if (marker_model_type == "aruconano") {
+            marker_model_ = std::make_shared<marker_model::aruconano>(
+                marker_model_yaml_node["width"].as<double>(),
+                marker_model_yaml_node["dict"].as<int>(0));
+        }
+#endif // USE_ARUCO_NANO
+
         else {
             throw std::runtime_error("Invalid marker model type :" + marker_model_type);
         }

src/stella_vslam/data/keyframe.cc

@@ -3,6 +3,7 @@
 #include "stella_vslam/data/keyframe.h"
 #include "stella_vslam/data/landmark.h"
 #include "stella_vslam/data/marker.h"
+#include "stella_vslam/data/marker2d.h"
 #include "stella_vslam/data/map_database.h"
 #include "stella_vslam/data/bow_database.h"
 #include "stella_vslam/data/camera_database.h"
@@ -14,14 +15,98 @@
 #include <nlohmann/json.hpp>
 #include <spdlog/spdlog.h>
 
+namespace {
+using namespace stella_vslam;
+using namespace stella_vslam::data;
+inline std::vector<double> markers2d_to_blob(const std::unordered_map<unsigned int, marker2d>& markers_2d) {
+    std::vector<double> blob;
+    for (auto& m_it : markers_2d) {
+        const auto& m2d = m_it.second;
+
+        // undist_corners_
+        assert(m2d.undist_corners_.size() == 4);
+        for (auto& c : m2d.undist_corners_) {
+            blob.push_back(c.x);
+            blob.push_back(c.y);
+        }
+
+        // bearings_
+        assert(m2d.bearings_.size() == 4);
+        for (auto& b : m2d.bearings_) {
+            blob.push_back(b.x());
+            blob.push_back(b.y());
+            blob.push_back(b.z());
+        }
+
+        // rot_cm_
+        for (size_t i = 0; i < 3; i++) {
+            for (size_t j = 0; j < 3; j++) {
+                blob.push_back(m2d.rot_cm_(i, j));
+            }
+        }
+
+        // trans_cm_
+        blob.push_back(m2d.trans_cm_.x());
+        blob.push_back(m2d.trans_cm_.y());
+        blob.push_back(m2d.trans_cm_.z());
+
+        // id_
+        blob.push_back(m2d.id_);
+    }
+    return blob;
+}
+
+std::unordered_map<unsigned int, marker2d>
+markers2d_from_blob(size_t amount,
+                    const std::vector<double>& blob) {
+    std::unordered_map<unsigned int, marker2d> markers_2d;
+    size_t offset = 0;
+    for (size_t i = 0; i < amount; i++) {
+        std::vector<cv::Point2f> undist_corners;
+        for (size_t i = 0; i < 4; i++) {
+            float x = (float)blob[offset++];
+            float y = (float)blob[offset++];
+            undist_corners.push_back(cv::Point2f(x, y));
+        }
+
+        eigen_alloc_vector<Vec3_t> bearings;
+        for (size_t i = 0; i < 4; i++) {
+            double x = blob[offset++];
+            double y = blob[offset++];
+            double z = blob[offset++];
+            bearings.push_back(Vec3_t(x, y, z));
+        }
+
+        Mat33_t rot_cm;
+        for (size_t i = 0; i < 3; i++)
+            for (size_t j = 0; j < 3; j++)
+                rot_cm(i, j) = blob[offset++];
+
+        Vec3_t trans_cm;
+
+        trans_cm(0) = blob[offset++];
+        trans_cm(1) = blob[offset++];
+        trans_cm(2) = blob[offset++];
+
+        unsigned int id = (unsigned int)blob[offset++];
+
+        marker2d m2d(undist_corners, bearings, rot_cm, trans_cm, id, nullptr, {}); // WARNING: marker_model is set to nullptr! Don't use yet!
+        assert(markers_2d.find(m2d.id_) == markers_2d.end());
+        markers_2d.emplace(m2d.id_, m2d);
+    }
+    return markers_2d;
+}
+} // namespace
+
 namespace stella_vslam {
 namespace data {
 
 keyframe::keyframe(unsigned int id, const frame& frm)
     : id_(id), timestamp_(frm.timestamp_),
       camera_(frm.camera_), orb_params_(frm.orb_params_),
-      frm_obs_(frm.frm_obs_), markers_2d_(frm.markers_2d_),
+      frm_obs_(frm.frm_obs_),
       bow_vec_(frm.bow_vec_), bow_feat_vec_(frm.bow_feat_vec_),
+      markers_2d_(frm.markers_2d_),
       landmarks_(frm.get_landmarks()) {
     // set pose parameters (pose_wc_, trans_wc_) using frm.pose_cw_
     set_pose_cw(frm.get_pose_cw());
@@ -30,11 +115,13 @@ keyframe::keyframe(unsigned int id, const frame& frm)
 keyframe::keyframe(const unsigned int id, const double timestamp,
                    const Mat44_t& pose_cw, camera::base* camera,
                    const feature::orb_params* orb_params, const frame_observation& frm_obs,
-                   const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec)
+                   const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec,
+                   std::unordered_map<unsigned int, marker2d> markers_2d)
     : id_(id),
       timestamp_(timestamp), camera_(camera),
       orb_params_(orb_params), frm_obs_(frm_obs),
       bow_vec_(bow_vec), bow_feat_vec_(bow_feat_vec),
+      markers_2d_(markers_2d),
       landmarks_(std::vector<std::shared_ptr<landmark>>(frm_obs_.undist_keypts_.size(), nullptr)) {
     // set pose parameters (pose_wc_, trans_wc_) using pose_cw_
     set_pose_cw(pose_cw);
@@ -62,12 +149,13 @@ std::shared_ptr<keyframe> keyframe::make_keyframe(
     const unsigned int id, const double timestamp,
     const Mat44_t& pose_cw, camera::base* camera,
     const feature::orb_params* orb_params, const frame_observation& frm_obs,
-    const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec) {
+    const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec,
+    std::unordered_map<unsigned int, marker2d> markers_2d) {
     auto ptr = std::allocate_shared<keyframe>(
         Eigen::aligned_allocator<keyframe>(),
         id, timestamp,
         pose_cw, camera, orb_params,
-        frm_obs, bow_vec, bow_feat_vec);
+        frm_obs, bow_vec, bow_feat_vec, markers_2d);
     // covisibility graph node (connections is not assigned yet)
     ptr->graph_node_ = stella_vslam::make_unique<graph_node>(ptr);
     return ptr;
@@ -126,6 +214,22 @@ std::shared_ptr<keyframe> keyframe::from_stmt(sqlite3_stmt* stmt,
     std::memcpy(descriptors.data, p, sqlite3_column_bytes(stmt, column_id));
     column_id++;
 
+    std::unordered_map<unsigned int, marker2d> markers_2d;
+    if (sqlite3_column_count(stmt) > column_id) {
+        auto num_markers = sqlite3_column_int64(stmt, column_id);
+        column_id++;
+
+        std::vector<double> markers_blob(num_markers * MARKERS2D_BLOB_NUM_DOUBLES);
+
+        p = reinterpret_cast<const char*>(sqlite3_column_blob(stmt, column_id));
+        assert(static_cast<size_t>(sqlite3_column_bytes(stmt, column_id)) == markers_blob.size() * sizeof(double));
+        std::memcpy(markers_blob.data(), p, sqlite3_column_bytes(stmt, column_id));
+        column_id++;
+
+        assert(markers_blob.size() == MARKERS2D_BLOB_NUM_DOUBLES * num_markers);
+        markers_2d = markers2d_from_blob(num_markers, markers_blob);
+    }
+
     auto bearings = eigen_alloc_vector<Vec3_t>();
     camera->convert_keypoints_to_bearings(undist_keypts, bearings);
     assert(bearings.size() == num_keypts);
@@ -137,9 +241,11 @@ std::shared_ptr<keyframe> keyframe::from_stmt(sqlite3_stmt* stmt,
     frame_observation frm_obs{descriptors, undist_keypts, bearings, stereo_x_right, depths};
     // Compute BoW
     data::bow_vocabulary_util::compute_bow(bow_vocab, descriptors, bow_vec, bow_feat_vec);
+    // NOTE: 3D marker info will be filled in later based on loaded markers
     auto keyfrm = data::keyframe::make_keyframe(
         id + next_keyframe_id, timestamp, pose_cw, camera, orb_params,
-        frm_obs, bow_vec, bow_feat_vec);
+        frm_obs, bow_vec, bow_feat_vec, markers_2d);
+
     return keyfrm;
 }
 
@@ -170,6 +276,8 @@ nlohmann::json keyframe::to_json() const {
         loop_edge_ids.push_back(loop_edge->id_);
     }
 
+    // TODO: msgpack format does not yet support markers save/load
+
     return {{"ts", timestamp_},
             {"cam", camera_->name_},
             {"orb_params", orb_params_->name_},
@@ -235,6 +343,17 @@ bool keyframe::bind_to_stmt(sqlite3* db, sqlite3_stmt* stmt) const {
         assert(descriptors.elemSize() == 1);
         ret = sqlite3_bind_blob(stmt, column_id++, descriptors.data, descriptors.total() * descriptors.elemSize(), SQLITE_TRANSIENT);
     }
+    size_t num_markers = 0;
+    if (ret == SQLITE_OK) {
+        num_markers = markers_2d_.size();
+        ret = sqlite3_bind_int64(stmt, column_id++, num_markers);
+    }
+
+    if (ret == SQLITE_OK) {
+        std::vector<double> marker2d_blob = markers2d_to_blob(markers_2d_);
+        assert(marker2d_blob.size() == MARKERS2D_BLOB_NUM_DOUBLES * markers_2d_.size());
+        ret = sqlite3_bind_blob(stmt, column_id++, marker2d_blob.data(), marker2d_blob.size() * sizeof(double), SQLITE_TRANSIENT);
+    }
     if (ret != SQLITE_OK) {
         spdlog::error("SQLite error (bind): {}", sqlite3_errmsg(db));
     }
@@ -522,6 +641,9 @@ void keyframe::prepare_for_erasing(map_database* map_db, bow_database* bow_db) {
                 lm->update_mean_normal_and_obs_scale_variance();
             }
         }
+        for (const auto& mkr : markers_) {
+            mkr.second->observations_.erase(id_);
+        }
     }
 
     // 3. recover covisibility graph and spanning tree

src/stella_vslam/data/keyframe.h

@@ -51,7 +51,8 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
     keyframe(const unsigned int id,
              const double timestamp, const Mat44_t& pose_cw, camera::base* camera,
              const feature::orb_params* orb_params, const frame_observation& frm_obs,
-             const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec);
+             const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec,
+             std::unordered_map<unsigned int, marker2d> markers_2d = {});
     virtual ~keyframe();
 
     // Factory method for create keyframe
@@ -60,7 +61,8 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
         const unsigned int id,
         const double timestamp, const Mat44_t& pose_cw, camera::base* camera,
         const feature::orb_params* orb_params, const frame_observation& frm_obs,
-        const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec);
+        const bow_vector& bow_vec, const bow_feature_vector& bow_feat_vec,
+        std::unordered_map<unsigned int, marker2d> markers_2d = {});
     static std::shared_ptr<keyframe> from_stmt(sqlite3_stmt* stmt,
                                                camera_database* cam_db,
                                                orb_params_database* orb_params_db,
@@ -94,7 +96,9 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
             {"undist_keypts", "BLOB"},
             {"x_rights", "BLOB"},
             {"depths", "BLOB"},
-            {"descs", "BLOB"}};
+            {"descs", "BLOB"},
+            {"n_markers", "INTEGER"},
+            {"markers", "BLOB"}};
     };
     bool bind_to_stmt(sqlite3* db, sqlite3_stmt* stmt) const;
 
@@ -271,13 +275,13 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
 
     frame_observation frm_obs_;
 
-    //! observed markers 2D (ID to marker2d map)
-    std::unordered_map<unsigned int, marker2d> markers_2d_;
-
     //! BoW features (DBoW2 or FBoW)
     bow_vector bow_vec_;
     bow_feature_vector bow_feat_vec_;
 
+    //! observed markers 2D (ID to marker2d map)
+    std::unordered_map<unsigned int, marker2d> markers_2d_;
+
     //-----------------------------------------
     // covisibility graph
 
@@ -319,6 +323,12 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
 
     //! flag which indicates this keyframe will be erased
     std::atomic<bool> will_be_erased_{false};
+
+    //-----------------------------------------
+    // misc
+
+    //!
+    static constexpr int MARKERS2D_BLOB_NUM_DOUBLES = 33;
 };
 
 } // namespace data

src/stella_vslam/data/map_database.cc

@@ -239,6 +239,7 @@ void map_database::clear() {
 
     landmarks_.clear();
     keyframes_.clear();
+    markers_.clear();
     last_inserted_keyfrm_ = nullptr;
     local_landmarks_.clear();
     spanning_roots_.clear();
@@ -517,6 +518,10 @@ bool map_database::from_db(sqlite3* db,
     if (!ok) {
         return false;
     }
+    bool have_markers = load_markers_from_db(db, "markers");
+    if (!have_markers) {
+        spdlog::warn("no such table: markers");
+    }
 
     // find root node
     std::unordered_set<unsigned int> already_found_root_ids;
@@ -673,9 +678,11 @@ bool map_database::to_db(sqlite3* db) const {
     bool ok = util::sqlite3_util::drop_table(db, "keyframes");
     ok = ok && util::sqlite3_util::drop_table(db, "landmarks");
     ok = ok && util::sqlite3_util::drop_table(db, "associations");
+    ok = ok && util::sqlite3_util::drop_table(db, "markers");
     ok = ok && save_keyframes_to_db(db, "keyframes");
     ok = ok && save_landmarks_to_db(db, "landmarks");
     ok = ok && save_associations_to_db(db, "associations");
+    ok = ok && save_markers_to_db(db, "markers");
     return ok;
 }
 
@@ -807,5 +814,47 @@ bool map_database::save_associations_to_db(sqlite3* db, const std::string& table
     return util::sqlite3_util::commit(db);
 }
 
+bool map_database::save_markers_to_db(sqlite3* db, const std::string& table_name) const {
+    const auto columns = data::marker::columns();
+    bool ok = util::sqlite3_util::create_table(db, table_name, columns);
+    ok = ok && util::sqlite3_util::begin(db);
+    if (!ok) {
+        return false;
+    }
+    sqlite3_stmt* stmt = util::sqlite3_util::create_insert_stmt(db, table_name, columns);
+    if (!stmt) {
+        return false;
+    }
+
+    for (const auto& id_marker : markers_) {
+        const auto mkr = id_marker.second;
+        assert(mkr);
+        bool ok = mkr->bind_to_stmt(db, stmt);
+        ok = ok && util::sqlite3_util::next(db, stmt);
+        if (!ok) {
+            return false;
+        }
+    }
+
+    sqlite3_finalize(stmt);
+    return util::sqlite3_util::commit(db);
+}
+
+bool map_database::load_markers_from_db(sqlite3* db, const std::string& table_name) {
+    sqlite3_stmt* stmt = util::sqlite3_util::create_select_stmt(db, table_name);
+    if (!stmt) {
+        return false;
+    }
+
+    int ret = SQLITE_ERROR;
+    while ((ret = sqlite3_step(stmt)) == SQLITE_ROW) {
+        auto mkr = data::marker::from_stmt(stmt, keyframes_);
+        assert(!markers_.count(mkr->id_));
+        markers_[mkr->id_] = mkr;
+    }
+    sqlite3_finalize(stmt);
+    return ret == SQLITE_DONE;
+}
+
 } // namespace data
 } // namespace stella_vslam

src/stella_vslam/data/map_database.h

@@ -333,6 +333,9 @@ class map_database {
                                   const std::shared_ptr<keyframe>& keyfrm) const;
     bool save_associations_to_db(sqlite3* db, const std::string& table_name) const;
 
+    bool load_markers_from_db(sqlite3* db, const std::string& table_name);
+    bool save_markers_to_db(sqlite3* db, const std::string& table_name) const;
+
     //! mutex for mutual exclusion controll between class methods
     mutable std::mutex mtx_map_access_;