Load multiple non-overlapping maps

example/run_video_localization.cc

@@ -8,6 +8,7 @@
 #include "openvslam/config.h"
 #include "openvslam/util/yaml.h"
 
+#include <cmath>
 #include <iostream>
 #include <chrono>
 #include <numeric>
@@ -28,15 +29,35 @@
 
 void mono_localization(const std::shared_ptr<openvslam::config>& cfg,
                        const std::string& vocab_file_path, const std::string& video_file_path, const std::string& mask_img_path,
-                       const std::string& map_db_path, const bool mapping,
-                       const unsigned int frame_skip, const bool no_sleep, const bool auto_term) {
+                       const std::string& map_db_path, const std::string& map_db_path2, const bool mapping,
+                       const unsigned int frame_skip, const bool no_sleep, const bool auto_term,
+                       const double scale, const std::vector<double> rotation_xyz, const std::vector<double> translation_xyz) {
     // load the mask image
     const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE);
 
     // build a SLAM system
     openvslam::system SLAM(cfg, vocab_file_path);
     // load the prebuilt map
     SLAM.load_map_database(map_db_path);
+    // if another map is passed load an merge it
+    if (!map_db_path2.empty()){
+        // Define Map Scale Factor
+        double map_scale = scale;
+        // Define Map Rotation with WC coordinates from Euler angles
+        openvslam::Mat33_t rotation_matrix;
+        rotation_matrix =   Eigen::AngleAxisd(rotation_xyz[0], Eigen::Vector3d::UnitX())
+                          * Eigen::AngleAxisd(rotation_xyz[1], Eigen::Vector3d::UnitY())
+                          * Eigen::AngleAxisd(rotation_xyz[2], Eigen::Vector3d::UnitZ());
+
+        // Define Map Translation with WC coordinates
+        openvslam::Vec3_t translation {translation_xyz[0], translation_xyz[1], translation_xyz[2]};
+
+        // Call load_new_map_to_merge
+        openvslam::Mat44_t transf_matrix = openvslam::Mat44_t::Identity();
+        transf_matrix.block<3, 3>(0, 0) = rotation_matrix;
+        transf_matrix.block<3, 1>(0, 3) = translation;
+        SLAM.load_new_map_database(map_db_path2, transf_matrix, map_scale);
+    }
     // startup the SLAM process (it does not need initialization of a map)
     SLAM.startup(false);
     // select to activate the mapping module or not
