├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── README.md
├── catkin_ws
    └── src
    │   ├── CMakeLists.txt
    │   └── sgloop_ros
    │       ├── CMakeLists.txt
    │       ├── include
    │           ├── SlidingWindow.h
    │           ├── Visualization.h
    │           ├── communication
    │           │   └── Communication.h
    │           ├── registration
    │           │   ├── Prune.h
    │           │   └── robustPoseAvg.h
    │           └── utility
    │           │   └── CameraPoseVisualization.h
    │       ├── launch
    │           ├── mapping.rviz
    │           ├── render_our_sgslam.launch
    │           ├── render_semantic_map.launch
    │           ├── semantic_mapping.launch
    │           ├── slabim_mapping.launch
    │           └── visualize.launch
    │       ├── msg
    │           ├── CoarseGraph.msg
    │           └── DenseGraph.msg
    │       ├── package.xml
    │       └── src
    │           ├── MappingNode.cpp
    │           ├── RenderNode.cpp
    │           ├── Visualization.cpp
    │           ├── communication
    │               └── Communication.cpp
    │           ├── registration
    │               ├── Prune.cpp
    │               └── robustPoseAvg.cpp
    │           └── utility
    │               └── CameraPoseVisualization.cpp
├── cmake
    ├── eigen.cmake
    ├── glog.cmake
    ├── gtsam.cmake
    ├── jsoncpp.cmake
    ├── open3d.cmake
    └── opencv.cmake
├── config
    ├── fusion_portable.yaml
    ├── matterport.yaml
    ├── realsense.yaml
    ├── render_3rscan.yaml
    ├── rio.yaml
    ├── scannet.yaml
    └── slabim.yaml
├── doc
    ├── DATA.md
    ├── rviz_gui.png
    ├── seng_map_720p.gif
    ├── system.png
    └── ust_atrium_light.gif
├── measurement_model
    ├── bayesian
    │   ├── detection_likelihood.png
    │   ├── likelihood_matrix.png
    │   └── ram_likelihood.png
    ├── bayesian_model.txt
    └── hardcode
    │   └── likelihood_matrix.png
├── scripts
    ├── process_slabim_pcd.py
    ├── rerun_visualize.py
    ├── uncompress_data.py
    └── utils.py
└── src
    ├── CMakeLists.txt
    ├── Common.h
    ├── GraphMatch.cpp
    ├── IntegrateInstanceMap.cpp
    ├── IntegrateRGBD.cpp
    ├── cluster
        ├── PoseGraph.cpp
        └── PoseGraph.h
    ├── mapping
        ├── BayesianLabel.h
        ├── Detection.cpp
        ├── Detection.h
        ├── Instance.cpp
        ├── Instance.h
        ├── SemanticDict.h
        ├── SemanticMapping.cpp
        ├── SemanticMapping.h
        ├── SubVolume.cpp
        └── SubVolume.h
    ├── sgloop
        ├── BertBow.cpp
        ├── BertBow.h
        ├── Graph.cpp
        ├── Graph.h
        ├── Initialization.h
        ├── LoopDetector.cpp
        ├── LoopDetector.h
        ├── SGNet.cpp
        ├── SGNet.h
        ├── ShapeEncoder.cpp
        └── ShapeEncoder.h
    ├── tokenizer
        ├── CMakeLists.txt
        ├── basic_string_util.h
        ├── bert_tokenizer.cc
        ├── bert_tokenizer.h
        ├── logging.h
        ├── source
        │   ├── utf8.h
        │   └── utf8
        │   │   ├── checked.h
        │   │   ├── core.h
        │   │   ├── cpp11.h
        │   │   ├── cpp17.h
        │   │   ├── cpp20.h
        │   │   └── unchecked.h
        ├── text_tokenizer.cc
        └── text_tokenizer.h
    └── tools
        ├── Color.h
        ├── Eval.h
        ├── IO.cpp
        ├── IO.h
        ├── TicToc.h
        ├── Tools.h
        ├── Utility.cpp
        ├── Utility.h
        ├── Visualization.cpp
        └── g3reg_api.h


/.gitignore:
--------------------------------------------------------------------------------
 1 | 
 2 | # Ignore folders
 3 | python/__pycache__/
 4 | measurement_model/prompt
 5 | build
 6 | .history
 7 | torchscript
 8 | catkin_ws/devel
 9 | catkin_ws/build
10 | catkin_ws/src/sgloop_ros/thirdparty
11 | catkin_ws/.catkin_workspace
12 | install
13 | eval/fmfusion
14 | tmp
15 | hydra_lcd
16 | images
17 | data
18 | output
19 | 
20 | # Ignore files 
21 | *.pyc
22 | *.zip
23 | *.npy
24 | .idea
25 | .vscode
26 | datasets
27 | Log


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "catkin_ws/src/perception_open3d"]
2 | 	path = catkin_ws/src/perception_open3d
3 | 	url = git@github.com:ros-perception/perception_open3d.git
4 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.18)
 2 | 
 3 | project(FmFusion LANGUAGES C CXX)
 4 | 
 5 | #### Options ####
 6 | option(LOOP_DETECTION OFF)
 7 | option(INSTALL_FMFUSION ON) # Install as static library
 8 | option(RUN_HYDRA OFF)
 9 | #################
