diff --git a/example/run_video_localization.cc b/example/run_video_localization.cc index fc0b40164..0b50873bd 100644 --- a/example/run_video_localization.cc +++ b/example/run_video_localization.cc @@ -8,6 +8,7 @@ #include "openvslam/config.h" #include "openvslam/util/yaml.h" +#include #include #include #include @@ -28,8 +29,9 @@ void mono_localization(const std::shared_ptr& 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 rotation_xyz, const std::vector translation_xyz) { // load the mask image const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE); @@ -37,6 +39,25 @@ void mono_localization(const std::shared_ptr& cfg, 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>("m", "video", "video file path"); auto config_file_path = op.add>("c", "config", "config file path"); auto map_db_path = op.add>("p", "map-db", "path to a prebuilt map database"); + auto map_db_path2 = op.add>("", "map-db2", "path to another prebuilt map database"); + auto map_2_scale = op.add>("s", "map-scale", "path to another prebuilt map database"); + auto map_2_rotation_x = op.add>("", "map-rotation-x", "Euler angle of X coordinate value to rotate map-db2 in rad"); + auto map_2_rotation_y = op.add>("", "map-rotation-y", "Euler angle of Y coordinate value to rotate map-db2 in rad"); + auto map_2_rotation_z = op.add>("", "map-rotation-z", "Euler angle of Z coordinate value to rotate map-db2 in rad"); + auto map_2_translation_x = op.add>("", "map-translation-x", "X coordinate value to translate map-db2"); + auto map_2_translation_y = op.add>("", "map-translation-y", "Y coordinate value to translate map-db2"); + auto map_2_translation_z = op.add>("", "map-translation-z", "Z coordinate value to translate map-db2"); auto mapping = op.add("", "mapping", "perform mapping as well as localization"); auto mask_img_path = op.add>("", "mask", "mask image path", ""); auto frame_skip = op.add>("", "frame-skip", "interval of frame skip", 1); @@ -197,6 +226,36 @@ 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 rotation_xyz = {map_2_rotation_x->value(), map_2_rotation_y->value(), map_2_rotation_z->value()}; + std::vector 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 @@ -204,8 +263,9 @@ int main(int argc, char* argv[]) { // 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()); diff --git a/src/openvslam/data/map_database.cc b/src/openvslam/data/map_database.cc index 9d2a06e9e..e2728a355 100644 --- a/src/openvslam/data/map_database.cc +++ b/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 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 diff --git a/src/openvslam/data/map_database.h b/src/openvslam/data/map_database.h index 57100bffa..9c546e1c7 100644 --- a/src/openvslam/data/map_database.h +++ b/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 diff --git a/src/openvslam/io/map_database_io.cc b/src/openvslam/io/map_database_io.cc index f0e2c4ddf..c8bd73089 100644 --- a/src/openvslam/io/map_database_io.cc +++ b/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 #include @@ -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 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 msgpack; + while (true) { + uint8_t buffer; + ifs.read(reinterpret_cast(&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 new_landmarks_ids; + new_landmarks_ids.reserve(json_keyfrm["lm_ids"].size()); + for (auto& landmark_id : json_keyfrm["lm_ids"].get>()) { + 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() + data::frame::next_id_; + + // shift id of 'span_parent' + tmp_json_kyfrm["span_parent"] = json_keyfrm["span_parent"].get() + data::keyframe::next_id_; + + // shift ids of 'span_children' + std::vector new_children_ids; + new_children_ids.reserve(json_keyfrm["span_children"].size()); + for (const auto& keyframe_id : json_keyfrm["span_children"].get>()) { + 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 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>()) { + 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() + data::keyframe::next_id_; + + // shift '1st_keyfrm' id + tmp_json_ldmrk["1st_keyfrm"] = json_landmark["1st_keyfrm"].get() + 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(); + data::keyframe::next_id_ += json.at("keyframe_next_id").get(); + data::landmark::next_id_ += json.at("landmark_next_id").get(); + + // 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 diff --git a/src/openvslam/io/map_database_io.h b/src/openvslam/io/map_database_io.h index bfc998b8f..d75723e63 100644 --- a/src/openvslam/io/map_database_io.h +++ b/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; diff --git a/src/openvslam/system.cc b/src/openvslam/system.cc index 8bfbf4f51..89e03ade6 100644 --- a/src/openvslam/system.cc +++ b/src/openvslam/system.cc @@ -232,6 +232,13 @@ void system::load_map_database(const std::string& path) const { resume_other_threads(); } +void system::load_new_map_database(const std::string& path, const Mat44_t transf_matrix, float scale_factor) const { + pause_other_threads(); + io::map_database_io map_db_io(cam_db_, orb_params_db_, map_db_, bow_db_, bow_vocab_); + map_db_io.load_new_message_pack(path, transf_matrix , scale_factor); + resume_other_threads(); +} + void system::save_map_database(const std::string& path) const { pause_other_threads(); io::map_database_io map_db_io(cam_db_, orb_params_db_, map_db_, bow_db_, bow_vocab_); diff --git a/src/openvslam/system.h b/src/openvslam/system.h index bb953890b..929e123bd 100644 --- a/src/openvslam/system.h +++ b/src/openvslam/system.h @@ -70,6 +70,9 @@ class system { //! Load the map database from the MessagePack file void load_map_database(const std::string& path) const; + + //! Load a new map database from the MessagePack file without deleting a precious one + void load_new_map_database(const std::string& path, const Mat44_t transf_matrix = Mat44_t::Identity() , float scale_factor = 1.0) const; //! Save the map database to the MessagePack file void save_map_database(const std::string& path) const;