@@ -149,6 +170,14 @@ int main(int argc, char* argv[]) {
     auto video_file_path = op.add<popl::Value<std::string>>("m", "video", "video file path");
     auto config_file_path = op.add<popl::Value<std::string>>("c", "config", "config file path");
     auto map_db_path = op.add<popl::Value<std::string>>("p", "map-db", "path to a prebuilt map database");
+    auto map_db_path2 = op.add<popl::Value<std::string>>("", "map-db2", "path to another prebuilt map database");
+    auto map_2_scale = op.add<popl::Value<double>>("s", "map-scale", "path to another prebuilt map database");
+    auto map_2_rotation_x = op.add<popl::Value<double>>("", "map-rotation-x", "Euler angle of X coordinate value to rotate map-db2 in rad");
+    auto map_2_rotation_y = op.add<popl::Value<double>>("", "map-rotation-y", "Euler angle of Y coordinate value to rotate map-db2 in rad");
+    auto map_2_rotation_z = op.add<popl::Value<double>>("", "map-rotation-z", "Euler angle of Z coordinate value to rotate map-db2 in rad");
+    auto map_2_translation_x = op.add<popl::Value<double>>("", "map-translation-x", "X coordinate value to translate map-db2");
+    auto map_2_translation_y = op.add<popl::Value<double>>("", "map-translation-y", "Y coordinate value to translate map-db2");
+    auto map_2_translation_z = op.add<popl::Value<double>>("", "map-translation-z", "Z coordinate value to translate map-db2");
     auto mapping = op.add<popl::Switch>("", "mapping", "perform mapping as well as localization");
     auto mask_img_path = op.add<popl::Value<std::string>>("", "mask", "mask image path", "");
     auto frame_skip = op.add<popl::Value<unsigned int>>("", "frame-skip", "interval of frame skip", 1);
@@ -197,15 +226,46 @@ int main(int argc, char* argv[]) {
         return EXIT_FAILURE;
     }
 
+    // transformation of second map
+    if (!map_db_path2->is_set()) {
+        map_db_path2->set_value("");
+    }
+    if (!map_2_scale->is_set()) {
+        map_2_scale->set_value(1.0);
+    }
+    if (!map_2_rotation_x->is_set()) {
+        map_2_rotation_x->set_value(0);
+    }
+    if (!map_2_rotation_y->is_set()) {
+        map_2_rotation_y->set_value(0);
+    }
+    if (!map_2_rotation_z->is_set()) {
+        map_2_rotation_z->set_value(0);
+    }
+    if (!map_2_translation_x->is_set()) {
+        map_2_translation_x->set_value(0);
+    }
+    if (!map_2_translation_y->is_set()) {
+        map_2_translation_y->set_value(0);
+    }
+    if (!map_2_translation_z->is_set()) {
+        map_2_translation_z->set_value(0);
+    }
+
+    std::vector<double> rotation_xyz = {map_2_rotation_x->value(), map_2_rotation_y->value(), map_2_rotation_z->value()};
+    std::vector<double> translation_xyz = {map_2_translation_x->value(), map_2_translation_y->value(), map_2_translation_z->value()};
+
+
 #ifdef USE_GOOGLE_PERFTOOLS
     ProfilerStart("slam.prof");
 #endif
 
     // run localization
     if (cfg->camera_->setup_type_ == openvslam::camera::setup_type_t::Monocular) {
         mono_localization(cfg, vocab_file_path->value(), video_file_path->value(), mask_img_path->value(),
-                          map_db_path->value(), mapping->is_set(),
-                          frame_skip->value(), no_sleep->is_set(), auto_term->is_set());
+                          map_db_path->value(), map_db_path2->value(), mapping->is_set(),
+                          frame_skip->value(), no_sleep->is_set(), auto_term->is_set(), 
+                          map_2_scale->value(), rotation_xyz, translation_xyz);
     }
     else {
         throw std::runtime_error("Invalid setup type: " + cfg->camera_->get_setup_type_string());

src/openvslam/data/map_database.cc

@@ -252,6 +252,84 @@ void map_database::from_json(camera_database* cam_db, orb_params_database* orb_p
     }
 }
 
