Merge branch 'MIT-SPARK:main' into main

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")

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 ""
+)

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()

include/kimera_multi_lcd/io.h

@@ -0,0 +1,44 @@
+/*
+ * Copyright Notes
+ *
+ * Authors: Yun Chang ([email protected])
+ */
+
+#pragma once
+
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
+
+#include <map>
+
+#include "kimera_multi_lcd/types.h"
+
+namespace kimera_multi_lcd {
+
+// Save BoW vectors
+void saveBowVectors(const std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors,
+                    const std::string& filename);
+
+void saveBowVectors(const std::map<PoseId, DBoW2::BowVector>& bow_vectors,
+                    const std::string& filename);
+
+// Save VLC Frames
+void saveVLCFrames(const std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames,
+                   const std::string& filename);
+
+void saveVLCFrames(const std::map<PoseId, VLCFrame>& vlc_frames,
+                   const std::string& filename);
+
+// Save BoW vectors
+void loadBowVectors(const std::string& filename,
+                    std::map<PoseId, pose_graph_tools_msgs::BowVector>& bow_vectors);
+
+void loadBowVectors(const std::string& filename,
+                    std::map<PoseId, DBoW2::BowVector>& bow_vectors);
+
+// Save VLC Frames
+void loadVLCFrames(const std::string& filename,
+                   std::map<PoseId, pose_graph_tools_msgs::VLCFrameMsg>& vlc_frames);
+
+void loadVLCFrames(const std::string& filename, std::map<PoseId, VLCFrame>& vlc_frames);
+}  // namespace kimera_multi_lcd

include/kimera_multi_lcd/LoopClosureDetector.h → include/kimera_multi_lcd/loop_closure_detector.h

@@ -9,7 +9,7 @@
 #include <DBoW2/DBoW2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
-#include <kimera_multi_lcd/LcdThirdPartyWrapper.h>
+#include <kimera_multi_lcd/lcd_third_party.h>
 #include <kimera_multi_lcd/types.h>
 #include <ros/ros.h>
 #include <ros/time.h>
@@ -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<PoseId, VLCFrame> 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<RobotId, std::unordered_map<PoseId, DBoW2::BowVector>>
-      bow_vectors_;
+  std::unordered_map<RobotId, PoseBowVector> bow_vectors_;
   std::unordered_map<RobotId, std::unique_ptr<OrbDatabase>> db_BoW_;
   // Keep track of latest pose Id with BoW for each robot
   std::unordered_map<RobotId, PoseId> bow_latest_pose_id_;

include/kimera_multi_lcd/serializer.h

@@ -0,0 +1,16 @@
+#pragma once
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
+
+#include <nlohmann/json.hpp>
+
+namespace pose_graph_tools_msgs {
+
+void to_json(nlohmann::json& j, const pose_graph_tools_msgs::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_msgs::VLCFrameMsg& vlc_frame);
+
+void from_json(const nlohmann::json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame);
+}  // namespace pose_graph_tools_msgs

include/kimera_multi_lcd/types.h

@@ -11,7 +11,7 @@
 #include <gtsam/inference/Symbol.h>
 #include <pcl/point_types.h>
 #include <pcl_ros/point_cloud.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
 #include <opencv/cv.hpp>
 #include <opencv2/features2d.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -27,6 +27,7 @@ typedef size_t PoseId;
 typedef std::pair<RobotId, PoseId> RobotPoseId;
 typedef std::set<RobotPoseId> RobotPoseIdSet;
 typedef std::vector<RobotPoseId> RobotPoseIdVector;
+typedef std::map<PoseId, DBoW2::BowVector> PoseBowVector;
 
 // Each edge in the pose graph is uniquely identified by four integers
 // (robot_src, frame_src, robot_dst, frame_dst)
@@ -70,7 +71,7 @@ class VLCFrame {
            const std::vector<gtsam::Vector3>& keypoints_3d,
            const std::vector<gtsam::Vector3>& 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)
@@ -80,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
 

include/kimera_multi_lcd/utils.h

@@ -7,31 +7,33 @@
 #pragma once
 
 #include <DBoW2/DBoW2.h>
-#include <pose_graph_tools/BowQuery.h>
-#include <pose_graph_tools/BowVector.h>
-#include <pose_graph_tools/PoseGraph.h>
-#include <pose_graph_tools/PoseGraphEdge.h>
-#include <pose_graph_tools/VLCFrameMsg.h>
-#include <pose_graph_tools/VLCFrameQuery.h>
+#include <pose_graph_tools_msgs/BowQuery.h>
+#include <pose_graph_tools_msgs/BowVector.h>
+#include <pose_graph_tools_msgs/PoseGraph.h>
+#include <pose_graph_tools_msgs/PoseGraphEdge.h>
+#include <pose_graph_tools_msgs/VLCFrameMsg.h>
+#include <pose_graph_tools_msgs/VLCFrameQuery.h>
+
+#include <map>
 
 #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

package.xml

@@ -23,7 +23,7 @@
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>image_transport</depend>
-  <depend>pose_graph_tools</depend>
+  <depend>pose_graph_tools_msgs</depend>
   <depend>pcl_ros</depend>
   <depend>rosbag</depend>
   <depend>cv_bridge</depend>