From f238a0f5ef9748e5b8eb00610ae82262aa15c586 Mon Sep 17 00:00:00 2001 From: Yun Chang Date: Sun, 29 Oct 2023 09:31:17 -0400 Subject: [PATCH 1/2] Feature/save load (#10) * initial save utils * start adding load utils * update CMakeLists and some clean up * json serialization * fix json load VLC * fix serialization vlc frames * add load save in new types * small fix --- CMakeLists.txt | 43 +++---- cmake/json.CMakeLists.txt.in | 17 +++ cmake/vocab_download.cmake | 21 ++++ include/kimera_multi_lcd/io.h | 44 +++++++ ...dThirdPartyWrapper.h => lcd_third_party.h} | 0 ...sureDetector.h => loop_closure_detector.h} | 9 +- include/kimera_multi_lcd/serializer.h | 16 +++ include/kimera_multi_lcd/types.h | 1 + include/kimera_multi_lcd/utils.h | 2 + src/io.cpp | 118 ++++++++++++++++++ ...rdPartyWrapper.cpp => lcd_third_party.cpp} | 6 +- ...Detector.cpp => loop_closure_detector.cpp} | 24 +++- src/serializer.cpp | 102 +++++++++++++++ src/utils.cpp | 2 + 14 files changed, 373 insertions(+), 32 deletions(-) create mode 100644 cmake/json.CMakeLists.txt.in create mode 100644 cmake/vocab_download.cmake create mode 100644 include/kimera_multi_lcd/io.h rename include/kimera_multi_lcd/{LcdThirdPartyWrapper.h => lcd_third_party.h} (100%) rename include/kimera_multi_lcd/{LoopClosureDetector.h => loop_closure_detector.h} (96%) create mode 100644 include/kimera_multi_lcd/serializer.h create mode 100644 src/io.cpp rename src/{LcdThirdPartyWrapper.cpp => lcd_third_party.cpp} (98%) rename src/{LoopClosureDetector.cpp => loop_closure_detector.cpp} (96%) create mode 100644 src/serializer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dfbe937..82dee56 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,14 +30,30 @@ if(NOT TARGET DBoW2::DBoW2) endif() find_package(PCL REQUIRED) +configure_file(cmake/json.CMakeLists.txt.in json-download/CMakeLists.txt) +execute_process( + COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/json-download" OUTPUT_QUIET +) +execute_process( + COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/json-download" OUTPUT_QUIET +) + +set(JSON_BuildTests OFF CACHE INTERNAL "") +set(JSON_Install OFF CACHE INTERNAL "") +add_subdirectory(${CMAKE_BINARY_DIR}/json-src ${CMAKE_BINARY_DIR}/json-build) + catkin_simple() cs_add_library(${PROJECT_NAME} - src/LoopClosureDetector.cpp - src/LcdThirdPartyWrapper.cpp + src/loop_closure_detector.cpp + src/lcd_third_party.cpp src/types.cpp src/utils.cpp + src/serializer.cpp + src/io.cpp ) target_link_libraries(${PROJECT_NAME} @@ -49,29 +65,10 @@ target_link_libraries(${PROJECT_NAME} DBoW2::DBoW2 tbb glog + nlohmann_json::nlohmann_json ) -### Download and unzip the vocabularly file -if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/vocab/mit_voc.yml) - message(STATUS "Downloading vocabulary file from drive.") - file(DOWNLOAD - https://drive.google.com/uc?export=download&confirm=9iBg&id=1N4y0HbgA3PHQ73ZxFJvy5dgvV_0cTBYF - ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip - SHOW_PROGRESS - STATUS voc_download_success - TIMEOUT 60) - if(voc_download_success) - message(STATUS "Unzipping vocabulary file.") - - execute_process(COMMAND ${CMAKE_COMMAND} -E tar xzf ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/) - execute_process(COMMAND ${CMAKE_COMMAND} -E remove ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip) - else(voc_download_success) - message(STATUS "Failed to download vocabulary file. Please download manually.") - endif(voc_download_success) -else() - message(STATUS "Vocabulary file exists, will not download.") -endif() +include(cmake/vocab_download.cmake) # Unit tests # set(TEST_DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/data") diff --git a/cmake/json.CMakeLists.txt.in b/cmake/json.CMakeLists.txt.in new file mode 100644 index 0000000..31889c4 --- /dev/null +++ b/cmake/json.CMakeLists.txt.in @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.1) + +project(json_download NONE) + +include(ExternalProject) +ExternalProject_Add( + json + URL https://github.com/nlohmann/json/archive/refs/tags/v3.10.5.zip + # GIT_REPOSITORY "https://github.com/nlohmann/json.git" GIT_TAG "v3.9.1" GIT_SHALLOW + # TRUE + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/json-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/json-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/cmake/vocab_download.cmake b/cmake/vocab_download.cmake new file mode 100644 index 0000000..eef7615 --- /dev/null +++ b/cmake/vocab_download.cmake @@ -0,0 +1,21 @@ +### Download and unzip the vocabularly file +if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/vocab/mit_voc.yml) + message(STATUS "Downloading vocabulary file from drive.") + file(DOWNLOAD + https://drive.google.com/uc?export=download&confirm=9iBg&id=1N4y0HbgA3PHQ73ZxFJvy5dgvV_0cTBYF + ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip + SHOW_PROGRESS + STATUS voc_download_success + TIMEOUT 60) + if(voc_download_success) + message(STATUS "Unzipping vocabulary file.") + + execute_process(COMMAND ${CMAKE_COMMAND} -E tar xzf ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/) + execute_process(COMMAND ${CMAKE_COMMAND} -E remove ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip) + else(voc_download_success) + message(STATUS "Failed to download vocabulary file. Please download manually.") + endif(voc_download_success) +else() + message(STATUS "Vocabulary file exists, will not download.") +endif() \ No newline at end of file diff --git a/include/kimera_multi_lcd/io.h b/include/kimera_multi_lcd/io.h new file mode 100644 index 0000000..59914cd --- /dev/null +++ b/include/kimera_multi_lcd/io.h @@ -0,0 +1,44 @@ +/* + * Copyright Notes + * + * Authors: Yun Chang (yunchang@mit.edu) + */ + +#pragma once + +#include +#include + +#include + +#include "kimera_multi_lcd/types.h" + +namespace kimera_multi_lcd { + +// Save BoW vectors +void saveBowVectors(const std::map& bow_vectors, + const std::string& filename); + +void saveBowVectors(const std::map& bow_vectors, + const std::string& filename); + +// Save VLC Frames +void saveVLCFrames(const std::map& vlc_frames, + const std::string& filename); + +void saveVLCFrames(const std::map& vlc_frames, + const std::string& filename); + +// Save BoW vectors +void loadBowVectors(const std::string& filename, + std::map& bow_vectors); + +void loadBowVectors(const std::string& filename, + std::map& bow_vectors); + +// Save VLC Frames +void loadVLCFrames(const std::string& filename, + std::map& vlc_frames); + +void loadVLCFrames(const std::string& filename, std::map& vlc_frames); +} // namespace kimera_multi_lcd diff --git a/include/kimera_multi_lcd/LcdThirdPartyWrapper.h b/include/kimera_multi_lcd/lcd_third_party.h similarity index 100% rename from include/kimera_multi_lcd/LcdThirdPartyWrapper.h rename to include/kimera_multi_lcd/lcd_third_party.h diff --git a/include/kimera_multi_lcd/LoopClosureDetector.h b/include/kimera_multi_lcd/loop_closure_detector.h similarity index 96% rename from include/kimera_multi_lcd/LoopClosureDetector.h rename to include/kimera_multi_lcd/loop_closure_detector.h index cc977ad..6c3218f 100644 --- a/include/kimera_multi_lcd/LoopClosureDetector.h +++ b/include/kimera_multi_lcd/loop_closure_detector.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -124,8 +124,12 @@ class LoopClosureDetector { DBoW2::BowVector getBoWVector(const RobotPoseId& id) const; + PoseBowVector getBoWVectors(const RobotId& robot_id) const; + VLCFrame getVLCFrame(const RobotPoseId& id) const; + std::map getVLCFrames(const RobotId& robot_id) const; + inline bool frameExists(const RobotPoseId& id) const { return vlc_frames_.find(id) != vlc_frames_.end(); } @@ -157,8 +161,7 @@ class LoopClosureDetector { size_t total_geometric_verifications_; // Database of BOW vectors from each robot (trajectory) - std::unordered_map> - bow_vectors_; + std::unordered_map bow_vectors_; std::unordered_map> db_BoW_; // Keep track of latest pose Id with BoW for each robot std::unordered_map bow_latest_pose_id_; diff --git a/include/kimera_multi_lcd/serializer.h b/include/kimera_multi_lcd/serializer.h new file mode 100644 index 0000000..c5d1234 --- /dev/null +++ b/include/kimera_multi_lcd/serializer.h @@ -0,0 +1,16 @@ +#pragma once +#include +#include + +#include + +namespace pose_graph_tools { + +void to_json(nlohmann::json& j, const pose_graph_tools::BowVector& bow_vector); + +void from_json(const nlohmann::json& j, pose_graph_tools::BowVector& bow_vector); + +void to_json(nlohmann::json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame); + +void from_json(const nlohmann::json& j, pose_graph_tools::VLCFrameMsg& vlc_frame); +} // namespace pose_graph_tools diff --git a/include/kimera_multi_lcd/types.h b/include/kimera_multi_lcd/types.h index 1783c9b..e283549 100644 --- a/include/kimera_multi_lcd/types.h +++ b/include/kimera_multi_lcd/types.h @@ -27,6 +27,7 @@ typedef size_t PoseId; typedef std::pair RobotPoseId; typedef std::set RobotPoseIdSet; typedef std::vector RobotPoseIdVector; +typedef std::map PoseBowVector; // Each edge in the pose graph is uniquely identified by four integers // (robot_src, frame_src, robot_dst, frame_dst) diff --git a/include/kimera_multi_lcd/utils.h b/include/kimera_multi_lcd/utils.h index 6da4393..e67085a 100644 --- a/include/kimera_multi_lcd/utils.h +++ b/include/kimera_multi_lcd/utils.h @@ -14,6 +14,8 @@ #include #include +#include + #include "kimera_multi_lcd/types.h" namespace kimera_multi_lcd { diff --git a/src/io.cpp b/src/io.cpp new file mode 100644 index 0000000..cfbb585 --- /dev/null +++ b/src/io.cpp @@ -0,0 +1,118 @@ +/* + * Copyright Notes + * + * Authors: Yun Chang (yunchang@mit.edu) + */ + +#include "kimera_multi_lcd/io.h" + +#include + +#include "kimera_multi_lcd/serializer.h" +#include "kimera_multi_lcd/utils.h" + +namespace kimera_multi_lcd { + +using nlohmann::json; + +void saveBowVectors(const std::map& bow_vectors, + const std::string& filename) { + std::ofstream outfile(filename); + json record; + record["ids"] = json::array(); + record["bow_vectors"] = json::array(); + for (const auto& id_bowvec : bow_vectors) { + record["ids"].push_back(id_bowvec.first); + record["bow_vectors"].push_back(id_bowvec.second); + } + outfile << record.dump(); +} + +void saveBowVectors(const std::map& bow_vectors, + const std::string& filename) { + std::map pg_bow_vectors; + for (const auto& id_bow : bow_vectors) { + pg_bow_vectors[id_bow.first] = pose_graph_tools::BowVector(); + BowVectorToMsg(id_bow.second, &pg_bow_vectors[id_bow.first]); + } + saveBowVectors(pg_bow_vectors, filename); +} + +void saveVLCFrames(const std::map& vlc_frames, + const std::string& filename) { + std::ofstream outfile(filename); + json record; + record["ids"] = json::array(); + record["frames"] = json::array(); + for (const auto& id_frame : vlc_frames) { + record["ids"].push_back(id_frame.first); + record["frames"].push_back(id_frame.second); + } + outfile << record.dump(); +} + +void saveVLCFrames(const std::map& vlc_frames, + const std::string& filename) { + std::map pg_vlc_frames; + for (const auto& id_frame : vlc_frames) { + pg_vlc_frames[id_frame.first] = pose_graph_tools::VLCFrameMsg(); + VLCFrameToMsg(id_frame.second, &pg_vlc_frames[id_frame.first]); + } + saveVLCFrames(pg_vlc_frames, filename); +} + +// Save BoW vectors +void loadBowVectors(const std::string& filename, + std::map& bow_vectors) { + std::stringstream ss; + std::ifstream infile(filename); + if (!infile) { + throw std::runtime_error(filename + " does not exist"); + } + ss << infile.rdbuf(); + + const auto record = json::parse(ss.str()); + + for (size_t i = 0; i < record.at("ids").size(); i++) { + bow_vectors[record.at("ids").at(i)] = record.at("bow_vectors").at(i); + } +} + +void loadBowVectors(const std::string& filename, + std::map& bow_vectors) { + std::map pg_bow_vectors; + loadBowVectors(filename, pg_bow_vectors); + + for (const auto& id_bow : pg_bow_vectors) { + bow_vectors[id_bow.first] = DBoW2::BowVector(); + BowVectorFromMsg(id_bow.second, &bow_vectors[id_bow.first]); + } +} + +// Save VLC Frames +void loadVLCFrames(const std::string& filename, + std::map& vlc_frames) { + std::stringstream ss; + std::ifstream infile(filename); + if (!infile) { + throw std::runtime_error(filename + " does not exist"); + } + ss << infile.rdbuf(); + + const auto record = json::parse(ss.str()); + + for (size_t i = 0; i < record.at("ids").size(); i++) { + vlc_frames[record.at("ids").at(i)] = record.at("frames").at(i); + } +} + +void loadVLCFrames(const std::string& filename, + std::map& vlc_frames) { + std::map pg_vlc_frames; + loadVLCFrames(filename, pg_vlc_frames); + for (const auto& id_frame : pg_vlc_frames) { + vlc_frames[id_frame.first] = VLCFrame(); + VLCFrameFromMsg(id_frame.second, &vlc_frames[id_frame.first]); + } +} +} // namespace kimera_multi_lcd \ No newline at end of file diff --git a/src/LcdThirdPartyWrapper.cpp b/src/lcd_third_party.cpp similarity index 98% rename from src/LcdThirdPartyWrapper.cpp rename to src/lcd_third_party.cpp index b738687..b86d493 100644 --- a/src/LcdThirdPartyWrapper.cpp +++ b/src/lcd_third_party.cpp @@ -38,16 +38,16 @@ POSSIBILITY OF SUCH DAMAGE. * -------------------------------------------------------------------------- */ /** - * @file ThirdPartyWrapper.cpp + * @file lcd_third_party.cpp * @brief Wrapper for functions of DLoopDetector (see copyright notice above). * @author Marcus Abate */ +#include "kimera_multi_lcd/lcd_third_party.h" + #include #include -#include "kimera_multi_lcd/LcdThirdPartyWrapper.h" - namespace kimera_multi_lcd { /* ------------------------------------------------------------------------ */ diff --git a/src/LoopClosureDetector.cpp b/src/loop_closure_detector.cpp similarity index 96% rename from src/LoopClosureDetector.cpp rename to src/loop_closure_detector.cpp index bbdf53b..7f05fee 100644 --- a/src/LoopClosureDetector.cpp +++ b/src/loop_closure_detector.cpp @@ -4,9 +4,9 @@ * Authors: Yun Chang (yunchang@mit.edu) Yulun Tian (yulun@mit.edu) */ -#include - #include +#include + #include #include #include @@ -101,12 +101,30 @@ DBoW2::BowVector LoopClosureDetector::getBoWVector( return bow_vectors_.at(robot_id).at(pose_id); } +PoseBowVector LoopClosureDetector::getBoWVectors(const RobotId& robot_id) const { + if (!bow_vectors_.count(robot_id)) { + return PoseBowVector(); + } + return bow_vectors_.at(robot_id); +} + VLCFrame LoopClosureDetector::getVLCFrame( const kimera_multi_lcd::RobotPoseId& id) const { CHECK(frameExists(id)); return vlc_frames_.at(id); } +std::map LoopClosureDetector::getVLCFrames( + const RobotId& robot_id) const { + std::map vlc_frames; + for (const auto& robot_pose_id_vlc : vlc_frames_) { + if (robot_pose_id_vlc.first.first == robot_id) { + vlc_frames[robot_pose_id_vlc.first.second] = robot_pose_id_vlc.second; + } + } + return vlc_frames; +} + void LoopClosureDetector::addBowVector(const RobotPoseId& id, const DBoW2::BowVector& bow_vector) { const size_t robot_id = id.first; @@ -115,7 +133,7 @@ void LoopClosureDetector::addBowVector(const RobotPoseId& id, if (bowExists(id)) return; if (db_BoW_.find(robot_id) == db_BoW_.end()) { db_BoW_[robot_id] = std::unique_ptr(new OrbDatabase(vocab_)); - bow_vectors_[robot_id] = std::unordered_map(); + bow_vectors_[robot_id] = PoseBowVector(); db_EntryId_to_PoseId_[robot_id] = std::unordered_map(); bow_latest_pose_id_[robot_id] = pose_id; ROS_INFO("Initialized BoW for robot %lu.", robot_id); diff --git a/src/serializer.cpp b/src/serializer.cpp new file mode 100644 index 0000000..915e32e --- /dev/null +++ b/src/serializer.cpp @@ -0,0 +1,102 @@ +#include "kimera_multi_lcd/serializer.h" + +#include +#include + +#include + +using json = nlohmann::json; + +namespace pcl { +void to_json(json& j, const pcl::PointXYZ& point) { + j = json{{"x", point.x}, {"y", point.y}, {"z", point.z}}; +} + +void from_json(const json& j, pcl::PointXYZ& point) { + point.x = j.at("x").is_null() ? std::numeric_limits::quiet_NaN() + : j.at("x").get(); + point.y = j.at("y").is_null() ? std::numeric_limits::quiet_NaN() + : j.at("y").get(); + point.z = j.at("z").is_null() ? std::numeric_limits::quiet_NaN() + : j.at("z").get(); +} + +void to_json(json& j, const pcl::PointCloud& points) { + j = json::array(); + for (const auto& point : points) { + json point_json = point; + j.push_back(point_json); + } +} + +void from_json(const json& j, pcl::PointCloud& points) { + for (const auto& point : j) { + points.push_back(point.get()); + } +} + +} // namespace pcl + +namespace pose_graph_tools { + +void to_json(json& j, const pose_graph_tools::BowVector& bow_vector) { + j = json{{"word_ids", bow_vector.word_ids}, {"word_values", bow_vector.word_values}}; +} + +void from_json(const json& j, pose_graph_tools::BowVector& bow_vector) { + j.at("word_ids").get_to(bow_vector.word_ids); + j.at("word_values").get_to(bow_vector.word_values); +} + +void to_json(json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame) { + pcl::PointCloud versors; + pcl::fromROSMsg(vlc_frame.versors, versors); + + j = json{{"robot_id", vlc_frame.robot_id}, + {"pose_id", vlc_frame.pose_id}, + {"submap_id", vlc_frame.submap_id}, + {"descriptors_mat", + {{"height", vlc_frame.descriptors_mat.height}, + {"width", vlc_frame.descriptors_mat.width}, + {"encoding", vlc_frame.descriptors_mat.encoding}, + {"step", vlc_frame.descriptors_mat.step}, + {"data", vlc_frame.descriptors_mat.data}}}, + {"versors", versors}, + {"depths", vlc_frame.depths}, + {"T_submap_pose", + {{"x", vlc_frame.T_submap_pose.position.x}, + {"y", vlc_frame.T_submap_pose.position.y}, + {"z", vlc_frame.T_submap_pose.position.z}, + {"qx", vlc_frame.T_submap_pose.orientation.x}, + {"qy", vlc_frame.T_submap_pose.orientation.y}, + {"qz", vlc_frame.T_submap_pose.orientation.z}, + {"qw", vlc_frame.T_submap_pose.orientation.w}}}}; +} + +void from_json(const json& j, pose_graph_tools::VLCFrameMsg& vlc_frame) { + j.at("robot_id").get_to(vlc_frame.robot_id); + j.at("pose_id").get_to(vlc_frame.pose_id); + j.at("submap_id").get_to(vlc_frame.submap_id); + j.at("descriptors_mat").at("height").get_to(vlc_frame.descriptors_mat.height); + j.at("descriptors_mat").at("width").get_to(vlc_frame.descriptors_mat.width); + j.at("descriptors_mat").at("encoding").get_to(vlc_frame.descriptors_mat.encoding); + j.at("descriptors_mat").at("step").get_to(vlc_frame.descriptors_mat.step); + j.at("descriptors_mat").at("data").get_to(vlc_frame.descriptors_mat.data); + + pcl::PointCloud versors; + j.at("versors").get_to(versors); + pcl::toROSMsg(versors, vlc_frame.versors); + + j.at("depths").get_to(vlc_frame.depths); + + geometry_msgs::Point& T_submap_t = vlc_frame.T_submap_pose.position; + geometry_msgs::Quaternion& T_submap_R = vlc_frame.T_submap_pose.orientation; + j.at("T_submap_pose").at("x").get_to(T_submap_t.x); + j.at("T_submap_pose").at("y").get_to(T_submap_t.y); + j.at("T_submap_pose").at("z").get_to(T_submap_t.z); + j.at("T_submap_pose").at("qx").get_to(T_submap_R.x); + j.at("T_submap_pose").at("qy").get_to(T_submap_R.y); + j.at("T_submap_pose").at("qz").get_to(T_submap_R.z); + j.at("T_submap_pose").at("qw").get_to(T_submap_R.w); +} +} // namespace pose_graph_tools diff --git a/src/utils.cpp b/src/utils.cpp index b6a48b5..460acc7 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -5,6 +5,7 @@ */ #include "kimera_multi_lcd/utils.h" + #include namespace kimera_multi_lcd { @@ -94,4 +95,5 @@ size_t computeVLCFramePayloadBytes(const pose_graph_tools::VLCFrameMsg& msg) { bytes += sizeof(msg.versors.data[0]) * msg.versors.data.size(); return bytes; } + } // namespace kimera_multi_lcd \ No newline at end of file From 215e1f59e1bf91a64ca9c7c176338febd75b87a6 Mon Sep 17 00:00:00 2001 From: Yun Chang Date: Thu, 25 Apr 2024 07:47:27 -0400 Subject: [PATCH 2/2] pose graph updates (#11) --- include/kimera_multi_lcd/io.h | 12 ++++++------ include/kimera_multi_lcd/serializer.h | 16 +++++++-------- include/kimera_multi_lcd/types.h | 6 +++--- include/kimera_multi_lcd/utils.h | 28 +++++++++++++-------------- package.xml | 2 +- src/io.cpp | 20 +++++++++---------- src/serializer.cpp | 12 ++++++------ src/types.cpp | 4 ++-- src/utils.cpp | 18 ++++++++--------- 9 files changed, 59 insertions(+), 59 deletions(-) diff --git a/include/kimera_multi_lcd/io.h b/include/kimera_multi_lcd/io.h index 59914cd..354adb8 100644 --- a/include/kimera_multi_lcd/io.h +++ b/include/kimera_multi_lcd/io.h @@ -6,8 +6,8 @@ #pragma once -#include -#include +#include +#include #include @@ -16,14 +16,14 @@ namespace kimera_multi_lcd { // Save BoW vectors -void saveBowVectors(const std::map& bow_vectors, +void saveBowVectors(const std::map& bow_vectors, const std::string& filename); void saveBowVectors(const std::map& bow_vectors, const std::string& filename); // Save VLC Frames -void saveVLCFrames(const std::map& vlc_frames, +void saveVLCFrames(const std::map& vlc_frames, const std::string& filename); void saveVLCFrames(const std::map& vlc_frames, @@ -31,14 +31,14 @@ void saveVLCFrames(const std::map& vlc_frames, // Save BoW vectors void loadBowVectors(const std::string& filename, - std::map& bow_vectors); + std::map& bow_vectors); void loadBowVectors(const std::string& filename, std::map& bow_vectors); // Save VLC Frames void loadVLCFrames(const std::string& filename, - std::map& vlc_frames); + std::map& vlc_frames); void loadVLCFrames(const std::string& filename, std::map& vlc_frames); } // namespace kimera_multi_lcd diff --git a/include/kimera_multi_lcd/serializer.h b/include/kimera_multi_lcd/serializer.h index c5d1234..37a7f77 100644 --- a/include/kimera_multi_lcd/serializer.h +++ b/include/kimera_multi_lcd/serializer.h @@ -1,16 +1,16 @@ #pragma once -#include -#include +#include +#include #include -namespace pose_graph_tools { +namespace pose_graph_tools_msgs { -void to_json(nlohmann::json& j, const pose_graph_tools::BowVector& bow_vector); +void to_json(nlohmann::json& j, const pose_graph_tools_msgs::BowVector& bow_vector); -void from_json(const nlohmann::json& j, pose_graph_tools::BowVector& bow_vector); +void from_json(const nlohmann::json& j, pose_graph_tools_msgs::BowVector& bow_vector); -void to_json(nlohmann::json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame); +void to_json(nlohmann::json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame); -void from_json(const nlohmann::json& j, pose_graph_tools::VLCFrameMsg& vlc_frame); -} // namespace pose_graph_tools +void from_json(const nlohmann::json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame); +} // namespace pose_graph_tools_msgs diff --git a/include/kimera_multi_lcd/types.h b/include/kimera_multi_lcd/types.h index e283549..1b9d0b8 100644 --- a/include/kimera_multi_lcd/types.h +++ b/include/kimera_multi_lcd/types.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -71,7 +71,7 @@ class VLCFrame { const std::vector& keypoints_3d, const std::vector& versors, const OrbDescriptor& descriptors_mat); - VLCFrame(const pose_graph_tools::VLCFrameMsg& msg); + VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg); size_t robot_id_; size_t pose_id_; size_t submap_id_; // ID of the submap that contains this frame (pose) @@ -81,7 +81,7 @@ class VLCFrame { OrbDescriptor descriptors_mat_; gtsam::Pose3 T_submap_pose_; // 3D pose in submap frame void initializeDescriptorsVector(); - void toROSMessage(pose_graph_tools::VLCFrameMsg* msg) const; + void toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const; // void pruneInvalidKeypoints(); }; // class VLCFrame diff --git a/include/kimera_multi_lcd/utils.h b/include/kimera_multi_lcd/utils.h index e67085a..08234c7 100644 --- a/include/kimera_multi_lcd/utils.h +++ b/include/kimera_multi_lcd/utils.h @@ -7,33 +7,33 @@ #pragma once #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include "kimera_multi_lcd/types.h" namespace kimera_multi_lcd { -void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector* msg); +void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg); -void BowVectorFromMsg(const pose_graph_tools::BowVector& msg, +void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg, DBoW2::BowVector* bow_vec); -void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools::VLCFrameMsg* msg); -void VLCFrameFromMsg(const pose_graph_tools::VLCFrameMsg& msg, VLCFrame* frame); +void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg); +void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame); -void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg); -void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge); +void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg); +void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge); // Compute the payload size in a BowQuery message -size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg); +size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg); // Compute the payload size of a VLC frame -size_t computeVLCFramePayloadBytes(const pose_graph_tools::VLCFrameMsg& msg); +size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg); } // namespace kimera_multi_lcd diff --git a/package.xml b/package.xml index 0c92a4a..08e5b8c 100644 --- a/package.xml +++ b/package.xml @@ -23,7 +23,7 @@ geometry_msgs nav_msgs image_transport - pose_graph_tools + pose_graph_tools_msgs pcl_ros rosbag cv_bridge diff --git a/src/io.cpp b/src/io.cpp index cfbb585..6c49dbf 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -15,7 +15,7 @@ namespace kimera_multi_lcd { using nlohmann::json; -void saveBowVectors(const std::map& bow_vectors, +void saveBowVectors(const std::map& bow_vectors, const std::string& filename) { std::ofstream outfile(filename); json record; @@ -30,15 +30,15 @@ void saveBowVectors(const std::map& bow_vec void saveBowVectors(const std::map& bow_vectors, const std::string& filename) { - std::map pg_bow_vectors; + std::map pg_bow_vectors; for (const auto& id_bow : bow_vectors) { - pg_bow_vectors[id_bow.first] = pose_graph_tools::BowVector(); + pg_bow_vectors[id_bow.first] = pose_graph_tools_msgs::BowVector(); BowVectorToMsg(id_bow.second, &pg_bow_vectors[id_bow.first]); } saveBowVectors(pg_bow_vectors, filename); } -void saveVLCFrames(const std::map& vlc_frames, +void saveVLCFrames(const std::map& vlc_frames, const std::string& filename) { std::ofstream outfile(filename); json record; @@ -53,9 +53,9 @@ void saveVLCFrames(const std::map& vlc_fr void saveVLCFrames(const std::map& vlc_frames, const std::string& filename) { - std::map pg_vlc_frames; + std::map pg_vlc_frames; for (const auto& id_frame : vlc_frames) { - pg_vlc_frames[id_frame.first] = pose_graph_tools::VLCFrameMsg(); + pg_vlc_frames[id_frame.first] = pose_graph_tools_msgs::VLCFrameMsg(); VLCFrameToMsg(id_frame.second, &pg_vlc_frames[id_frame.first]); } saveVLCFrames(pg_vlc_frames, filename); @@ -63,7 +63,7 @@ void saveVLCFrames(const std::map& vlc_frames, // Save BoW vectors void loadBowVectors(const std::string& filename, - std::map& bow_vectors) { + std::map& bow_vectors) { std::stringstream ss; std::ifstream infile(filename); if (!infile) { @@ -80,7 +80,7 @@ void loadBowVectors(const std::string& filename, void loadBowVectors(const std::string& filename, std::map& bow_vectors) { - std::map pg_bow_vectors; + std::map pg_bow_vectors; loadBowVectors(filename, pg_bow_vectors); for (const auto& id_bow : pg_bow_vectors) { @@ -91,7 +91,7 @@ void loadBowVectors(const std::string& filename, // Save VLC Frames void loadVLCFrames(const std::string& filename, - std::map& vlc_frames) { + std::map& vlc_frames) { std::stringstream ss; std::ifstream infile(filename); if (!infile) { @@ -108,7 +108,7 @@ void loadVLCFrames(const std::string& filename, void loadVLCFrames(const std::string& filename, std::map& vlc_frames) { - std::map pg_vlc_frames; + std::map pg_vlc_frames; loadVLCFrames(filename, pg_vlc_frames); for (const auto& id_frame : pg_vlc_frames) { vlc_frames[id_frame.first] = VLCFrame(); diff --git a/src/serializer.cpp b/src/serializer.cpp index 915e32e..8e9f1f8 100644 --- a/src/serializer.cpp +++ b/src/serializer.cpp @@ -37,18 +37,18 @@ void from_json(const json& j, pcl::PointCloud& points) { } // namespace pcl -namespace pose_graph_tools { +namespace pose_graph_tools_msgs { -void to_json(json& j, const pose_graph_tools::BowVector& bow_vector) { +void to_json(json& j, const pose_graph_tools_msgs::BowVector& bow_vector) { j = json{{"word_ids", bow_vector.word_ids}, {"word_values", bow_vector.word_values}}; } -void from_json(const json& j, pose_graph_tools::BowVector& bow_vector) { +void from_json(const json& j, pose_graph_tools_msgs::BowVector& bow_vector) { j.at("word_ids").get_to(bow_vector.word_ids); j.at("word_values").get_to(bow_vector.word_values); } -void to_json(json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame) { +void to_json(json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) { pcl::PointCloud versors; pcl::fromROSMsg(vlc_frame.versors, versors); @@ -73,7 +73,7 @@ void to_json(json& j, const pose_graph_tools::VLCFrameMsg& vlc_frame) { {"qw", vlc_frame.T_submap_pose.orientation.w}}}}; } -void from_json(const json& j, pose_graph_tools::VLCFrameMsg& vlc_frame) { +void from_json(const json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) { j.at("robot_id").get_to(vlc_frame.robot_id); j.at("pose_id").get_to(vlc_frame.pose_id); j.at("submap_id").get_to(vlc_frame.submap_id); @@ -99,4 +99,4 @@ void from_json(const json& j, pose_graph_tools::VLCFrameMsg& vlc_frame) { j.at("T_submap_pose").at("qz").get_to(T_submap_R.z); j.at("T_submap_pose").at("qw").get_to(T_submap_R.w); } -} // namespace pose_graph_tools +} // namespace pose_graph_tools_msgs diff --git a/src/types.cpp b/src/types.cpp index 7781fb0..8d03aa3 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -26,7 +26,7 @@ VLCFrame::VLCFrame(const RobotId& robot_id, initializeDescriptorsVector(); } -VLCFrame::VLCFrame(const pose_graph_tools::VLCFrameMsg& msg) +VLCFrame::VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg) : robot_id_(msg.robot_id), pose_id_(msg.pose_id), submap_id_(msg.submap_id) { T_submap_pose_ = gtsam::Pose3(gtsam::Rot3(msg.T_submap_pose.orientation.w, msg.T_submap_pose.orientation.x, @@ -72,7 +72,7 @@ VLCFrame::VLCFrame(const pose_graph_tools::VLCFrameMsg& msg) } } -void VLCFrame::toROSMessage(pose_graph_tools::VLCFrameMsg* msg) const { +void VLCFrame::toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const { msg->robot_id = robot_id_; msg->pose_id = pose_id_; diff --git a/src/utils.cpp b/src/utils.cpp index 460acc7..9ce2139 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -10,7 +10,7 @@ namespace kimera_multi_lcd { -void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector* msg) { +void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg) { msg->word_ids.clear(); msg->word_values.clear(); for (auto it = bow_vec.begin(); it != bow_vec.end(); ++it) { @@ -19,7 +19,7 @@ void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools::BowVector } } -void BowVectorFromMsg(const pose_graph_tools::BowVector& msg, +void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg, DBoW2::BowVector* bow_vec) { assert(msg.word_ids.size() == msg.word_values.size()); bow_vec->clear(); @@ -28,22 +28,22 @@ void BowVectorFromMsg(const pose_graph_tools::BowVector& msg, } } -void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools::VLCFrameMsg* msg) { +void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg) { frame.toROSMessage(msg); } -void VLCFrameFromMsg(const pose_graph_tools::VLCFrameMsg& msg, VLCFrame* frame) { +void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame) { *frame = VLCFrame(msg); } -void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg) { +void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg) { // Yulun: this function currently does not assign covariance! msg->robot_from = edge.vertex_src_.first; msg->key_from = edge.vertex_src_.second; msg->robot_to = edge.vertex_dst_.first; msg->key_to = edge.vertex_dst_.second; - msg->type = pose_graph_tools::PoseGraphEdge::LOOPCLOSE; + msg->type = pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE; gtsam::Pose3 pose = edge.T_src_dst_; gtsam::Quaternion quat = pose.rotation().toQuaternion(); @@ -59,7 +59,7 @@ void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools::PoseGraphEdge* msg) { msg->pose.position.z = position.z(); } -void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge) { +void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge) { edge->vertex_src_ = std::make_pair(msg.robot_from, msg.key_from); edge->vertex_dst_ = std::make_pair(msg.robot_to, msg.key_to); @@ -74,7 +74,7 @@ void VLCEdgeFromMsg(const pose_graph_tools::PoseGraphEdge& msg, VLCEdge* edge) { edge->T_src_dst_ = T_src_dst; } -size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg) { +size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg) { size_t bytes = 0; bytes += sizeof(msg.robot_id); bytes += sizeof(msg.pose_id); @@ -83,7 +83,7 @@ size_t computeBowQueryPayloadBytes(const pose_graph_tools::BowQuery& msg) { return bytes; } -size_t computeVLCFramePayloadBytes(const pose_graph_tools::VLCFrameMsg& msg) { +size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg) { size_t bytes = 0; bytes += sizeof(msg.robot_id); bytes += sizeof(msg.pose_id);