+void map_database::add_from_json(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab,
+                             const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks) {
+    std::lock_guard<std::mutex> lock(mtx_map_access_);
+
+    // When loading the map, leave last_inserted_keyfrm_ as nullptr.
+    last_inserted_keyfrm_ = nullptr;
+    local_landmarks_.clear();
+    origin_keyfrm_ = nullptr;
+
+    // Step 1. Register keyframes
+    // If the object does not exist at this step, the corresponding pointer is set as nullptr.
+    spdlog::info("decoding {} keyframes to load", json_keyfrms.size());
+    for (const auto& json_id_keyfrm : json_keyfrms.items()) {
+        const auto id = std::stoi(json_id_keyfrm.key());
+        assert(0 <= id);
+        const auto json_keyfrm = json_id_keyfrm.value();
+
+        register_keyframe(cam_db, orb_params_db, bow_vocab, id, json_keyfrm);
+    }
+
+    // Step 2. Register 3D landmark point
+    // If the object does not exist at this step, the corresponding pointer is set as nullptr.
+    spdlog::info("decoding {} landmarks to load", json_landmarks.size());
+    for (const auto& json_id_landmark : json_landmarks.items()) {
+        const auto id = std::stoi(json_id_landmark.key());
+        assert(0 <= id);
+        const auto json_landmark = json_id_landmark.value();
+
+        register_landmark(id, json_landmark);
+    }
+
+    // Step 3. Register graph information
+    spdlog::info("registering essential graph");
+    for (const auto& json_id_keyfrm : json_keyfrms.items()) {
+        const auto id = std::stoi(json_id_keyfrm.key());
+        assert(0 <= id);
+        const auto json_keyfrm = json_id_keyfrm.value();
+
+        register_graph(id, json_keyfrm);
+    }
+
+    // Step 4. Register association between keyframs and 3D points
+    spdlog::info("registering keyframe-landmark association");
+    for (const auto& json_id_keyfrm : json_keyfrms.items()) {
+        const auto id = std::stoi(json_id_keyfrm.key());
+        assert(0 <= id);
+        const auto json_keyfrm = json_id_keyfrm.value();
+
+        register_association(id, json_keyfrm);
+    }
+
+    // Step 5. Update graph
+    spdlog::info("updating covisibility graph");
+    for (const auto& json_id_keyfrm : json_keyfrms.items()) {
+        const auto id = std::stoi(json_id_keyfrm.key());
+        assert(0 <= id);
+
+        assert(keyframes_.count(id));
+        auto keyfrm = keyframes_.at(id);
+
+        keyfrm->graph_node_->update_connections();
+        keyfrm->graph_node_->update_covisibility_orders();
+    }
+
+    // Step 6. Update geometry
+    spdlog::info("updating landmark geometry");
+    for (const auto& json_id_landmark : json_landmarks.items()) {
+        const auto id = std::stoi(json_id_landmark.key());
+        assert(0 <= id);
+
+        assert(landmarks_.count(id));
+        const auto& lm = landmarks_.at(id);
+
+        lm->update_mean_normal_and_obs_scale_variance();
+        lm->compute_descriptor();
+    }
+}
+
 void map_database::register_keyframe(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab,
                                      const unsigned int id, const nlohmann::json& json_keyfrm) {
     // Metadata

src/openvslam/data/map_database.h

@@ -173,6 +173,18 @@ class map_database {
     void from_json(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab,
                    const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks);
 
+
+    /**
+     * Load keyframes and landmarks from JSON without deleting previous ones
+     * @param cam_db
+     * @param orb_params_db
+     * @param bow_vocab
+     * @param json_keyfrms
+     * @param json_landmarks
+     */
+    void add_from_json(camera_database* cam_db, orb_params_database* orb_params_db, bow_vocabulary* bow_vocab,
+                       const nlohmann::json& json_keyfrms, const nlohmann::json& json_landmarks);
+
     /**
      * Dump keyframes and landmarks as JSON
      * @param json_keyfrms

src/openvslam/io/map_database_io.cc

@@ -6,6 +6,7 @@
 #include "openvslam/data/bow_database.h"
 #include "openvslam/data/map_database.h"
 #include "openvslam/io/map_database_io.h"
+#include "openvslam/data/common.h"
 
 #include <spdlog/spdlog.h>
 #include <nlohmann/json.hpp>
@@ -103,5 +104,163 @@ void map_database_io::load_message_pack(const std::string& path) {
     }
 }
 
+void map_database_io::load_new_message_pack(const std::string& path, const Mat44_t transf_matrix, float scale_factor) {
+    std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);
+
+    // 1. Check if exists a map databse already
+    assert(cam_db_ && map_db_ && bow_db_ && bow_vocab_);
+
+    // 2. Load binary bytes
+
+    std::ifstream ifs(path, std::ios::in | std::ios::binary);
+    if (!ifs.is_open()) {
+        spdlog::critical("cannot load the file at {}", path);
+        throw std::runtime_error("cannot load the file at " + path);
+    }
+
+    spdlog::info("load the MessagePack file of database from {}", path);
+    std::vector<uint8_t> msgpack;
+    while (true) {
+        uint8_t buffer;
+        ifs.read(reinterpret_cast<char*>(&buffer), sizeof(uint8_t));
+        if (ifs.eof()) {
+            break;
+        }
+        msgpack.push_back(buffer);
+    }
+    ifs.close();
+
+    // 3. parse into JSON
+
+    const auto json = nlohmann::json::from_msgpack(msgpack);
+
+    // 4. load database
+
+    // add the new cameras to 'cam_db'
+    const auto json_cameras = json.at("cameras");
+    cam_db_->from_json(json_cameras);
+
+    // add the new orb_params to 'orb_params_db'
+    const auto json_orb_params = json.at("orb_params");
+    orb_params_db_->from_json(json_orb_params);
+
+    // shift keyframes and landmarks ids to not collide with first map loaded
+    const auto json_keyfrms = json.at("keyframes");
+    const auto json_landmarks = json.at("landmarks");
+
+    // create temporaly json to sotre the new values
+    nlohmann::json tmp_json_keyframes;
+    nlohmann::json tmp_json_landmarks;
+
+    // Translation anb rotation
+    Mat33_t rotation_matrix = transf_matrix.block<3,3>(0,0);
+    Vec3_t translation = transf_matrix.block<3,1>(0,3);
+
+    // change values of keyframes
+    for (auto& [keyfrm_id, json_keyfrm] : json_keyfrms.items()) {
+        nlohmann::json tmp_json_kyfrm;
+
+        // Transform keyframes
+        if (json_keyfrm.contains("rot_cw")) {
+            Mat33_t rot_wc = openvslam::data::convert_json_to_rotation(json_keyfrm["rot_cw"]).transpose();
+            Vec3_t trans_wc = (-rot_wc) * openvslam::data::convert_json_to_translation(json_keyfrm["trans_cw"]);
+            Vec3_t new_position = (scale_factor * rotation_matrix) * trans_wc + translation;
+            nlohmann::json new_rot_cw =  openvslam::data::convert_rotation_to_json((rotation_matrix * rot_wc).transpose());
+            tmp_json_kyfrm["rot_cw"] = std::move(new_rot_cw);
+            nlohmann::json new_trans_cw =  openvslam::data::convert_translation_to_json((-rotation_matrix * rot_wc).inverse() * new_position);
+            tmp_json_kyfrm["trans_cw"] = std::move(new_trans_cw);
+        }
+        else {
+            tmp_json_kyfrm["trans_cw"] = std::move(json_keyfrm["trans_cw"]);
+            tmp_json_kyfrm["rot_cw"] = std::move(json_keyfrm["rot_cw"]);
+        }
+        // shift ids of landmarks associated to keyframe
+        std::vector<int> new_landmarks_ids;
+        new_landmarks_ids.reserve(json_keyfrm["lm_ids"].size());
+        for (auto& landmark_id : json_keyfrm["lm_ids"].get<std::vector<int>>()) {
+            if (landmark_id != -1) {
+                new_landmarks_ids.push_back(landmark_id + data::landmark::next_id_);
+            } else {
+                new_landmarks_ids.push_back(landmark_id);
+            }
+        }
+        tmp_json_kyfrm["lm_ids"] = std::move(new_landmarks_ids);
+
+        // shift id of frame associated to keyframe
+        tmp_json_kyfrm["src_frm_id"] = json_keyfrm["src_frm_id"].get<unsigned int>() + data::frame::next_id_;
+
+        // shift id of 'span_parent'
+        tmp_json_kyfrm["span_parent"] = json_keyfrm["span_parent"].get<int>() + data::keyframe::next_id_;
+
+        // shift ids of 'span_children'
+        std::vector<int> new_children_ids;
+        new_children_ids.reserve(json_keyfrm["span_children"].size());
+        for (const auto& keyframe_id : json_keyfrm["span_children"].get<std::vector<int>>()) {
+            new_children_ids.push_back(keyframe_id + data::keyframe::next_id_);
+        }
+        tmp_json_kyfrm["span_children"] = std::move(new_children_ids);
+
+        // shift ids of 'loop_edges'
+        std::vector<int> new_loop_edges_ids;
+        new_loop_edges_ids.reserve(json_keyfrm["loop_edges"].size());
+        for (const auto& loop_edge_id : json_keyfrm["loop_edges"].get<std::vector<int>>()) {
+            new_loop_edges_ids.push_back(loop_edge_id + data::keyframe::next_id_);
+        }
+        tmp_json_kyfrm["loop_edges"] = std::move(new_loop_edges_ids);
+
+        // copy other values unchanged
+        tmp_json_kyfrm["cam"] = std::move(json_keyfrm["cam"]);
+        tmp_json_kyfrm["depth_thr"] = std::move(json_keyfrm["depth_thr"]);
+        tmp_json_kyfrm["depths"] = std::move(json_keyfrm["depths"]);
+        tmp_json_kyfrm["descs"] = std::move(json_keyfrm["descs"]);
+        tmp_json_kyfrm["keypts"] = std::move(json_keyfrm["keypts"]);
+        tmp_json_kyfrm["n_keypts"] = std::move(json_keyfrm["n_keypts"]);
+        tmp_json_kyfrm["n_scale_levels"] = std::move(json_keyfrm["n_scale_levels"]);
+        tmp_json_kyfrm["orb_params"] = std::move(json_keyfrm["orb_params"]);
+        tmp_json_kyfrm["scale_factor"] = std::move(json_keyfrm["scale_factor"]);
+        tmp_json_kyfrm["ts"] = std::move(json_keyfrm["ts"]);
+        tmp_json_kyfrm["undists"] = std::move(json_keyfrm["undists"]);
+        tmp_json_kyfrm["x_rights"] = std::move(json_keyfrm["x_rights"]);
+
+        // finally, move modified keyframe value to temporal json with the new id
+        tmp_json_keyframes[std::to_string(std::stoi(keyfrm_id) + data::keyframe::next_id_)] = std::move(tmp_json_kyfrm);
+    }
+
+    // change values of landmarks
+    for (auto& [landmark_id, json_landmark] : json_landmarks.items()) {
+        nlohmann::json tmp_json_ldmrk;
+
+        // transform the position in world
+        tmp_json_ldmrk["pos_w"] = std::move(openvslam::data::convert_translation_to_json((scale_factor * rotation_matrix)*openvslam::data::convert_json_to_translation(json_landmark["pos_w"]) + translation));
+
+        // shift 'ref_keyfrm' id
+        tmp_json_ldmrk["ref_keyfrm"] = json_landmark["ref_keyfrm"].get<int>() + data::keyframe::next_id_;
+
+        // shift '1st_keyfrm' id
+        tmp_json_ldmrk["1st_keyfrm"] = json_landmark["1st_keyfrm"].get<int>() + data::keyframe::next_id_;
+
+        // copy other values unchanged
+        tmp_json_ldmrk["n_vis"] = std::move(json_landmark.at("n_vis"));
+        tmp_json_ldmrk["n_fnd"] = std::move(json_landmark.at("n_fnd"));
+
+        // finally, move modified landmark value to temporal json with the new id
+        tmp_json_landmarks[std::to_string(std::stoi(landmark_id) + data::landmark::next_id_)] = std::move(tmp_json_ldmrk);
+    }
+
+    // increase static variables
+    data::frame::next_id_    += json.at("frame_next_id").get<unsigned int>();
+    data::keyframe::next_id_ += json.at("keyframe_next_id").get<unsigned int>();
+    data::landmark::next_id_ += json.at("landmark_next_id").get<unsigned int>();
+
+    // add to database with the new keyfrms and landmarks
+    map_db_->add_from_json(cam_db_, orb_params_db_, bow_vocab_, tmp_json_keyframes, tmp_json_landmarks);
+    const auto keyfrms = map_db_->get_all_keyframes();
+    for (const auto keyfrm : keyfrms) {
+        bow_db_->add_keyframe(keyfrm);
+
+    }
+}
+
+
 } // namespace io
 } // namespace openvslam

src/openvslam/io/map_database_io.h

@@ -38,6 +38,11 @@ class map_database_io {
      */
     void load_message_pack(const std::string& path);
 
+    /**
+     * Load a map database from MessagePack without deleting a previous one
+     */
+    void load_new_message_pack(const std::string& path, const Mat44_t transf_matrix = Mat44_t::Identity() , float scale_factor = 1.0);
+
 private:
     //! camera database
     data::camera_database* const cam_db_ = nullptr;