10 | 
11 | set(ALL_TARGET_LIBRARIES "")
12 | include(cmake/eigen.cmake)
13 | include(cmake/glog.cmake)
14 | include(cmake/opencv.cmake)
15 | include(cmake/open3d.cmake)
16 | include(cmake/jsoncpp.cmake)
17 | 
18 | set(PROJECT_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})
19 | set(CMAKE_INSTALL_PREFIX ${CMAKE_CURRENT_SOURCE_DIR}/install)
20 | message(STATUS "Project root directory: ${PROJECT_ROOT_DIR}")
21 | message(STATUS "CMake install prefix: ${CMAKE_INSTALL_PREFIX}")
22 | add_subdirectory(src)
23 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | <div align="center">
  2 |     <h1>FM-Fusion: Instance-aware Semantic Mapping <br> Boosted by Vision-Language Foundation Models</h2>
  3 |     <strong>IEEE RA-L 2024</strong>
  4 |     <br>
  5 |         <a href="https://uav.hkust.edu.hk/current-members/" target="_blank">Chuhao Liu</a><sup>1</sup>,
  6 |         <a href="https://uav.hkust.edu.hk/group/alumni/" target="_blank">Ke Wang</a><sup>2,*</sup>,
  7 |         <a href="https://jayceeshi.github.io/" target="_blank">Jieqi Shi</a><sup>1</sup>,
  8 |         <a href="https://qiaozhijian.github.io/" target="_blank">Zhijian Qiao</a><sup>1</sup>, and
  9 |         <a href="https://uav.hkust.edu.hk/group/" target="_blank">Shaojie Shen</a><sup>1</sup>
 10 |     <p>
 11 |         <h45>
 12 |             <sup>1</sup>HKUST Aerial Robotics Group &nbsp;&nbsp;
 13 |             <sup>2</sup>Chang'an University, China &nbsp;&nbsp;
 14 |             <br>
 15 |         </h5>
 16 |         <sup>*</sup>Corresponding Author
 17 |     </p>
 18 |     <a href="https://ieeexplore.ieee.org/abstract/document/10403989"> <img src="https://img.shields.io/badge/IEEE-RA--L-004c99"> </a>
 19 |     <a href='https://arxiv.org/abs/2402.04555'><img src='https://img.shields.io/badge/arXiv-2402.04555-990000' alt='arxiv'></a>
 20 |     <a href="https://www.youtube.com/watch?v=zrzcjj9-ydk&t=9s"><img alt="YouTube" src="https://img.shields.io/badge/YouTube-Video-red"/></a>
 21 | </div>
 22 | 
 23 | <p align="center">
 24 |     <img src="doc/system.png" width="800"/>
 25 | </p>
 26 | 
 27 | ### News 
 28 | * [27th Nov 2024] Support semantic reconstruction using [SLABIM](https://github.com/HKUST-Aerial-Robotics/SLABIM) dataset.
 29 | * [25th Oct 2024] Publish code and RGB-D sequences from ScanNet and SgSlam. 
 30 | * [5th Jan 2024] Paper accepted by IEEE RA-L.
 31 | 
 32 | **FM-Fusion** utilizes [RAM](https://github.com/xinyu1205/recognize-anything), [GroundingDINO](https://github.com/IDEA-Research/GroundingDINO) and [SAM](https://github.com/facebookresearch/segment-anything) to reconstruct an instance-aware semantic map. Boosted by the vision foundational models, FM-Fusion can reconstruct semantic instances in real-world cluttered indoor environments. 
 33 | <p align="center">
 34 |     <img src="doc/seng_map_720p.gif" width = "800"/>
 35 | </p>
 36 | <p align="center">
 37 |     <img src="doc/ust_atrium_light.gif" width = "800"/>
 38 | </p>
 39 | 
 40 | The following instruction explains how to run FM-Fusion on our RGB-D sequences or ScanNet sequences. If you find its useful, please cite our paper.
 41 | 
 42 | ```latex
 43 | @article{
 44 |   title={{FM-Fusion}: Instance-aware Semantic Mapping Boosted by Vision-Language Foundation Models}, 
 45 |   author={Liu, Chuhao and Wang, Ke and Shi, Jieqi and Qiao, Zhijian and Shen, Shaojie},
 46 |   journal={IEEE Robotics and Automation Letters(RA-L)}, 
 47 |   year={2024}, 
 48 |   volume={9}, 
 49 |   number={3},
 50 |   pages={2232-2239}
 51 | }
 52 | ```
 53 | 
 54 | ## Tabel of Contents
 55 | 1. Install
 56 | 2. Download Data
 57 | 3. Run Instance-aware Semantic Mapping
 58 | 4. Use it in Your RGB-D Camera
 59 | 5. Acknowledge
 60 | 6. License
 61 | 
 62 | ## 1. Install
 63 | Install dependency packages from Ubuntu Source
 64 | ```
 65 | sudo apt-get install libboost-dev libomp-dev libeigen3-dev
 66 | ```
 67 | Install Open3D from its source code.([Install Tutorial](https://www.open3d.org/docs/release/compilation.html#compilation))
 68 | ```
 69 | git clone https://github.com/isl-org/Open3D
 70 | cd Open3D
 71 | mkdir build && cd build
 72 | cmake -DBUILD_SHARED_LIBS=ON ..
 73 | make -j12
 74 | sudo make install
 75 | ```
 76 | Follow the official tutorials to install [OpenCV](https://docs.opencv.org/4.x/d7/d9f/tutorial_linux_install.html), [GLOG](https://github.com/google/glog), [jsoncpp](https://github.com/open-source-parsers/jsoncpp/blob/master/README.md).
 77 | To make it compatible with ROS, please install ```OpenCV 3.4.xx```.
 78 | 
 79 | Clone and compile FM-Fusion,
 80 | ```
 81 | git clone git@github.com:HKUST-Aerial-Robotics/FM-Fusion.git
 82 | mkdir build && cd build
 83 | cmake .. -DINSTALL_FMFUSION=ON
 84 | make -j12
 85 | make install
 86 | ```
 87 | 
 88 | Install the ROS node program, which renders the semantic instance map in Rviz. Install ROS platform following its [official guidance](http://wiki.ros.org/noetic/Installation/Ubuntu). Then, build the ros node we provide,
 89 | ```
 90 | git submodule update --init --recursive
 91 | cd catkin_ws && catkin_make
 92 | source devel/setup.bash
 93 | ```
 94 | 
 95 | ## 2. Download Data
 96 | We provide two datasets to evaluate: SgSlam (captured using Intel [Realsense L-515](https://www.intelrealsense.com/lidar-camera-l515/)) and ScanNet. Their sequences can be downloaded:
 97 | * [SgSlam_OneDrive](https://hkustconnect-my.sharepoint.com/:f:/g/personal/cliuci_connect_ust_hk/EnIjnIl7a2NBtioZFfireM4B_PnZcIpwcB-P2-Fnh3FEPg?e=BKsgLQ) or [SgSlam_Nutstore(坚果云)](https://www.jianguoyun.com/p/DSM4iw0Q4cyEDRjUvekFIAA).
 98 | * [ScanNet_OneDrive](https://hkustconnect-my.sharepoint.com/:f:/g/personal/cliuci_connect_ust_hk/EhUu2zzwUGNKq8h2yUGHIawBpC8EI73YB_GTG9BUXXBoVA?e=o4TfsA) or [ScanNet_NutStore(坚果云)](https://www.jianguoyun.com/p/DVuHdogQ4cyEDRjQvekFIAA).
 99 | 
100 | Please check [data format](doc/DATA.md) for the illustration about data in each sequence.
101 | After download the ```scans``` folder in each dataset, go to [uncompress_data.py](scripts/uncompress_data.py) and set the data directories to your local directories. Then, un-compress the sequence data.
102 | ```
103 | python scripts/uncompress_data.py
104 | ```
105 | 
106 | ## 3. Run Instance-aware Semantic Mapping
107 | #### a. Run with Rviz.
108 | 
109 | In ```launch/semantic_mapping.launch```, set the directories to your local dataset directories. Then, launch Rviz and the ROS node,
110 | ```
111 | roslaunch sgloop_ros visualize.launch
112 | roslaunch sgloop_ros semantic_mapping.launch
113 | ```
114 | <p align="center">
115 |     <img src="doc/rviz_gui.png" width="600"/>
116 | </p>
117 | 
118 | It should incremental reconstruct the semantic map and render the results on Rviz. At the end of the sequence, the program save the output results, where the output format is illustrated in the [data format](doc/DATA.md). To visualize the results that are previously reconstructed, open ```launch/render_semantic_map.launch``` and set the ```result_folder``` directory accordingly. Then,
119 | ```
120 | roslaunch sgloop_ros render_semantic_map.launch
121 | ```
122 | 
123 | ***Tips***: If you are running the program on a remote server, you can utilize the [ROS across machine](http://wiki.ros.org/ROS/Tutorials/MultipleMachines) function. After set the ```rosmaster``` following the ROS tutorial, you can launch ```visualize.launch``` at your local machine and ```semantic_mapping.launch``` at the server. So, you can still visualize the result on your local machine.
124 | 
125 | #### b. Run without visualization.
126 | 
127 | If you do not need the ROS node to visualize, you can skip its install in the above instruction. Then, simply run the C++ executable program and the results will be saved at ```${SGSLAM_DATAROOT}/output```. The output directory can be set before run the program.
128 | ```
129 | ./build/src/IntegrateInstanceMap --config config/realsense.yaml --root ${SGSLAM_DATAROOT}/scans/ab0201_03a --output ${SGSLAM_DATAROOT}/output
130 | ```
131 | 
132 | ## 4. Use it in Your RGB-D Camera
133 | In ```SgSlam``` dataset, we use Intel Realsense-D515 camera and DJI A3 flight controller to collect data sequence Details of the hardware suite can be found in this [paper](https://arxiv.org/abs/2201.03312). You can also collect your own dataset using a similar hardware suite. 
134 | #### a. Prepare RGB-D and Camera poses.
135 | We use [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) to compute visual-inertial odometry (VIO). We save the camera poses of its keyframes in a ```pose``` folder.
136 | 
137 | #### b. Run [RAM](https://github.com/xinyu1205/recognize-anything), [GroundingDINO](https://github.com/IDEA-Research/GroundingDINO) and [SAM](https://github.com/facebookresearch/segment-anything).
138 | 
139 | The three models are combined to run in [Grounded-SAM](https://github.com/IDEA-Research/Grounded-Segment-Anything). Please find our adopted Grounded-SAM [here](https://github.com/glennliu/Grounded-Segment-Anything). It should generate a ```prediction``` folder as explained in [data format](DATA.md). Then, you can run the semantic mapping on your dataset.
140 | 
141 | ## 5. Acknowledge
142 | The hardware used in SgSlam is supported by [Luqi Wang](https://lwangax.wordpress.com). The lidar-camera hardware used in [SLABIM](https://github.com/HKUST-Aerial-Robotics/SLABIM) is supported by [Skyland Innovation](https://www.skylandx.com).
143 | In our program, we use and adopt [Open3D](https://www.open3d.org) to reconstruct instance sub-volume. The vision foundation models RAM, GroundingDINO, and SAM provide instance segmentation on images.
144 | 
145 | ## 6. License
146 | The source code is released under [GPLv3](https://www.gnu.org/licenses/) license.
147 | For technical issues, please contact Chuhao LIU (cliuci@connect.ust.hk). 
148 | 


--------------------------------------------------------------------------------
/catkin_ws/src/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.0.2)
  2 | project(sgloop_ros)
  3 | 
  4 | # add_compile_options(-std=c++11)
  5 | add_compile_options(-fPIC)
  6 | set(CMAKE_CXX_STANDARD 17)
  7 | option(LOOP_DETECTION OFF)
  8 | 
  9 | find_package(catkin REQUIRED COMPONENTS
 10 |   roscpp
 11 |   rospy
 12 |   std_msgs
 13 |   geometry_msgs
 14 |   sensor_msgs
 15 |   nav_msgs
 16 |   visualization_msgs
 17 |   open3d_conversions
 18 |   cv_bridge
 19 |   tf
 20 |   message_generation
 21 | )
 22 | 
 23 | set(FMFUSION_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/../../../build/src/libfmfusion.so)
 24 | set(FMFUSION_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../install/include/fmfusion)
 25 | find_package(OpenCV REQUIRED)
 26 | find_package(Open3D REQUIRED)
 27 | message(STATUS "FMFUSION LIBRARIES: ${FMFUSION_LIBRARIES}")
 28 | message(STATUS "FMFUSION INCLUDE DIR: ${FMFUSION_INCLUDE_DIR}")
 29 | 
 30 | # 
 31 | if(LOOP_DETECTION)
 32 |   set(CMAKE_PREFIX_PATH "/home/cliuci/tools/libtorch")
 33 |   find_package(Torch REQUIRED)
 34 |   add_subdirectory(thirdparty/G3Reg)
 35 |   # add_subdirectory(thirdparty/Kimera-RPGO)
 36 |   # include_directories("thirdparty/Kimera-RPGO/include")
 37 |   message(STATUS "TORCH_LIBRARIES: ${TORCH_LIBRARIES}")
 38 |   message(STATUS "TORCH_INCLUDE_DIRS: ${TORCH_INCLUDE_DIRS}")
 39 |   add_message_files(
 40 |     FILES
 41 |     CoarseGraph.msg
 42 |     DenseGraph.msg
 43 |   )
 44 |   
 45 |   generate_messages(
 46 |     DEPENDENCIES
 47 |     std_msgs
 48 |     geometry_msgs
 49 |   )
 50 | endif()
 51 | 
 52 | catkin_package(
 53 |   INCLUDE_DIRS include
 54 |   CATKIN_DEPENDS roscpp std_msgs sensor_msgs tf open3d_conversions cv_bridge message_runtime
 55 |   DEPENDS Open3D
 56 | )
 57 | 
 58 | ###########
 59 | ## Build ##
 60 | ###########
 61 | 
 62 | ## Specify additional locations of header files
 63 | ## Your package locations should be listed before other locations
 64 | include_directories(
 65 |   include
 66 |   ${catkin_INCLUDE_DIRS}
 67 |   ${FMFUSION_INCLUDE_DIR}
 68 |   ${GTSAM_INCLUDE_DIR}
 69 | )
 70 | 
 71 | set(ALL_TARGET_LINK_LIBARIES
 72 |   ${catkin_LIBRARIES}
 73 |   Open3D::Open3D
 74 |   ${FMFUSION_LIBRARIES}
 75 |   ${OpenCV_LIBS}
 76 | )
 77 | 
 78 | # Build Library
 79 | set(SGLOOP_SRC src/utility/CameraPoseVisualization.cpp
 80 |                src/Visualization.cpp)
 81 | if(LOOP_DETECTION)
 82 |   list(APPEND SGLOOP_SRC 
 83 |               src/communication/Communication.cpp
 84 |               src/registration/Prune.cpp
 85 |               src/registration/robustPoseAvg.cpp)
 86 | 
 87 |   list(APPEND ALL_TARGET_LINK_LIBARIES 
 88 |               ${TORCH_LIBRARIES}
 89 |               G3REG::g3reg
 90 |               gtsam
 91 |         )
 92 |   message("Enable Loop Detection")
 93 | endif(LOOP_DETECTION)
 94 | 
 95 | add_library(sgloop_ros SHARED ${SGLOOP_SRC})
 96 | target_link_libraries(sgloop_ros ${ALL_TARGET_LINK_LIBARIES})
 97 | 
 98 | # Build Executables
 99 | add_executable(MappingNode src/MappingNode.cpp)
100 | target_link_libraries(MappingNode ${ALL_TARGET_LINK_LIBARIES} sgloop_ros)
101 | 
102 | add_executable(RenderNode src/RenderNode.cpp)
103 | target_link_libraries(RenderNode ${ALL_TARGET_LINK_LIBARIES} sgloop_ros)
104 | 
105 | if(LOOP_DETECTION)
106 |   add_executable(PoseGraphNode src/PoseGraphNode.cpp)
107 |   target_link_libraries(PoseGraphNode PUBLIC ${ALL_TARGET_LINK_LIBARIES} sgloop_ros)
108 | 
109 |   add_executable(OfflineLoop src/OfflineLoopNode.cpp)
110 |   target_link_libraries(OfflineLoop ${ALL_TARGET_LINK_LIBARIES} sgloop_ros)
111 | 
112 |   add_executable(OnlineLoop src/OnlineLoopNode.cpp)
113 |   target_link_libraries(OnlineLoop ${ALL_TARGET_LINK_LIBARIES} sgloop_ros)
114 | endif(LOOP_DETECTION)
115 | 
116 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/SlidingWindow.h:
--------------------------------------------------------------------------------
 1 | #include <unordered_map>
 2 | #include <vector>
 3 | #include <string>
 4 | #include <iostream>
 5 | #include <eigen3/Eigen/Dense>
 6 | 
 7 | namespace sgloop_ros
 8 | {
 9 |     class SlidingWindow
10 |     {
11 |     private:
12 |         float translation_threshold;
13 |         std::vector<int> frame_ids;
14 |         std::vector<float> translations;
15 |         float total_translation;
16 | 
17 |         Eigen::Matrix4d last_pose;
18 | 
19 |     public:
20 |         SlidingWindow(float translation_threshold_ = 100.0);
21 |         ~SlidingWindow(){};
22 | 
23 |         void update_translation(const int &frame_id, const Eigen::Matrix4d &cur_pose); 
24 |                             // float translation);
25 | 
26 |         int get_window_start_frame();
27 | 
28 |     };
29 |     
30 |     SlidingWindow::SlidingWindow(float translation_threshold_): 
31 |         translation_threshold(translation_threshold_), total_translation(0.0)
32 |     {
33 | 
34 |     }
35 | 
36 |     void SlidingWindow::update_translation(const int &frame_id, const Eigen::Matrix4d &cur_pose)
37 |     {
38 |         float translation = (cur_pose.block<3,1>(0,3) - last_pose.block<3,1>(0,3)).norm();
39 | 
40 |         translations.push_back(translation);
41 |         frame_ids.push_back(frame_id);
42 |         total_translation += translation;
43 | 
44 |         if(total_translation > translation_threshold)
45 |         { // remove frames from the start
46 |             while(total_translation > translation_threshold)
47 |             {
48 |                 total_translation -= translations[0];
49 |                 frame_ids.erase(frame_ids.begin());
50 |                 translations.erase(translations.begin());
51 |             }
52 |         }
53 | 
54 |         last_pose = cur_pose;
55 |     }
56 | 
57 |     int SlidingWindow::get_window_start_frame()
58 |     {
59 |         int start_frame;
60 |         if(frame_ids.empty()) start_frame = 0;
61 |         else start_frame = std::max(0,frame_ids[0]-1);
62 | 
63 |         // std::cout<<"start_frame: "<<start_frame<<", "
64 |         //         <<"translation: " <<total_translation <<"\n";
65 |         return start_frame;
66 |     }
67 |     
68 |     
69 | }
70 | 
71 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/Visualization.h:
--------------------------------------------------------------------------------
  1 | #include "unordered_map"
  2 | #include "ros/ros.h"
  3 | 
  4 | #include "std_msgs/ColorRGBA.h"
  5 | #include "geometry_msgs/PointStamped.h"
  6 | #include "nav_msgs/Odometry.h"
  7 | #include "nav_msgs/Path.h"
  8 | #include "sensor_msgs/PointCloud2.h"
  9 | #include "sensor_msgs/Image.h"
 10 | #include "visualization_msgs/Marker.h"
 11 | #include "visualization_msgs/MarkerArray.h"
 12 | 
 13 | #include "cv_bridge/cv_bridge.h"
 14 | #include "open3d_conversions/open3d_conversions.h"
 15 | 
 16 | #include "mapping/Detection.h"
 17 | 
 18 | namespace Visualization
 19 | {
 20 |     struct VizParam{
 21 |         float edge_width = 0.02;
 22 |         std::array<float,3> edge_color;
 23 |         float centroid_size = 0.1;
 24 |         std::array<float,3> centroid_color = {0.0,0.0,0.0};
 25 |         float centroid_voffset = 0.0;
 26 |         float annotation_size = 0.2;
 27 |         float annotation_voffset = 0.3;
 28 |         std::array<float,3> annotation_color = {0.0,0.0,0.0};
 29 |     }; 
 30 | 
 31 |     class Visualizer
 32 |     {
 33 |         public:
 34 |             Visualizer(ros::NodeHandle &nh, ros::NodeHandle &nh_private, std::vector<std::string> remote_agents={});
 35 | 
 36 |             ~Visualizer(){};
 37 | 
 38 |         public:
 39 |             ros::Publisher ref_graph, src_graph;
 40 |             ros::Publisher ref_global_map, src_global_map;
 41 |             ros::Publisher ref_centroids, src_centroids;
 42 |             ros::Publisher ref_edges, src_edges;
 43 |             ros::Publisher instance_match, point_match;
 44 | 
 45 |             ros::Publisher rgb_image, pred_image;
 46 |             ros::Publisher camera_pose, path;
 47 |             ros::Publisher node_annotation, node_dir_lines;
 48 | 
 49 |             ros::Publisher src_map_aligned, path_aligned;
 50 | 
 51 |             nav_msgs::Path path_msg;
 52 |             VizParam param;
 53 |             std::array<float,3> local_frame_offset;        
 54 | 
 55 |             std::map<std::string, Eigen::Vector3d> t_local_remote;    
 56 |             std::map<std::string, Eigen::Matrix4d> Transfrom_local_remote;
 57 | 
 58 |     };
 59 | 
 60 |     bool render_point_cloud(const std::shared_ptr<open3d::geometry::PointCloud> &pcd, ros::Publisher pub, std::string frame_id="world");
 61 | 
 62 |     bool inter_graph_edges(const std::vector<Eigen::Vector3d> &centroids,
 63 |                             const std::vector<std::pair<int,int>> &edges,
 64 |                             ros::Publisher pub,
 65 |                             float width=0.02,
 66 |                             std::array<float,3> color={0.0,0.0,1.0},
 67 |                             std::string frame_id="world");
 68 | 
 69 |     bool correspondences(const std::vector<Eigen::Vector3d> &src_centroids,
 70 |                         const std::vector<Eigen::Vector3d> &ref_centroids,
 71 |                         ros::Publisher pub, 
 72 |                         std::string frame_id="world",
 73 |                         std::vector<bool> pred_masks={},
 74 |                         Eigen::Matrix4d T_local_remote = Eigen::Matrix4d::Identity(),
 75 |                         float line_width=0.02,
 76 |                         float alpha=1.0);
 77 | 
 78 |     bool instance_centroids(const std::vector<Eigen::Vector3d> &centroids,
 79 |                             ros::Publisher pub, 
 80 |                             std::string frame_id="world", 
 81 |                             float scale=0.1,
 82 |                             std::array<float,3> color={0.0,0.0,1.0},
 83 |                             float offset_z=0.0);
 84 |     
 85 |     bool node_annotation(const std::vector<Eigen::Vector3d> &centroids,
 86 |                         const std::vector<std::string> &annotations,
 87 |                         ros::Publisher pub, 
 88 |                         std::string frame_id="world", 
 89 |                         float scale=0.1,
 90 |                         float z_offset=0.3,
 91 |                         std::array<float,3> color={0.0,0.0,1.0});
 92 | 
 93 |     bool render_image(const cv::Mat &image, ros::Publisher pub, std::string frame_id="world");
 94 | 
 95 |     bool render_camera_pose(const Eigen::Matrix4d &pose, ros::Publisher pub, 
 96 |                             std::string frame_id="world", int sequence_id = 0);
 97 | 
 98 |     bool render_path(const Eigen::Matrix4d &pose, 
 99 |                     nav_msgs::Path &path_msg,
100 |                     ros::Publisher pub, 
101 |                     std::string frame_id="world", 
102 |                     int sequence_id = 0);
103 | 
104 |     bool render_path(const std::vector<Eigen::Matrix4d> &T_a_camera,
105 |                     const Eigen::Matrix4d & T_b_a,
106 |                     const std::string &agnetB_frame_id,
107 |                     nav_msgs::Path &path_msg,
108 |                     ros::Publisher pub);
109 | 
110 |     bool render_rgb_detections(const open3d::geometry::Image &color,
111 |                             const std::vector<fmfusion::DetectionPtr> &detections,
112 |                             ros::Publisher pub,
113 |                             std::string frame_id="world");
114 | 
115 |     int render_semantic_map(const std::shared_ptr<open3d::geometry::PointCloud> &cloud, 
116 |                         const std::vector<Eigen::Vector3d> &instance_centroids, 
117 |                         const std::vector<std::string> &instance_annotations,
118 |                         const Visualization::Visualizer &viz,
119 |                         const std::string &agent_name);                            
120 | }


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/communication/Communication.h:
--------------------------------------------------------------------------------
  1 | #ifndef COMMUNICATION_H
  2 | #define COMMUNICATION_H
  3 | 
  4 | #include <map>
  5 | #include <fstream>
  6 | #include <torch/torch.h>
  7 | 
  8 | #include <ros/ros.h>
  9 | #include <std_msgs/Int32.h>
 10 | #include <std_msgs/String.h>
 11 | #include <sgloop_ros/CoarseGraph.h>
 12 | #include <sgloop_ros/DenseGraph.h>
 13 | #include <eigen3/Eigen/Dense>
 14 | 
 15 | namespace SgCom
 16 | {
 17 |     struct Log
 18 |     {
 19 |         int frame_id;
 20 |         float timestamp;
 21 |         int N;
 22 |         int X=0;
 23 |         std::string direction; // "pub" or "sub"
 24 |         std::string msg_type; // "CoarseGraph", "DenseGraph"
 25 |         bool checksum;
 26 |     };
 27 |     
 28 |     struct AgentDataDict{
 29 |         int frame_id=-1;
 30 |         float received_timestamp;
 31 |         int N=0;
 32 |         int X=0; // number of points
 33 |         int D=0; // coarse node feature dimension
 34 |         std::vector<uint32_t> nodes; // (N,)
 35 |         std::vector<uint32_t> instances; // (N,)
 36 |         std::vector<Eigen::Vector3d> centroids; // (N,3)
 37 |         std::vector<std::vector<float>> features_vec; // (N,D)
 38 |         std::vector<Eigen::Vector3d> xyz; // (X,3)
 39 |         std::vector<uint32_t> labels; // (X,) node ids, each label in [0,N)
 40 | 
 41 |         void clear(){
 42 |             nodes.clear();
 43 |             instances.clear();
 44 |             centroids.clear();
 45 |             features_vec.clear();
 46 |             xyz.clear();
 47 |             labels.clear();
 48 |             N=0;
 49 |             X=0;
 50 |             D=0;
 51 |         };
 52 | 
 53 |         std::string print_msg()const{
 54 |             std::stringstream ss;
 55 |             for(int i=0;i<N;i++){
 56 |                 ss<<"instance: "<<instances[i]<<" centroid: "<<centroids[i].transpose();
 57 |                 ss<<"\n";
 58 |             }
 59 |             return ss.str();
 60 |         }
 61 | 
 62 |     };
 63 | 
 64 |     void convert_tensor(const std::vector<std::vector<float>> &features_vector, 
 65 |                     const int &N, const int &D, torch::Tensor &features_tensor);
 66 |                     
 67 |     class Communication
 68 |     {
 69 |         public:
 70 |             /// @brief Each MASTER_AGENT publish their features to "MASTER_AGENT/graph_topics"
 71 |             ///        And it subscribes to "OTHER_AGENT/graph_topics" for other agents' features.
 72 |             /// @param nh 
 73 |             /// @param nh_private 
 74 |             /// @param master_agent 
 75 |             /// @param other_agents 
 76 |             Communication(ros::NodeHandle &nh, ros::NodeHandle &nh_private,
 77 |                         std::string master_agent="agent_a", std::vector<std::string> other_agents={});
 78 |             ~Communication(){};
 79 | 
 80 |             // bool broadcast_coarse_graph(int frame_id,
 81 |             //                     const std::vector<uint32_t> &instances,
 82 |             //                     const std::vector<Eigen::Vector3d> &centroids,
 83 |             //                     const int& N, const int &D,
 84 |             //                     const std::vector<std::vector<float>> &features);
 85 | 
 86 | 
 87 |             /// @brief  Broadcast dense graph to other agents.
 88 |             ///         If X<1, broadcast the coarse graph.
 89 |             /// @param frame_id
 90 |             /// @param nodes (N,)
 91 |             /// @param instances (N,)
 92 |             /// @param centroids (N,3)
 93 |             /// @param features (N,D) coarse node features
 94 |             /// @param xyz (X,3) global point cloud
 95 |             /// @param labels (X,) node ids, each label in [0,N)
 96 |             bool broadcast_dense_graph(int frame_id,
 97 |                                     const std::vector<uint32_t> &nodes,
 98 |                                     const std::vector<uint32_t> &instances,
 99 |                                     const std::vector<Eigen::Vector3d> &centroids,
100 |                                     const std::vector<std::vector<float>> &features,
101 |                                     const std::vector<Eigen::Vector3d> &xyz,
102 |                                     const std::vector<uint32_t> &labels);
103 | 
104 |             /// @brief Publish request message to the target agent.
105 |             /// @param target_agent_name 
106 |             /// @return 
107 |             bool send_request_dense(const std::string &target_agent_name);
108 | 
109 |             // void coarse_graph_callback(const sgloop_ros::CoarseGraph::ConstPtr &msg,std::string agent_name);
110 | 
111 |             void dense_graph_callback(const sgloop_ros::DenseGraph::ConstPtr &msg,std::string agent_name);
112 |             
113 |             void request_dense_callback(const std_msgs::Int32::ConstPtr &msg);
114 | 
115 |             const AgentDataDict& get_remote_agent_data(const std::string agent_name)const;
116 | 
117 |             bool write_logs(const std::string &out_dir);
118 | 
119 |             bool get_pub_dense_msg()const{return pub_dense_msg;};
120 | 
121 |             void reset_pub_dense_msg(){pub_dense_msg=false;};
122 | 
123 |         public:
124 |             int pub_dense_frame_id; // master publish dense graph message     
125 | 
126 |         private:
127 |             float frame_duration; // in seconds
128 |             float time_offset; // in seconds
129 | 
130 |             // In masterAgent
131 |             ros::Publisher pub_dense_sg;
132 |             ros::Subscriber sub_request_dense;
133 |             std::string local_agent;
134 | 
135 |             // In otherAgents
136 |             std::vector<ros::Subscriber> agents_dense_subscriber;
137 |             std::map<std::string,ros::Publisher> pub_agents_request_dense;
138 |             std::map<std::string,AgentDataDict> agents_data_dict;
139 | 
140 |             std::vector<Log> pub_logs;
141 |             std::vector<Log> sub_logs;
142 | 
143 |             bool pub_dense_msg; 
144 | 
145 |             int last_exchange_frame_id; // At an exchange frame, the latest agent data dict is exported.
146 | 
147 |     };
148 |     
149 |     
150 | 
151 | }
152 | 
153 | 
154 | #endif // COMMUNICATION_H
155 | 
156 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/registration/Prune.h:
--------------------------------------------------------------------------------
 1 | #include <memory>
 2 | 
 3 | #include "Common.h"
 4 | #include "back_end/reglib.h"
 5 | // #include "tools/g3reg_api.h"
 6 | #include "sgloop/Graph.h"
 7 | 
 8 | 
 9 | namespace Registration
10 | {
11 | 
12 |     void pruneInsOutliers(const fmfusion::RegistrationConfig &config,
13 |                         const std::vector<fmfusion::NodePtr> &src_nodes,
14 |                         const std::vector<fmfusion::NodePtr> &ref_nodes,
15 |                         const std::vector<fmfusion::NodePair> &match_pairs,
16 |                         std::vector<bool> &pruned_true_masks);
17 |     
18 |     
19 | } // namespace Registration
20 | 
21 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/registration/robustPoseAvg.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by qzj on 6/24/24.
 3 | //
 4 | #include <gtsam/nonlinear/GncOptimizer.h>
 5 | #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 6 | #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 7 | #include <gtsam/geometry/Pose3.h>
 8 | 
 9 | namespace registration
10 | {
11 | gtsam::Pose3 gncRobustPoseAveraging(const std::vector<gtsam::Pose3> &input_poses,
12 |                                     const double &rot_sigma = 0.1,
13 |                                     const double &trans_sigma = 0.5);
14 | } // namespace registration


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/include/utility/CameraPoseVisualization.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <ros/ros.h>
 4 | #include <std_msgs/ColorRGBA.h>
 5 | #include <visualization_msgs/Marker.h>
 6 | #include <visualization_msgs/MarkerArray.h>
 7 | #include <Eigen/Dense>
 8 | #include <Eigen/Geometry>
 9 | #include <opencv2/opencv.hpp>
10 | // #include "../parameters.h"
11 | 
12 | class CameraPoseVisualization {
13 | public:
14 | 	std::string m_marker_ns;
15 | 
16 | 	CameraPoseVisualization(float r, float g, float b, float a);
17 | 	
18 | 	void setImageBoundaryColor(float r, float g, float b, float a=1.0);
19 | 	void setOpticalCenterConnectorColor(float r, float g, float b, float a=1.0);
20 | 	void setScale(double s);
21 | 	void setLineWidth(double width);
22 | 
23 | 	void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q);
24 | 	void reset();
25 | 
26 | 	void publish_by(ros::Publisher& pub, const std_msgs::Header& header);
27 | 	void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
28 | 	void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1);
29 | 	//void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src);
30 | 	void publish_image_by( ros::Publisher &pub, const std_msgs::Header &header);
31 | private:
32 | 	std::vector<visualization_msgs::Marker> m_markers;
33 | 	std_msgs::ColorRGBA m_image_boundary_color;
34 | 	std_msgs::ColorRGBA m_optical_center_connector_color;
35 | 	double m_scale;
36 | 	double m_line_width;
37 | 	visualization_msgs::Marker image;
38 | 	int LOOP_EDGE_NUM;
39 | 	int tmp_loop_edge_num;
40 | 
41 | 	static const Eigen::Vector3d imlt;
42 | 	static const Eigen::Vector3d imlb;
43 | 	static const Eigen::Vector3d imrt;
44 | 	static const Eigen::Vector3d imrb;
45 | 	static const Eigen::Vector3d oc  ;
46 | 	static const Eigen::Vector3d lt0 ;
47 | 	static const Eigen::Vector3d lt1 ;
48 | 	static const Eigen::Vector3d lt2 ;
49 | };
50 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/launch/render_our_sgslam.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="dataroot" value="/data2/sgslam"/>
 4 |   <!-- <arg name="agentA_scene" value="uc0204_00a"/> -->
 5 |   <!-- <arg name="agentB_scene" value="uc0204_00b"/> -->
 6 |   <arg name="agentA_scene" value="ab0201_03a"/>
 7 |   <arg name="agentB_scene" value="ab0201_03a"/>
 8 | 
 9 |   <arg name="agentA_agnetB_x" value="0"/>
10 |   <arg name="agentA_agentB_y" value="0"/>
11 |   <arg name="agentA_agentB_z" value="-4.0"/>
12 |   <arg name="agentA_agentB_yaw" value="0"/>
13 | 
14 |   <group ns="viz">
15 |     <param name="edge_width" value="0.03"/>
16 |     <param name="edge_color/r" value="0.0"/>
17 |     <param name="edge_color/g" value="1.0"/>
18 |     <param name="edge_color/b" value="1.0"/>
19 |     <param name="centroid_size" value="0.3"/>
20 |   </group>
21 | 
22 |   <!-- inital tf to enable visulization -->
23 |   <group ns="br">
24 |     <param name="agentA/x" value="0.0"/>
25 |     <param name="agentA/y" value="0.0"/>
26 |     <param name="agentA/z" value="0.0"/>
27 | 
28 |     <param name="agentB/x" value="$(arg agentA_agnetB_x)"/>
29 |     <param name="agentB/y" value="$(arg agentA_agentB_y)"/>
30 |     <param name="agentB/z" value="$(arg agentA_agentB_z)"/>
31 |     <param name="agentB/yaw" value="$(arg agentA_agentB_yaw)"/>
32 |   </group>
33 | 
34 |   <node pkg="sgloop_ros" name="agentA" type="PoseGraphNode" output="screen" clear_params="true">
35 |       <param name="cfg_file" value="/home/cliuci/code_ws/OpensetFusion/config/realsense.yaml"/>
36 |       <param name="src_name" value="agentA"/>
37 |       <param name="ref_name" value="agentB"/>
38 |       <param name="camera_marker/r" value="0.8"/>
39 |       <param name="camera_marker/g" value="0.0"/>
40 |       <param name="camera_marker/b" value="0.8"/>
41 |       <param name="camera_marker/scale" value="0.3"/>
42 |       <param name="camera_marker/line_width" value="0.05"/>
43 |       <param name="map_name" value="mesh_o3d.ply"/>
44 |       <!-- <param name="map_name" value="frame-000671_src.ply"/> -->
45 |       <!-- <param name="select_frame" value="frame-000671"/> -->
46 |       <param name="mode" value="15"/>
47 |       <param name="loop_filename" value="loops_tp_raw.txt"/>
48 |       <!-- <param name="pose_graph_folder" value="/data2/sgslam/output/v9/$(arg agentA_scene)/$(arg agentB_scene)"/> -->
49 |       <param name="pose_graph_folder" value="/data2/sgslam/output/v9/$(arg agentA_scene)"/>
50 |       <param name="src_scene_dir" value="$(arg dataroot)/scans/$(arg agentA_scene)"/>
51 |       <param name="ref_scene_dir" value="$(arg dataroot)/scans/$(arg agentB_scene)"/>
52 |       <param name="gt_file" value="$(arg dataroot)/gt/$(arg agentA_scene)-$(arg agentB_scene).txt"/>
53 |       <param name="ate_threshold" value="0.2"/>
54 |       <param name="render_loops" value="true"/>
55 |       <param name="render_trajectory" value="true"/>
56 |   </node>
57 | 
58 |   <!-- <node pkg="sgloop_ros" name="agentB" type="PoseGraphNode" output="screen" clear_params="true">
59 |       <param name="cfg_file" value="/home/cliuci/code_ws/OpensetFusion/config/realsense.yaml"/>
60 |       <param name="src_name" value="agentB"/>
61 |       <param name="ref_name" value="agentA"/>
62 |       <param name="camera_marker/r" value="0.0"/>
63 |       <param name="camera_marker/g" value="0.0"/>
64 |       <param name="camera_marker/b" value="1.0"/>
65 |       <param name="camera_marker/scale" value="0.3"/>
66 |       <param name="camera_marker/line_width" value="0.05"/>
67 |       <param name="map_name" value="mesh_o3d.ply"/>
68 |       <param name="mode" value="99"/>
69 |       <param name="pose_graph_folder" value="/data2/sgslam/output/v9/$(arg agentB_scene)/fakeScene"/>
70 |       <param name="src_scene_dir" value="$(arg dataroot)/scans/$(arg agentB_scene)"/>
71 |       <param name="render_loops" value="false"/>
72 |       <param name="render_trajectory" value="true"/>
73 |   </node> -->
74 | 
75 |   <node pkg="tf" type="static_transform_publisher" name="link_1st_agent" args="0 0 0 0 0 0 1 world agentA 100" />
76 |   <node pkg="tf" type="static_transform_publisher" name="link_2nd_agent" args="$(arg agentA_agnetB_x) $(arg agentA_agentB_y) $(arg agentA_agentB_z) 0 0 0 1 world agentB 100" />
77 | 
78 | </launch>
79 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/launch/render_semantic_map.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- <arg name="result_folder" value="/data2/sample_data/output"/> -->
 3 |   <arg name="cfg_file" value="$(find sgloop_ros)/../../../config/scannet.yaml"/>
 4 |   <arg name="result_folder" value="/data2/ScanNet/output/online_mapping"/>
 5 |   <arg name="agentA_scene" value="scene0025_00"/>
 6 | 
 7 |   <group ns="viz">
 8 |     <param name="edge_width" value="0.03"/>
 9 |     <param name="edge_color/r" value="0.0"/>
10 |     <param name="edge_color/g" value="1.0"/>
11 |     <param name="edge_color/b" value="1.0"/>
12 |     <param name="centroid_size" value="0.2"/>
13 |     <param name="centroid_v_offset" value="3.0"/>
14 |     <param name="annotation_size" value="0.3"/>
15 |     <param name="annotation_v_offset" value="0.3"/>
16 |   </group>
17 | 
18 |   <node pkg="sgloop_ros" name="agentA" type="RenderNode" output="screen" clear_params="true">
19 |       <param name="agent_name" value="agentA"/>
20 |       <param name="cfg_file" value="$(arg cfg_file)"/>
21 |       <param name="sequence_result_folder" value="$(arg result_folder)/$(arg agentA_scene)"/>
22 |       <param name="map_name" value=""/>
23 |       <!-- <param name="map_name" value="mesh_o3d.ply"/> -->
24 |   </node>
25 | 
26 |   <node pkg="tf" type="static_transform_publisher" name="link_1st_agent" args="0 0 0 0 0 0 1 world agentA 100" />
27 | 
28 | </launch>
29 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/launch/semantic_mapping.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- SET CONFIGURATION HERE -->
 3 |   <arg name="dataroot" value="/data2/ScanNet"/>
 4 |   <arg name="sequence_name" default="scene0025_00"/>
 5 |   <!-- <arg name="sequence_name" default="ab0201_03a"/> -->
 6 | 
 7 |   <!-- global settings  -->
 8 |   <arg name="cfg_file" value="$(find sgloop_ros)/../../../config/scannet.yaml"/>
 9 |   <arg name="verbose_level" value="0"/>
10 |   <arg name="output_folder" value="$(arg dataroot)/output/online_mapping"/>
11 | 
12 |   <group ns="viz"> 
13 |       <param name="edge_width" value="0.03"/>
14 |       <param name="edge_color/r" value="0.0"/>
15 |       <param name="edge_color/g" value="1.0"/>
16 |       <param name="edge_color/b" value="1.0"/>
17 |       <param name="centroid_size" value="0.2"/>
18 |       <param name="centroid_color/r" value="0.0"/>
19 |       <param name="centroid_color/g" value="0.0"/>
20 |       <param name="centroid_color/b" value="0.0"/>
21 |       <param name="annotation_size" value="0.3"/>
22 |       <param name="centroid_v_offset" value="0.0"/>
23 |       <param name="annotation_v_offset" value="0.3"/>
24 |   </group>
25 | 
26 |   <node pkg="sgloop_ros" name="agentA" type="MappingNode" output="screen" clear_params="true">
27 |     <param name="cfg_file" value="$(arg cfg_file)"/>
28 |     <param name="output_folder" value="$(arg output_folder)"/>
29 |     <param name="active_sequence_dir" value="$(arg dataroot)/val/$(arg sequence_name)"/>
30 |     <param name="local_agent" value="agentA"/>
31 |     <!-- <param name="association_name" value="data_association.txt"/> -->
32 |     <param name="association_name" value=""/>
33 |     <param name="max_frames" value="500"/>
34 |     <param name="frame_gap" value="5"/>
35 |     <param name="visualization" value="1"/>
36 |     <param name="o3d_verbose_level" value="$(arg verbose_level)"/>
37 |   </node>
38 | 
39 |   <node pkg="tf" type="static_transform_publisher" name="link_1st_agent" args="0 0 0 0 0 0 1 world agentA 100" />
40 | 
41 | </launch>
42 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/launch/slabim_mapping.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- SET CONFIGURATION HERE -->
 3 |   <arg name="dataroot" value="/data2/slabim"/>
 4 |   <arg name="sequence_name" default="xb0101_02"/>
 5 | 
 6 |   <!-- global settings  -->
 7 |   <arg name="cfg_file" value="$(find sgloop_ros)/../../../config/slabim.yaml"/>
 8 |   <arg name="verbose_level" value="2"/>
 9 |   <arg name="output_folder" value="$(arg dataroot)/output/online_mapping"/>
10 | 
11 |   <group ns="viz"> 
12 |       <param name="edge_width" value="0.03"/>
13 |       <param name="edge_color/r" value="0.0"/>
14 |       <param name="edge_color/g" value="1.0"/>
15 |       <param name="edge_color/b" value="1.0"/>
16 |       <param name="centroid_size" value="0.38"/>
17 |       <param name="centroid_color/r" value="0.0"/>
18 |       <param name="centroid_color/g" value="0.0"/>
19 |       <param name="centroid_color/b" value="0.0"/>
20 |       <param name="annotation_size" value="0.5"/>
21 |       <param name="centroid_v_offset" value="0.0"/>
22 |       <param name="annotation_v_offset" value="0.3"/>
23 |   </group>
24 | 
25 |   <node pkg="sgloop_ros" name="agentA" type="MappingNode" output="screen" clear_params="true">
26 |     <param name="cfg_file" value="$(arg cfg_file)"/>
27 |     <param name="output_folder" value="$(arg output_folder)"/>
28 |     <param name="active_sequence_dir" value="$(arg dataroot)/scans/$(arg sequence_name)"/>
29 |     <param name="local_agent" value="agentA"/>
30 | <!-- <param name="association_name" value="data_association.txt"/> -->
31 |     <param name="association_name" value=""/>
32 |     <param name="max_frames" value="5000"/>
33 |     <param name="frame_gap" value="1"/>
34 |     <param name="visualization" value="1"/>
35 |     <param name="o3d_verbose_level" value="$(arg verbose_level)"/>
36 |     <param name="debug" value="false"/>
37 |   </node>
38 | 
39 |   <node pkg="tf" type="static_transform_publisher" name="link_1st_agent" args="0 0 0 0 0 0 1 world agentA 100" />
40 | 
41 | </launch>
42 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/launch/visualize.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find sgloop_ros)/launch/mapping.rviz" />
3 | </launch>
4 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/msg/CoarseGraph.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header header
2 | int16[] instances
3 | geometry_msgs/Point[] centroids
4 | std_msgs/Float32MultiArray features


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/msg/DenseGraph.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header header
2 | int32 nodes_number
3 | int32 points_number
4 | int16[] nodes
5 | int16[] instances
6 | geometry_msgs/Point[] centroids
7 | std_msgs/Float32MultiArray features
8 | geometry_msgs/Point[] points
9 | int16[] labels


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>sgloop_ros</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The sgloop_ros package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="cliuci@todo.todo">cliuci</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/sgloop_ros</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>geometry_msgs</build_depend>
53 |   <build_depend>roscpp</build_depend>
54 |   <build_depend>rospy</build_depend>
55 |   <build_depend>std_msgs</build_depend>
56 |   <build_depend>sensor_msgs</build_depend>
57 |   <build_depend>open3d_conversions</build_depend>
58 |   <build_depend>message_generation</build_depend>
59 |   <build_export_depend>geometry_msgs</build_export_depend>
60 |   <build_export_depend>roscpp</build_export_depend>
61 |   <build_export_depend>rospy</build_export_depend>
62 |   <build_export_depend>std_msgs</build_export_depend>
63 |   <build_export_depend>sensor_msgs</build_export_depend>
64 |   <build_export_depend>open3d_conversions</build_export_depend>
65 |   <exec_depend>geometry_msgs</exec_depend>
66 |   <exec_depend>roscpp</exec_depend>
67 |   <exec_depend>rospy</exec_depend>
68 |   <exec_depend>std_msgs</exec_depend>
69 |   <exec_depend>sensor_msgs</exec_depend>
70 |   <exec_depend>open3d_conversions</exec_depend>
71 |   <exec_depend>message_runtime</exec_depend>
72 |   <depend>cv_bridge</depend>
73 |   <depend>nav_msgs</depend>
74 |   <depend>tf</depend>
75 | 
76 | 
77 |   <!-- The export tag contains other, unspecified, tags -->
78 |   <export>
79 |     <!-- Other tools can request additional information be placed here -->
80 | 
81 |   </export>
82 | </package>
83 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/src/MappingNode.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <memory>
  3 | #include <sys/stat.h>
  4 | #include <vector>
  5 | #include <fstream>
  6 | 
  7 | #include <ros/ros.h>
  8 | #include <tf/transform_broadcaster.h>
  9 | #include "tf/transform_listener.h"
 10 | 
 11 | #include <open3d/Open3D.h>
 12 | #include <opencv2/opencv.hpp>
 13 | 
 14 | #include "tools/Utility.h"
 15 | #include "tools/IO.h"
 16 | #include "tools/TicToc.h"
 17 | #include "mapping/SemanticMapping.h"
 18 | 
 19 | #include "Visualization.h"
 20 | 
 21 | int main(int argc, char **argv)
 22 | {
 23 |     using namespace fmfusion;
 24 |     using namespace open3d::utility::filesystem;
 25 |     using namespace open3d::io;
 26 | 
 27 |     ros::init(argc, argv, "MappingNode");
 28 |     ros::NodeHandle n;
 29 |     ros::NodeHandle nh_private("~");
 30 | 
 31 |     // Settings
 32 |     std::string config_file;
 33 |     std::string root_dir;
 34 |     std::string LOCAL_AGENT;
 35 | 
 36 |     assert(nh_private.getParam("cfg_file", config_file));
 37 |     assert(nh_private.getParam("active_sequence_dir", root_dir));
 38 |     nh_private.getParam("local_agent", LOCAL_AGENT);
 39 |     int frame_gap = nh_private.param("frame_gap", 1);
 40 |     std::string prediction_folder = nh_private.param("prediction_folder", std::string("prediction_no_augment"));
 41 |     std::string output_folder = nh_private.param("output_folder", std::string(""));
 42 |     std::string association_name = nh_private.param("association_name", std::string(""));
 43 |     std::string trajectory_name = nh_private.param("trajectory_name", std::string("trajectory.log"));
 44 |     int o3d_verbose_level = nh_private.param("o3d_verbose_level", 2);
 45 |     int visualization = nh_private.param("visualization", 0);
 46 |     int max_frames = nh_private.param("max_frames", 5000);
 47 |     bool debug = nh_private.param("debug", false);
 48 |     ROS_WARN("MappingNode started");
 49 | 
 50 |     // Inits
 51 |     Config *global_config;
 52 |     SemanticMapping *semantic_mapping;
 53 |     std::string sequence_name = *GetPathComponents(root_dir).rbegin();
 54 |     Visualization::Visualizer viz(n,nh_private);
 55 |     {
 56 |         global_config = utility::create_scene_graph_config(config_file, true);
 57 |         open3d::utility::SetVerbosityLevel((open3d::utility::VerbosityLevel)o3d_verbose_level);
 58 |         if(output_folder.size()>0 && !DirectoryExists(output_folder)) 
 59 |             MakeDirectory(output_folder);
 60 | 
 61 |         std::ofstream out_file(output_folder+"/config.txt");
 62 |         out_file<<utility::config_to_message(*global_config);
 63 |         out_file.close();
 64 | 
 65 |         semantic_mapping = new SemanticMapping(global_config->mapping_cfg, global_config->instance_cfg);
 66 |     }
 67 | 
 68 |     // Load frames information
 69 |     std::vector<fmfusion::IO::RGBDFrameDirs> rgbd_table;
 70 |     std::vector<Eigen::Matrix4d> pose_table;
 71 |     if(association_name.empty()) {
 72 |         ROS_WARN("--- Read all RGB-D frames in %s ---",root_dir.c_str());
 73 |         IO::construct_sorted_frame_table(root_dir,
 74 |                                         rgbd_table,
 75 |                                         pose_table);
 76 |         if(rgbd_table.size()>4000) return 0;
 77 |     }
 78 |     else{
 79 |         ROS_WARN("--- Read RGB-D frames from %s/%s ---",
 80 |                 root_dir.c_str(),
 81 |                 association_name.c_str());
 82 |         bool read_ret = IO::construct_preset_frame_table(root_dir,
 83 |                                                         association_name,
 84 |                                                         trajectory_name,
 85 |                                                         rgbd_table,
 86 |                                                         pose_table);
 87 |         if(!read_ret) return 0;
 88 |     }
 89 | 
 90 |     if(rgbd_table.empty()){
 91 |         ROS_ERROR("No RGB-D frames found in %s",root_dir.c_str());
 92 |         return 0;
 93 |     }    
 94 | 
 95 |     // Integration
 96 |     open3d::geometry::Image depth, color;
 97 |     int prev_frame_id = -100;
 98 |     int prev_save_frame = -100;
 99 |     fmfusion::TicTocSequence tic_toc_seq("# Load Integration Export", 3);
100 | 
101 |     for(int k=0;k<rgbd_table.size();k++){
102 |         IO::RGBDFrameDirs frame_dirs = rgbd_table[k];
103 |         std::string frame_name = frame_dirs.first.substr(frame_dirs.first.find_last_of("/")+1); // eg. frame-000000.png
104 |         frame_name = frame_name.substr(0,frame_name.find_last_of("."));
105 |         int seq_id = stoi(frame_name.substr(frame_name.find_last_of("-")+1));
106 |         if((seq_id-prev_frame_id)<frame_gap) continue;
107 |         if(seq_id>max_frames) break;
108 |         // map_timing.create_frame(seq_id);
109 |         std::cout<<"Processing frame "<<frame_name<<" ..."<<std::endl;
110 |         tic_toc_seq.tic();
111 | 
112 |         //
113 |         bool loaded;
114 |         std::vector<DetectionPtr> detections;
115 |         std::shared_ptr<open3d::geometry::RGBDImage> rgbd;
116 |         { // load RGB-D, detections
117 |             ReadImage(frame_dirs.second, depth);
118 |             ReadImage(frame_dirs.first, color);
119 |             rgbd = open3d::geometry::RGBDImage::CreateFromColorAndDepth(
120 |                 color, depth, global_config->mapping_cfg.depth_scale, global_config->mapping_cfg.depth_max, false);
121 |             
122 |             loaded = fmfusion::utility::LoadPredictions(root_dir+'/'+prediction_folder, frame_name, 
123 |                                                             global_config->mapping_cfg, global_config->instance_cfg.intrinsic.width_, global_config->instance_cfg.intrinsic.height_,
124 |                                                             detections);
125 |             tic_toc_seq.toc();
126 |         }
127 |         if(!loaded) {
128 |             ROS_WARN("Cannot find detection file at %s",root_dir+'/'+prediction_folder+"/"+frame_name);
129 |             continue;
130 |         }
131 |         semantic_mapping->integrate(seq_id,rgbd, pose_table[k], detections);
132 |         tic_toc_seq.toc();
133 |         prev_frame_id = seq_id;
134 | 
135 |         { // Viz poses
136 |             Visualization::render_camera_pose(pose_table[k], 
137 |                                             viz.camera_pose, 
138 |                                             LOCAL_AGENT, 
139 |                                             seq_id);
140 |             Visualization::render_path(pose_table[k], 
141 |                                         viz.path_msg, 
142 |                                         viz.path, 
143 |                                         LOCAL_AGENT, 
144 |                                         seq_id);
145 |             
146 |         }
147 | 
148 |         if(viz.pred_image.getNumSubscribers()>0) Visualization::render_rgb_detections(color, 
149 |                                                                                     detections, 
150 |                                                                                     viz.pred_image,
151 |                                                                                     LOCAL_AGENT);
152 | 
153 |         {// Viz 3D
154 |             Visualization::render_semantic_map(semantic_mapping->export_global_pcd(true,0.05),
155 |                                                     semantic_mapping->export_instance_centroids(0, debug),
156 |                                                     semantic_mapping->export_instance_annotations(0),
157 |                                                     viz,
158 |                                                     LOCAL_AGENT);
159 |         }
160 |         tic_toc_seq.toc();
161 |     }
162 | 
163 |     ROS_WARN("Finished sequence with %ld frames",rgbd_table.size());
164 |     { // Process at the end of the sequence
165 |         semantic_mapping->extract_point_cloud();
166 |         semantic_mapping->merge_floor(true);
167 |         // semantic_mapping->merge_overlap_instances();
168 | 
169 |         Visualization::render_semantic_map(semantic_mapping->export_global_pcd(true,0.05),
170 |                             semantic_mapping->export_instance_centroids(0, debug),
171 |                             semantic_mapping->export_instance_annotations(0),
172 |                             viz,
173 |                             LOCAL_AGENT);
174 |     }
175 | 
176 |     // Save
177 |     ROS_WARN("Save results to %s",output_folder.c_str());
178 |     semantic_mapping->Save(output_folder+"/"+sequence_name);
179 |     tic_toc_seq.export_data(output_folder+"/"+sequence_name+"/time_records.txt");
180 |     fmfusion::utility::write_config(output_folder+"/"+sequence_name+"/config.txt",*global_config);
181 | 
182 | 
183 |     return 0;
184 | }
185 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/src/RenderNode.cpp:
--------------------------------------------------------------------------------
 1 | #include <sstream>
 2 | #include <unordered_map>
 3 | 
 4 | #include "ros/ros.h"
 5 | #include "std_msgs/String.h"
 6 | #include "sensor_msgs/PointCloud2.h"
 7 | #include "open3d_conversions/open3d_conversions.h"
 8 | 
 9 | #include "tools/IO.h"
10 | 
11 | #include "Visualization.h"
12 | #include "mapping/SemanticMapping.h"
13 | 
14 | int main(int argc, char **argv)
15 | {
16 |     using namespace open3d::utility::filesystem;
17 |     using namespace fmfusion;
18 | 
19 |     ros::init(argc, argv, "RenderNode");
20 |     ros::NodeHandle nh;
21 |     ros::NodeHandle nh_private("~");
22 | 
23 |     // Params
24 |     std::string config_file;
25 |     std::string scene_result_folder;
26 |     assert(nh_private.getParam("cfg_file", config_file));
27 |     assert(nh_private.getParam("sequence_result_folder", scene_result_folder));
28 |     std::string agent_name = nh_private.param("agent_name", std::string("agentA"));
29 |     std::string map_name = nh_private.param("map_name", std::string(""));
30 |     int iter_num = nh_private.param("iter_num", 3);
31 |     std::string sequence_name = *GetPathComponents(scene_result_folder).rbegin();
32 |     ROS_WARN("Render %s semantic mapping results from %s", agent_name.c_str(),
33 |                                                         scene_result_folder.c_str());
34 | 
35 |     // Init
36 |     Config *global_config;
37 |     SemanticMapping *semantic_mapping;
38 |     Visualization::Visualizer viz(nh,nh_private);
39 |     {
40 |         global_config = utility::create_scene_graph_config(config_file, false);
41 |         semantic_mapping = new SemanticMapping(global_config->mapping_cfg, global_config->instance_cfg);
42 |     }
43 | 
44 |     // Load
45 |     semantic_mapping->load(scene_result_folder);
46 |     semantic_mapping->refresh_all_semantic_dict();
47 |     semantic_mapping->merge_floor(false);
48 |     semantic_mapping->merge_overlap_instances();
49 | 
50 |     // Viz
51 |     for(int i=0;i<iter_num;i++){
52 |         O3d_Cloud_Ptr render_map_pcd;
53 |         if(map_name.size()>0){
54 |             render_map_pcd = open3d::io::CreatePointCloudFromFile(scene_result_folder+"/"+map_name);
55 |             ROS_WARN("Load and render map from %s", map_name.c_str());
56 |         }
57 |         else
58 |             render_map_pcd = semantic_mapping->export_global_pcd(true,0.05);
59 | 
60 |         Visualization::render_semantic_map(render_map_pcd,
61 |                                         semantic_mapping->export_instance_centroids(0),
62 |                                         semantic_mapping->export_instance_annotations(0),
63 |                                         viz,
64 |                                         agent_name);
65 |         ros::Duration(0.5).sleep();
66 |         ros::spinOnce();
67 |     }
68 | 
69 | 
70 |     return 0;
71 | 
72 | }
73 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/src/registration/Prune.cpp:
--------------------------------------------------------------------------------
  1 | #include "registration/Prune.h"
  2 | 
  3 | namespace Registration
  4 | {
  5 | 
  6 | void pruneInsOutliers(const fmfusion::RegistrationConfig &config,
  7 |                       const std::vector<fmfusion::NodePtr> &src_nodes,
  8 |                       const std::vector<fmfusion::NodePtr> &ref_nodes,
  9 |                       const std::vector<fmfusion::NodePair> &match_pairs,
 10 |                       std::vector<bool> &pruned_true_masks) {
 11 | 
 12 | //    存储配准单位
 13 |     std::vector<clique_solver::GraphVertex::Ptr> v1, v2;
 14 | //    一致性图的节点有多种类型
 15 |     clique_solver::VertexInfo vertex_info;
 16 | //    不同的节点会有不同的一致性测量函数
 17 |     vertex_info.type = clique_solver::VertexType::POINT;
 18 | //    一致性判断的阈值，可以设置为多个，从小到大，会得到多个内点结果。
 19 | //  由于Instance中心点不确定性较大，我们这里只选取一个比较大的阈值，不做精确估计
 20 | //  注意，阈值越大，图越稠密，最大团求解时间也会上升
 21 |     vertex_info.noise_bound_vec = config.noise_bound_vec; //{1.0};
 22 | //    初始化配准节点
 23 |     for (const auto &src_node: src_nodes) {
 24 |         const Eigen::Vector3d &center = src_node->centroid;
 25 |         v1.push_back(clique_solver::create_vertex(center, vertex_info));
 26 |     }
 27 |     for (const auto &ref_node: ref_nodes) {
 28 |         const Eigen::Vector3d &center = ref_node->centroid;
 29 |         v2.push_back(clique_solver::create_vertex(center, vertex_info));
 30 |     }
 31 | 
 32 | //    匹配的数量
 33 |     uint64_t num_corr = match_pairs.size();
 34 | //    TRIM(Translation Rotation Invariant Measurements)的数量
 35 |     uint64_t num_tims = num_corr * (num_corr - 1) / 2;
 36 | //    初始化一致性图和最大团
 37 |     std::vector<clique_solver::Graph> graphs;
 38 |     std::vector<std::vector<int>> max_cliques;
 39 |     int num_graphs = vertex_info.noise_bound_vec.size();
 40 |     for (int i = 0; i < num_graphs; ++i) {
 41 |         clique_solver::Graph graph;
 42 |         graph.clear();
 43 |         graph.setType(false);
 44 |         graph.populateVertices(num_corr);
 45 |         graphs.push_back(graph);
 46 |         max_cliques.push_back(std::vector<int>());
 47 |     }
 48 | 
 49 | //    构建多个一致性图
 50 | #pragma omp parallel for default(none) shared(num_corr, num_tims, v1, v2, graphs, match_pairs, num_graphs)
 51 |     for (size_t k = 0; k < num_tims; ++k) {
 52 |         size_t i, j;
 53 | //        一共需要进行num_tims次边的计算，第k次对应第i，j个节点
 54 |         std::tie(i, j) = clique_solver::k2ij(k, num_corr);
 55 | //        由于之前的noise_bound_vec会有多个阈值，因此results也是多个
 56 |         const auto &results = (*v1[match_pairs[j].first] - *v1[match_pairs[i].first])->consistent(
 57 |                 *(*v2[match_pairs[j].second] - *v2[match_pairs[i].second]));
 58 |         for (int level = 0; level < num_graphs; ++level) {
 59 | //            判断是否通过一致性检验
 60 |             if (results(level) > 0.0) {
 61 | #pragma omp critical
 62 |                 {
 63 |                     graphs[level].addEdge(i, j);
 64 |                 }
 65 |             }
 66 |         }
 67 |     }
 68 | 
 69 | 
 70 | //  求解最大团
 71 |     clique_solver::MaxCliqueSolver::Params clique_params;
 72 |     clique_params.solver_mode = clique_solver::MaxCliqueSolver::CLIQUE_SOLVER_MODE::PMC_EXACT;
 73 |     int prune_level = 0;
 74 | //    渐进式求解每个图的最大团
 75 |     for (int level = 0; level < num_graphs; ++level) {
 76 |         clique_solver::MaxCliqueSolver mac_solver(clique_params);
 77 |         max_cliques[level] = mac_solver.findMaxClique(graphs[level], prune_level);
 78 |         prune_level = max_cliques[level].size();
 79 |     }
 80 | //    最大团的元素表示第几个匹配关系是正确的。排序，为了好看。
 81 |     for (int level = 0; level < num_graphs; ++level) {
 82 |         auto &clique = max_cliques[level];
 83 |         std::sort(clique.begin(), clique.end());
 84 |     }
 85 | 
 86 |     std::stringstream msg;
 87 |     msg << "The inlier match pairs are: \n";
 88 | //    默认只有一个noise bound，我们只看第一个最大团的结果
 89 |     for (auto i: max_cliques[0]) {
 90 |         auto pair = match_pairs[i];
 91 |         pruned_true_masks[i] = true;
 92 |         const auto &src_node = src_nodes[pair.first];
 93 |         const auto &ref_node = ref_nodes[pair.second];
 94 |         msg << "(" << pair.first << "," << pair.second << ") "
 95 |             << "(" << src_node->semantic << "," << ref_node->semantic << ")\n";
 96 |     }
 97 |     // std::cout << msg.str() << std::endl;
 98 | }
 99 | 
100 |     
101 | }
102 | 


--------------------------------------------------------------------------------
/catkin_ws/src/sgloop_ros/src/registration/robustPoseAvg.cpp:
--------------------------------------------------------------------------------
 1 | #include "registration/robustPoseAvg.h"
 2 | 
 3 | namespace registration
 4 | {
 5 | gtsam::Pose3 gncRobustPoseAveraging(const std::vector<gtsam::Pose3> &input_poses,
 6 |                                     const double &rot_sigma,
 7 |                                     const double &trans_sigma) {
 8 |     gtsam::Values initial;
 9 |     initial.insert(0, gtsam::Pose3());  // identity pose as initialization
10 | 
11 |     gtsam::NonlinearFactorGraph graph;
12 |     gtsam::Vector sigmas;
13 |     sigmas.resize(6);
14 |     sigmas.head(3).setConstant(rot_sigma);
15 |     sigmas.tail(3).setConstant(trans_sigma);
16 |     const gtsam::noiseModel::Diagonal::shared_ptr noise =
17 |             gtsam::noiseModel::Diagonal::Sigmas(sigmas);
18 |     // add measurements
19 |     for (const auto &pose: input_poses) {
20 |         graph.add(gtsam::PriorFactor<gtsam::Pose3>(0, pose, noise));
21 |     }
22 | 
23 |     gtsam::GncParams<gtsam::LevenbergMarquardtParams> gncParams;
24 |     auto gnc = gtsam::GncOptimizer<gtsam::GncParams<gtsam::LevenbergMarquardtParams>>(graph, initial, gncParams);
25 | 
26 |     gnc.setInlierCostThresholdsAtProbability(0.99);
27 | 
28 |     gtsam::Values estimate = gnc.optimize();
29 |     return estimate.at<gtsam::Pose3>(0);
30 | }
31 | 
32 | 
33 | } // namespace registration


--------------------------------------------------------------------------------
/cmake/eigen.cmake:
--------------------------------------------------------------------------------
1 | find_package(Eigen3 REQUIRED)
2 | include_directories(${EIGEN3_INCLUDE_DIR})
3 | message(${EIGEN3_INCLUDE_DIR})


--------------------------------------------------------------------------------
/cmake/glog.cmake:
--------------------------------------------------------------------------------
1 | find_package(glog REQUIRED)
2 | include_directories(${GLOG_INCLUDE_DIRS})
3 | list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES})
4 | 


--------------------------------------------------------------------------------
/cmake/gtsam.cmake:
--------------------------------------------------------------------------------
1 | find_package(GTSAM REQUIRED)
2 | include_directories(${GTSAM_INCLUDE_DIR})
3 | list(APPEND ALL_TARGET_LIBRARIES gtsam)
4 | message(STATUS "GTSAM VERSION: ${GTSAM_VERSION} ${GTSAM_INCLUDE_DIR} ${gtsam}")


--------------------------------------------------------------------------------
/cmake/jsoncpp.cmake:
--------------------------------------------------------------------------------
1 | find_package(jsoncpp REQUIRED)
2 | include_directories(${jsoncpp_INCLUDE_DIRS})
3 | list(APPEND ALL_TARGET_LIBRARIES jsoncpp_lib)


--------------------------------------------------------------------------------
/cmake/open3d.cmake:
--------------------------------------------------------------------------------
1 | find_package(Open3D REQUIRED)
2 | include_directories(${Open3D_INCLUDE_DIRS})
3 | list(APPEND ALL_TARGET_LIBRARIES Open3D::Open3D)
4 | message(STATUS "Open3D_LIBRARIES: ${Open3D_LIBRARIES}")
5 | 


--------------------------------------------------------------------------------
/cmake/opencv.cmake:
--------------------------------------------------------------------------------
1 | find_package(OpenCV REQUIRED)
2 | include_directories(${OpenCV_INCLUDE_DIRS})
3 | list(APPEND ALL_TARGET_LIBRARIES ${OpenCV_LIBS})


--------------------------------------------------------------------------------
/config/fusion_portable.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "fusion_portable"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 1024
 7 | image_height: 768
 8 | camera_fx: 605.128
 9 | camera_fy: 604.974
10 | camera_cx: 520.45
11 | camera_cy: 393.88
12 | depth_scale: 1000.0
13 | depth_max: 20.0
14 | 
15 | # volumetric 
16 | voxel_length: 0.04
17 | sdf_trunc: 0.6
18 | min_instance_points: 100 # for initial new instance
19 | 
20 | # associations
21 | min_det_masks: 30 # pixel-wise
22 | max_box_area_ratio: 0.9 # box_area/image_area
23 | # min_instance_masks: 20 # pixel-wise
24 | dilate_kernal: 3 
25 | min_iou: 0.3
26 | 
27 | # shape
28 | min_voxel_weight: 0.05 # weight/observation_count
29 | 
30 | # merge over-segmentation
31 | merge_inflation: 3.5
32 | merge_iou: 0.4
33 | 
34 | # output
35 | save_da_images: 1
36 | tmp_dir: "/media/lch/SeagateExp/dataset_/FusionPortable/output"
37 | 


--------------------------------------------------------------------------------
/config/matterport.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "scannet"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 640
 7 | image_height: 512
 8 | camera_fx: 535.825
 9 | camera_fy: 536.13
10 | camera_cx: 315.4215
11 | camera_cy: 252.159
12 | 
13 | Mapping:
14 |   # Instance 
15 |   voxel_length: 0.02
16 |   sdf_trunc: 0.04
17 |   min_voxel_weight: 0.1 # weight/observation_count
18 | 
19 |   #
20 |   depth_scale: 4000.0
21 |   depth_max: 4.0
22 | 
23 |   # associations
24 |   min_det_masks: 50 # pixel-wise
25 |   min_active_points: 50 # active instances
26 |   max_box_area_ratio: 0.9 # box_area/image_area
27 |   query_depth_vx_size: -1.0
28 |   dilate_kernal: 3 
29 |   min_iou: 0.2
30 |   search_radius: 4.0
31 | 
32 |   # merge over-segmentation
33 |   merge_inflation: 4.0
34 |   merge_iou: 0.15
35 |   realtime_merge_floor: 1
36 |   recent_window_size: 240
37 |   update_period: 20
38 | 
39 |   # Bayesian fusion
40 |   bayesian_semantic_likelihood: ""
41 | 
42 |   # export
43 |   shape_min_points: 50 # export and bbox extract
44 | 
45 |   save_da_dir: "" #"/data2/sgslam/output/da"
46 | # 
47 | Graph:
48 |   edge_radius_ratio: 2.0
49 |   voxel_size: 0.02
50 |   involve_floor_edge: 1
51 |   ignore_labels: "ceiling." # Append " floor. carpet." if ignore floors
52 | 
53 | SGNet:
54 |   triplet_number: 20
55 |   instance_match_threshold: 0.05
56 |   warm_up: 10
57 | 
58 | ShapeEncoder:
59 |   init_voxel_size: 0.05
60 |   init_radius: 0.0625
61 |   padding: "random"
62 |   K_shape_samples: 1024
63 |   K_match_samples: 128
64 | 
65 | LoopDetector:
66 |   fuse_shape: 1 # set to 1 if fuse node feature with shape feature
67 |   lcd_nodes: 3
68 |   recall_nodes: 2
69 | 
70 | Registration:
71 |   noise_bound: 1.0
72 | 
73 | # output
74 | save_da_images: 0
75 | tmp_dir: "/media/lch/SeagateExp/bag/sgslam/output"
76 | 


--------------------------------------------------------------------------------
/config/realsense.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "realsense"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 640
 7 | image_height: 480
 8 | camera_fx: 619.18
 9 | camera_fy: 618.17
10 | camera_cx: 336.51
11 | camera_cy: 246.95
12 | 
13 | Mapping:
14 |   # Instance 
15 |   voxel_length: 0.02
16 |   sdf_trunc: 0.04
17 |   min_voxel_weight: 0.1 # weight/observation_count
18 | 
19 |   #
20 |   depth_scale: 1000.0
21 |   depth_max: 4.0
22 | 
23 |   # associations
24 |   min_det_masks: 150 # pixel-wise
25 |   min_active_points: 80 # active instances
26 |   max_box_area_ratio: 0.9 # box_area/image_area
27 |   query_depth_vx_size: -1.0
28 |   dilate_kernal: 3 
29 |   min_iou: 0.2
30 |   search_radius: 4.0
31 | 
32 |   # merge over-segmentation
33 |   merge_inflation: 3.5
34 |   merge_iou: 0.4
35 |   realtime_merge_floor: 1
36 |   recent_window_size: 240
37 |   update_period: 20
38 | 
39 |   # Bayesian fusion
40 |   bayesian_semantic_likelihood: "" # To enable it, set it to the likelihood matrix path
41 | 
42 |   # export
43 |   shape_min_points: 500 # export and bbox extract
44 | 
45 |   save_da_dir: "" #"/data2/sgslam/output/da"
46 | # 
47 | Graph:
48 |   edge_radius_ratio: 2.0
49 |   voxel_size: 0.02
50 |   involve_floor_edge: 1
51 |   ignore_labels: "ceiling." # Append " floor. carpet." if ignore floors
52 | 
53 | SGNet:
54 |   triplet_number: 20
55 |   instance_match_threshold: 0.05
56 |   warm_up: 10
57 | 
58 | ShapeEncoder:
59 |   init_voxel_size: 0.05
60 |   init_radius: 0.0625
61 |   padding: "random"
62 |   K_shape_samples: 1024
63 |   K_match_samples: 128
64 | 
65 | LoopDetector:
66 |   fuse_shape: 1 # set to 1 if fuse node feature with shape feature
67 |   lcd_nodes: 3
68 |   recall_nodes: 2
69 | 
70 | Registration:
71 |   noise_bound: 1.0
72 | 
73 | # output
74 | save_da_images: 0
75 | tmp_dir: "/media/lch/SeagateExp/bag/sgslam/output"
76 | 


--------------------------------------------------------------------------------
/config/render_3rscan.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "realsense"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 640
 7 | image_height: 480
 8 | camera_fx: 619.18
 9 | camera_fy: 618.17
10 | camera_cx: 336.51
11 | camera_cy: 246.95
12 | 
13 | Mapping:
14 |   # Instance 
15 |   voxel_length: 0.02
16 |   sdf_trunc: 0.04
17 |   min_voxel_weight: 0.1 # weight/observation_count
18 | 
19 |   #
20 |   depth_scale: 1000.0
21 |   depth_max: 4.0
22 | 
23 |   # associations
24 |   min_det_masks: 150 # pixel-wise
25 |   min_active_points: 80 # active instances
26 |   max_box_area_ratio: 0.9 # box_area/image_area
27 |   query_depth_vx_size: -1.0
28 |   dilate_kernal: 3 
29 |   min_iou: 0.2
30 |   search_radius: 4.0
31 | 
32 |   # merge over-segmentation
33 |   merge_inflation: 3.5
34 |   merge_iou: 0.4
35 |   cleanup_period: 6600
36 |   recent_window_size: 240
37 |   update_period: 20
38 | 
39 |   # export
40 |   shape_min_points: 100 # export and bbox extract
41 | 
42 |   save_da_dir: "" #"/data2/sgslam/output/da"
43 | # 
44 | Graph:
45 |   edge_radius_ratio: 2.0
46 |   voxel_size: 0.02
47 |   involve_floor_edge: 1
48 |   ignore_labels: "ceiling" # Append " floor. carpet." if ignore floors
49 | 
50 | SGNet:
51 |   triplet_number: 20
52 |   instance_match_threshold: 0.05
53 |   warm_up: 10
54 | 
55 | ShapeEncoder:
56 |   init_voxel_size: 0.05
57 |   init_radius: 0.0625
58 |   padding: "random"
59 |   K_shape_samples: 1024
60 |   K_match_samples: 128
61 | 
62 | LoopDetector:
63 |   fuse_shape: 1 # set to 1 if fuse node feature with shape feature
64 |   lcd_nodes: 3
65 |   recall_nodes: 2
66 | 
67 | Registration:
68 |   noise_bound: 1.0
69 | 
70 | # output
71 | save_da_images: 0
72 | tmp_dir: "/media/lch/SeagateExp/bag/sgslam/output"
73 | 


--------------------------------------------------------------------------------
/config/rio.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "rio"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 960
 7 | image_height: 540
 8 | camera_fx: 756.832
 9 | camera_fy: 756.026
10 | camera_cx: 492.889
11 | camera_cy: 270.419
12 | depth_scale: 1000.0
13 | depth_max: 5.0
14 | 
15 | # volumetric 
16 | voxel_length: 0.02
17 | sdf_trunc: 0.04
18 | min_active_points: 100 # active instances
19 | shape_min_points: 500 # export and bbox extract
20 | 
21 | # associations
22 | min_det_masks: 100 # pixel-wise
23 | max_box_area_ratio: 0.9 # box_area/image_area
24 | query_depth_vx_size: -1.0
25 | dilate_kernal: 3 
26 | min_iou: 0.3
27 | search_radius: 10.0
28 | 
29 | # shape
30 | min_voxel_weight: 0.1 # weight/observation_count
31 | 
32 | # merge over-segmentation
33 | merge_inflation: 3.5
34 | merge_iou: 0.4
35 | cleanup_period: 6600
36 | 
37 | # output
38 | save_da_images: 0 # 1 for save
39 | tmp_dir: "/data2/3rscan_raw/viz"
40 | 


--------------------------------------------------------------------------------
/config/scannet.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "scannet"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 640
 7 | image_height: 480
 8 | camera_fx: 557.59
 9 | camera_fy: 578.73
10 | camera_cx: 318.91
11 | camera_cy: 242.68
12 | 
13 | Mapping:
14 |   # Instance 
15 |   voxel_length: 0.02
16 |   sdf_trunc: 0.04
17 |   min_voxel_weight: 0.1 # weight/observation_count
18 | 
19 |   #
20 |   depth_scale: 1000.0
21 |   depth_max: 4.0
22 | 
23 |   # associations
24 |   min_det_masks: 100 # pixel-wise
25 |   min_active_points: 80 # active instances
26 |   max_box_area_ratio: 0.9 # box_area/image_area
27 |   query_depth_vx_size: -1.0
28 |   dilate_kernal: 3 
29 |   min_iou: 0.3
30 |   search_radius: 4.0
31 | 
32 |   # merge over-segmentation
33 |   merge_inflation: 4.0
34 |   merge_iou: 0.3
35 |   realtime_merge_floor: 1
36 |   recent_window_size: 240
37 |   update_period: 20
38 | 
39 |   # Bayesian fusion
40 |   bayesian_semantic_likelihood: "/home/cliuci/code_ws/OpensetFusion/measurement_model/bayesian_model.txt"
41 | 
42 |   # export
43 |   shape_min_points: 500 # export and bbox extract
44 | 
45 |   save_da_dir: "" #"/data2/sgslam/output/da"
46 | # 
47 | Graph:
48 |   edge_radius_ratio: 2.0
49 |   voxel_size: 0.02
50 |   involve_floor_edge: 1
51 |   ignore_labels: "ceiling." # Append " floor. carpet." if ignore floors
52 | 
53 | SGNet:
54 |   triplet_number: 20
55 |   instance_match_threshold: 0.05
56 |   warm_up: 10
57 | 
58 | ShapeEncoder:
59 |   init_voxel_size: 0.05
60 |   init_radius: 0.0625
61 |   padding: "random"
62 |   K_shape_samples: 1024
63 |   K_match_samples: 128
64 | 
65 | LoopDetector:
66 |   fuse_shape: 1 # set to 1 if fuse node feature with shape feature
67 |   lcd_nodes: 3
68 |   recall_nodes: 2
69 | 
70 | Registration:
71 |   noise_bound: 1.0
72 | 
73 | # output
74 | save_da_images: 0
75 | tmp_dir: "/media/lch/SeagateExp/bag/sgslam/output"
76 | 


--------------------------------------------------------------------------------
/config/slabim.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | dataset: "realsense"  # scannet, fusion_portable, realsense
 4 | 
 5 | # camera parameters 
 6 | image_width: 1600
 7 | image_height: 1600
 8 | camera_fx: 800.0
 9 | camera_fy: 800.0
10 | camera_cx: 800.0
11 | camera_cy: 800.0
12 | 
13 | Mapping:
14 |   # Instance 
15 |   voxel_length: 0.05
16 |   sdf_trunc: 0.04
17 |   min_voxel_weight: 0.1 # weight/observation_count
18 | 
19 |   #
20 |   depth_scale: 1000.0
21 |   depth_max: 10.0
22 | 
23 |   # associations
24 |   min_det_masks: 50 # pixel-wise
25 |   min_active_points: 80 # active instances
26 |   max_box_area_ratio: 0.9 # box_area/image_area
27 |   query_depth_vx_size: -1.0
28 |   dilate_kernal: 3 
29 |   min_iou: 0.2
30 |   search_radius: 4.0
31 | 
32 |   # merge over-segmentation
33 |   merge_inflation: 3.5
34 |   merge_iou: 0.4
35 |   realtime_merge_floor: 1
36 |   recent_window_size: 240
37 |   update_period: 1
38 | 
39 |   # Bayesian fusion
40 |   bayesian_semantic_likelihood: "" # To enable it, set it to the likelihood matrix path
41 | 
42 |   # export
43 |   shape_min_points: 20 # export and bbox extract
44 | 
45 |   save_da_dir: "" #"/data2/sgslam/output/da"
46 | # 
47 | Graph:
48 |   edge_radius_ratio: 2.0
49 |   voxel_size: 0.02
50 |   involve_floor_edge: 1
51 |   ignore_labels: "ceiling." # Append " floor. carpet." if ignore floors
52 | 
53 | SGNet:
54 |   triplet_number: 20
55 |   instance_match_threshold: 0.05
56 |   warm_up: 10
57 | 
58 | ShapeEncoder:
59 |   init_voxel_size: 0.05
60 |   init_radius: 0.0625
61 |   padding: "random"
62 |   K_shape_samples: 1024
63 |   K_match_samples: 128
64 | 
65 | LoopDetector:
66 |   fuse_shape: 1 # set to 1 if fuse node feature with shape feature
67 |   lcd_nodes: 3
68 |   recall_nodes: 2
69 | 
70 | Registration:
71 |   noise_bound: 1.0
72 | 
73 | # output
74 | save_da_images: 0
75 | tmp_dir: "/media/lch/SeagateExp/bag/sgslam/output"
76 | 


--------------------------------------------------------------------------------
/doc/DATA.md:
--------------------------------------------------------------------------------
 1 | # Data Format
 2 | 
 3 | ## 1. Input RGB-D Sequence
 4 | In SgSlam and ScanNet, their data sequence are collected from RGB-D sensors.
 5 | The data format is as follow,
 6 | ```bash
 7 | |---ab0201_03a
 8 |     |---color
 9 |     |---depth
10 |     |---pose
11 |     |---intrinsic
12 |     |---prediction_no_augment
13 |         |---frame-000000_label.json # Image taging and detection results
14 |         |---frame-000000_mask.png # Pixel-wise instance mask
15 |         |--- ...
16 | ```
17 | In the sequence name ```ab0201_03a```, ```ab0201``` is a scene id and ```03a``` is a scanning index.
18 | 
19 | ## 2. Output Results
20 | At the end of each sequence, the mapping node save the results in a pre-defined output folder. The output format is as below.
21 | ```bash
22 | |---output
23 |     |---ab0201_03a
24 |         |---instance_map.ply # points are colored instance-wise
25 |         |---instance_info.txt
26 |         |---instance_box.txt
27 |         |---${idx}.ply (intanace-wise point cloud)
28 |         |--- ...
29 | ```
30 | 
31 | ## 3. Dataset Folder
32 | We organize the dataset folder as below. 
33 | ```bash
34 | ${SGSLAM_DATAROOT}
35 | |---splits
36 |     |---val.txt
37 | |---scans
38 |     |---ab0201_03a
39 |     |--- ...
40 | |---output
41 |     |---online_mapping # mapping results
42 | ```
43 | ```ScanNet``` dataset folder is organized in the same way.


--------------------------------------------------------------------------------
/doc/rviz_gui.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/doc/rviz_gui.png


--------------------------------------------------------------------------------
/doc/seng_map_720p.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/doc/seng_map_720p.gif


--------------------------------------------------------------------------------
/doc/system.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/doc/system.png


--------------------------------------------------------------------------------
/doc/ust_atrium_light.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/doc/ust_atrium_light.gif


--------------------------------------------------------------------------------
/measurement_model/bayesian/detection_likelihood.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/measurement_model/bayesian/detection_likelihood.png


--------------------------------------------------------------------------------
/measurement_model/bayesian/likelihood_matrix.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/measurement_model/bayesian/likelihood_matrix.png


--------------------------------------------------------------------------------
/measurement_model/bayesian/ram_likelihood.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/measurement_model/bayesian/ram_likelihood.png


--------------------------------------------------------------------------------
/measurement_model/bayesian_model.txt:
--------------------------------------------------------------------------------
 1 | # openset_names closet_names likelihood_matrix
 2 | wall,tile wall,floor,carpet,cabinet,closet,file cabinet,bed,dresser,chair,armchair,swivel chair,couch,table,side table,round table,kitchen table,door,glass door,window,bookshelf,shelf,picture,picture frame,poster,counter top,kitchen counter,computer desk,office desk,curtain,fridge,shower curtain,toilet bowl,sink,bathroom sink,bath,tub,bin
 3 | wall,floor,cabinet,bed,chair,sofa,table,door,window,bookshelf,picture,counter,desk,curtain,refrigerator,shower curtain,toilet,sink,bathtub,otherfurniture
 4 | 0.075617,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.009470,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
 5 | 0.080379,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.011047,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
 6 | 0.009790,0.720144,0.011175,0.000000,0.002166,0.000000,0.009081,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.009830
 7 | 0.000000,0.074926,0.000000,0.000000,0.000000,0.000000,0.019295,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.006994
 8 | 0.026366,0.000000,0.397324,0.000000,0.000000,0.000000,0.000000,0.027258,0.015156,0.121678,0.000000,0.000000,0.021404,0.000000,0.069495,0.000000,0.000000,0.000000,0.000000,0.049126
 9 | 0.065171,0.000000,0.120085,0.000000,0.000000,0.000000,0.000000,0.215352,0.019302,0.063645,0.000000,0.000000,0.000000,0.000000,0.029548,0.000000,0.000000,0.000000,0.000000,0.056627
10 | 0.000000,0.000000,0.079965,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.056741,0.000000,0.000000,0.023806,0.000000,0.027828,0.000000,0.000000,0.000000,0.000000,0.016295
11 | 0.014688,0.000000,0.000000,0.763208,0.000000,0.016948,0.008452,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.042558
12 | 0.000000,0.000000,0.096581,0.000000,0.000000,0.000000,0.004585,0.000000,0.000000,0.047322,0.000000,0.000000,0.052364,0.000000,0.036029,0.000000,0.000000,0.000000,0.000000,0.032088
13 | 0.000000,0.000000,0.000000,0.017061,0.644090,0.186446,0.030744,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.090755
14 | 0.000000,0.000000,0.000000,0.000000,0.161128,0.210941,0.022109,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.067746
15 | 0.000000,0.000000,0.000000,0.000000,0.318009,0.022146,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
16 | 0.000000,0.000000,0.000000,0.000000,0.053638,0.685248,0.008946,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.035547
17 | 0.000000,0.002227,0.056196,0.000000,0.005525,0.000000,0.666827,0.000000,0.000000,0.000000,0.000000,0.062246,0.377809,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.070300
18 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.043157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
19 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.062319,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
20 | 0.000000,0.000000,0.000000,0.000000,0.001482,0.000000,0.146021,0.000000,0.000000,0.000000,0.000000,0.000000,0.023519,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
21 | 0.041309,0.000000,0.022602,0.000000,0.000000,0.000000,0.000000,0.321690,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.010685
22 | 0.007111,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.033882,0.025441,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
23 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.007158,0.336141,0.000000,0.000000,0.000000,0.000000,0.055282,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
24 | 0.025842,0.000000,0.043706,0.000000,0.000000,0.000000,0.005226,0.000000,0.000000,0.622479,0.000000,0.000000,0.075105,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.018120
25 | 0.030105,0.000000,0.055712,0.000000,0.000000,0.000000,0.000000,0.008287,0.000000,0.432767,0.000000,0.000000,0.026736,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.021360
26 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.151566,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
27 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.518353,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
28 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.033725,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
29 | 0.000000,0.000000,0.045695,0.000000,0.000000,0.000000,0.008059,0.000000,0.000000,0.000000,0.000000,0.536332,0.000000,0.000000,0.000000,0.000000,0.000000,0.063300,0.000000,0.000000
30 | 0.000000,0.000000,0.029269,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.506114,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
31 | 0.000000,0.003906,0.057221,0.000000,0.003223,0.000000,0.165106,0.000000,0.000000,0.000000,0.000000,0.028585,0.603372,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.018662
32 | 0.000000,0.003917,0.039822,0.000000,0.000000,0.000000,0.129846,0.000000,0.000000,0.000000,0.000000,0.000000,0.398751,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
33 | 0.028696,0.000000,0.015369,0.000000,0.000000,0.000000,0.000000,0.008345,0.021142,0.000000,0.000000,0.000000,0.000000,0.663413,0.000000,0.664545,0.000000,0.000000,0.000000,0.007796
34 | 0.014792,0.000000,0.010071,0.000000,0.000000,0.000000,0.000000,0.017180,0.000000,0.017153,0.000000,0.000000,0.000000,0.000000,0.460342,0.000000,0.000000,0.000000,0.000000,0.000000
35 | 0.008267,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.044914,0.000000,0.765570,0.000000,0.000000,0.000000,0.000000
36 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.948199,0.000000,0.000000,0.008644
37 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.670517,0.000000,0.000000
38 | 0.000000,0.000000,0.022246,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.343074,0.000000,0.000000
39 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.757643,0.000000
40 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.659859,0.000000
41 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.292872
42 | 


--------------------------------------------------------------------------------
/measurement_model/hardcode/likelihood_matrix.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FM-Fusion/84f6a0f971ca3c4a7dd7ddd6342ca7bd08aff740/measurement_model/hardcode/likelihood_matrix.png


--------------------------------------------------------------------------------
/scripts/process_slabim_pcd.py:
--------------------------------------------------------------------------------
  1 | import os, sys, glob
  2 | import cv2
  3 | import open3d as o3d
  4 | import numpy as np
  5 | import multiprocessing as mp
  6 | 
  7 | def read_sensor_shapes(dir):
  8 |     assert os.path.exists(dir), 'File not found: {}'.format(dir)
  9 |     with open(dir, 'r') as f:
 10 |         lines = f.readlines()
 11 |         height, width = 0, 0
 12 |         for line in lines:
 13 |             if 'color_width' in line:
 14 |                 width = int(line.split(':')[1].strip())
 15 |             elif 'color_height' in line:
 16 |                 height = int(line.split(':')[1].strip()) 
 17 |         return height, width
 18 | 
 19 | def read_intrinsics(intrinsic_folder):
 20 |     
 21 |     h,w = read_sensor_shapes(os.path.join(intrinsic_folder, 
 22 |                                           'sensor_shapes.txt'))
 23 |     K = np.loadtxt(os.path.join(intrinsic_folder, 'intrinsic_depth.txt'))
 24 | 
 25 |     return K, h, w
 26 | 
 27 | def project_xyz(xyz, K, h, w, depth_scale):
 28 |     z = xyz[2]
 29 |     u, v = np.dot(K, xyz)[:2] / z
 30 |     u, v = int(u), int(v)
 31 |     if u>=0 and u<w and v>=0 and v<h:
 32 |         # out_array = np.array([u, v, z * depth_scale], dtype=np.float32)
 33 |         # out_mat[v, u] = z * depth_scale
 34 |         return u, v, z * depth_scale
 35 |     else:
 36 |         return -1, -1, -1
 37 | 
 38 | def project_pcd2image(points:np.ndarray, 
 39 |                       K:np.ndarray, 
 40 |                       height:int, width:int,
 41 |                       DEPTH_SCALE:float=1000):
 42 |     depth_map = np.zeros((height, width), dtype=np.float32)
 43 |     N = points.shape[0]
 44 |     uvd_mat = np.zeros((N, 3), dtype=np.float32)
 45 |     
 46 |     for i in range(N):
 47 |         u,v,d = project_xyz(points[i], K, height, width, DEPTH_SCALE)
 48 |         uvd_mat[i] = np.array([u,v,d])
 49 |     
 50 |     for i in range(N):
 51 |         u,v,d = uvd_mat[i]
 52 |         if d>0: depth_map[int(v), int(u)] = d
 53 |         
 54 |     return depth_map
 55 | 
 56 | def project_pcd2image_cuda(points:np.ndarray,
 57 |                            K:np.ndarray,
 58 |                            height:int, width:int,
 59 |                            DEPTH_SCALE:float=1000):
 60 |     import torch
 61 |     depth_map = torch.zeros((height, width)).cuda()
 62 |     N = points.shape[0]
 63 |     
 64 |     points = torch.from_numpy(points).float().cuda() # (N,3)
 65 |     height = torch.tensor(height).cuda()
 66 |     width = torch.tensor(width).cuda()
 67 |     DEPTH_SCALE = torch.tensor(DEPTH_SCALE).cuda()
 68 |     K = torch.from_numpy(K).float().cuda() # (3,3)
 69 |     
 70 |     # extend K to (N,3,3)
 71 |     K = K.unsqueeze(0).expand(N, -1, -1)
 72 |     z = points[:,2] # (N,)
 73 |     points = points.unsqueeze(-1) # (N,3,1)
 74 | 
 75 |     uv = torch.matmul(K, points) # (N,3,3) x (N,3,1) -> (N,3,1)    
 76 |     uv = uv.squeeze() / z.unsqueeze(-1) # (N,3)
 77 |     u, v = uv[:, 0], uv[:, 1]
 78 |     u, v = u.int(), v.int()
 79 |     
 80 |     mask = (u>=0) & (u<width) & (v>=0) & (v<height) &(z>0)
 81 |     u, v, z = u[mask], v[mask], z[mask]
 82 |     depth_map[v, u] = z * DEPTH_SCALE
 83 |     
 84 |     depth_map = depth_map.cpu().numpy()
 85 |     return depth_map
 86 | 
 87 | if __name__=='__main__':
 88 |     ############ SET CONFIG ############
 89 |     DATA_ROOT = '/data2/slabim/scans'
 90 |     DEPTH_SCALE = 1000
 91 |     DEPTH_POSFIX = '.png'
 92 |     PROJECT_DEPTH = -1 # 0: run on CPU, 1: run on GPU, -1: not run
 93 |     MERGE_GLOBAL_MAP=True
 94 |     SAVE_VIZ = False
 95 |     
 96 |     ###################################
 97 |     scans = [
 98 |             # 'ab0202_00a',
 99 |             # 'ab0205_00a',
100 |             # 'ab0206_00a',
101 |             # 'ab0304_00a',
102 |             'jk0001_00a',
103 |             # 'jk0102_00a'
104 |             ]
105 |     
106 |     if False: # debug
107 |         # tmpdir = '/data2/sgslam/scans/uc0204_00a/depth/frame-000338.png'
108 |         tmpdir = '/data2/slabim/scans/ab0202_00a/depth/frame-000000.png'
109 |         depth = cv2.imread(tmpdir, cv2.IMREAD_UNCHANGED)
110 |         print(depth.dtype, depth.shape)
111 |         exit(0)
112 |     
113 |     for scan in scans:
114 |         print('------------ Processing {} ------------'.format(scan))
115 |         
116 |         pcd_files = glob.glob(os.path.join(DATA_ROOT, scan, 'pcd', '*.pcd'))
117 |         K, h, w = read_intrinsics(os.path.join(DATA_ROOT, scan, 'intrinsic'))
118 |         os.makedirs(os.path.join(DATA_ROOT, scan, 'depth'), exist_ok=True)
119 |         global_map = o3d.geometry.PointCloud()
120 |         
121 |         for pcd_file in sorted(pcd_files):
122 |             frame_name = os.path.basename(pcd_file).split('.')[0]
123 |             pcd = o3d.io.read_point_cloud(pcd_file) # in world coordinate
124 |             print('Load {} points at {}'.format(len(pcd.points), os.path.basename(pcd_file)))
125 |             
126 |             if MERGE_GLOBAL_MAP:
127 |                 global_map += pcd
128 |             
129 |             if PROJECT_DEPTH<0: continue
130 |             # Run depth projection
131 |             T_wc = np.loadtxt(os.path.join(DATA_ROOT, scan, 'pose', frame_name+'.txt'))
132 |             pcd.transform(np.linalg.inv(T_wc)) # in camera coordinate
133 |             if PROJECT_DEPTH==1:
134 |                 depth = project_pcd2image_cuda(np.asarray(pcd.points), K, h, w)
135 |             elif PROJECT_DEPTH==0:
136 |                 depth = project_pcd2image(np.asarray(pcd.points), K, h, w)
137 |             
138 |             depth = depth.astype(np.uint16)            
139 |             cv2.imwrite(os.path.join(DATA_ROOT, scan, 'depth', frame_name+DEPTH_POSFIX), 
140 |                         depth)
141 |             print('Write {} depth image'.format(frame_name+DEPTH_POSFIX))
142 |             
143 |             if SAVE_VIZ:
144 |                 depth_viz = cv2.applyColorMap(cv2.convertScaleAbs(depth, alpha=0.03), cv2.COLORMAP_JET)
145 |                 cv2.imwrite(os.path.join(DATA_ROOT, scan, 'viz', frame_name+'_depth'+DEPTH_POSFIX), 
146 |                             depth_viz)
147 |             
148 |         if global_map.has_points():
149 |             global_map = global_map.voxel_down_sample(voxel_size=0.05)
150 |             o3d.io.write_point_cloud(os.path.join(DATA_ROOT, scan, 'mesh_o3d.ply'), 
151 |                                      global_map)
152 |             print('Write global map')
153 |             
154 | 


--------------------------------------------------------------------------------
/scripts/uncompress_data.py:
--------------------------------------------------------------------------------
 1 | import os,glob
 2 | import zipfile
 3 | 
 4 | 
 5 | if __name__=='__main__':
 6 |     ############## SET Configurations Here ##############
 7 |     dataroot = '/data2/sample_data/ScanNet'
 8 |     DELETE_ZIP_FILE = True
 9 |     #####################################################
10 | 
11 |     sequence_zips = glob.glob(os.path.join(dataroot, 'scans', '*.zip'))
12 |     
13 |     for sequence_zip in sequence_zips:
14 |         sequence_name = os.path.basename(sequence_zip).split('.')[0]
15 |         sequence_folder = os.path.join(dataroot, 'scans', sequence_name)
16 |         if os.path.exists(sequence_folder):
17 |             print('[Warning] Skip {} because it already exists.'.format(sequence_folder))
18 |             continue        
19 |         print('------- {} -------'.format(sequence_name))
20 |         
21 |         # unzip the sequence
22 |         with zipfile.ZipFile(sequence_zip, 'r') as z:
23 |             z.extractall(os.path.join(dataroot, 'scans'))
24 |         
25 |         if DELETE_ZIP_FILE: # after unzipping, delete the zip file
26 |             os.remove(sequence_zip)
27 |             print('deleted {}'.format(sequence_zip))
28 |         
29 |     


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.18)
  2 | 
  3 | project(fmfusion LANGUAGES CXX VERSION 1.0)
  4 | 
  5 | set(CMAKE_BUILD_TYPE Release)
  6 | set(CMAKE_CXX_STANDARD 17)
  7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
  8 | 
  9 | # Dependencies
 10 | add_definitions(-D _GLIBCXX_USE_CXX11_ABI=1)
 11 | include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 12 | 
 13 | # FMFusion::Mapping
 14 | set(FMFUSION_HEADER
 15 |         mapping/SubVolume.h
 16 |         mapping/Detection.h
 17 |         mapping/Instance.h
 18 |         mapping/SemanticMapping.h
 19 |         cluster/PoseGraph.h
 20 |         tools/Tools.h
 21 |         tools/Utility.h
 22 |         tools/IO.h
 23 |         tools/TicToc.h
 24 |         Common.h
 25 | )
 26 | 
 27 | set(FMFUSION_SRC
 28 |         mapping/SubVolume.cpp
 29 |         mapping/Detection.cpp
 30 |         mapping/Instance.cpp
 31 |         mapping/SemanticMapping.cpp
 32 |         cluster/PoseGraph.cpp
 33 |         tools/Visualization.cpp
 34 |         tools/Utility.cpp
 35 |         tools/IO.cpp
 36 | )
 37 | 
 38 | # FMFusion::LoopDetection
 39 | if (LOOP_DETECTION)
 40 |     include(${PROJECT_ROOT_DIR}/cmake/gtsam.cmake)
 41 |     include_directories("thirdparty/Kimera-RPGO/include")
 42 |     include_directories("thirdparty/extensions")
 43 |     set(CMAKE_PREFIX_PATH "/home/cliuci/tools/libtorch")
 44 |     set(LIB_UTF8PROC "/usr/local/lib/libutf8proc.so")
 45 | 
 46 |     add_subdirectory(tokenizer)
 47 |     add_subdirectory(thirdparty/extensions)
 48 |     add_subdirectory(thirdparty/G3Reg)
 49 |     add_subdirectory(thirdparty/Kimera-RPGO)
 50 |     find_package(Torch REQUIRED)
 51 | 
 52 |     list(APPEND FMFUSION_HEADER
 53 |             tools/g3reg_api.h
 54 |             sgloop/Graph.h
 55 |             sgloog/BertBow.h
 56 |             sgloop/SGNet.h
 57 |             sgloop/ShapeEncoder.h
 58 |             sgloop/LoopDetector.h
 59 |     )
 60 | 
 61 |     list(APPEND FMFUSION_SRC
 62 |             tools/IO.cpp
 63 |             sgloop/Graph.cpp
 64 |             sgloop/BertBow.cpp
 65 |             sgloop/SGNet.cpp
 66 |             sgloop/ShapeEncoder.cpp
 67 |             sgloop/LoopDetector.cpp
 68 |     )
 69 | 
 70 |     list(APPEND ALL_TARGET_LIBRARIES
 71 |             ${TORCH_LIBRARIES}
 72 |             G3REG::g3reg
 73 |             KimeraRPGO
 74 |             tokenizer
 75 |             extensions
 76 |         )
 77 |     message(STATUS "TORCH_LIBRARIES: ${TORCH_LIBRARIES}")
 78 |     message(STATUS "TORCH_INCLUDE_DIRS: ${TORCH_INCLUDE_DIRS}")
 79 | endif()
 80 | 
 81 | add_library(fmfusion SHARED ${FMFUSION_SRC})
 82 | target_link_libraries(fmfusion PRIVATE ${ALL_TARGET_LIBRARIES})
 83 | 
 84 | # Executables
 85 | add_executable(IntegrateRGBD)
 86 | target_sources(IntegrateRGBD PRIVATE IntegrateRGBD.cpp)
 87 | target_link_libraries(IntegrateRGBD PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion)
 88 | 
 89 | add_executable(IntegrateInstanceMap)
 90 | target_sources(IntegrateInstanceMap PRIVATE IntegrateInstanceMap.cpp)
 91 | target_link_libraries(IntegrateInstanceMap PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion)
 92 | 
 93 | # Install loop closure detection(LCD) module
 94 | # The LCD is in developing. More complete instruction will be released soon.
 95 | if (LOOP_DETECTION)
 96 |     add_executable(TestLoop)
 97 |     target_sources(TestLoop PRIVATE TestLoop.cpp)
 98 |     target_link_libraries(TestLoop PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion)
 99 | 
100 |     add_executable(TestRegister)
101 |     target_sources(TestRegister PRIVATE TestRegister.cpp)
102 |     target_link_libraries(TestRegister PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion)
103 | 
104 |     add_executable(Test3RScanRegister)
105 |     target_sources(Test3RScanRegister PRIVATE Test3RscanRegister.cpp)
106 |     target_link_libraries(Test3RScanRegister PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion)
107 |         
108 |     add_executable(robustPoseAvg)
109 |     target_sources(robustPoseAvg PRIVATE robustPoseAvg.cpp)
110 |     target_link_libraries(robustPoseAvg PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion -ltbb)
111 | 
112 |     add_executable(testTwoAgentsPGO)
113 |     target_sources(testTwoAgentsPGO PRIVATE testTwoAgentsPGO.cpp)
114 |     target_link_libraries(testTwoAgentsPGO PRIVATE ${ALL_TARGET_LIBRARIES} fmfusion -ltbb)
115 | 
116 | endif ()
117 | 
118 | if (RUN_HYDRA)
119 |     find_package(GTest REQUIRED)
120 |     find_package(DBoW2 REQUIRED)
121 |     add_library(DBoW2::DBoW2 INTERFACE IMPORTED)
122 |     set_target_properties(DBoW2::DBoW2 PROPERTIES
123 |                         INTERFACE_LINK_LIBRARIES "${DBoW2_LIBRARIES}"
124 |                         INTERFACE_INCLUDE_DIRECTORIES "${DBoW2_INCLUDE_DIRS}")
125 | 
126 |     add_executable(TestHydra)
127 |     target_sources(TestHydra PRIVATE TestHydra.cpp)
128 |     target_link_libraries(TestHydra PRIVATE ${ALL_TARGET_LIBRARIES} 
129 |                                         fmfusion 
130 |                                         DBoW2::DBoW2
131 |                                         GTest::gtest_main)
132 | endif()
133 | 
134 | # Install fmfusion library
135 | if (INSTALL_FMFUSION)
136 |     message(STATUS "Installing fmfusion library. So it can be used in ros warpper.")
137 | 
138 |     install(TARGETS fmfusion
139 |             LIBRARY DESTINATION lib
140 |             ARCHIVE DESTINATION lib
141 |             RUNTIME DESTINATION bin
142 |             INCLUDES DESTINATION include
143 |     )
144 |     install(FILES
145 |             Common.h
146 |             DESTINATION include/fmfusion
147 |     )
148 |     install(FILES
149 |             cluster/PoseGraph.h
150 |             DESTINATION include/fmfusion/cluster
151 |     )
152 |     install(FILES
153 |             mapping/BayesianLabel.h
154 |             mapping/SemanticDict.h
155 |             mapping/SubVolume.h
156 |             mapping/Detection.h
157 |             mapping/Instance.h
158 |             mapping/SemanticMapping.h
159 |             DESTINATION include/fmfusion/mapping
160 |     )
161 |     install(FILES
162 |            sgloop/Graph.h
163 |            sgloop/SGNet.h
164 |            sgloop/ShapeEncoder.h
165 |            sgloop/LoopDetector.h
166 |            sgloop/BertBow.h
167 |            sgloop/Initialization.h
168 |            DESTINATION include/fmfusion/sgloop
169 |     )
170 |     install(FILES
171 |             tools/Tools.h
172 |             tools/Eval.h
173 |             tools/Utility.h
174 |             tools/Color.h
175 |             tools/IO.h
176 |             tools/TicToc.h
177 |             DESTINATION include/fmfusion/tools
178 |     )
179 | 
180 |     if(LOOP_DETECTION)
181 |         install(FILES
182 |                 tools/g3reg_api.h
183 |                 DESTINATION include/fmfusion/tools
184 |         )       
185 | 
186 |         install(FILES
187 |                 tokenizer/logging.h
188 |                 tokenizer/basic_string_util.h
189 |                 tokenizer/text_tokenizer.h
190 |                 tokenizer/bert_tokenizer.h
191 |                 DESTINATION include/fmfusion/tokenizer
192 |         )
193 | 
194 |         install(FILES
195 |                 thirdparty/extensions/cpu/torch_helper.h
196 |                 thirdparty/extensions/cpu/grid_subsampling.h
197 |                 thirdparty/extensions/cpu/radius_neighbors.h
198 |                 DESTINATION include/fmfusion/thirdparty/extensions/cpu
199 |         )
200 |      endif()
201 | endif ()
202 | 


--------------------------------------------------------------------------------
/src/Common.h:
--------------------------------------------------------------------------------
  1 | #ifndef FMFUSOIN_COMMON_H
  2 | #define FMFUSOIN_COMMON_H
  3 | 
  4 | #include "open3d/Open3D.h"
  5 | #include "opencv2/opencv.hpp"
  6 | #include "cluster/PoseGraph.h"
  7 | 
  8 | namespace fmfusion
  9 | {
 10 | using namespace std;
 11 | 
 12 | struct InstanceConfig{
 13 |     double voxel_length = 0.02;
 14 |     double sdf_trunc = 0.04;
 15 |     open3d::camera::PinholeCameraIntrinsic intrinsic;
 16 |     int max_label_measures = 20;
 17 |     double min_voxel_weight = 2.0;
 18 |     double cluster_eps = 0.05;
 19 |     int cluster_min_points = 20;
 20 |     bool bayesian_semantic = false;
 21 | 
 22 |     const std::string print_msg() const{
 23 |         std::stringstream msg;
 24 |         msg<<" - voxel_length: "<<voxel_length<<std::endl;
 25 |         msg<<" - sdf_trunc: "<<sdf_trunc<<std::endl;
 26 |         msg<<" - image shape: "<<intrinsic.width_<<", "<<intrinsic.height_<<std::endl;
 27 |         msg<<" - intrinsic fx,fy,cx,cy: "<<intrinsic.intrinsic_matrix_(0,0)<<", "<<intrinsic.intrinsic_matrix_(1,1)<<", "<<intrinsic.intrinsic_matrix_(0,2)<<", "<<intrinsic.intrinsic_matrix_(1,2)<<std::endl;
 28 |         msg<<" - max_label_measures: "<<max_label_measures<<std::endl;
 29 |         msg<<" - min_voxel_weight: "<<min_voxel_weight<<std::endl;
 30 |         msg<<" - cluster_eps: "<<cluster_eps<<std::endl;
 31 |         msg<<" - cluster_min_points: "<<cluster_min_points<<std::endl;
 32 |         // msg<<" - bayesian_semantic: "<<bayesian_semantic<<std::endl;
 33 |         return msg.str();
 34 |     }
 35 | 
 36 | };
 37 | 
 38 | struct MappingConfig{
 39 |     double depth_scale;
 40 |     double depth_max;
 41 |     int min_active_points;    
 42 | 
 43 |     // associations
 44 |     int min_det_masks;
 45 |     double max_box_area_ratio;
 46 |     double query_depth_vx_size; // <0 if skipped
 47 |     double search_radius;
 48 |     int dilation_size;
 49 |     double min_iou;
 50 | 
 51 |     // shape
 52 |     double min_voxel_weight;
 53 |     int shape_min_points=1000;
 54 | 
 55 |     // merge
 56 |     double merge_iou;
 57 |     double merge_inflation;
 58 |     int recent_window_size = 200; // in frames
 59 |     bool realtime_merge_floor = false;
 60 |     int min_observation=2;
 61 | 
 62 |     //
 63 |     int update_period=20; // in frames
 64 |     std::string bayesian_semantic_likelihood = ""; // The path to the likelihood model
 65 |     bool bayesian_semantic = false;
 66 | 
 67 |     //
 68 |     std::string save_da_dir = "";
 69 | 
 70 |     const std::string print_msg()const{
 71 |         std::stringstream msg;
 72 |         msg<<" - depth_scale: "<<depth_scale<<std::endl;
 73 |         msg<<" - depth_max: "<<depth_max<<std::endl;
 74 |         msg<<" - min_active_points: "<<min_active_points<<std::endl;
 75 |         msg<<" - min_det_masks: "<<min_det_masks<<std::endl;
 76 |         msg<<" - max_box_area_ratio: "<<max_box_area_ratio<<std::endl;
 77 |         msg<<" - query_depth_vx_size: "<<query_depth_vx_size<<std::endl;
 78 |         msg<<" - search_radius: "<<search_radius<<std::endl;
 79 |         msg<<" - dilation_size: "<<dilation_size<<std::endl;
 80 |         msg<<" - min_iou: "<<min_iou<<std::endl;
 81 |         msg<<" - min_voxel_weight: "<<min_voxel_weight<<std::endl;
 82 |         msg<<" - shape_min_points: "<<shape_min_points<<std::endl;
 83 |         msg<<" - merge_iou: "<<merge_iou<<std::endl;
 84 |         msg<<" - merge_inflation: "<<merge_inflation<<std::endl;
 85 |         msg<<" - realtime_merge_floor: "<<realtime_merge_floor<<std::endl;
 86 |         msg<<" - min_observation: "<<min_observation<<std::endl;
 87 |         msg<<" - update_period: "<<update_period<<std::endl;
 88 |         msg<<" - recent_window_size: "<<recent_window_size<<std::endl;
 89 |         msg<<" - bayesian_semantic_likelihood: "<<bayesian_semantic_likelihood<<std::endl;
 90 |         msg<<" - bayesian_semantic: "<<bayesian_semantic<<std::endl;
 91 | 
 92 |         msg<<" - save_da_dir: "<<save_da_dir<<std::endl;
 93 |         return msg.str();
 94 |     }
 95 | 
 96 | };
 97 | 
 98 | struct GraphConfig{
 99 |     float edge_radius_ratio = 2.0;
100 |     float voxel_size = 0.02;
101 |     bool involve_floor_edge = false;
102 |     std::string ignore_labels = "floor. carpet. ceiling.";
103 | 
104 |     const std::string print_msg()const{
105 |         std::stringstream msg;
106 |         msg<<" - edge_radius_ratio: "<<edge_radius_ratio<<std::endl;
107 |         msg<<" - voxel_size: "<<voxel_size<<std::endl;
108 |         msg<<" - involve_floor_edge: "<<involve_floor_edge<<std::endl;
109 |         msg<<" - ignore_labels: "<<ignore_labels<<std::endl;
110 |         return msg.str();
111 |     }
112 | };
113 | 
114 | struct SgNetConfig
115 | {
116 |     int token_padding=8;
117 |     int triplet_number=20; // number of triplets for each node
118 |     int warm_up_iter=10;
119 |     float instance_match_threshold=0.1;
120 |     
121 |     const std::string print_msg()const{
122 |         std::stringstream msg;
123 |         msg<<" - token_padding: "<<token_padding<<std::endl;
124 |         msg<<" - triplet_number: "<<triplet_number<<std::endl;
125 |         msg<<" - warm_up_iter: "<<warm_up_iter<<std::endl;
126 |         msg<<" - instance_match_threshold: "<<instance_match_threshold<<std::endl;
127 |         return msg.str();
128 |     }
129 | };
130 | 
131 | struct ShapeEncoderConfig
132 | {
133 |     int num_stages = 4;
134 |     float init_voxel_size = 0.05;
135 |     int neighbor_limits[4] = {33, 9, 9, 9};
136 |     float init_radius = 2.0 * init_voxel_size;
137 |     int K_shape_samples = 1024;
138 |     int K_match_samples = 512;
139 |     std::string padding = "zero"; // zero, random
140 | 
141 |     const std::string print_msg()const{
142 |         std::stringstream msg;
143 |         msg<<" - num_stages: "<<num_stages<<std::endl;
144 |         msg<<" - voxel_size: "<<init_voxel_size<<std::endl;
145 |         msg<<" - neighbor_limits: "<<neighbor_limits[0]<<", "<<neighbor_limits[1]<<", "<<neighbor_limits[2]<<", "<<neighbor_limits[3]<<std::endl;
146 |         msg<<" - init_radius: "<<init_radius<<std::endl;
147 |         msg<<" - K_shape_samples: "<<K_shape_samples<<std::endl;
148 |         msg<<" - K_match_samples: "<<K_match_samples<<std::endl;
149 |         msg<<" - padding: "<<padding<<std::endl;
150 |         return msg.str();
151 |     }
152 | };
153 | 
154 | struct LoopDetectorConfig
155 | {
156 |     bool fuse_shape = false;
157 |     int lcd_nodes = 12;
158 |     int recall_nodes = 8;
159 | 
160 |     const std::string print_msg()const{
161 |         std::stringstream msg;
162 |         msg<<" - fuse_shape: "<<fuse_shape<<std::endl;
163 |         msg<<" - lcd_nodes: "<<lcd_nodes<<std::endl;
164 |         msg<<" - recall_nodes: "<<recall_nodes<<std::endl;
165 |         return msg.str();
166 |     }
167 | };
168 | 
169 | struct RegistrationConfig
170 | {
171 |     std::vector<double> noise_bound_vec = {1.0};
172 |     const std::string print_msg() const{
173 |         std::stringstream msg;
174 |         msg<<" - noise_bound_vec: "<<std::fixed<<std::setprecision(2);
175 |         for(auto &v: noise_bound_vec) msg<<v<<", ";
176 |     
177 |         msg<<std::endl;
178 |         return msg.str();
179 |     
180 |     }
181 | };
182 | 
183 | struct Config
184 | {
185 |     enum DATASET_TYPE{
186 |         REALSENSE,
187 |         FUSION_PORTABLE,
188 |         SCANNET,
189 |         MATTERPORT,
190 |         RIO
191 |     }dataset;
192 |     
193 |     InstanceConfig instance_cfg;
194 |     MappingConfig mapping_cfg;
195 | 
196 |     // output
197 |     // bool save_da_images;
198 |     // std::string tmp_dir; // For debug
199 | 
200 |     //
201 |     GraphConfig graph;
202 |     SgNetConfig sgnet;
203 |     LoopDetectorConfig loop_detector;
204 |     ShapeEncoderConfig shape_encoder;
205 | 
206 |     //
207 |     RegistrationConfig reg;
208 |         
209 | };
210 | 
211 | typedef std::shared_ptr<cv::Mat> CvMatPtr;
212 | typedef open3d::geometry::Image O3d_Image;
213 | typedef open3d::geometry::Geometry O3d_Geometry;
214 | typedef std::shared_ptr<const open3d::geometry::Geometry> O3d_Geometry_Ptr;
215 | typedef std::shared_ptr<open3d::geometry::Image> O3d_Image_Ptr;
216 | typedef std::pair<uint32_t, uint32_t> NodePair;
217 | typedef std::pair<std::string, std::string> LoopPair;
218 | 
219 | typedef uint32_t InstanceId;
220 | typedef std::vector<InstanceId> InstanceIdList;
221 | typedef open3d::geometry::PointCloud O3d_Cloud;
222 | typedef std::shared_ptr<open3d::geometry::PointCloud> O3d_Cloud_Ptr;
223 | 
224 | }
225 | 
226 | #endif //FMFUSOIN_COMMON_H


--------------------------------------------------------------------------------
/src/GraphMatch.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <memory>
 3 | #include <vector>
 4 | #include <fstream>
 5 | 
 6 | #include "open3d/Open3D.h"
 7 | #include "opencv2/opencv.hpp"
 8 | 
 9 | #include "tools/Utility.h"
10 | #include "mapping/SemanticMapping.h"
11 | 
12 | using namespace open3d::visualization;
13 | 
14 | class InstanceWindow: public gui::Window{
15 |     // using Super = gui::window;
16 | 
17 | public:
18 |     InstanceWindow(const std::string &title)
19 |         : gui::Window(title, 1280, 720) {
20 |         // gui::Application::GetInstance().Initialize();
21 |         // gui::Application::GetInstance().AddWindow(this);
22 |         auto pcd = open3d::io::CreatePointCloudFromFile("/media/lch/SeagateExp/dataset_/FusionPortable/output/bday_04/instance_map.ply");
23 |         std::cout<<"pcd size: "<<pcd->points_.size()<<std::endl;
24 |     }
25 | 
26 | 
27 | };
28 | 
29 | int main(int argc, char *argv[]) 
30 | {
31 |     using namespace open3d;
32 | 
33 |     std::string config_file = utility::GetProgramOptionAsString(argc, argv, "--config");
34 |     std::string map_folder =
35 |             utility::GetProgramOptionAsString(argc, argv, "--map_folder");
36 |     std::string src_sequence = 
37 |         utility::GetProgramOptionAsString(argc, argv, "--src_sequence");
38 |     std::string tar_sequence = 
39 |         utility::GetProgramOptionAsString(argc, argv, "--tar_sequence");
40 | 
41 | 
42 |     // init
43 |     auto sg_config = fmfusion::utility::create_scene_graph_config(config_file, true);
44 |     if(sg_config==nullptr) {
45 |         utility::LogWarning("Failed to create scene graph config.");
46 |         return 0;
47 |     }
48 |     fmfusion::SemanticMapping scene_graph_src(sg_config->mapping_cfg, sg_config->instance_cfg);
49 |     fmfusion::SemanticMapping scene_graph_tar(sg_config->mapping_cfg, sg_config->instance_cfg);
50 | 
51 |     // load
52 |     scene_graph_src.load(map_folder+"/"+src_sequence);
53 | 
54 |     // Update 
55 |     scene_graph_src.merge_overlap_instances();
56 |     scene_graph_src.extract_bounding_boxes();
57 |     scene_graph_src.merge_overlap_structural_instances();
58 |     if(tar_sequence!=""){
59 |         fmfusion::o3d_utility::LogInfo("Todo {}.",tar_sequence);
60 |         Eigen::Matrix4d T_tar_offset;
61 |         T_tar_offset<<1,0,0,0,
62 |             0,1,0,0,
63 |             0,0,1,6.0,
64 |             0,0,0,1;
65 |         scene_graph_tar.load(map_folder+"/"+tar_sequence);
66 |         scene_graph_tar.Transform(T_tar_offset);
67 |         scene_graph_tar.merge_overlap_instances();
68 |         scene_graph_tar.extract_bounding_boxes();
69 |         scene_graph_tar.merge_overlap_structural_instances();
70 |     }
71 | 
72 |     // Visualize
73 |     auto geometries = scene_graph_src.get_geometries(true,true);
74 |     if(!scene_graph_tar.is_empty()){
75 |         auto geometries_tar = scene_graph_tar.get_geometries(true,true);
76 |         geometries.insert(geometries.end(),geometries_tar.begin(),geometries_tar.end());
77 |         std::cout<<"Update tar scene graph from "<<tar_sequence<<std::endl;
78 |     }
79 | 
80 |     open3d::visualization::DrawGeometries(geometries, src_sequence, 1920, 1080);
81 |     
82 | 
83 |     return 0;
84 | 
85 | }


--------------------------------------------------------------------------------
/src/cluster/PoseGraph.cpp:
--------------------------------------------------------------------------------
 1 | #include "json/json.h"
 2 | #include "PoseGraph.h"
 3 | 
 4 | namespace fmfusion
 5 | {
 6 | 
 7 | bool PoseGraphFile::ConvertFromJsonValue(const Json::Value &value)
 8 | {
 9 |     using namespace open3d;
10 |     
11 |     if (!value.isObject()) {
12 |         open3d::utility::LogWarning(
13 |                 "PoseGraph read JSON failed: unsupported json "
14 |                 "format.");
15 |         return false;
16 |     }
17 |     const Json::Value scans_pose = value;
18 |     // std::cout << scans_pose << std::endl;
19 | 
20 |     if (scans_pose.size() <1) {
21 |         open3d::utility::LogWarning(
22 |                 "PoseGraph cannot find valid objects.");
23 |         return false;
24 |     }
25 |     
26 | 
27 |     for (auto it=scans_pose.begin();it!=scans_pose.end();it++){
28 |         std::string scene_name = it.key().asString();
29 |         Eigen::Matrix4d_u T;
30 |         for(int i=0;i<4;++i){
31 |             for(int j=0;j<4;++j){
32 |                 T(i,j) = (*it)[i*4+j].asDouble();
33 |             }
34 |         }
35 |         poses_[scene_name] = T;
36 |         // std::cout<<scene_name<<"\n";
37 |         // std::cout<<T<<"\n";
38 | 
39 |     }
40 | 
41 |     std::cout<<"Read "<<poses_.size()<<" poses.\n";
42 |     return true;
43 | }
44 | 
45 | 
46 | 
47 | }
48 | 
49 | 


--------------------------------------------------------------------------------
/src/cluster/PoseGraph.h:
--------------------------------------------------------------------------------
 1 | #ifndef FMFUSION_POSEGRAPH_H
 2 | #define FMFUSION_POSEGRAPH_H
 3 | 
 4 | #include <unordered_map>
 5 | #include "open3d/Open3D.h"
 6 | #include "open3d/utility/IJsonConvertible.h"
 7 | 
 8 | namespace fmfusion
 9 | {
10 | 
11 | class PoseGraphFile: public open3d::utility::IJsonConvertible
12 | {
13 | public:
14 |     PoseGraphFile() {};
15 | 
16 | public:
17 |     bool ConvertToJsonValue(Json::Value &value) const override {return true;};
18 |     bool ConvertFromJsonValue(const Json::Value &value) override;
19 | 
20 | public:
21 |     std::unordered_map<std::string, Eigen::Matrix4d_u> poses_;
22 | 
23 | };
24 | 
25 | }
26 | #endif


--------------------------------------------------------------------------------
/src/mapping/BayesianLabel.h:
--------------------------------------------------------------------------------
  1 | #ifndef BAYESIANLABEL_H
  2 | #define BAYESIANLABEL_H
  3 | 
  4 | #include <vector>
  5 | #include <string>
  6 | #include <iostream>
  7 | #include <eigen3/Eigen/Core>
  8 | 
  9 | #include "Detection.h"
 10 | #include "tools/Utility.h"
 11 | 
 12 | namespace fmfusion
 13 | {
 14 | 
 15 | class BayesianLabel
 16 | {
 17 |     public:
 18 |         /// \brief  Load the likelihood matrix from file.
 19 |         BayesianLabel(const std::string &likelihood_file, bool verbose=false)
 20 |         {
 21 |             std::cout<<"Semantic fusion load likelihood matrix from "
 22 |                     <<likelihood_file<<std::endl;
 23 |             if(load_likelihood_matrix(likelihood_file, verbose)){
 24 |                 is_loaded = true;
 25 |             }
 26 |             else{
 27 |                 std::cerr<<"Failed to load likelihood matrix from "
 28 |                          <<likelihood_file<<std::endl;
 29 |             }
 30 | 
 31 |         };
 32 |         ~BayesianLabel(){};
 33 | 
 34 |         /// \brief  Update the probability vector from the measurements.
 35 |         bool update_measurements(const std::vector<LabelScore> &measurements,
 36 |                                 Eigen::VectorXf &probability_vector)
 37 |         {
 38 |             if(measurements.empty() || !is_loaded) return false;
 39 | 
 40 |             int rows = measurements.size();
 41 |             int cols = predict_label_vec.size();
 42 |             probability_vector = Eigen::VectorXf::Zero(cols);
 43 |             for(int i=0;i<rows;i++){
 44 |                 std::string measure_label = measurements[i].first;
 45 |                 float measure_score = measurements[i].second;
 46 |                 if(measure_label_map.find(measure_label)!=measure_label_map.end()){
 47 |                     probability_vector += measure_score 
 48 |                                         * likelihood_matrix.row(measure_label_map[measure_label]).transpose();
 49 |                 }
 50 |             }
 51 | 
 52 |             return true;
 53 |         }
 54 |         
 55 |         bool update_measurements(const std::unordered_map<std::string, float> &measurements,
 56 |                                 Eigen::VectorXf &probability_vector)
 57 |         {
 58 |             if(measurements.empty() || !is_loaded) return false;
 59 |             std::vector<LabelScore> measurements_vec;
 60 |             for(const auto &label_score:measurements){
 61 |                 measurements_vec.emplace_back(std::make_pair(label_score.first, label_score.second));
 62 |             }
 63 | 
 64 |             return update_measurements(measurements_vec, probability_vector);
 65 |         }
 66 | 
 67 |         std::vector<std::string> get_label_vec()const{
 68 |             return predict_label_vec;
 69 |         }
 70 | 
 71 |         int get_num_classes() const{
 72 |             return predict_label_vec.size();
 73 |         }
 74 | 
 75 |     
 76 |     private:
 77 | 
 78 |         /// \brief  Load the likelihood matrix from text file.
 79 |         bool load_likelihood_matrix(const std::string &likelihood_file, bool verbose)
 80 |         {
 81 |             std::ifstream file(likelihood_file);
 82 |             std::stringstream msg;
 83 |             if(file.is_open()){
 84 |                 // read in lines
 85 |                 std::string line;
 86 |                 std::getline(file, line); // header
 87 |                 std::getline(file, line); // 
 88 |                 // std::cout<<line<<std::endl;
 89 |                 measure_label_vec = utility::split_str(line, ",");
 90 | 
 91 |                 std::getline(file, line); //
 92 |                 predict_label_vec = utility::split_str(line, ",");
 93 |                 
 94 |                 int rows = measure_label_vec.size();
 95 |                 int cols = predict_label_vec.size();
 96 |                 std::cout<<"rows: "<<rows<<" cols: "<<cols<<std::endl;
 97 |                 likelihood_matrix = Eigen::MatrixXf::Zero(rows, cols);
 98 |                 for(int i=0;i<rows;i++){
 99 |                     std::getline(file, line);
100 |                     std::vector<std::string> values = utility::split_str(line, ",");
101 |                     for(int j=0;j<cols;j++){
102 |                         likelihood_matrix(i,j) = std::stof(values[j]);
103 |                     }
104 |                 }
105 |                 
106 |                 msg<<"Measured label-set: ";
107 |                 for (int i=0;i<measure_label_vec.size();i++) {
108 |                     measure_label_map[measure_label_vec[i]] = i;
109 |                     msg<<measure_label_vec[i]<<" ";
110 |                 }
111 |                 msg<<"\n"
112 |                    <<"Predict label-set: ";
113 |                 for (auto label : predict_label_vec) {
114 |                     msg<<label<<" ";
115 |                 }
116 |                 msg<<"\n"
117 |                    <<"Likelihood matrix: "<<likelihood_matrix.rows()<<"x"
118 |                    <<likelihood_matrix.cols()<<std::endl;
119 | 
120 |                 assert(measure_label_vec.size()==likelihood_matrix.rows());
121 |                 assert(predict_label_vec.size()==likelihood_matrix.cols());
122 |                 if(verbose) std::cout<<msg.str();
123 |                 return true;
124 |             }
125 |             else return false;
126 | 
127 |         }
128 | 
129 |     private:
130 |         bool is_loaded = false;
131 |         std::vector<std::string> predict_label_vec;
132 |         std::vector<std::string> measure_label_vec;
133 |         std::map<std::string, int> measure_label_map;
134 |         
135 |         Eigen::MatrixXf likelihood_matrix;
136 | 
137 | 
138 | };
139 | 
140 | }
141 | 
142 | #endif // BAYESIANLABEL_H
143 | 


--------------------------------------------------------------------------------
/src/mapping/Detection.cpp:
--------------------------------------------------------------------------------
  1 | #include "json/json.h"
  2 | #include "Detection.h"
  3 | 
  4 | 
  5 | namespace fmfusion
  6 | {
  7 | 
  8 | Detection::Detection(const int id): id_(id)
  9 | {
 10 | 
 11 | }
 12 | 
 13 | std::string Detection::extract_label_string() const
 14 | {
 15 |     std::stringstream ss;
 16 |     for(auto label_score:labels_){
 17 |         ss<<label_score.first<<"("<<std::fixed<<std::setprecision(2)<<label_score.second<<"), ";
 18 |         // <<label_score.second<<"), ";
 19 |     }
 20 |     return ss.str();
 21 | }
 22 | 
 23 | std::vector<std::string> DetectionFile::split_str(
 24 |     const std::string s, const std::string delim) 
 25 | {
 26 |     std::vector<std::string> list;
 27 |     auto start = 0U;
 28 |     auto end = s.find(delim);
 29 |     while (true) {
 30 |         list.push_back(s.substr(start, end - start));
 31 |         if (end == std::string::npos)
 32 |             break;
 33 |         start = end + delim.length();
 34 |         end = s.find(delim, start);
 35 |     }
 36 |     return list;
 37 | }
 38 | 
 39 | bool DetectionFile::ConvertFromJsonValue(const Json::Value &value)
 40 | {
 41 |     using namespace open3d;
 42 |     
 43 |     if (!value.isObject()) {
 44 |         std::cerr<<"DetectionField read JSON failed: unsupported json format."<<std::endl;
 45 |         // open3d::utility::LogWarning(
 46 |         //         "DetectionField read JSON failed: unsupported json "
 47 |         //         "format.");
 48 |         return false;
 49 |     }
 50 |     const Json::Value object_array = value["mask"];
 51 | 
 52 |     if (!object_array.isArray() || object_array.size() <2) {
 53 |         std::cerr<<"DetectionField cannot find valid objects."<<std::endl;
 54 |         return false;
 55 |     }
 56 |     
 57 |     // open3d::utility::LogInfo("DetectionField read {:d} objects.", object_array.size()-1);
 58 |     std::stringstream msg;
 59 | 
 60 |     for(int i=0;i<object_array.size();++i){ // update detected objects
 61 |         const Json::Value object = object_array[i];
 62 |         int detection_id = object["value"].asInt();
 63 |         if(detection_id==0) continue;
 64 |         auto detection = std::make_shared<Detection>(detection_id);
 65 | 
 66 |         auto label_score_map = object["labels"]; // {label:score}
 67 |         msg<<detection_id<<": ";
 68 |         for(auto it=label_score_map.begin();it!=label_score_map.end();++it){
 69 |             std::string label = it.key().asString();
 70 |             float score = (*it).asFloat();
 71 |             detection->labels_.push_back(std::make_pair(label,score));
 72 |             msg<<"("<<label<<","<<score<<") ";
 73 |         }
 74 | 
 75 |         auto box_array = object["box"];
 76 |         detection->bbox_.u0 = box_array[0].asDouble();
 77 |         detection->bbox_.v0 = box_array[1].asDouble();
 78 |         detection->bbox_.u1 = box_array[2].asDouble();
 79 |         detection->bbox_.v1 = box_array[3].asDouble();
 80 | 
 81 |         // update
 82 |         // int box_area = (detection->bbox_.u1-detection->bbox_.u0)*(detection->bbox_.v1-detection->bbox_.v0);
 83 |         // if(box_area<max_box_area_){
 84 |         detections.push_back(detection);
 85 |         msg<<"bbox("<<detection->bbox_.u0<<","<<detection->bbox_.v0<<","<<detection->bbox_.u1<<","<<detection->bbox_.v1<<") ";
 86 |         msg<<"\n";
 87 |         // }
 88 |     }
 89 | 
 90 |     // update raw tags
 91 |     auto raw_tag_array = split_str(value["raw_tags"].asString(), ".");
 92 |     msg<<"raw tags (";
 93 |     for(auto tag:raw_tag_array){
 94 |         raw_tags.push_back(tag.substr(tag.find_first_not_of(" ")));
 95 |         msg<<"$"<<tag.substr(tag.find_first_not_of(" "))<<"$";
 96 |     }
 97 |     msg<<")\n";
 98 | 
 99 |     // update filter tags
100 |     auto filter_tag_array = split_str(value["tags"].asString(), ".");
101 |     msg<<"filter tags (";
102 |     for(auto tag:filter_tag_array){
103 |         filter_tags.push_back(tag.substr(tag.find_first_not_of(" ")));
104 |         msg<<"$"<<tag.substr(tag.find_first_not_of(" "))<<"$";
105 |     }
106 |     msg<<")\n";
107 | 
108 |     // for debug
109 |     // std::cout<<msg.str()<<endl;
110 | 
111 |     return true;
112 | }
113 | 
114 | bool DetectionFile::updateInstanceMap(const std::string &instance_file)
115 | {
116 |     const int K = detections.size();
117 |     cv::Mat detection_map = cv::imread(instance_file, -1);
118 |     double min, max;
119 |     cv::Point min_loc, max_loc;
120 |     cv::minMaxLoc(detection_map, &min, &max, &min_loc, &max_loc);
121 | 
122 |     // todo: run sam with larger number of boxes
123 |     if(max!=K){
124 |         std::cerr<<"instance map has different number of instances"<<std::endl;
125 |         return false;
126 |     }
127 |     assert (max == K), "instance map has different number of instances";
128 |     std::vector<int> to_remove_detections;
129 | 
130 |     for(int idx=1;idx<=K;++idx){
131 |         cv::Mat mask = (detection_map == idx);
132 |         assert (detections[idx-1]->id_ == idx), "detection id is not consistent";
133 |         detections[idx-1]->instances_idxs_ = mask; // [H,W], CV_8UC1
134 |         if(cv::countNonZero(mask)<min_mask_ || detections[idx-1]->get_box_area()>max_box_area_){
135 |             to_remove_detections.push_back(idx-1);
136 |         }
137 |     }
138 | 
139 |     // remove invalid detections 
140 |     if(to_remove_detections.size()>0){
141 |         std::cerr<<"Remove "<<to_remove_detections.size()<<" invalid detections"<<std::endl;
142 |         for(int i=to_remove_detections.size()-1;i>=0;--i){
143 |             detections.erase(detections.begin()+to_remove_detections[i]);
144 |         }
145 |     }
146 | 
147 |     return true;
148 | }
149 | 
150 | }
151 | 


--------------------------------------------------------------------------------
/src/mapping/Detection.h:
--------------------------------------------------------------------------------
 1 | #ifndef FMFUSION_DETECTION_H
 2 | #define FMFUSION_DETECTION_H
 3 | 
 4 | #include "opencv2/opencv.hpp"
 5 | #include "open3d/utility/IJsonConvertible.h"
 6 | 
 7 | 
 8 | namespace fmfusion
 9 | {
10 |     
11 | typedef std::pair<std::string,float> LabelScore;
12 | 
13 | struct BoundingBox{
14 |     double u0,v0,u1,v1;
15 | };
16 | 
17 | class Detection
18 | {
19 | public:
20 |     Detection(const int id);
21 |     Detection(const std::vector<LabelScore> &labels, const BoundingBox &bbox, const cv::Mat &instances_idxs): 
22 |         labels_(labels), bbox_(bbox), instances_idxs_(instances_idxs) {};
23 |     std::string extract_label_string() const;
24 |     const cv::Point get_box_center(){return cv::Point((bbox_.u0+bbox_.u1)/2,(bbox_.v0+bbox_.v1)/2);};
25 |     const int get_box_area(){return (bbox_.u1-bbox_.u0)*(bbox_.v1-bbox_.v0);}
26 | 
27 | public:
28 |     unsigned int id_;
29 |     std::vector<LabelScore> labels_;
30 |     BoundingBox bbox_;
31 |     cv::Mat instances_idxs_; // [H,W], CV_8UC1
32 | };
33 | 
34 | typedef std::shared_ptr<Detection> DetectionPtr;
35 | 
36 | class DetectionFile: public open3d::utility::IJsonConvertible
37 | {
38 | public:
39 |     DetectionFile(int min_mask, int max_box_area): min_mask_(min_mask), max_box_area_(max_box_area) {};
40 | 
41 |     bool updateInstanceMap(const std::string &instance_file);    
42 | 
43 | public:
44 |     bool ConvertToJsonValue(Json::Value &value) const override {return true;};
45 |     bool ConvertFromJsonValue(const Json::Value &value) override;
46 | 
47 | protected:
48 |     std::vector<std::string> split_str(const std::string s, const std::string delim);
49 |     int min_mask_;
50 |     int max_box_area_;
51 | 
52 | public:
53 |     std::vector<DetectionPtr> detections;
54 |     std::vector<std::string> raw_tags;
55 |     std::vector<std::string> filter_tags;
56 | };
57 | 
58 | }
59 | 
60 | #endif //FMFUSION_DETECTION_H


--------------------------------------------------------------------------------
/src/mapping/Instance.h:
--------------------------------------------------------------------------------
  1 | #ifndef FMFUSION_INSTANCE_H
  2 | #define FMFUSION_INSTANCE_H
  3 | 
  4 | #include <list>
  5 | #include <string>
  6 | 
  7 | #include "open3d/Open3D.h"
  8 | #include "Detection.h"
  9 | #include "Common.h"
 10 | #include "SubVolume.h"
 11 | 
 12 | namespace fmfusion {
 13 | 
 14 |     namespace o3d_utility = open3d::utility;
 15 | 
 16 |     class Instance {
 17 | 
 18 |     public:
 19 |         Instance(const InstanceId id, const unsigned int frame_id, const InstanceConfig &config);
 20 | 
 21 |         ~Instance() {};
 22 | 
 23 |         void init_bayesian_fusion(const std::vector<std::string> &label_set);
 24 |     public:
 25 |         void integrate(const int &frame_id,
 26 |                        const std::shared_ptr<open3d::geometry::RGBDImage> &rgbd_image, const Eigen::Matrix4d &pose);
 27 | 
 28 |         // Record measured labels and update the predicted label.
 29 |         void update_label(const DetectionPtr &detection);
 30 | 
 31 |         /// \brief  Update the centroid from volume units origins.
 32 |         void fast_update_centroid() { centroid = volume_->get_centroid(); };
 33 | 
 34 |         bool update_point_cloud(int cur_frame_id, int min_frame_gap = 10);
 35 | 
 36 |         /// @brief  add the probability vector from each measurement.
 37 |         ///         And update the predicted label.
 38 |         /// @param probability_vector_ computed from BayesianLabel
 39 |         /// @return success flag
 40 |         bool update_semantic_probability(const Eigen::VectorXf &probability_vector_);
 41 | 
 42 |         void merge_with(const O3d_Cloud_Ptr &other_cloud,
 43 |                         const std::unordered_map<std::string, float> &label_measurements, 
 44 |                         const int &observations_);
 45 | 
 46 |         void extract_write_point_cloud();
 47 |     
 48 |         void filter_pointcloud_statistic();
 49 | 
 50 |         bool filter_pointcloud_by_cluster();
 51 | 
 52 |         void CreateMinimalBoundingBox();
 53 | 
 54 |         /// \brief  label_str:label_name(score),label_name(score),...
 55 |         ///         record previouse measured labels
 56 |         void load_previous_labels(const std::string &labels_str);
 57 | 
 58 |         void load_obser_count(const int &obs_count){
 59 |             observation_count = obs_count;
 60 |         }
 61 | 
 62 |         void save(const std::string &path);
 63 | 
 64 |         void load(const std::string &path);
 65 | 
 66 |     public:
 67 |         SubVolume *get_volume() { return volume_; }
 68 | 
 69 |         LabelScore get_predicted_class() const { 
 70 |             return predicted_label;
 71 |         }
 72 | 
 73 |         std::unordered_map<std::string, float> get_measured_labels() const { 
 74 |             return measured_labels; 
 75 |         }
 76 | 
 77 |         size_t get_cloud_size() const;
 78 | 
 79 |         O3d_Cloud_Ptr get_complete_cloud() const;
 80 | 
 81 |         InstanceConfig get_config() const { return config_; }
 82 | 
 83 |         std::string get_measured_labels_string() const {
 84 |             std::stringstream label_measurements;
 85 |             for (const auto &label_score: measured_labels) {
 86 |                 label_measurements << label_score.first
 87 |                                    << "(" << std::fixed << std::setprecision(2) << label_score.second << "),";
 88 |             }
 89 |             return label_measurements.str();
 90 |         }
 91 | 
 92 |         int get_observation_count() const {
 93 |             return observation_count;
 94 |         }
 95 | 
 96 |         InstanceId get_id() const { return id_; }
 97 | 
 98 |         void change_id(InstanceId new_id) { 
 99 |             id_ = new_id; 
100 |         }
101 | 
102 |         O3d_Cloud_Ptr get_point_cloud() const;
103 | 
104 |         bool isBayesianFusion() const { return bayesian_label; }
105 | 
106 |     private:
107 |         /// @brief Extract the label with the maximum probability
108 |         ///        as the predicted class.
109 |         void extract_bayesian_prediciton();
110 | 
111 |     public:
112 |         unsigned int frame_id_; // latest integration frame id
113 |         unsigned int update_frame_id; // update point cloud and bounding box
114 |         Eigen::Vector3d color_;
115 |         std::shared_ptr<cv::Mat> observed_image_mask; // Poject volume on image plane;
116 |         // open3d::pipelines::integration::InstanceTSDFVolume *volume_;
117 |         SubVolume *volume_;
118 |         O3d_Cloud_Ptr point_cloud;
119 |         Eigen::Vector3d centroid;
120 |         Eigen::Vector3d normal;
121 |         std::shared_ptr<open3d::geometry::OrientedBoundingBox> min_box;
122 | 
123 |     private:
124 |         InstanceId id_; // >=1
125 |         InstanceConfig config_;
126 |         std::unordered_map<std::string, float> measured_labels;
127 |         LabelScore predicted_label;
128 |         int observation_count;
129 |         O3d_Cloud_Ptr merged_cloud;
130 | 
131 |         // For Bayesian likelihood
132 |         std::vector<std::string> semantic_labels;
133 |         Eigen::VectorXf probability_vector;
134 |         bool bayesian_label;
135 |     };
136 | 
137 |     typedef std::shared_ptr<Instance> InstancePtr;
138 | 
139 | } // namespace fmfusion
140 | 
141 | #endif // FMFUSION_INSTANCE_H
142 | 
143 |     


--------------------------------------------------------------------------------
/src/mapping/SemanticDict.h:
--------------------------------------------------------------------------------
 1 | #ifndef SEMANTICDICT_H
 2 | #define SEMANTICDICT_H
 3 | 
 4 | #include <unordered_map>
 5 | 
 6 | namespace fmfusion
 7 | {
 8 |     typedef unordered_map<std::string, InstanceIdList> SemanticDictionary;
 9 | 
10 |     class SemanticDictServer
11 |     {
12 |     public:
13 |         SemanticDictServer():semantic_dict()
14 |         {
15 |             semantic_dict.clear();
16 |         };
17 | 
18 |         ~SemanticDictServer(){};
19 | 
20 |         /// @brief  Update the semantic dictionary.
21 |         /// @param semantic_label 
22 |         /// @param instance_id 
23 |         void update_instance(const std::string &semantic_label, 
24 |                             const InstanceId &instance_id)
25 |         {
26 |             if(semantic_label=="floor" ||semantic_label=="carpet"){
27 |                 if(semantic_dict.find(semantic_label)==semantic_dict.end())
28 |                     semantic_dict[semantic_label] = {instance_id};
29 |                 else
30 |                     semantic_dict[semantic_label].push_back(instance_id);
31 |             }
32 |         };
33 | 
34 |         void clear()
35 |         {
36 |             semantic_dict.clear();  
37 |         };
38 | 
39 |         std::vector<InstanceId> query_instances(const std::string &label)
40 |         {
41 |             if(semantic_dict.find(label)==semantic_dict.end())
42 |                 return {};
43 |             return semantic_dict[label];
44 |         }
45 | 
46 |         // SemanticDictionary get_semantic_dict()
47 |         // {
48 |         //     return semantic_dict;
49 |         // }
50 | 
51 |     private:
52 |         SemanticDictionary semantic_dict;
53 |     };
54 | 
55 | }
56 | 
57 | #endif
58 | 


--------------------------------------------------------------------------------
/src/mapping/SemanticMapping.h:
--------------------------------------------------------------------------------
  1 | #ifndef FMFUSION_SEMANTICMAPPING_H
  2 | #define FMFUSION_SEMANTICMAPPING_H
  3 | 
  4 | #include <unordered_map>
  5 | #include <fstream>
  6 | 
  7 | #include "Common.h"
  8 | #include "tools/Color.h"
  9 | #include "tools/Utility.h"
 10 | #include "Instance.h"
 11 | #include "SemanticDict.h"
 12 | #include "BayesianLabel.h"
 13 | 
 14 | namespace fmfusion {
 15 | 
 16 | class SemanticMapping {
 17 | public:
 18 |     SemanticMapping(const MappingConfig &mapping_cfg, const InstanceConfig &instance_cfg);
 19 | 
 20 |     ~SemanticMapping() {};
 21 | 
 22 | public:
 23 |     void integrate(const int &frame_id,
 24 |                     const std::shared_ptr<open3d::geometry::RGBDImage> &rgbd_image, const Eigen::Matrix4d &pose,
 25 |                     std::vector<DetectionPtr> &detections);
 26 | 
 27 |     int merge_overlap_instances(std::vector<InstanceId> instance_list = std::vector<InstanceId>());
 28 | 
 29 |     int merge_floor(bool verbose=false);
 30 | 
 31 |     int merge_overlap_structural_instances(bool merge_all = true);
 32 | 
 33 |     int merge_other_instances(std::vector<InstancePtr> &instances);
 34 |     
 35 |     void extract_point_cloud(const std::vector<InstanceId> instance_list = std::vector<InstanceId>());
 36 | 
 37 |     /// \brief  Extract and update bounding box for each instance.
 38 |     void extract_bounding_boxes();
 39 | 
 40 |     int update_instances(const int &cur_frame_id, const std::vector<InstanceId> &instance_list);
 41 | 
 42 |     /// @brief  clean all and update all.
 43 |     void refresh_all_semantic_dict();
 44 | 
 45 |     std::shared_ptr<open3d::geometry::PointCloud> export_global_pcd(bool filter = false, float vx_size = -1.0);
 46 | 
 47 |     std::vector<Eigen::Vector3d> export_instance_centroids(int earliest_frame_id = -1,
 48 |                                                         bool verbose=false) const;
 49 | 
 50 |     std::vector<std::string> export_instance_annotations(int earliest_frame_id = -1
 51 |                                                         ) const;
 52 | 
 53 |     bool query_instance_info(const std::vector<InstanceId> &names,
 54 |                                 std::vector<Eigen::Vector3f> &centroids,
 55 |                                 std::vector<std::string> &labels);
 56 | 
 57 |     void remove_invalid_instances();
 58 | 
 59 |     /// \brief  Get geometries for each instance.
 60 |     std::vector<std::shared_ptr<const open3d::geometry::Geometry>>
 61 |     get_geometries(bool point_cloud = true, bool bbox = false);
 62 | 
 63 |     bool is_empty() { return instance_map.empty(); }
 64 | 
 65 |     InstancePtr get_instance(const InstanceId &name) { return instance_map[name]; }
 66 | 
 67 |     void Transform(const Eigen::Matrix4d &pose);
 68 | 
 69 |     /// @brief
 70 |     /// @param path output sequence folder
 71 |     /// @return
 72 |     bool Save(const std::string &path);
 73 | 
 74 |     bool load(const std::string &path);
 75 | 
 76 |     /// \brief  Export instances to the vector.
 77 |     ///         Filter instances that are too small or not been observed for a long time.
 78 |     void export_instances(std::vector<InstanceId> &names, std::vector<InstancePtr> &instances,
 79 |                             int earliest_frame_id = 0);
 80 | 
 81 | protected:
 82 |     /// \brief  match vector in [K,1], K is the number of detections;
 83 |     /// If detection k is associated, match[k] = matched_instance_id
 84 |     int
 85 |     data_association(const std::vector<DetectionPtr> &detections, const std::vector<InstanceId> &active_instances,
 86 |                         Eigen::VectorXi &matches,
 87 |                         std::vector<std::pair<InstanceId, InstanceId>> &ambiguous_pairs);
 88 | 
 89 |     int create_new_instance(const DetectionPtr &detection, const unsigned int &frame_id,
 90 |                             const std::shared_ptr<open3d::geometry::RGBDImage> &rgbd_image,
 91 |                             const Eigen::Matrix4d &pose);
 92 | 
 93 |     std::vector<InstanceId> search_active_instances(const O3d_Cloud_Ptr &depth_cloud, const Eigen::Matrix4d &pose,
 94 |                                                     const double search_radius = 5.0);
 95 | 
 96 |     void update_active_instances(const std::vector<InstanceId> &active_instances);
 97 | 
 98 |     void update_recent_instances(const int &frame_id,
 99 |                                     const std::vector<InstanceId> &active_instances,
100 |                                     const std::vector<InstanceId> &new_instances);
101 | 
102 |     bool IsSemanticSimilar(const std::unordered_map<std::string, float> &measured_labels_a,
103 |                             const std::unordered_map<std::string, float> &measured_labels_b);
104 | 
105 |     /// \brief  Compute the 2D IoU (horizontal plane) between two oriented bounding boxes.
106 |     double Compute2DIoU(const open3d::geometry::OrientedBoundingBox &box_a,
107 |                         const open3d::geometry::OrientedBoundingBox &box_b);
108 | 
109 |     /// \brief  Compute the 3D IoU between two point clouds.
110 |     /// \param cloud_a, point cloud of the larger instance
111 |     /// \param cloud_b, point cloud of the smaller instance
112 |     double Compute3DIoU(const O3d_Cloud_Ptr &cloud_a, const O3d_Cloud_Ptr &cloud_b, double inflation = 1.0);
113 | 
114 |     int merge_ambiguous_instances(const std::vector<std::pair<InstanceId, InstanceId>> &ambiguous_pairs);
115 | 
116 |     // Recent observed instances
117 |     std::unordered_set<InstanceId> recent_instances;
118 | 
119 | private:
120 |     // Config config_;
121 |     MappingConfig mapping_config;
122 |     InstanceConfig instance_config;
123 |     std::unordered_map<InstanceId, InstancePtr> instance_map;
124 |     std::unordered_map<std::string, std::vector<InstanceId>> label_instance_map;
125 |     SemanticDictServer semantic_dict_server;
126 |     BayesianLabel *bayesian_label;
127 | 
128 |     InstanceId latest_created_instance_id;
129 |     int last_cleanup_frame_id;
130 |     int last_update_frame_id;
131 | 
132 | };
133 | 
134 | 
135 | } // namespace fmfusion
136 | 
137 | 
138 | #endif //FMFUSION_SEMANTICMAPPING_H
139 | 


--------------------------------------------------------------------------------
/src/mapping/SubVolume.cpp:
--------------------------------------------------------------------------------
  1 | #include "SubVolume.h"
  2 | 
  3 | namespace fmfusion
  4 | {
  5 | 
  6 | SubVolume::SubVolume(double voxel_length,
  7 |                      double sdf_trunc,
  8 |                      TSDFVolumeColorType color_type,
  9 |                      int volume_unit_resolution,
 10 |                      int depth_sampling_stride)
 11 |     : ScalableTSDFVolume(voxel_length, 
 12 |                         sdf_trunc, 
 13 |                         color_type, 
 14 |                         volume_unit_resolution, 
 15 |                         depth_sampling_stride){}
 16 | 
 17 | SubVolume::~SubVolume() {}
 18 | 
 19 | bool SubVolume::query_observed_points(const PointCloudPtr &cloud_scan,
 20 |                                  PointCloudPtr &cloud_observed,
 21 |                                  const float max_dist)
 22 | {
 23 | 
 24 |     for (size_t i = 0; i < cloud_scan->points_.size(); i++)
 25 |     {
 26 |         Eigen::Vector3d point = cloud_scan->points_[i];
 27 |         // if (volume_units_.find(index)!=volume_units_.end())
 28 |         //     observed_units.insert(index);
 29 | 
 30 |         Eigen::Vector3d p_locate =
 31 |             point - Eigen::Vector3d(0.5, 0.5, 0.5) * voxel_length_;
 32 |         Eigen::Vector3i index0 = LocateVolumeUnit(p_locate);
 33 |         auto unit_itr = volume_units_.find(index0);
 34 |         if (unit_itr == volume_units_.end()) {
 35 |             continue;
 36 |         }
 37 | 
 38 |         const auto &volume0 = *unit_itr->second.volume_;
 39 |         Eigen::Vector3i idx0; // root voxel index
 40 |         Eigen::Vector3d p_grid =
 41 |                 (p_locate - index0.cast<double>() * volume_unit_length_) /
 42 |                 voxel_length_;  // point position in voxels
 43 |         for (int i = 0; i < 3; i++) {
 44 |             idx0(i) = (int)std::floor(p_grid(i));
 45 |             if (idx0(i) < 0) idx0(i) = 0;
 46 |             if (idx0(i) >= volume_unit_resolution_)
 47 |                 idx0(i) = volume_unit_resolution_ - 1;
 48 |         }
 49 | 
 50 |         // iterate over neighbor voxels
 51 |         for (int i = 0; i < 8; i++) {
 52 |             float w0 = 0.0f;
 53 |             float f0 = 0.0f;
 54 |             Eigen::Vector3i idx1 = idx0 + shift[i];
 55 |             if (idx1(0) < volume_unit_resolution_ &&
 56 |                 idx1(1) < volume_unit_resolution_ &&
 57 |                 idx1(2) < volume_unit_resolution_) {
 58 |                 w0 = volume0.voxels_[volume0.IndexOf(idx1)].weight_;
 59 |                 f0 = volume0.voxels_[volume0.IndexOf(idx1)].tsdf_;
 60 |             } 
 61 |             else {
 62 |                 Eigen::Vector3i index1 = index0;
 63 |                 for (int j = 0; j < 3; j++) {
 64 |                     if (idx1(j) >= volume_unit_resolution_) {
 65 |                         idx1(j) -= volume_unit_resolution_;
 66 |                         index1(j) += 1;
 67 |                     }
 68 |                 }
 69 |                 auto unit_itr1 = volume_units_.find(index1);
 70 |                 if (unit_itr1 != volume_units_.end()) {
 71 |                     const auto &volume1 = *unit_itr1->second.volume_;
 72 |                     w0 = volume1.voxels_[volume1.IndexOf(idx1)].weight_;
 73 |                     f0 = volume1.voxels_[volume1.IndexOf(idx1)].tsdf_;
 74 |                 }
 75 |             }
 76 |             if(w0!=0.0f && f0<0.98f && f0>=-0.98f){
 77 |                 cloud_observed->points_.push_back(point);
 78 |                 break;
 79 |             }
 80 |         }
 81 | 
 82 |     }
 83 |     return true;
 84 | 
 85 | }
 86 |     
 87 | Eigen::Vector3d SubVolume::get_centroid()
 88 | {
 89 |     Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
 90 |     int count = 0;
 91 |     for(const auto&unit:volume_units_){
 92 |         if(unit.second.volume_){
 93 |             centroid += unit.second.volume_->origin_;
 94 |             count++;
 95 |         }
 96 |     }
 97 | 
 98 |     if(count>0) return centroid/count;
 99 |     else return Eigen::Vector3d::Zero();
100 | }
101 | 
102 | } // namespace fmfusion
103 | 
104 | 


--------------------------------------------------------------------------------
/src/mapping/SubVolume.h:
--------------------------------------------------------------------------------
 1 | 
 2 | #pragma once
 3 | 
 4 | #include <memory>
 5 | #include <string>
 6 | #include <vector>
 7 | #include <unordered_map>
 8 | #include <unordered_set>
 9 | 
10 | #include "open3d/utility/Logging.h"
11 | #include "open3d/geometry/PointCloud.h"
12 | #include "open3d/pipelines/integration/UniformTSDFVolume.h"
13 | #include "open3d/pipelines/integration/ScalableTSDFVolume.h"
14 | #include "open3d/pipelines/integration/MarchingCubesConst.h"
15 | 
16 | namespace fmfusion
17 | {
18 |     // class UniformTSDFVolume;
19 |     typedef open3d::pipelines::integration::ScalableTSDFVolume ScalableTSDFVolume;
20 |     typedef std::shared_ptr<open3d::geometry::PointCloud> PointCloudPtr;
21 |     typedef open3d::pipelines::integration::TSDFVolumeColorType TSDFVolumeColorType;
22 | 
23 |     class SubVolume : public ScalableTSDFVolume {
24 |     public:
25 |         SubVolume(double voxel_length,
26 |                         double sdf_trunc,
27 |                         TSDFVolumeColorType color_type,
28 |                         int volume_unit_resolution = 16,
29 |                         int depth_sampling_stride = 4);
30 |         ~SubVolume() override;
31 | 
32 |     public:
33 |         // std::shared_ptr<geometry::PointCloud> ExtractWeightedPointCloud(const float min_weight=0.0);
34 | 
35 |         /// @brief Generate scan cloud from depth image. 
36 |         ///         And query observed voxel points from the volume.
37 |         bool query_observed_points(const PointCloudPtr &cloud_scan,
38 |                                 PointCloudPtr &cloud_observed,
39 |                                 const float max_dist=0.98f);
40 | 
41 |         //todo: inaccurate
42 |         /// @brief  Get the centroid of all volume units origin.
43 |         /// @return 
44 |         Eigen::Vector3d get_centroid();
45 | 
46 |     protected:
47 |         Eigen::Vector3i LocateVolumeUnit(const Eigen::Vector3d &point) {
48 |             return Eigen::Vector3i((int)std::floor(point(0) / volume_unit_length_),
49 |                                 (int)std::floor(point(1) / volume_unit_length_),
50 |                                 (int)std::floor(point(2) / volume_unit_length_));
51 |         };
52 | 
53 |         Eigen::Vector3d GetNormalAt(const Eigen::Vector3d &p);
54 | 
55 |         double GetTSDFAt(const Eigen::Vector3d &p);
56 | 
57 |     };
58 | 
59 | 
60 | }
61 | 
62 | 


--------------------------------------------------------------------------------
/src/sgloop/BertBow.cpp:
--------------------------------------------------------------------------------
  1 | #include "BertBow.h"
  2 | 
  3 | 
  4 | namespace fmfusion
  5 | {
  6 | 
  7 |     std::vector<char> get_the_bytes(const std::string &filename){
  8 |         std::ifstream input(filename, std::ios::binary);
  9 |         if(!input.is_open()){
 10 |             throw std::runtime_error("Cannot open file " + filename + " for reading.");
 11 |             return std::vector<char>();
 12 |         }
 13 | 
 14 |         std::vector<char> bytes(
 15 |                 (std::istreambuf_iterator<char>(input)),
 16 |                 (std::istreambuf_iterator<char>()));
 17 |         input.close();
 18 |         return bytes;
 19 |     }
 20 | 
 21 | 
 22 |     BertBow::BertBow(const std::string &words_file, const std::string &word_features_file, bool cuda_device_):
 23 |         cuda_device(cuda_device_)
 24 |     {
 25 |         if(load_word2int(words_file) && load_word_features(word_features_file))
 26 |         {
 27 |             N = word2int.size();
 28 |             assert(N==(word_features.size(0)-1));
 29 |             load_success = true;
 30 |             if(cuda_device) word_features = word_features.to(torch::kCUDA);
 31 |             std::cout<<"Load "<< N <<" words features on CUDA\n";
 32 |                         // <<cuda_device<<"\n" ;  //A zero feature is padded in the first row.\n"; 
 33 |             warm_up();
 34 |                               
 35 |         }
 36 |         else{
 37 |             load_success = false;
 38 |             std::cerr<<"Error: Bert Bag of Words Loaded Wrong!\n";
 39 |         }
 40 |     }
 41 | 
 42 |     bool BertBow::query_semantic_features(const std::vector<std::string> &words, 
 43 |                                     torch::Tensor &features)
 44 |     {
 45 |         int Q = words.size();
 46 |         if(Q<1) return false;
 47 |         float indices[Q];
 48 |         // float mask[Q]; // mask the invalid query
 49 | 
 50 |         for(int i=0; i<Q; i++){
 51 |             if(word2int.find(words[i]) == word2int.end()){
 52 |                 std::cerr<<"Error: word "<<words[i]<<" not found in the dictionary. Todo\n";
 53 |                 indices[i] = 0;
 54 |             }
 55 |             else{
 56 |                 indices[i] = word2int[words[i]];
 57 |             } 
 58 |         }
 59 |  
 60 |         indices_tensor = torch::from_blob(indices, {Q}, torch::kFloat32).toType(torch::kInt64);
 61 |         // if(cuda_device) indices_tensor = indices_tensor.to(torch::kCUDA);
 62 |         indices_tensor = indices_tensor.to(word_features.device());
 63 |         features = torch::index_select(word_features, 0, indices_tensor);
 64 | 
 65 |         return true;
 66 |     }
 67 | 
 68 | 
 69 |     bool BertBow::load_word2int(const std::string &word2int_file)
 70 |     {
 71 |         // std::cout<<"Load word2int file from "<<word2int_file<<"\n";
 72 |         std::ifstream file(word2int_file, std::ifstream::in);
 73 |         if (!file.is_open()){
 74 |             std::cerr << "Error: cannot open file " << word2int_file << std::endl;
 75 |             return false;
 76 |         }
 77 |         else{
 78 |             std::string line;
 79 |             while (std::getline(file, line)){
 80 |                 std::string label;
 81 |                 int index;
 82 |                 index = std::stoi(line.substr(0, line.find(".")));
 83 |                 label = line.substr(line.find(".")+1, line.size()-line.find("."));
 84 | 
 85 |                 word2int[label] = index;
 86 |                 // std::cout<<index<<":"<<label<<"\n";
 87 |             }
 88 |             return true;
 89 |         }
 90 | 
 91 |     }
 92 | 
 93 |     bool BertBow::load_word_features(const std::string &word_features_file)
 94 |     {
 95 | 
 96 |         std::vector<char> bytes = get_the_bytes(word_features_file);
 97 |         if(bytes.empty()){
 98 |             return false;
 99 |         }
100 |         else{
101 |             // std::cout<<"Load word features from "<<word_features_file<<"\n";
102 |             torch::IValue ivalue = torch::pickle_load(bytes);
103 |             word_features = ivalue.toTensor();
104 |             assert(torch::isnan(word_features).sum().item<int>()==0);
105 | 
106 |             torch::Tensor zero_features = torch::zeros({1, word_features.size(1)});
107 |             word_features = torch::cat({zero_features, word_features}, 0);
108 |             if(cuda_device) word_features = word_features.to(torch::kCUDA);
109 |             // std::cout<<"feature shape "<<word_features.sizes()<<"\n";
110 |             return true;
111 |         }
112 | 
113 |     }
114 | 
115 |     void BertBow::warm_up()
116 |     {
117 |         indices_tensor = torch::randint(0, N, {30}).toType(torch::kInt64);
118 |         indices_tensor = indices_tensor.to(word_features.device());
119 |         torch::Tensor tmp_features = torch::index_select(word_features, 0, indices_tensor);
120 |         std::cout<<"Warm up BertBow with random quries \n";
121 |     }
122 | 
123 | }
124 | 


--------------------------------------------------------------------------------
/src/sgloop/BertBow.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * @Author: Glen LIU 
 3 |  * @Date: 2024-06-25 16:27:24 
 4 |  * @Last Modified by: Glen LIU
 5 |  * @Last Modified time: 2024-06-25 16:36:43
 6 |  */
 7 | /// \file This file load a pre-generated words and semantic features.
 8 | ///     Given a vector of N words, it returns a semantic features in NxD.
 9 | 
10 | #ifndef BERTBOW_H
11 | #define BERTBOW_H
12 | 
13 | #include <fstream>
14 | #include <iostream>
15 | #include <map>
16 | #include <torch/torch.h>
17 | 
18 | 
19 | namespace fmfusion
20 | {
21 | 
22 | std::vector<char> get_the_bytes(const std::string &filename);
23 | 
24 | class BertBow
25 | {
26 |     public:
27 |         BertBow(const std::string &words_file, const std::string &word_features_file, bool cuda_device_=false);
28 |         ~BertBow() {};
29 | 
30 |         /// Read N words, and return NxD features.
31 |         bool query_semantic_features(const std::vector<std::string> &words, 
32 |                                     torch::Tensor &features);
33 |                                     
34 |         bool is_loaded() const {return load_success;};
35 | 
36 |         void get_word2int(std::map<std::string, int>& word2int_map)const{
37 |             word2int_map = word2int;
38 |         }
39 | 
40 |     private:
41 |         bool load_word2int(const std::string &word2int_file);
42 |         bool load_word_features(const std::string &word_features_file);
43 | 
44 |         void warm_up();
45 | 
46 |     private:
47 |         bool load_success;
48 |         bool cuda_device;
49 |         std::map<std::string, int> word2int;
50 |         torch::Tensor word_features;
51 |         int N;
52 | 
53 |         torch::Tensor indices_tensor;
54 | 
55 | };
56 | 
57 | } // namespace fmfusion
58 | 
59 | 
60 | #endif // BERTBOW_H
61 | 
62 | 


--------------------------------------------------------------------------------
/src/sgloop/Graph.h:
--------------------------------------------------------------------------------
  1 | #ifndef FMFUSION_GRAPH_H
  2 | #define FMFUSION_GRAPH_H
  3 | 
  4 | #include <Common.h>
  5 | #include <mapping/Instance.h>
  6 | 
  7 | namespace fmfusion
  8 | {
  9 | 
 10 | typedef std::array<uint32_t,2> Corner;
 11 | 
 12 | class Node
 13 | {
 14 |     public:
 15 |         Node(uint32_t node_id_, InstanceId instance_id_):
 16 |             id(node_id_), instance_id(instance_id_){};
 17 |         
 18 |         /// @brief 
 19 |         /// @param max_corner_number 
 20 |         /// @param corner_vector 
 21 |         /// @param padding_value For the corners less than max_corner_number, padding with this value. 
 22 |         void sample_corners(const int &max_corner_number, std::vector<Corner> &corner_vector, int padding_value=0);
 23 | 
 24 |         ~Node(){};
 25 |     public:
 26 |         uint32_t id; // node id, reordered from 0
 27 |         InstanceId instance_id; // corresponds to original instance id
 28 |         std::string semantic;
 29 |         std::vector<uint32_t> neighbors;
 30 |         std::vector<Corner> corners; // corner1, corner2. The anchor node and corner nodes form a triplet.
 31 |         O3d_Cloud_Ptr cloud;
 32 |         Eigen::Vector3d centroid;
 33 |         Eigen::Vector3d bbox_shape; // x,y,z
 34 | 
 35 | };
 36 | typedef std::shared_ptr<Node> NodePtr;
 37 | 
 38 | 
 39 | class Edge
 40 | {
 41 | public:
 42 |     Edge(const uint32_t &id1, const uint32_t &id2): src_id(id1), ref_id(id2) {
 43 |         distance = 0.0;
 44 |     }
 45 |     ~Edge() {};
 46 | 
 47 | public:
 48 |     uint32_t src_id; // start node
 49 |     uint32_t ref_id; // end node
 50 |     double distance;
 51 | };
 52 | typedef std::shared_ptr<Edge> EdgePtr;
 53 | 
 54 | 
 55 | struct DataDict{
 56 |     std::vector<Eigen::Vector3d> xyz; // (X,3)
 57 |     std::vector<int> length_vec; // (B,) [|X1|,|X2|,...,|XB|]
 58 |     std::vector<uint32_t> labels; // (X,)
 59 |     std::vector<Eigen::Vector3d> centroids; // (N,3)
 60 |     std::vector<uint32_t> nodes; // (N,)
 61 |     std::vector<uint32_t> instances; // (N,)
 62 |     std::string print_instances(){
 63 |         std::stringstream msg;
 64 |         for (auto instance:instances){
 65 |             msg<<instance<<",";
 66 |         }
 67 |         return msg.str();
 68 |     }
 69 | 
 70 |     void clear(){
 71 |         xyz.clear();
 72 |         length_vec.clear();
 73 |         labels.clear();
 74 |         centroids.clear();
 75 |         nodes.clear();
 76 |         instances.clear();
 77 |     }
 78 | 
 79 | };
 80 | 
 81 | class Graph
 82 | {
 83 |     /// \brief  Explicit Scene Graph Representations.
 84 |     public:
 85 |         Graph(GraphConfig config_);
 86 |         void initialize(const std::vector<InstancePtr> &instances);
 87 | 
 88 |         /// \brief Update the node information received from communication server.
 89 |         ///        It cleared the graph and insert.
 90 |         int subscribe_coarse_nodes(const float &latest_timestamp,
 91 |                                     const std::vector<uint32_t> &node_indices,
 92 |                                     const std::vector<uint32_t> &instances,
 93 |                                     const std::vector<Eigen::Vector3d> &centroids);
 94 | 
 95 |         /// \brief Update the dense point cloud information received from communication server.
 96 |         ///        It should be updated after the coarse nodes are updated.
 97 |         int subscribde_dense_points(const float &sub_timestamp,
 98 |                                     const std::vector<Eigen::Vector3d> &xyz,
 99 |                                     const std::vector<uint32_t> &labels);
100 | 
101 |         void construct_edges();
102 | 
103 |         void construct_triplets();
104 | 
105 |         const std::vector<NodePtr> get_const_nodes() const { return nodes; }
106 | 
107 |         const std::vector<EdgePtr> get_const_edges() const { return edges; }
108 | 
109 |         const std::vector<std::pair<int,int>> get_edges() const;
110 | 
111 |         const std::vector<Eigen::Vector3d> get_centroids()const;
112 | 
113 |         void paint_all_floor(const Eigen::Vector3d &color);
114 | 
115 |         /// \brief  Extract global point cloud from the graph.
116 |         /// @param xyz Global point cloud.
117 |         /// @param labels Node ids.
118 |         void extract_global_cloud(std::vector<Eigen::Vector3d> &xyz,std::vector<uint32_t> &labels);
119 | 
120 |         /// \brief  Extract data dictionary.
121 |         /// @param coarse If true, extract only nodes, instances and centroids data.
122 |         DataDict extract_data_dict(bool coarse=false);
123 | 
124 |         O3d_Cloud_Ptr extract_global_cloud(float vx_size=-1.0)const;
125 | 
126 |         /// \brief  Extract instances and centroids.
127 |         DataDict extract_coarse_data_dict();
128 | 
129 |         std::string print_nodes_names()const{
130 |             std::stringstream ss;
131 |             for (auto node: nodes) ss<<node->instance_id<<",";
132 |             return ss.str();
133 |         }
134 | 
135 |         float get_timestamp()const{return timestamp;}
136 | 
137 |         void clear();
138 | 
139 |         ~Graph() {};
140 | 
141 |     private:
142 |         void update_neighbors();
143 | 
144 |     public:
145 |         int max_neighbor_number;
146 |         int max_corner_number;        
147 |         
148 |     private:
149 |         GraphConfig config;
150 |         std::map<InstanceId, int> instance2node_idx; // instance id to node idx
151 |         std::vector<InstanceId> node_instance_idxs; // corresponds to original instance ids
152 |         std::vector<NodePtr> nodes;
153 |         std::vector<EdgePtr> edges; // Each edge is a pair of node id
154 | 
155 |         int frame_id; // The latest received sg frame id
156 |         float timestamp; // The latest received sg timestamp
157 | 
158 | 
159 | };
160 | typedef std::shared_ptr<Graph> GraphPtr;
161 | 
162 | }
163 | 
164 | 
165 | #endif
166 | 
167 | 


--------------------------------------------------------------------------------
/src/sgloop/Initialization.h:
--------------------------------------------------------------------------------
 1 | #ifndef INITIALIZATION_H
 2 | #define INITIALIZATION_H
 3 | 
 4 | #include <mapping/SemanticMapping.h>
 5 | #include <sgloop/Graph.h>
 6 | 
 7 | 
 8 | namespace fmfusion
 9 | {
10 | 
11 |     bool init_scene_graph(const Config &config, const std::string &scene_dir,
12 |                           std::shared_ptr<Graph> &src_graph)
13 |     {
14 |         std::vector<InstanceId> instance_idxs;
15 |         std::vector<InstancePtr> instances;
16 | 
17 |         auto src_map = std::make_shared<SemanticMapping>(
18 |             SemanticMapping(config.mapping_cfg, config.instance_cfg));
19 |         src_graph = std::make_shared<Graph>(config.graph);
20 | 
21 |         // Prepare map
22 |         src_map->load(scene_dir);
23 |         src_map->extract_bounding_boxes();
24 |         src_map->export_instances(instance_idxs, instances);
25 |         assert(instance_idxs.size()>0 && instances.size()>0);
26 | 
27 |         // Prepare graph
28 |         src_graph->initialize(instances);
29 |         src_graph->construct_edges();
30 |         src_graph->construct_triplets();
31 |         // fmfusion::DataDict src_data_dict = src_graph->extract_data_dict();
32 | 
33 |         return true;
34 |     }
35 | 
36 | }
37 | 
38 | #endif // INITIALIZATION_H
39 | 


--------------------------------------------------------------------------------
/src/sgloop/LoopDetector.h:
--------------------------------------------------------------------------------
  1 | #ifndef LOOP_DETECTOR_H_
  2 | #define LOOP_DETECTOR_H_
  3 | #include <Common.h>
  4 | #include <sgloop/SGNet.h>
  5 | #include <sgloop/Graph.h>
  6 | #include <sgloop/ShapeEncoder.h>
  7 | 
  8 | 
  9 | namespace fmfusion
 10 | {
 11 |     struct ImplicitGraph{
 12 |         torch::Tensor node_features; // (N, D0)
 13 |         torch::Tensor shape_features; // (N, D1)
 14 |         torch::Tensor node_knn_points; // (N, K, 3)
 15 |         torch::Tensor node_knn_features; // (N, K, D2)
 16 |         bool shape_embedded = false;
 17 |     };
 18 | 
 19 |     class LoopDetector
 20 |     {
 21 |     public:
 22 |         LoopDetector(LoopDetectorConfig &lcd_config,
 23 |                     ShapeEncoderConfig &shape_encoder_config, 
 24 |                     SgNetConfig &sgnet_config, 
 25 |                     const std::string weight_folder,
 26 |                     int cuda_number=0,
 27 |                     std::vector<std::string> ref_graph_names={});
 28 |         ~LoopDetector() {};
 29 | 
 30 |         /// \brief  Encode the reference scene graph. 
 31 |         /// If \param data_dict is empty, it only run triplet gnn and skip dense point cloud encoding.
 32 |         bool encode_ref_scene_graph(const std::string &ref_name, const std::vector<NodePtr> &nodes);
 33 | 
 34 |         /// \brief  Encode the source scene graph. 
 35 |         /// If \param data_dict is empty, it only run triplet gnn and skip dense point cloud encoding.
 36 |         bool encode_src_scene_graph(const std::vector<NodePtr> &nodes);
 37 | 
 38 |         /// \brief Update the ref sg features from subscribed com server.
 39 |         bool subscribe_ref_coarse_features(const std::string &ref_name,
 40 |                                         const float &cur_timestamp, 
 41 |                                         const std::vector<std::vector<float>> &coarse_features_vec,
 42 |                                         torch::Tensor coarse_features);
 43 | 
 44 |         /// \brief the point cloud and instance points from two scene graphs to encode.
 45 |         /// @param hidden_feat_dir: the directory to save the input sent to shape encoder.
 46 |         bool encode_concat_sgs(const std::string &ref_name, 
 47 |                                 const int &Nr, const DataDict& ref_data_dict,
 48 |                                 const int& Ns, const DataDict& src_data_dict,
 49 |                                 float &encoding_time,
 50 |                                 bool fused=false,
 51 |                                 std::string hidden_feat_dir="");
 52 | 
 53 |         int match_nodes(const std::string &ref_name,
 54 |                         std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 55 |                         std::vector<float> &match_scores,
 56 |                         bool fused=false,
 57 |                         std::string dir="");
 58 | 
 59 |         /// \brief  Match the points of the corresponding instances.
 60 |         /// \param  match_pairs         The matched node pairs.
 61 |         /// \param  corr_src_points     (C,3) The corresponding points in the source instance.
 62 |         /// \param  corr_ref_points     (C,3) The corresponding points in the reference instance.
 63 |         /// \param  corr_match_indices  (C,) The indices of the matched node pairs.
 64 |         /// \param  corr_scores_vec     (C,) The matching scores of each point correspondence.
 65 |         int match_instance_points(const std::string &ref_name,
 66 |                                 const std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 67 |                                 std::vector<Eigen::Vector3d> &corr_src_points,
 68 |                                 std::vector<Eigen::Vector3d> &corr_ref_points,
 69 |                                 std::vector<int> &corr_match_indices,
 70 |                                 std::vector<float> &corr_scores_vec,
 71 |                                 std::string dir="");
 72 | 
 73 |         // void clear();
 74 | 
 75 |         torch::Tensor get_active_node_feats()const{
 76 |             return src_features.node_features.clone();
 77 |         }
 78 | 
 79 |         /// \brief  Get the active node features in 2-level vectors.
 80 |         /// \param  feat_vector (N, D) The active node features.
 81 |         /// \param  N           The number of nodes.
 82 |         /// \param  D           The dimension of the node features.
 83 |         bool get_active_node_feats(std::vector<std::vector<float>> &feat_vector,
 84 |                                 int &N, int &D)const;
 85 | 
 86 |         torch::Tensor get_ref_node_feats(const std::string &ref_name)const{
 87 |             if(ref_graphs.find(ref_name)==ref_graphs.end()){
 88 |                 return torch::zeros({0,0},torch::kFloat32);
 89 |             }
 90 |             else{
 91 |                 return ref_graphs.at(ref_name).node_features.clone();
 92 |             }
 93 |         }
 94 | 
 95 |         int get_ref_feats_number(const std::string &ref_name)const{
 96 |             if(ref_graphs.find(ref_name)==ref_graphs.end()){
 97 |                 return 0;
 98 |             }
 99 |             else{
100 |                 return ref_graphs.at(ref_name).node_features.size(0);
101 |             }
102 |         }
103 | 
104 |         bool IsSrcShapeEmbedded()const{
105 |             return src_features.shape_embedded;
106 |         }
107 | 
108 |         bool IsRefShapeEmbedded(const std::string &ref_name)const{
109 |             if(ref_graphs.find(ref_name)==ref_graphs.end()){
110 |                 return false;
111 |             }
112 |             else{
113 |                 return ref_graphs.at(ref_name).shape_embedded;
114 |             }
115 |         }
116 | 
117 |         bool save_middle_features(const std::string &dir);
118 | 
119 |     private:
120 |         bool encode_scene_graph(const std::vector<NodePtr> &nodes, ImplicitGraph &graph_features);
121 | 
122 |         /// @brief  Reserve the features memories on GPU. 
123 |         void initialize_graph_features();
124 | 
125 |     private:
126 |         std::string cuda_device_string;
127 |         ImplicitGraph src_features;    
128 |         std::unordered_map<std::string, ImplicitGraph> ref_graphs;
129 |         std::unordered_map<std::string, float> ref_sg_timestamps; // The latest received sg frame id
130 | 
131 |         LoopDetectorConfig config;
132 |         std::shared_ptr<ShapeEncoder> shape_encoder;
133 |         std::shared_ptr<SgNet> sgnet;
134 |         std::string weight_folder_dir;
135 | 
136 |     };
137 |     
138 | } // namespace fmfusion
139 | #endif


--------------------------------------------------------------------------------
/src/sgloop/SGNet.h:
--------------------------------------------------------------------------------
  1 | #ifndef SGNet_H_
  2 | #define SGNet_H_
  3 | 
  4 | #include <torch/script.h> 
  5 | #include <torch/torch.h>
  6 | #include <array>
  7 | #include <iostream>
  8 | #include <memory>
  9 | 
 10 | #include <mapping/Instance.h>
 11 | #include <sgloop/Graph.h>
 12 | #include <sgloop/BertBow.h>
 13 | #include <tokenizer/text_tokenizer.h>
 14 | #include <Common.h>
 15 | 
 16 | namespace fmfusion
 17 | {
 18 | 
 19 | int extract_corr_points(const torch::Tensor &src_guided_knn_points,
 20 |                         const torch::Tensor &ref_guided_knn_points,
 21 |                         const torch::Tensor &corr_points, 
 22 |                         std::vector<Eigen::Vector3d> &corr_src_points, 
 23 |                         std::vector<Eigen::Vector3d> &corr_ref_points);
 24 | 
 25 | /// @brief  
 26 | /// @param features, (N, D) 
 27 | /// @return number_nan_features
 28 | int check_nan_features(const torch::Tensor &features);
 29 | 
 30 | class SgNet
 31 | {
 32 | 
 33 | public:
 34 |     SgNet(const SgNetConfig &config_, const std::string weight_folder, int cuda_number_=0);
 35 |     ~SgNet() {};
 36 | 
 37 |     /// @brief Modality encoder and graph encoder.
 38 |     /// @param nodes 
 39 |     /// @param node_features 
 40 |     /// @return  
 41 |     bool graph_encoder(const std::vector<NodePtr> &nodes, torch::Tensor &node_features);
 42 | 
 43 | 
 44 |     void match_nodes(const torch::Tensor &src_node_features, const torch::Tensor &ref_node_features,
 45 |         std::vector<std::pair<uint32_t,uint32_t>> &match_pairs, std::vector<float> &match_scores, bool fused=false);
 46 | 
 47 |     /// @brief  Match the points of the matched nodes. Each node sampled 512 points.
 48 |     /// @param src_guided_knn_feats (M,512,256)
 49 |     /// @param ref_guided_knn_feats (M,512,256)
 50 |     /// @param src_guided_knn_points (M,512,3)
 51 |     /// @param ref_guided_knn_points (M,512,3)
 52 |     /// @param corr_src_points (C,3)
 53 |     /// @param corr_ref_points (C,3)
 54 |     /// @param corr_match_indices (C,)
 55 |     /// @param corr_scores_vec (C,)
 56 |     /// @return C, number of matched points
 57 |     int match_points(const torch::Tensor &src_guided_knn_feats, 
 58 |                     const torch::Tensor &ref_guided_knn_feats,
 59 |                     const torch::Tensor &src_guided_knn_points,
 60 |                     const torch::Tensor &ref_guided_knn_points,
 61 |                     std::vector<Eigen::Vector3d> &corr_src_points,
 62 |                     std::vector<Eigen::Vector3d> &corr_ref_points,
 63 |                     std::vector<int> &corr_match_indices,
 64 |                     std::vector<float> &corr_scores_vec);
 65 | 
 66 |     bool load_bert(const std::string weight_folder);
 67 | 
 68 |     bool is_online_bert()const{return !enable_bert_bow;};
 69 | 
 70 |     bool save_hidden_features(const std::string &dir);
 71 | 
 72 | private:
 73 |     /// @brief  Run the SGNet with fake tensor data to warm up the model.
 74 |     /// @param iter iteration number
 75 |     void warm_up(int iter=10,bool verbose=true);
 76 | 
 77 | private:
 78 |     std::string cuda_device_string;
 79 |     std::shared_ptr<radish::TextTokenizer> tokenizer;
 80 |     torch::jit::script::Module bert_encoder;
 81 |     torch::jit::script::Module sgnet_lt;
 82 |     torch::jit::script::Module light_match_layer;
 83 |     torch::jit::script::Module fused_match_layer;
 84 |     torch::jit::script::Module point_match_layer;
 85 | 
 86 |     std::shared_ptr<BertBow> bert_bow_ptr;
 87 |     bool enable_bert_bow;
 88 | 
 89 |     SgNetConfig config;
 90 | 
 91 | private: // hidden states; Reserve at the initialization to boost speed.
 92 |     torch::Tensor semantic_embeddings;
 93 |     torch::Tensor boxes, centroids, anchors, corners, corners_mask;
 94 |     torch::Tensor triplet_verify_mask;
 95 | 
 96 | };
 97 | 
 98 | 
 99 | 
100 | 
101 | }
102 | 
103 | 
104 | #endif


--------------------------------------------------------------------------------
/src/sgloop/ShapeEncoder.h:
--------------------------------------------------------------------------------
 1 | #ifndef SHAPE_ENCODER_H_
 2 | #define SHAPE_ENCODER_H_
 3 | 
 4 | #include <vector>
 5 | #include <Eigen/Core>
 6 | #include <torch/torch.h>
 7 | #include <torch/script.h> 
 8 | #include <open3d/Open3D.h>
 9 | 
10 | #include "Common.h"
11 | #include "thirdparty/extensions/cpu/grid_subsampling.h"
12 | #include "thirdparty/extensions/cpu/radius_neighbors.h"
13 | 
14 | namespace fmfusion
15 | {
16 |     at::Tensor radius_search(at::Tensor q_points, 
17 |                         at::Tensor s_points, 
18 |                         at::Tensor q_lengths, 
19 |                         at::Tensor s_lengths, 
20 |                         float radius, 
21 |                         int neighbor_limit);
22 | 
23 |     class ShapeEncoder
24 |     {
25 |     /// \brief  Shape encoder for one single scene graph.
26 |     public:
27 |         ShapeEncoder(const ShapeEncoderConfig &config_, const std::string weight_folder, int cuda_number=0);
28 |         ~ShapeEncoder(){};
29 | 
30 |         /// \brief  Encode the shape of the scene graph.
31 |         /// \param  xyz_        (X,3), The point cloud of the scene graph.
32 |         /// \param  labels_     (X,), The node indices of each point.
33 |         /// \param  centroids_  (N,3), The centroid of each node.
34 |         /// \param  nodes       (N,), The index of each node.
35 |         void encode(const std::vector<Eigen::Vector3d> &xyz, const std::vector<int> &length_vec, const std::vector<uint32_t> &labels, 
36 |                     const std::vector<Eigen::Vector3d> &centroids_, const std::vector<uint32_t> &nodes,
37 |                     torch::Tensor &node_shape_feats, 
38 |                     torch::Tensor &node_knn_points,
39 |                     torch::Tensor &node_knn_feats,
40 |                     float &encoding_time,
41 |                     std::string hidden_feat_dir="");
42 | 
43 |     private:
44 |         void precompute_data_stack_mode(at::Tensor points, at::Tensor lengths,
45 |                                         std::vector<at::Tensor> &points_list,
46 |                                         std::vector<at::Tensor> &lengths_list,
47 |                                         std::vector<at::Tensor> &neighbors_list,
48 |                                         std::vector<at::Tensor> &subsampling_list,
49 |                                         std::vector<at::Tensor> &upsampling_list);
50 | 
51 |         void associate_f_points(const std::vector<Eigen::Vector3d> &xyz, const std::vector<uint32_t> &labels, 
52 |                     const at::Tensor &points_f, at::Tensor &labels_f);
53 | 
54 |         void sample_node_f_points(const at::Tensor &labels_f, const std::vector<uint32_t> &nodes,
55 |                                         at::Tensor &node_f_points, int K=512, int padding_mode=0);
56 |     
57 |     private:
58 |         std::string cuda_device_string;
59 |         ShapeEncoderConfig config;
60 |         torch::jit::script::Module encoder; // abandoned
61 |         torch::jit::script::Module encoder_v2;
62 | 
63 |     };
64 |     
65 |     typedef std::shared_ptr<ShapeEncoder> ShapeEncoderPtr;
66 | } // namespace fmfusion
67 | 
68 | 
69 | 
70 | #endif
71 | 


--------------------------------------------------------------------------------
/src/tokenizer/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.18)
 2 | 
 3 | project(tokenizer VERSION 1.0)
 4 | 
 5 | find_package(spdlog REQUIRED) # PATHS ${SPDLOG_ROOT})
 6 | find_package(glog REQUIRED)
 7 | 
 8 | add_library(tokenizer SHARED 
 9 |         text_tokenizer.cc
10 |         bert_tokenizer.cc
11 |         )
12 | target_link_libraries(tokenizer PRIVATE spdlog::spdlog ${LIB_UTF8PROC} glog::glog)
13 | set_target_properties(tokenizer PROPERTIES CXX_STANDARD 17)
14 | 
15 | 
16 | 


--------------------------------------------------------------------------------
/src/tokenizer/bert_tokenizer.cc:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * File: bert_tokenizer.cc
  3 |  * Project: bert
  4 |  * File Created: Saturday, 19th October 2019 11:26:14 am
  5 |  * Author: Koth (yovnchine@163.com)
  6 |  * -----
  7 |  * Last Modified: Saturday, 19th October 2019 11:26:17 am
  8 |  * Modified By: Koth (yovnchine@163.com>)
  9 |  * -----
 10 |  */
 11 | 
 12 | #include "bert_tokenizer.h"
 13 | 
 14 | #include <cwctype>
 15 | #include <fstream>
 16 | 
 17 | #include "basic_string_util.h"
 18 | #include "logging.h"
 19 | #include "source/utf8.h"
 20 | #include "utf8proc.h"
 21 | 
 22 | namespace radish {
 23 | 
 24 | std::string BertTokenizer::kUnkToken = "[UNK]";
 25 | std::string BertTokenizer::kMaskToken = "[MASK]";
 26 | std::string BertTokenizer::kSepToken = "[SEP]";
 27 | std::string BertTokenizer::kPadToken = "[PAD]";
 28 | std::string BertTokenizer::kClsToken = "[CLS]";
 29 | static std::unordered_set<uint16_t> kChinesePunts = {
 30 |     12290, 65306, 65311, 8212, 8216, 12304, 12305, 12298, 12299, 65307};
 31 | static int kMaxCharsPerWords = 100;
 32 | 
 33 | bool BertTokenizer::Init(std::string vocab_file) {
 34 |   std::ifstream ifs(vocab_file);
 35 |   if (!ifs) {
 36 |     return false;
 37 |   }
 38 |   std::string content((std::istreambuf_iterator<char>(ifs)),
 39 |                       (std::istreambuf_iterator<char>()));
 40 |   return InitByFileContent(content);
 41 | }
 42 | 
 43 | bool BertTokenizer::InitByFileContent(std::string content) {
 44 | 
 45 |   std::vector<std::string> lines;
 46 |   BasicStringUtil::SplitString(content.c_str(), content.size(),'\n',&lines);
 47 |   init_from_lines(lines);
 48 |   if (token_2_id_map_.find(kPadToken) == token_2_id_map_.end()) {
 49 |     return false;
 50 |   }
 51 |   if (token_2_id_map_.find(kUnkToken) == token_2_id_map_.end()) {
 52 |     return false;
 53 |   }
 54 |   if (token_2_id_map_.find(kClsToken) == token_2_id_map_.end()) {
 55 |     return false;
 56 |   }
 57 |   if (token_2_id_map_.find(kSepToken) == token_2_id_map_.end()) {
 58 |     return false;
 59 |   }
 60 |   if (token_2_id_map_.find(kMaskToken) == token_2_id_map_.end()) {
 61 |     return false;
 62 |   }
 63 |   int v = token_2_id_map_.at(kPadToken);
 64 |   if (v != 0) {
 65 |     return false;
 66 |   }
 67 |   return true;
 68 | }
 69 | 
 70 | std::vector<int> BertTokenizer::Encode(std::string text) {
 71 |   (void)s_bRegistered;  // force the registeration
 72 |   std::vector<int> results;
 73 |   text =BasicStringUtil::StripStringASCIIWhole(text);
 74 |   char* nfkcstr = reinterpret_cast<char*>(
 75 |       utf8proc_NFD(reinterpret_cast<const unsigned char*>(text.c_str())));
 76 |   if (nfkcstr == nullptr) {
 77 |     spdlog::info("do NFD error");
 78 |     return {};
 79 |   }
 80 |   text.assign(nfkcstr, strlen(nfkcstr));
 81 |   free(nfkcstr);
 82 |   BasicStringUtil::ToLower(text);
 83 |   UString unicodes;
 84 |   utf8::utf8to16(text.c_str(), text.c_str() + text.size(),
 85 |                  std::back_inserter(unicodes));
 86 |   unicodes = _clean(unicodes);
 87 |   unicodes = _basic_tokenize(unicodes);
 88 |   std::string newtext;
 89 |   utf8::utf16to8(
 90 |       reinterpret_cast<const uint16_t*>(unicodes.c_str()),
 91 |       reinterpret_cast<const uint16_t*>(unicodes.c_str() + unicodes.size()),
 92 |       std::back_inserter(newtext));
 93 |   std::vector<std::string> tokens;
 94 |   BasicStringUtil::SplitString(newtext.c_str(), newtext.size(), ' ', &tokens);
 95 |   for (auto s : tokens) {
 96 |     if (s.size() > kMaxCharsPerWords) {
 97 |       results.push_back(token_2_id_map_.at(kUnkToken));
 98 |     } else {
 99 |       max_seg_(s, results);
100 |     }
101 |   }
102 |   return results;
103 | }
104 | 
105 | int BertTokenizer::PadId() const { return token_2_id_map_.at(kPadToken); }
106 | int BertTokenizer::MaskId() const { return token_2_id_map_.at(kMaskToken); }
107 | int BertTokenizer::SepId() const { return token_2_id_map_.at(kSepToken); }
108 | int BertTokenizer::ClsId() const { return token_2_id_map_.at(kClsToken); }
109 | int BertTokenizer::UnkId() const { return token_2_id_map_.at(kUnkToken); }
110 | 
111 | int BertTokenizer::TotalSize() const { return tokens_.size(); }
112 | void BertTokenizer::max_seg_(std::string s, std::vector<int>& results) {
113 |   int end = s.size();
114 |   int start = 0;
115 |   // spdlog::info("now s:[{}]", s);
116 |   bool firstOne = true;
117 |   while (start < end) {
118 |     std::string test(s.c_str() + start, end - start);
119 |     if (!firstOne) {
120 |       test = std::string("##") + test;
121 |     }
122 |     auto it = token_2_id_map_.find(test);
123 |     if (it == token_2_id_map_.end()) {
124 |       end -= 1;
125 |     } else {
126 |       // spdlog::info("now got :{}", test);
127 |       results.push_back(it->second);
128 |       start = end;
129 |       end = s.size();
130 |       firstOne = false;
131 |     }
132 |   }
133 |   if (firstOne) {
134 |     // not any one matched
135 |     results.push_back(token_2_id_map_.at(kUnkToken));
136 |   }
137 | }
138 | int BertTokenizer::Word2Id(std::string s) const {
139 |   if (s.size() > kMaxCharsPerWords) {
140 |     return token_2_id_map_.at(kUnkToken);
141 |   }
142 |   auto it = token_2_id_map_.find(s);
143 |   if (it == token_2_id_map_.end()) {
144 |     return token_2_id_map_.at(kUnkToken);
145 |   } else {
146 |     return it->second;
147 |   }
148 | }
149 | std::string BertTokenizer::Id2Word(int id) const {
150 |   if (id >= 0 && id < static_cast<int>(tokens_.size())) {
151 |     return tokens_[id];
152 |   }
153 |   return kUnkToken;
154 | }
155 | 
156 | void BertTokenizer::init_from_lines(const std::vector<std::string>& lines) {
157 |   int idx = 0;
158 |   for (size_t i = 0; i < lines.size(); i++) {
159 |     std::string line = lines[i];
160 |     size_t nn = line.size();
161 |     while (nn > 0 && (line[nn - 1] == '\n' || line[nn - 1] == '\r')) {
162 |       nn -= 1;
163 |     }
164 |     if (nn == 0) {
165 |       continue;
166 |     }
167 |     std::string token = line.substr(0, nn);
168 |     tokens_.push_back(token);
169 |     token_2_id_map_[token] = idx;
170 |     idx += 1;
171 |   }
172 | }
173 | void BertTokenizer::load_vocab_(std::string path,
174 |                                 std::vector<std::string>& lines) {
175 |   FILE* fp = fopen(path.c_str(), "r");
176 |   CHECK(fp != NULL) << "open file error:" << path;
177 |   char line[4096] = {0};
178 |   int idx = 0;
179 |   while (fgets(line, sizeof(line) - 1, fp)) {
180 |     int nn = strlen(line);
181 |     while (nn && (line[nn - 1] == '\n' || line[nn - 1] == '\r')) {
182 |       nn -= 1;
183 |     }
184 |     if (nn <= 0) {
185 |       continue;
186 |     }
187 |     lines.push_back(std::string(line, nn));
188 |   }
189 |   fclose(fp);
190 | }
191 | 
192 | static bool _is_whitespace(uint16_t c) {
193 |   if (c == '\t' || c == '\n' || c == '\r' || c == ' ') {
194 |     return true;
195 |   }
196 |   return (UTF8PROC_CATEGORY_ZS == utf8proc_category(c));
197 | }
198 | 
199 | static bool _is_control(uint16_t c) {
200 |   if (c == '\t' || c == '\n' || c == '\r') {
201 |     return false;
202 |   }
203 |   utf8proc_category_t cat = utf8proc_category(c);
204 |   return (cat == UTF8PROC_CATEGORY_CC || cat == UTF8PROC_CATEGORY_CF);
205 | }
206 | 
207 | static bool _is_chinese_char(uint16_t cp) {
208 |   if ((cp >= 0x4E00 && cp <= 0x9FFF) || (cp >= 0x3400 && cp <= 0x4DBF) ||
209 |       (cp >= 0x20000 && cp <= 0x2A6DF) || (cp >= 0x2A700 && cp <= 0x2B73F) ||
210 |       (cp >= 0x2B740 && cp <= 0x2B81F) || (cp >= 0x2B820 && cp <= 0x2CEAF) ||
211 |       (cp >= 0xF900 && cp <= 0xFAFF) || (cp >= 0x2F800 && cp <= 0x2FA1F)) {
212 |     return true;
213 |   }
214 |   return false;
215 | }
216 | static bool _is_punct_char(uint16_t cp) {
217 |   if ((cp >= 33 && cp <= 47) || (cp >= 58 && cp <= 64) ||
218 |       (cp >= 91 && cp <= 96) || (cp >= 123 && cp <= 126)) {
219 |     return true;
220 |   }
221 |   if (cp == ' ') {
222 |     return false;
223 |   }
224 |   // we can remove this part code  now !!!!
225 |   if (kChinesePunts.find(cp) != kChinesePunts.end()) {
226 |     return true;
227 |   }
228 |   int cate = static_cast<int>(utf8proc_category(cp));
229 |   return (cate >= 12 && cate <= 18);
230 | }
231 | UString BertTokenizer::_basic_tokenize(UString text) {
232 |   UString ret;
233 |   size_t len = text.size();
234 |   for (size_t i = 0; i < len; i++) {
235 |     uint16_t c = text[i];
236 |     if (_is_chinese_char(c) || _is_punct_char(c)) {
237 |       if (!ret.empty() && ret.back() != ' ') {
238 |         ret.append(1, ' ');
239 |       }
240 |       ret.append(1, c);
241 |       ret.append(1, ' ');
242 |     } else if (c == ' ') {
243 |       if (!ret.empty() && ret.back() != ' ') {
244 |         ret.append(1, c);
245 |       }
246 |     } else {
247 |       ret.append(1, c);
248 |     }
249 |   }
250 |   if (!ret.empty() && ret.back() == ' ') {
251 |     ret.erase(ret.end() - 1);
252 |   }
253 |   return ret;
254 | }
255 | UString BertTokenizer::_clean(UString text) {
256 |   size_t len = text.size();
257 |   UString ret;
258 |   for (size_t i = 0; i < len; i++) {
259 |     uint16_t c = text[i];
260 |     if (c == 0 || c == 0xFFFD || _is_control(c) ||
261 |         utf8proc_category(c) == UTF8PROC_CATEGORY_MN) {
262 |       continue;
263 |     }
264 |     if (_is_whitespace(c)) {
265 |       ret.append(1, ' ');
266 |     } else {
267 |       ret.append(1, c);
268 |     }
269 |   }
270 |   return ret;
271 | }
272 | 
273 | }  // namespace radish


--------------------------------------------------------------------------------
/src/tokenizer/bert_tokenizer.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * This is a thirdparty module adopted from: https://github.com/LieluoboAi/radish
 3 |  * File: bert_tokenizer.h
 4 |  * Project: bert
 5 |  * File Created: Saturday, 19th October 2019 10:41:25 am
 6 |  * Author: Koth (yovnchine@163.com)
 7 |  * -----
 8 |  * Last Modified: Saturday, 19th October 2019 10:41:38 am
 9 |  * Modified By: Koth (yovnchine@163.com>)
10 |  * -----
11 |  */
12 | #pragma once
13 | #include <string>
14 | #include <unordered_map>
15 | #include <unordered_set>
16 | #include <vector>
17 | 
18 | #include "text_tokenizer.h"
19 | 
20 | 
21 | namespace radish {
22 | using UString = std::basic_string<uint16_t>;
23 | class BertTokenizer : public TextTokenizer,
24 |                       TextTokenizerRegisteeStub<BertTokenizer> {
25 |  public:
26 |   bool Init(std::string vocab) override;
27 |   bool InitByFileContent(std::string content);
28 |   std::vector<int> Encode(std::string text) override;
29 |   int Word2Id(std::string word) const override;
30 |   std::string Id2Word(int id) const override;
31 |   int PadId() const override;
32 |   int MaskId() const override;
33 |   int SepId() const override;
34 |   int ClsId() const override;
35 |   int UnkId() const override;
36 |   int TotalSize() const override;
37 |  private:
38 |   void max_seg_(std::string s, std::vector<int>& results);
39 |   void load_vocab_(std::string path, std::vector<std::string>& lines);
40 |   void init_from_lines(const std::vector<std::string>& lines);
41 |   UString _basic_tokenize(UString text);
42 |   UString _clean(UString text);
43 |   std::unordered_map<std::string, int> token_2_id_map_;
44 |   std::vector<std::string> tokens_;
45 |   static std::string kUnkToken;
46 |   static std::string kMaskToken;
47 |   static std::string kSepToken;
48 |   static std::string kPadToken;
49 |   static std::string kClsToken;
50 | };
51 | }  // namespace radish
52 | 


--------------------------------------------------------------------------------
/src/tokenizer/logging.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * File: logging.h
 3 |  * Project: utils
 4 |  * Author: koth (Koth Chen)
 5 |  * -----
 6 |  * Last Modified: 2019-09-24 10:06:23
 7 |  * Modified By: koth (nobody@verycool.com)
 8 |  * -----
 9 |  * Copyright 2020 - 2019
10 |  */
11 | #pragma once
12 | #include "glog/logging.h"
13 | #include "spdlog/spdlog.h"
14 | #include "spdlog/fmt/ostr.h"
15 | 
16 | 
17 | // #define SPDLOG_TRACE(logger, ...) if (logger->should_log(level::trace)){logger->trace("{}::{}()#{}: ", __FILE__ , __FUNCTION__, __LINE__, fmt::format(__VA_ARGS__));}


--------------------------------------------------------------------------------
/src/tokenizer/source/utf8.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2006 Nemanja Trifunovic
 2 | 
 3 | /*
 4 | Permission is hereby granted, free of charge, to any person or organization
 5 | obtaining a copy of the software and accompanying documentation covered by
 6 | this license (the "Software") to use, reproduce, display, distribute,
 7 | execute, and transmit the Software, and to prepare derivative works of the
 8 | Software, and to permit third-parties to whom the Software is furnished to
 9 | do so, all subject to the following:
10 | 
11 | The copyright notices in the Software and this entire statement, including
12 | the above license grant, this restriction and the following disclaimer,
13 | must be included in all copies of the Software, in whole or in part, and
14 | all derivative works of the Software, unless such copies or derivative
15 | works are solely in the form of machine-executable object code generated by
16 | a source language processor.
17 | 
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
21 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
22 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
23 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
24 | DEALINGS IN THE SOFTWARE.
25 | */
26 | 
27 | 
28 | #ifndef UTF8_FOR_CPP_2675DCD0_9480_4c0c_B92A_CC14C027B731
29 | #define UTF8_FOR_CPP_2675DCD0_9480_4c0c_B92A_CC14C027B731
30 | 
31 | /*
32 | To control the C++ language version used by the library, you can define UTF_CPP_CPLUSPLUS macro
33 | and set it to one of the values used by the __cplusplus predefined macro.
34 | 
35 | For instance,
36 |     #define UTF_CPP_CPLUSPLUS 199711L
37 | will cause the UTF-8 CPP library to use only types and language features available in the C++ 98 standard.
38 | Some library features will be disabled.
39 | 
40 | If you leave UTF_CPP_CPLUSPLUS undefined, it will be internally assigned to __cplusplus.
41 | */
42 | 
43 | #include "utf8/checked.h"
44 | #include "utf8/unchecked.h"
45 | 
46 | #endif // header guard
47 | 


--------------------------------------------------------------------------------
/src/tokenizer/source/utf8/cpp11.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2018 Nemanja Trifunovic
 2 | 
 3 | /*
 4 | Permission is hereby granted, free of charge, to any person or organization
 5 | obtaining a copy of the software and accompanying documentation covered by
 6 | this license (the "Software") to use, reproduce, display, distribute,
 7 | execute, and transmit the Software, and to prepare derivative works of the
 8 | Software, and to permit third-parties to whom the Software is furnished to
 9 | do so, all subject to the following:
10 | 
11 | The copyright notices in the Software and this entire statement, including
12 | the above license grant, this restriction and the following disclaimer,
13 | must be included in all copies of the Software, in whole or in part, and
14 | all derivative works of the Software, unless such copies or derivative
15 | works are solely in the form of machine-executable object code generated by
16 | a source language processor.
17 | 
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
21 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
22 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
23 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
24 | DEALINGS IN THE SOFTWARE.
25 | */
26 | 
27 | 
28 | #ifndef UTF8_FOR_CPP_a184c22c_d012_11e8_a8d5_f2801f1b9fd1
29 | #define UTF8_FOR_CPP_a184c22c_d012_11e8_a8d5_f2801f1b9fd1
30 | 
31 | #include "checked.h"
32 | 
33 | namespace utf8
34 | {
35 |     inline void append16(utfchar32_t cp, std::u16string& s)
36 |     {
37 |         append16(cp, std::back_inserter(s));
38 |     }
39 | 
40 |     inline std::string utf16to8(const std::u16string& s)
41 |     {
42 |         std::string result;
43 |         utf16to8(s.begin(), s.end(), std::back_inserter(result));
44 |         return result;
45 |     }
46 | 
47 |     inline std::u16string utf8to16(const std::string& s)
48 |     {
49 |         std::u16string result;
50 |         utf8to16(s.begin(), s.end(), std::back_inserter(result));
51 |         return result;
52 |     }
53 | 
54 |     inline std::string utf32to8(const std::u32string& s)
55 |     {
56 |         std::string result;
57 |         utf32to8(s.begin(), s.end(), std::back_inserter(result));
58 |         return result;
59 |     }
60 | 
61 |     inline std::u32string utf8to32(const std::string& s)
62 |     {
63 |         std::u32string result;
64 |         utf8to32(s.begin(), s.end(), std::back_inserter(result));
65 |         return result;
66 |     }
67 | } // namespace utf8
68 | 
69 | #endif // header guard
70 | 
71 | 


--------------------------------------------------------------------------------
/src/tokenizer/source/utf8/cpp17.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2018 Nemanja Trifunovic
 2 | 
 3 | /*
 4 | Permission is hereby granted, free of charge, to any person or organization
 5 | obtaining a copy of the software and accompanying documentation covered by
 6 | this license (the "Software") to use, reproduce, display, distribute,
 7 | execute, and transmit the Software, and to prepare derivative works of the
 8 | Software, and to permit third-parties to whom the Software is furnished to
 9 | do so, all subject to the following:
10 | 
11 | The copyright notices in the Software and this entire statement, including
12 | the above license grant, this restriction and the following disclaimer,
13 | must be included in all copies of the Software, in whole or in part, and
14 | all derivative works of the Software, unless such copies or derivative
15 | works are solely in the form of machine-executable object code generated by
16 | a source language processor.
17 | 
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
21 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
22 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
23 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
24 | DEALINGS IN THE SOFTWARE.
25 | */
26 | 
27 | 
28 | #ifndef UTF8_FOR_CPP_7e906c01_03a3_4daf_b420_ea7ea952b3c9
29 | #define UTF8_FOR_CPP_7e906c01_03a3_4daf_b420_ea7ea952b3c9
30 | 
31 | #include "cpp11.h"
32 | 
33 | namespace utf8
34 | {
35 |     inline std::string utf16to8(std::u16string_view s)
36 |     {
37 |         std::string result;
38 |         utf16to8(s.begin(), s.end(), std::back_inserter(result));
39 |         return result;
40 |     }
41 | 
42 |     inline std::u16string utf8to16(std::string_view s)
43 |     {
44 |         std::u16string result;
45 |         utf8to16(s.begin(), s.end(), std::back_inserter(result));
46 |         return result;
47 |     }
48 | 
49 |     inline std::string utf32to8(std::u32string_view s)
50 |     {
51 |         std::string result;
52 |         utf32to8(s.begin(), s.end(), std::back_inserter(result));
53 |         return result;
54 |     }
55 | 
56 |     inline std::u32string utf8to32(std::string_view s)
57 |     {
58 |         std::u32string result;
59 |         utf8to32(s.begin(), s.end(), std::back_inserter(result));
60 |         return result;
61 |     }
62 | 
63 |     inline std::size_t find_invalid(std::string_view s)
64 |     {
65 |         std::string_view::const_iterator invalid = find_invalid(s.begin(), s.end());
66 |         return (invalid == s.end()) ? std::string_view::npos : static_cast<std::size_t>(invalid - s.begin());
67 |     }
68 | 
69 |     inline bool is_valid(std::string_view s)
70 |     {
71 |         return is_valid(s.begin(), s.end());
72 |     }
73 | 
74 |     inline std::string replace_invalid(std::string_view s, char32_t replacement)
75 |     {
76 |         std::string result;
77 |         replace_invalid(s.begin(), s.end(), std::back_inserter(result), replacement);
78 |         return result;
79 |     }
80 | 
81 |     inline std::string replace_invalid(std::string_view s)
82 |     {
83 |         std::string result;
84 |         replace_invalid(s.begin(), s.end(), std::back_inserter(result));
85 |         return result;
86 |     }
87 | 
88 |     inline bool starts_with_bom(std::string_view s)
89 |     {
90 |         return starts_with_bom(s.begin(), s.end());
91 |     }
92 |  
93 | } // namespace utf8
94 | 
95 | #endif // header guard
96 | 
97 | 


--------------------------------------------------------------------------------
/src/tokenizer/source/utf8/cpp20.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2022 Nemanja Trifunovic
  2 | 
  3 | /*
  4 | Permission is hereby granted, free of charge, to any person or organization
  5 | obtaining a copy of the software and accompanying documentation covered by
  6 | this license (the "Software") to use, reproduce, display, distribute,
  7 | execute, and transmit the Software, and to prepare derivative works of the
  8 | Software, and to permit third-parties to whom the Software is furnished to
  9 | do so, all subject to the following:
 10 | 
 11 | The copyright notices in the Software and this entire statement, including
 12 | the above license grant, this restriction and the following disclaimer,
 13 | must be included in all copies of the Software, in whole or in part, and
 14 | all derivative works of the Software, unless such copies or derivative
 15 | works are solely in the form of machine-executable object code generated by
 16 | a source language processor.
 17 | 
 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 20 | FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 21 | SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 22 | FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 23 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 24 | DEALINGS IN THE SOFTWARE.
 25 | */
 26 | 
 27 | 
 28 | #ifndef UTF8_FOR_CPP_207e906c01_03a3_4daf_b420_ea7ea952b3c9
 29 | #define UTF8_FOR_CPP_207e906c01_03a3_4daf_b420_ea7ea952b3c9
 30 | 
 31 | #include "cpp17.h"
 32 | 
 33 | namespace utf8
 34 | {
 35 |     inline std::u8string utf16tou8(const std::u16string& s)
 36 |     {
 37 |         std::u8string result;
 38 |         utf16to8(s.begin(), s.end(), std::back_inserter(result));
 39 |         return result;
 40 |     }
 41 | 
 42 |     inline std::u8string utf16tou8(std::u16string_view s)
 43 |     {
 44 |         std::u8string result;
 45 |         utf16to8(s.begin(), s.end(), std::back_inserter(result));
 46 |         return result;
 47 |     }
 48 | 
 49 |     inline std::u16string utf8to16(const std::u8string& s)
 50 |     {
 51 |         std::u16string result;
 52 |         utf8to16(s.begin(), s.end(), std::back_inserter(result));
 53 |         return result;
 54 |     }
 55 | 
 56 |     inline std::u16string utf8to16(const std::u8string_view& s)
 57 |     {
 58 |         std::u16string result;
 59 |         utf8to16(s.begin(), s.end(), std::back_inserter(result));
 60 |         return result;
 61 |     }
 62 | 
 63 |     inline std::u8string utf32tou8(const std::u32string& s)
 64 |     {
 65 |         std::u8string result;
 66 |         utf32to8(s.begin(), s.end(), std::back_inserter(result));
 67 |         return result;
 68 |     }
 69 | 
 70 |     inline std::u8string utf32tou8(const std::u32string_view& s)
 71 |     {
 72 |         std::u8string result;
 73 |         utf32to8(s.begin(), s.end(), std::back_inserter(result));
 74 |         return result;
 75 |     }
 76 | 
 77 |     inline std::u32string utf8to32(const std::u8string& s)
 78 |     {
 79 |         std::u32string result;
 80 |         utf8to32(s.begin(), s.end(), std::back_inserter(result));
 81 |         return result;
 82 |     }
 83 | 
 84 |     inline std::u32string utf8to32(const std::u8string_view& s)
 85 |     {
 86 |         std::u32string result;
 87 |         utf8to32(s.begin(), s.end(), std::back_inserter(result));
 88 |         return result;
 89 |     }
 90 | 
 91 |     inline std::size_t find_invalid(const std::u8string& s)
 92 |     {
 93 |         std::u8string::const_iterator invalid = find_invalid(s.begin(), s.end());
 94 |         return (invalid == s.end()) ? std::string_view::npos : static_cast<std::size_t>(invalid - s.begin());
 95 |     }
 96 | 
 97 |     inline bool is_valid(const std::u8string& s)
 98 |     {
 99 |         return is_valid(s.begin(), s.end());
100 |     }
101 | 
102 |     inline std::u8string replace_invalid(const std::u8string& s, char32_t replacement)
103 |     {
104 |         std::u8string result;
105 |         replace_invalid(s.begin(), s.end(), std::back_inserter(result), replacement);
106 |         return result;
107 |     }
108 | 
109 |     inline std::u8string replace_invalid(const std::u8string& s)
110 |     {
111 |         std::u8string result;
112 |         replace_invalid(s.begin(), s.end(), std::back_inserter(result));
113 |         return result;
114 |     }
115 | 
116 |     inline bool starts_with_bom(const std::u8string& s)
117 |     {
118 |         return starts_with_bom(s.begin(), s.end());
119 |     }
120 |  
121 | } // namespace utf8
122 | 
123 | #endif // header guard
124 | 
125 | 


--------------------------------------------------------------------------------
/src/tokenizer/text_tokenizer.cc:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * File: text_tokenizer.cc
 3 |  * Project: utils
 4 |  * File Created: Sunday, 20th October 2019 10:33:18 am
 5 |  * Author: Koth (yovnchine@163.com)
 6 |  * -----
 7 |  * Last Modified: Sunday, 20th October 2019 10:34:00 am
 8 |  * Modified By: Koth (yovnchine@163.com>)
 9 |  * -----
10 |  */
11 | 
12 | #include "text_tokenizer.h"
13 | #include "logging.h"
14 | namespace radish {
15 | 
16 | std::map<std::string, TextTokenizerFactory::TCreateMethod>*
17 |     TextTokenizerFactory::sMethods =
18 |         nullptr;  // Use pointer to walk around wild init issue!!!!
19 | 
20 | bool TextTokenizerFactory::Register(
21 |     const std::string name, TextTokenizerFactory::TCreateMethod funcCreate) {
22 |   if (sMethods == nullptr) {
23 |     sMethods = new std::map<std::string, TextTokenizerFactory::TCreateMethod>();
24 |   }
25 |   auto it = sMethods->find(name);
26 |   if (it == sMethods->end()) {
27 |     sMethods->insert(std::make_pair(name, funcCreate));
28 |     return true;
29 |   }
30 |   return false;
31 | }
32 | 
33 | TextTokenizer* TextTokenizerFactory::Create(const std::string& name) {
34 |   auto it = sMethods->find(name);
35 |   if (it == sMethods->end()) {
36 |     return nullptr;
37 |   }
38 |   return it->second();
39 | }
40 | }  // namespace radish
41 | 


--------------------------------------------------------------------------------
/src/tokenizer/text_tokenizer.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * File: text_tokenizer.h
 3 |  * Project: utils
 4 |  * File Created: Sunday, 20th October 2019 9:53:58 am
 5 |  * Author: Koth (yovnchine@163.com)
 6 |  * -----
 7 |  * Last Modified: Sunday, 20th October 2019 10:05:56 am
 8 |  * Modified By: Koth (yovnchine@163.com>)
 9 |  * -----
10 |  */
11 | 
12 | #pragma once
13 | 
14 | #include <cxxabi.h>
15 | 
16 | #include <map>
17 | #include <string>
18 | #include <vector>
19 | 
20 | namespace radish {
21 | class TextTokenizer {
22 |  public:
23 |   TextTokenizer() = default;
24 |   virtual ~TextTokenizer() = default;
25 |   virtual bool Init(std::string vocab) = 0;
26 |   virtual std::vector<int> Encode(std::string text) = 0;
27 |   virtual int Word2Id(std::string word) const = 0;
28 |   virtual std::string Id2Word(int id) const = 0;
29 |   virtual int PadId() const { return 0; }
30 |   virtual int MaskId() const = 0;
31 |   virtual int SepId() const = 0;
32 |   virtual int ClsId() const = 0;
33 |   virtual int UnkId() const = 0;
34 |   virtual int TotalSize()  const =0;
35 | };
36 | 
37 | class TextTokenizerFactory {
38 |  public:
39 |   using TCreateMethod = TextTokenizer* (*)();
40 | 
41 |  public:
42 |   TextTokenizerFactory() = delete;
43 | 
44 |   static bool Register(const std::string name, TCreateMethod funcCreate);
45 | 
46 |   static TextTokenizer* Create(const std::string& name);
47 | 
48 |  private:
49 |   static std::map<std::string, TCreateMethod>* sMethods;
50 | };
51 | 
52 | template <typename T>
53 | class TextTokenizerRegisteeStub {
54 |  public:
55 |   static std::string factory_name() {
56 |     int status = 0;
57 |     return std::string(abi::__cxa_demangle(typeid(T).name(), 0, 0, &status));
58 |   }
59 | 
60 |  protected:
61 |   static bool s_bRegistered;
62 | };
63 | 
64 | template <typename T>
65 | bool TextTokenizerRegisteeStub<T>::s_bRegistered =
66 |     TextTokenizerFactory::Register(TextTokenizerRegisteeStub<T>::factory_name(),
67 |                                    []() {
68 |                                      return dynamic_cast<TextTokenizer*>(new T);
69 |                                    });
70 | 
71 | }  // namespace radish
72 | 


--------------------------------------------------------------------------------
/src/tools/Color.h:
--------------------------------------------------------------------------------
 1 | #ifndef FMFUSOIN_COLOR_H
 2 | #define FMFUSOIN_COLOR_H
 3 | 
 4 | 
 5 | namespace fmfusion
 6 | {
 7 |     // Generate 40 colors as a color bar
 8 |     std::array<Eigen::Vector3d, 40> InstanceColorBar40 =
 9 |     {
10 |         Eigen::Vector3d(0.0, 0.0, 1.0), 
11 |         Eigen::Vector3d(1.0, 1.0, 0.0), 
12 |         Eigen::Vector3d(1.0, 0.0, 1.0), 
13 |         Eigen::Vector3d(0.0, 1.0, 1.0),
14 |         Eigen::Vector3d(0.5, 0.0, 0.0), 
15 |         Eigen::Vector3d(0.0, 0.5, 0.0), 
16 |         Eigen::Vector3d(0.0, 0.0, 0.5), 
17 |         Eigen::Vector3d(0.5, 0.5, 0.0), 
18 |         Eigen::Vector3d(0.5, 0.0, 0.5), 
19 |         Eigen::Vector3d(0.0, 0.5, 0.5),
20 |         Eigen::Vector3d(0.75, 0.0, 0.0), 
21 |         Eigen::Vector3d(0.0, 0.75, 0.0), 
22 |         Eigen::Vector3d(0.0, 0.0, 0.75), 
23 |         Eigen::Vector3d(0.75, 0.75, 0.0), 
24 |         Eigen::Vector3d(0.75, 0.0, 0.75), 
25 |         Eigen::Vector3d(0.0, 0.75, 0.75),
26 |         Eigen::Vector3d(0.25, 0.0, 0.0), 
27 |         Eigen::Vector3d(0.0, 0.25, 0.0), 
28 |         Eigen::Vector3d(0.0, 0.0, 0.25), 
29 |         Eigen::Vector3d(0.25, 0.25, 0.0), 
30 |         Eigen::Vector3d(0.25, 0.0, 0),
31 |         Eigen::Vector3d(0.0, 0.25, 0.25),
32 |         Eigen::Vector3d(0.375, 0.0, 0.0),
33 |         Eigen::Vector3d(0.0, 0.375, 0.0),
34 |         Eigen::Vector3d(0.0, 0.0, 0.375),
35 |         Eigen::Vector3d(0.375, 0.375, 0.0),
36 |         Eigen::Vector3d(0.375, 0.0, 0.375),
37 |         Eigen::Vector3d(0.0, 0.375, 0.375),
38 |         Eigen::Vector3d(0.125, 0.0, 0.0),
39 |         Eigen::Vector3d(0.0, 0.125, 0.0),
40 |         Eigen::Vector3d(0.0, 0.0, 0.125),
41 |         Eigen::Vector3d(0.125, 0.125, 0.0),
42 |         Eigen::Vector3d(0.125, 0.0, 0.125),
43 |         Eigen::Vector3d(0.0, 0.125, 0.125),
44 |         Eigen::Vector3d(0.625, 0.0, 0.0),
45 |         Eigen::Vector3d(0.0, 0.625, 0.0),
46 |         Eigen::Vector3d(0.0, 0.0, 0.625),
47 |         Eigen::Vector3d(0.625, 0.625, 0.0),
48 |         Eigen::Vector3d(0.625, 0.0, 0.625),
49 |         Eigen::Vector3d(0.0, 0.625, 0.625)
50 |     };
51 | 
52 | 
53 |     std::array<Eigen::Vector3d,20> InstanceColorBar20 = 
54 |     {
55 |         Eigen::Vector3d(0.65098039, 0.80784314, 0.89019608),
56 |         Eigen::Vector3d(0.12156863, 0.47058824, 0.70588235),
57 |         Eigen::Vector3d(0.69803922, 0.8745098 , 0.54117647),
58 |         Eigen::Vector3d(0.2       , 0.62745098, 0.17254902),
59 |         Eigen::Vector3d(0.98431373, 0.60392157, 0.6       ),
60 |         Eigen::Vector3d(0.89019608, 0.10196078, 0.10980392),
61 |         Eigen::Vector3d(0.99215686, 0.74901961, 0.43529412),
62 |         Eigen::Vector3d(1.        , 0.49803922, 0.        ),
63 |         Eigen::Vector3d(0.79215686, 0.69803922, 0.83921569),
64 |         Eigen::Vector3d(0.41568627, 0.23921569, 0.60392157),
65 |         Eigen::Vector3d(1.        , 1.        , 0.6       ),
66 |         Eigen::Vector3d(0.69411765, 0.34901961, 0.15686275),
67 |         Eigen::Vector3d(0.97647059, 0.45098039, 0.02352941),
68 |         Eigen::Vector3d(0.63529412, 0.07843137, 0.18431373),
69 |         Eigen::Vector3d(0.6       , 0.9       , 0.6       ),
70 |         Eigen::Vector3d(0.4       , 0.7       , 0.4       ),
71 |         Eigen::Vector3d(0.6       , 0.6       , 0.6       ),
72 |         Eigen::Vector3d(0.4       , 0.4       , 0.7       ),
73 |         Eigen::Vector3d(0.6       , 0.6       , 0.6       ),
74 |         Eigen::Vector3d(0.4       , 0.4       , 0.4       )
75 |     };
76 | 
77 | }
78 | 
79 | #endif
80 | 
81 | 


--------------------------------------------------------------------------------
/src/tools/Eval.h:
--------------------------------------------------------------------------------
 1 | #include <fstream>
 2 | #include <vector>
 3 | #include <chrono>
 4 | #include "Utility.h"
 5 | 
 6 | namespace fmfusion
 7 | {
 8 |     
 9 |     int maks_true_instance(const std::string &gt_file_dir, 
10 |                             const std::vector<std::pair<uint32_t,uint32_t>> &pred_instances,
11 |                             std::vector<bool> &pred_masks)
12 |                             // std::string &msg)
13 |     {
14 |         std::cout<<"Loading gt instance file: "<<gt_file_dir<<std::endl;
15 |         std::set<std::string> gt_instance_pairs;
16 |         int M = pred_instances.size();
17 |         int count_true = 0;
18 | 
19 |         // Load gt
20 |         std::ifstream gt_file(gt_file_dir, std::ifstream::in);
21 |         if (!gt_file.is_open()){
22 |             std::cerr<<"Error opening file: "<<gt_file_dir<<std::endl;
23 |             return -1;
24 |         }
25 | 
26 |         std::string line;
27 |         while (std::getline(gt_file, line)){
28 |             // std::cout<<line<<std::endl;
29 |             auto eles = utility::split_str(line, " ");
30 |             std::stringstream pair_name;
31 |             pair_name<<eles[0]<<"_"<<eles[1];
32 |             // std::cout<<pair_name.str()<<std::endl;
33 |             gt_instance_pairs.insert(pair_name.str());
34 |         }
35 | 
36 |         //
37 |         for (int i=0; i<M; i++){
38 |             std::stringstream pair_name;
39 |             pair_name<<pred_instances[i].first<<"_"<<pred_instances[i].second;
40 |             bool true_mask=false;
41 |             if(gt_instance_pairs.find(pair_name.str())!=gt_instance_pairs.end()){
42 |                 true_mask=true;
43 |                 count_true++;
44 |             }
45 |             pred_masks.push_back(true_mask);
46 |             // std::cout<<pair_name.str()<<" "<<true_mask<<std::endl;
47 |         }
48 | 
49 |         return count_true;
50 |     }    
51 | 
52 |     int mark_tp_instances(const Eigen::Matrix4d & gt_pose,
53 |                         const std::vector<Eigen::Vector3d> &src_centroids,
54 |                         const std::vector<Eigen::Vector3d> &ref_centroids,
55 |                         std::vector<bool> &pred_masks,
56 |                         float dist_threshold = 0.5)
57 |     {
58 |         int M = ref_centroids.size();
59 |         int count_tp = 0;
60 |         for (int i=0; i<M; i++){
61 |             Eigen::Vector3d aligned_src
62 |                 = gt_pose.block<3,3>(0,0)*src_centroids[i]+gt_pose.block<3,1>(0,3);
63 |             double dist = (ref_centroids[i] - aligned_src).norm();
64 |             if(dist<dist_threshold){
65 |                 pred_masks.push_back(true);
66 |                 count_tp++;
67 |             }
68 |             else pred_masks.push_back(false);
69 |         }
70 |         
71 |         return count_tp;
72 |     }
73 | 
74 | } // namespace fmfusion
75 | 
76 | 
77 | 
78 | 
79 | 
80 | 


--------------------------------------------------------------------------------
/src/tools/IO.h:
--------------------------------------------------------------------------------
  1 | #ifndef IO_H_
  2 | #define IO_H_
  3 | #include <vector>
  4 | #include <Common.h>
  5 | 
  6 | #include <tools/Utility.h>
  7 | #include <sgloop/Graph.h>
  8 | 
  9 | namespace fmfusion
 10 | {
 11 | namespace IO
 12 | {
 13 |     typedef std::pair<std::string, std::string> RGBDFrameDirs;
 14 | 
 15 |     // Read
 16 |     bool read_rs_intrinsic(const std::string intrinsic_dir, 
 17 |                         open3d::camera::PinholeCameraIntrinsic &intrinsic_);
 18 | 
 19 |     bool read_scannet_intrinsic(const std::string intrinsic_folder,
 20 |                         open3d::camera::PinholeCameraIntrinsic &intrinsic_);
 21 |     bool read_transformation(const std::string &transformation_file, 
 22 |                         Eigen::Matrix4d &transformation);
 23 | 
 24 |     bool frames_srt_func(const std::string &a, const std::string &b);
 25 | 
 26 |     void construct_sorted_frame_table(const std::string &scene_dir,
 27 |                                     std::vector<RGBDFrameDirs> &frame_table, 
 28 |                                     std::vector<Eigen::Matrix4d> &pose_table);
 29 | 
 30 |     bool construct_preset_frame_table(const std::string &root_dir,
 31 |                                     const std::string &association_name,
 32 |                                     const std::string &trajectory_name,
 33 |                                     std::vector<RGBDFrameDirs> &rgbd_table,
 34 |                                     std::vector<Eigen::Matrix4d> &pose_table);
 35 | 
 36 |     // Write
 37 |     void extract_match_instances(const std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 38 |         const std::vector<fmfusion::NodePtr> &src_nodes,const std::vector<fmfusion::NodePtr> &ref_nodes,
 39 |         std::vector<std::pair<fmfusion::InstanceId,fmfusion::InstanceId>> &match_instances);
 40 | 
 41 |     void extract_instance_correspondences(const std::vector<fmfusion::NodePtr> &src_nodes, 
 42 |                                         const std::vector<fmfusion::NodePtr> &ref_nodes, 
 43 |                                         const std::vector<std::pair<uint32_t,uint32_t>> &match_pairs, 
 44 |                                         const std::vector<float> &match_scores,
 45 |                                         std::vector<Eigen::Vector3d> &src_centroids, 
 46 |                                         std::vector<Eigen::Vector3d> &ref_centroids);
 47 | 
 48 |     bool save_match_results(const Eigen::Matrix4d &pose,
 49 |                         const std::vector<std::pair<uint32_t,uint32_t>> &match_pairs, 
 50 |                         const std::vector<float> &match_scores,
 51 |                         const std::string &output_file_dir);
 52 | 
 53 |     bool save_match_results(const float &timestamp,
 54 |                         const Eigen::Matrix4d &pose,
 55 |                         const std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 56 |                         const std::vector<Eigen::Vector3d> &src_centroids,
 57 |                         const std::vector<Eigen::Vector3d> &ref_centroids,
 58 |                         const std::string &output_file_dir);
 59 | 
 60 |     bool save_corrs_match_indices(const std::vector<int> &corrs_match_indices,
 61 |                                 const std::vector<float> &corrs_match_scores,
 62 |                                 const std::string &output_file_dir);
 63 |     
 64 |     bool load_corrs_match_indices(const std::string &corrs_match_indices_file,
 65 |                                 std::vector<int> &corrs_match_indices,
 66 |                                 std::vector<float> &corrs_match_scores);
 67 | 
 68 |     bool load_match_results(const std::string &match_file_dir,
 69 |                         float &timestamp,
 70 |                         Eigen::Matrix4d &pose,
 71 |                         std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 72 |                         std::vector<Eigen::Vector3d> &src_centroids,
 73 |                         std::vector<Eigen::Vector3d> &ref_centroids,
 74 |                         bool verbose=false);
 75 | 
 76 |     bool load_node_matches(const std::string &match_file_dir,
 77 |                         std::vector<std::pair<uint32_t,uint32_t>> &match_pairs,
 78 |                         std::vector<bool> &match_tp_masks,
 79 |                         std::vector<Eigen::Vector3d> &src_centroids,
 80 |                         std::vector<Eigen::Vector3d> &ref_centroids,
 81 |                         bool verbose=false);
 82 | 
 83 |     bool load_single_col_mask(const std::string &mask_file_dir, std::vector<bool> &mask);
 84 | 
 85 |     bool load_pose_file(const std::string &pose_file_dir, Eigen::Matrix4d &pose, bool verbose=false);
 86 | 
 87 |     bool read_loop_transformations(const std::string &loop_file_dir,
 88 |                         std::vector<LoopPair> &loop_pairs,
 89 |                         std::vector<Eigen::Matrix4d> &loop_transformations);
 90 | 
 91 |     bool read_loop_pairs(const std::string &loop_file_dir,
 92 |                         std::vector<LoopPair> &loop_pairs,
 93 |                         std::vector<bool> &loop_tp_masks);
 94 | 
 95 |     bool read_frames_poses(const std::string &frame_pose_file,
 96 |                     std::unordered_map<std::string, Eigen::Matrix4d> &frame_poses);
 97 | 
 98 |     bool read_entire_camera_poses(const std::string &scene_folder,
 99 |                         std::unordered_map<std::string, Eigen::Matrix4d> &src_poses_map);
100 | 
101 |     bool save_pose(const std::string &output_dir, const Eigen::Matrix4d &pose);
102 | 
103 |     /// \brief  Save the all of the centroids from two graph to the output directory.
104 |     bool save_graph_centroids(const std::vector<Eigen::Vector3d> &src_centroids, 
105 |                             const std::vector<Eigen::Vector3d> &ref_centroids,
106 |                             const std::string &output_dir);
107 | 
108 |     bool save_instance_info(const std::vector<Eigen::Vector3d> &centroids,
109 |                         const std::vector<std::string> &labels,
110 |                         const std::string &output_dir);
111 |     
112 |     bool load_instance_info(const std::string &instance_info_file,
113 |                         std::vector<Eigen::Vector3d> &centroids,
114 |                         std::vector<std::string> &labels);
115 | 
116 |     bool write_time(const std::vector<std::string> & header,
117 |                 const std::vector<double> & time,
118 |                 const std::string & file_name);
119 | 
120 | }
121 | 
122 | }
123 | #endif
124 | 


--------------------------------------------------------------------------------
/src/tools/TicToc.h:
--------------------------------------------------------------------------------
  1 | #include <fstream>
  2 | #include <iostream>
  3 | #include <vector>
  4 | #include <array>
  5 | #include <chrono>
  6 | 
  7 | 
  8 | namespace fmfusion
  9 | {
 10 |     class TicToc {
 11 |         public:
 12 |             TicToc() {
 13 |                 tic();
 14 |             }
 15 | 
 16 |             void tic() {
 17 |                 start = std::chrono::system_clock::now();
 18 |             }
 19 | 
 20 |             double toc(bool restart = false) {
 21 |                 end = std::chrono::system_clock::now();
 22 |                 std::chrono::duration<double> elapsed_seconds = end - start;
 23 |                 if(restart) tic();
 24 |                 return elapsed_seconds.count() * 1000;
 25 |             }
 26 | 
 27 |         private:
 28 |             std::chrono::time_point<std::chrono::system_clock> start, end;
 29 |     };
 30 | 
 31 |     class TicTocArray
 32 |     {
 33 |         TicTocArray() {
 34 |             tic_toc.tic();
 35 |         }
 36 | 
 37 |         double record(const std::string &name){
 38 |             double record = tic_toc.toc();
 39 |             tic_toc.tic();
 40 |             time_records.push_back(std::make_pair(name, record));
 41 |         }
 42 | 
 43 |         std::string print(){
 44 |             std::stringstream msg;
 45 |             for (auto record:time_records){
 46 |                 msg<<record.first<<": "<<record.second<<"ms, ";
 47 |             }
 48 |             msg<<"\n";
 49 |             return msg.str();
 50 |         }
 51 | 
 52 |         public:
 53 |             TicToc tic_toc;
 54 |             std::vector<std::pair<std::string,double>> time_records;
 55 |     };
 56 | 
 57 |     class TicTocSequence
 58 |     {
 59 |         public:
 60 |             TicTocSequence(string header_,int K_=10):header(header_),K(K_){
 61 | 
 62 |             }
 63 | 
 64 |             void tic(){
 65 |                 tic_toc.tic();
 66 |                 frame_records.push_back(cur_frame);
 67 |                 cur_frame.clear();
 68 |             };
 69 | 
 70 |             void toc(){
 71 |                 float duration = tic_toc.toc();
 72 |                 if(!cur_frame.empty()) duration -= cur_frame.back();
 73 |                 cur_frame.push_back(duration);
 74 |             }
 75 | 
 76 |             void fill_zeros(int fill_segments=1){
 77 |                 std::vector<float> zeros(fill_segments,0);
 78 |                 cur_frame.insert(cur_frame.end(),zeros.begin(),zeros.end());
 79 |             }
 80 | 
 81 |             bool export_data(std::string output_dir){
 82 |                 float sum_val[10]= {0,0,0,0,0,0,0,0,0,0};
 83 |                 int count[10]={0,0,0,0,0,0,0,0,0,0};
 84 |                 int num_segments=0;
 85 | 
 86 |                 std::ofstream file;
 87 |                 file.open(output_dir);
 88 |                 if (!file.is_open()){
 89 |                     std::cout<<"Failed to open file: "<<output_dir<<std::endl;
 90 |                     return false;
 91 |                 }
 92 |                 file<<header<<std::endl;
 93 |                 file<<std::fixed<<std::setprecision(1);
 94 |                 for (auto frame: frame_records){
 95 |                     for(int i=0;i<frame.size();i++){
 96 |                         file<<frame[i]<<" ";
 97 |                         sum_val[i] += frame[i];
 98 |                         count[i] ++;
 99 |                         if(i>num_segments) num_segments=i;
100 |                     }
101 |                     file<<std::endl;
102 |                 }
103 | 
104 |                 // 
105 |                 file<<"# Sum: ";
106 |                 for(int i=0;i<=num_segments;i++){
107 |                     file<<sum_val[i]/count[i]<<" ";
108 |                 }
109 |                 file<<std::endl;
110 |                 file<<"# Count: ";
111 |                 for(int i=0;i<=num_segments;i++){
112 |                     file<<count[i]<<" ";
113 |                 }
114 | 
115 |                 file.close();
116 |                 std::cout<<"Write "
117 |                         <<num_segments<<" segments and "
118 |                         <<frame_records.size()<<" frames to "<<output_dir<<std::endl;
119 |                 return true;
120 |             }
121 | 
122 |         private:
123 |             int K; // number of max segments in each frame
124 |             std::string header;
125 |             std::vector<float> cur_frame;
126 |             std::vector<vector<float>> frame_records;
127 |             TicToc tic_toc;
128 |             
129 | 
130 |     };
131 |     
132 |     class TimingSequence
133 |     {
134 |         public:
135 |             TimingSequence(std::string header_):header(header_) {
136 | 
137 |             }
138 | 
139 |             void create_frame(int frame_id){
140 |                 cur_frame_id = frame_id;
141 |                 cur_frame.clear();           
142 |             }
143 |             
144 |             void finish_frame(){
145 |                 frame_idxs.push_back(cur_frame_id);
146 |                 timings.push_back(cur_frame);
147 |                 cur_frame.clear();
148 |             }
149 | 
150 |             void record(double duration){
151 |                 cur_frame.push_back(duration);
152 |             }
153 | 
154 |             bool write_log(std::string output_dir)
155 |             {
156 |                 std::ofstream file;
157 |                 file.open(output_dir);
158 |                 if (!file.is_open()){
159 |                     std::cout<<"Failed to open file: "<<output_dir<<std::endl;
160 |                     return false;
161 |                 }
162 |                 file<<header<<std::endl;
163 |                 file<<std::fixed<<std::setprecision(1);
164 |                 for (int i=0;i<frame_idxs.size();i++){
165 |                     file<<frame_idxs[i]<<" ";
166 |                     for (int j=0;j<timings[i].size();j++){
167 |                         file<<timings[i][j]<<" ";
168 |                     }
169 |                     file<<std::endl;
170 |                 }
171 |                 file.close();
172 |                 std::cout<<"Write "<<frame_idxs.size()<<" frames to "<<output_dir<<std::endl;
173 |                 return true;
174 |             
175 |             }
176 | 
177 |         private:
178 |             std::string header;
179 |             int cur_frame_id;
180 |             std::vector<float> cur_frame;
181 | 
182 |             std::vector<int> frame_idxs;
183 |             std::vector<std::vector<float>> timings;
184 |     };
185 | 
186 | }


--------------------------------------------------------------------------------
/src/tools/Tools.h:
--------------------------------------------------------------------------------
 1 | #ifndef FMFUSION_TOOLS_H
 2 | #define FMFUSION_TOOLS_H
 3 | 
 4 | #include "mapping/Instance.h"
 5 | #include "sgloop/Graph.h"
 6 | 
 7 | namespace fmfusion
 8 | {
 9 | 
10 | namespace visualization
11 | {
12 | /// \brief  Visualization functions in Open3D lib.
13 | 
14 |     /// \brief  Draw intra-graph edges between instances.
15 |     std::shared_ptr<const open3d::geometry::LineSet> draw_edges(
16 |         const std::vector<NodePtr> &nodes, const std::vector<EdgePtr> &edges);
17 |     
18 | 
19 |     std::shared_ptr<const open3d::geometry::LineSet> draw_instance_correspondences(
20 |         const std::vector<Eigen::Vector3d> &src_centroids, const std::vector<Eigen::Vector3d> &ref_centroids);
21 | 
22 | }
23 | 
24 | 
25 | }
26 | 
27 | 
28 | #endif // FMFUSION_TOOLS_H
29 | 
30 | 
31 | 
32 | 


--------------------------------------------------------------------------------
/src/tools/Utility.h:
--------------------------------------------------------------------------------
 1 | #ifndef FMFUSION_UTILITY_H
 2 | #define FMFUSION_UTILITY_H
 3 | #include <fstream>
 4 | 
 5 | #include "open3d/Open3D.h"
 6 | #include "opencv2/opencv.hpp"
 7 | #include "Common.h"
 8 | #include "mapping/Instance.h"
 9 | 
10 | namespace fmfusion
11 | {
12 | 
13 | namespace utility
14 | {
15 | 
16 | std::vector<std::string> 
17 |     split_str(const std::string s, const std::string delim);
18 | 
19 | fmfusion::Config *create_scene_graph_config(const std::string &config_file, bool verbose);
20 | 
21 | std::string config_to_message(const fmfusion::Config &config);
22 | 
23 | template <typename T>
24 | inline std::vector<T> update_masked_vec(const std::vector<T> &pairs, const std::vector<bool> &prune_masks)
25 | {
26 |     assert(pairs.size() == prune_masks.size() && "Size mismatch");
27 |     std::vector<T> pruned_pairs;
28 |     for(int i=0;i<prune_masks.size();i++){
29 |         if(prune_masks[i]) pruned_pairs.push_back(pairs[i]);
30 |     }
31 |     return pruned_pairs;
32 | }
33 | 
34 | bool LoadPredictions(const std::string &folder_path, const std::string &frame_name, 
35 |                     const MappingConfig &mapping_cfg, const int &img_width, const int &img_height,
36 |                     std::vector<DetectionPtr> &detections);
37 | 
38 | std::shared_ptr<cv::Mat> RenderDetections(const std::shared_ptr<cv::Mat> &rgb_img,
39 |     const std::vector<fmfusion::DetectionPtr> &detections, const std::unordered_map<InstanceId,CvMatPtr> &instances_mask,
40 |     const Eigen::VectorXi &matches, const std::unordered_map<InstanceId,Eigen::Vector3d> &instance_colors);
41 | 
42 | std::shared_ptr<cv::Mat> PrjectionCloudToDepth(const open3d::geometry::PointCloud& cloud, 
43 |     const Eigen::Matrix4d &pose_inverse,const open3d::camera::PinholeCameraIntrinsic& intrinsic, int dilation_size);
44 | 
45 | bool create_masked_rgbd(
46 |     const open3d::geometry::Image &rgb, const open3d::geometry::Image &float_depth, const cv::Mat &mask,
47 |     const int &min_points,
48 |     std::shared_ptr<open3d::geometry::RGBDImage> &masked_rgbd);
49 | 
50 | bool write_config(const std::string &output_dir, const fmfusion::Config &config);
51 |  
52 | }
53 | 
54 | O3d_Image_Ptr extract_masked_o3d_image(const O3d_Image &depth, const O3d_Image &mask);
55 | 
56 | void random_sample(const std::vector<int> &indices, const int &sample_size, std::vector<int> &sampled_indices);
57 | 
58 | 
59 | }
60 | 
61 | #endif //FMFUSION_UTILITY_H
62 | 


--------------------------------------------------------------------------------
/src/tools/Visualization.cpp:
--------------------------------------------------------------------------------
 1 | #include "Tools.h"
 2 | 
 3 | namespace fmfusion::visualization
 4 | {
 5 |     std::shared_ptr<const open3d::geometry::LineSet> draw_edges(
 6 |         const std::vector<NodePtr> &nodes, const std::vector<EdgePtr> &edges)
 7 |     {
 8 |         std::vector<Eigen::Vector3d> points;
 9 |         std::vector<Eigen::Vector2i> lines;
10 | 
11 |         for (const auto &edge : edges)
12 |         {
13 |             assert (edge->src_id < nodes.size());
14 |             auto node1 = nodes[edge->src_id];
15 |             auto node2 = nodes[edge->ref_id];
16 | 
17 |             Eigen::Vector3d center1 = node1->centroid;
18 |             Eigen::Vector3d center2 = node2->centroid;
19 | 
20 |             points.push_back(center1);
21 |             points.push_back(center2);
22 | 
23 |             lines.push_back(Eigen::Vector2i(points.size() - 2, points.size() - 1));
24 |         }
25 |         std::shared_ptr<const open3d::geometry::LineSet> line_set = std::make_shared<open3d::geometry::LineSet>(points,lines);
26 | 
27 |         return line_set;
28 |     }
29 | 
30 |     std::shared_ptr<const open3d::geometry::LineSet> draw_instance_correspondences(
31 |         const std::vector<Eigen::Vector3d> &src_centroids, const std::vector<Eigen::Vector3d> &ref_centroids)
32 |     {
33 |         std::vector<Eigen::Vector3d> points;
34 |         std::vector<Eigen::Vector2i> lines;
35 | 
36 |         for (size_t i = 0; i < src_centroids.size(); i++)
37 |         {
38 |             Eigen::Vector3d center1 = src_centroids[i];
39 |             Eigen::Vector3d center2 = ref_centroids[i];
40 | 
41 |             points.push_back(center1);
42 |             points.push_back(center2);
43 | 
44 |             lines.push_back(Eigen::Vector2i(points.size() - 2, points.size() - 1));
45 |         }
46 |         std::shared_ptr<const open3d::geometry::LineSet> line_set = std::make_shared<open3d::geometry::LineSet>(points,lines);
47 |         return line_set;
48 |     }
49 | 
50 | 
51 | }
52 | 
53 | 


--------------------------------------------------------------------------------