├── LICENSE ├── README.md ├── doc ├── challenges.png ├── ltmapper.pdf ├── outputs.png ├── parkinglot_ltslam.png └── pipeline.png ├── docker ├── Dockerfile ├── build.sh └── run.sh ├── ltmapper-final.pdf ├── ltremovert ├── CMakeLists.txt ├── config │ └── params_ltmapper.yaml ├── include │ └── removert │ │ ├── Removerter.h │ │ ├── RosParamServer.h │ │ ├── Session.h │ │ └── utility.h ├── launch │ ├── removert_visualization.rviz │ └── run_ltmapper.launch ├── package.xml └── src │ ├── Removerter.cpp │ ├── RosParamServer.cpp │ ├── Session.cpp │ ├── removert_main.cpp │ └── utility.cpp └── ltslam ├── CMakeLists.txt ├── README.md ├── config └── params.yaml ├── include └── ltslam │ ├── BetweenFactorWithAnchoring.h │ ├── KDTreeVectorOfVectorsAdaptor.h │ ├── LTslam.h │ ├── RosParamServer.h │ ├── Scancontext.h │ ├── Session.h │ ├── nanoflann.hpp │ ├── tictoc.h │ └── utility.h ├── launch ├── ltmapper_visualization.rviz └── run.launch ├── package.xml └── src ├── LTslam.cpp ├── RosParamServer.cpp ├── Scancontext.cpp ├── Session.cpp ├── main.cpp └── utility.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Giseop Kim 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LT-mapper: A Modular Framework for LiDAR-based Lifelong Mapping 2 | 3 | 4 | 5 | 6 | **LT-mapper** is an open-source, modular framework designed for **LiDAR-based lifelong mapping** in dynamic environments. It enables robots to maintain accurate 3D maps over long time scales, addressing challenges such as: 7 | 8 | - Multi-session trajectory alignment (LT-SLAM) 9 | - High/low dynamic object filtering and change detection (LT-removert) 10 | - Efficient map management and change composition (LT-map) 11 |

12 | The framework supports real-world scenarios, handling ephemeral and persistent changes across multiple sessions without requiring perfect initial alignment. 13 | 14 | For more deatils, please refer to our [paper](./doc/ltmapper.pdf) and [video](https://youtu.be/vlYKfp1p2j8). 15 | 16 | 17 | 18 | 19 | 20 | ## Features 21 | - **Multi-Session SLAM (LT-SLAM):** Align multiple sessions in a shared coordinate system using anchor-node-based pose graph optimization. 22 | - **Dynamic Object Removal (LT-removert):** Efficiently filter high dynamic points and detect low-dynamic changes between sessions. 23 | - **Map Management (LT-map):** Maintain live maps, generate meta-maps, and efficiently compose changes using delta maps. 24 | - **Seamless Modular Integration:** Each module can run seperately via file-based in/out protocol. 25 |

99 | 100 | 101 | 102 | ## Example: LT-mapper on ParkingLot dataset 103 | 104 | You can download the ParkingLot dataset from this [link](https://bit.ly/ltmapper_parkinglot_data). To replay the data, you may need a [file player](https://github.com/RPM-Robotics-Lab/file_player_mulran). 105 | 106 | ParkingLot dataset includes 6 sequences captured over three days at the same spatial site but with varying initial poses. 107 | 108 | #### Step 1: Run LT-SLAM 109 | Update paths in ``ltslam/config/param.yaml`` and execute LT-SLAM. 110 | 111 | LT-SLAM will automatically align multiple sessions within a shared coordinate system. 112 | 113 |

114 | 115 | #### Step 2: Run LT-Removert & LT-Map 116 | Update paths in ``ltremovert/config/param_ltmapper.yaml`` and execute LT-removert & LT-map. 117 | 118 | After running LT-Removert & LT-Map, you will get cleaned session data and detected changes between sessions. 119 | 120 | 121 | 122 | 123 | 124 | ## Citation 125 | ```bibtex 126 | @inproceedings{kim2022lt, 127 | title={Lt-mapper: A modular framework for lidar-based lifelong mapping}, 128 | author={Kim, Giseop and Kim, Ayoung}, 129 | booktitle={2022 International Conference on Robotics and Automation (ICRA)}, 130 | pages={7995--8002}, 131 | year={2022}, 132 | organization={IEEE} 133 | } 134 | ``` 135 | 136 | 137 | ## Contact 138 | - Maintained by Giseop Kim and please contact the author via ``gsk@dgist.ac.kr`` 139 | 140 | ### Acknowledgement 141 | - Big thanks to [Dongjae Lee](https://dongjae0107.github.io/) for contributions in refactoring, dockerization, documentation, and various verifications. 142 | 143 | ### Contributors 144 | 145 |

146 | 147 | -------------------------------------------------------------------------------- /doc/challenges.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/doc/challenges.png -------------------------------------------------------------------------------- /doc/ltmapper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/doc/ltmapper.pdf -------------------------------------------------------------------------------- /doc/outputs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/doc/outputs.png -------------------------------------------------------------------------------- /doc/parkinglot_ltslam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/doc/parkinglot_ltslam.png -------------------------------------------------------------------------------- /doc/pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/doc/pipeline.png -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:noetic-desktop-full 2 | 3 | ENV DEBIAN_FRONTEND=noninteractive 4 | 5 | RUN apt update && apt upgrade -y 6 | 7 | # Essential tools 8 | RUN apt install -y \ 9 | build-essential \ 10 | cmake \ 11 | git \ 12 | gedit \ 13 | unzip \ 14 | curl \ 15 | vim \ 16 | wget \ 17 | software-properties-common \ 18 | libboost-all-dev \ 19 | libeigen3-dev \ 20 | python3-catkin-tools 21 | 22 | # ROS 23 | RUN apt install -y ros-noetic-navigation \ 24 | ros-noetic-robot-localization \ 25 | ros-noetic-robot-state-publisher 26 | 27 | # catkin workspace 28 | ARG CATKIN_WS_DIR=/root/catkin_ws 29 | RUN . /opt/ros/noetic/setup.sh && mkdir -p ${CATKIN_WS_DIR}/src \ 30 | && cd ${CATKIN_WS_DIR} \ 31 | && catkin build \ 32 | && echo "source ${CATKIN_WS_DIR}/devel/setup.bash" >> ~/.bashrc 33 | 34 | # gtsam 35 | RUN apt update 36 | RUN add-apt-repository ppa:borglab/gtsam-release-4.0 37 | RUN apt update 38 | RUN apt install -y libgtsam-dev libgtsam-unstable-dev 39 | RUN rm -rf /var/lib/apt/lists/* 40 | 41 | # livox_ros_driver 42 | RUN git clone https://github.com/Livox-SDK/livox_ros_driver ${CATKIN_WS_DIR}/src/livox_ros_driver 43 | RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git ${CATKIN_WS_DIR}/src/Livox-SDK2 44 | RUN cd ${CATKIN_WS_DIR}/src/Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j && make install 45 | 46 | # ceres-solver 47 | RUN git clone https://ceres-solver.googlesource.com/ceres-solver -b 2.0.0 ${CATKIN_WS_DIR}/src/ceres-solver 48 | RUN apt update 49 | RUN apt install -y libgoogle-glog-dev libgflags-dev 50 | RUN apt install -y libatlas-base-dev 51 | RUN cd ${CATKIN_WS_DIR}/src/ceres-solver/ && mkdir build && cd build && cmake .. && make -j3 && make install 52 | 53 | # SC-LIO-SAM 54 | RUN git clone https://github.com/dongjae0107/SC-LIO-SAM.git ${CATKIN_WS_DIR}/src/SC-LIO-SAM 55 | RUN cd ${CATKIN_WS_DIR} && catkin build lio_sam 56 | 57 | # FAST-LIO-SLAM 58 | RUN git clone https://github.com/dongjae0107/FAST_LIO_SLAM.git ${CATKIN_WS_DIR}/src/FAST_LIO_SLAM 59 | # RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO_SLAM && git submodule update --init 60 | RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO_SLAM 61 | RUN cd ${CATKIN_WS_DIR} && catkin build livox_ros_driver fast_lio aloam_velodyne 62 | 63 | # LT-mapper 64 | RUN git clone https://github.com/dongjae0107/lt-mapper ${CATKIN_WS_DIR}/src/lt-mapper 65 | RUN cd ${CATKIN_WS_DIR} && catkin build ltslam removert 66 | 67 | RUN echo "source /opt/ros/noetic/setup.bash" >> /root/.bashrc 68 | RUN echo "source /root/catkin_ws/devel/setup.bash" >> /root/.bashrc 69 | 70 | # nvidia-container-runtime 71 | ENV NVIDIA_VISIBLE_DEVICES \ 72 | ${NVIDIA_VISIBLE_DEVICES:-all} 73 | ENV NVIDIA_DRIVER_CAPABILITIES \ 74 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 75 | 76 | 77 | ENTRYPOINT [ "/bin/bash" ] -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #! /bin/sh 2 | 3 | docker build --no-cache --force-rm -f Dockerfile -t dongjae0107/lt-mapper:latest . -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | xhost + 2 | docker run --rm -it --ipc=host --net=host --privileged \ 3 | --env="DISPLAY" \ 4 | --volume="/etc/localtime:/etc/localtime:ro" \ 5 | dongjae0107/lt-mapper:latest 6 | xhost - -------------------------------------------------------------------------------- /ltmapper-final.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/lt-mapper/80b67567b356850a10e883c1530e4e69314eaa62/ltmapper-final.pdf -------------------------------------------------------------------------------- /ltremovert/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(removert) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_STANDARD 17) # C++17 to use stdc++fs 6 | # set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | tf 10 | roscpp 11 | rospy 12 | cv_bridge 13 | # pcl library 14 | pcl_conversions 15 | # msgs 16 | std_msgs 17 | sensor_msgs 18 | geometry_msgs 19 | nav_msgs 20 | message_generation 21 | image_transport 22 | ) 23 | 24 | find_package(OpenMP REQUIRED) 25 | find_package(PCL REQUIRED QUIET) 26 | find_package(OpenCV REQUIRED QUIET) 27 | # find_package(GTSAM REQUIRED QUIET) 28 | 29 | # add_message_files( 30 | # DIRECTORY msg 31 | # FILES 32 | # # cloud_info.msg 33 | # ) 34 | 35 | # generate_messages( 36 | # DEPENDENCIES 37 | # geometry_msgs 38 | # std_msgs 39 | # nav_msgs 40 | # sensor_msgs 41 | # ) 42 | 43 | catkin_package( 44 | INCLUDE_DIRS include 45 | DEPENDS PCL 46 | # GTSAM 47 | 48 | CATKIN_DEPENDS 49 | std_msgs 50 | nav_msgs 51 | geometry_msgs 52 | sensor_msgs 53 | message_runtime 54 | message_generation 55 | ) 56 | 57 | # include directories 58 | include_directories( 59 | include 60 | ${catkin_INCLUDE_DIRS} 61 | ${PCL_INCLUDE_DIRS} 62 | ${OpenCV_INCLUDE_DIRS} 63 | # ${GTSAM_INCLUDE_DIR} 64 | ) 65 | 66 | # link directories 67 | link_directories( 68 | include 69 | ${PCL_LIBRARY_DIRS} 70 | ${OpenCV_LIBRARY_DIRS} 71 | # ${GTSAM_LIBRARY_DIRS} 72 | ) 73 | 74 | ########### 75 | ## Build ## 76 | ########### 77 | 78 | # Range Image Projection 79 | add_executable( ${PROJECT_NAME}_removert src/removert_main.cpp 80 | src/utility.cpp 81 | src/RosParamServer.cpp 82 | src/Session.cpp 83 | src/Removerter.cpp 84 | ) 85 | 86 | add_dependencies(${PROJECT_NAME}_removert ${catkin_EXPORTED_TARGETS}) 87 | target_compile_options(${PROJECT_NAME}_removert PRIVATE ${OpenMP_CXX_FLAGS}) 88 | target_link_libraries(${PROJECT_NAME}_removert 89 | ${catkin_LIBRARIES} 90 | ${PCL_LIBRARIES} 91 | ${OpenCV_LIBRARIES} 92 | ${OpenMP_CXX_FLAGS} 93 | stdc++fs 94 | ) 95 | 96 | 97 | -------------------------------------------------------------------------------- /ltremovert/config/params_ltmapper.yaml: -------------------------------------------------------------------------------- 1 | removert: 2 | 3 | # 4 | isScanFileKITTIFormat: false 5 | 6 | # @ save option 7 | saveMapPCD: true 8 | saveCleanScansPCD: true 9 | save_pcd_directory: "/home/user/Desktop/ltslam-tutorial-kitti/data_ltmapper_mulran/riverside-rs/out/" # replace to your path (please use an absolute path) 10 | 11 | # @ sequence info (replace to your paths) 12 | # please follow the KITTI odometry dataset's scan and pose formats (i.e., make them .bin and line-by-line corresponding se3 poses composed of 12 numbers) 13 | central_sess_scan_dir: "/home/user/Desktop/ltslam-tutorial-kitti/data_scaloam_mulran/riverside/01/Scans/" 14 | central_sess_pose_path: "/home/user/Desktop/ltslam-tutorial-kitti/data_ltslam_mulran/riverside0102rs/01_central_aft_intersession_loops.txt" 15 | 16 | query_sess_scan_dir: "/home/user/Desktop/ltslam-tutorial-kitti/data_scaloam_mulran/riverside/02/Scans/" 17 | query_sess_pose_path: "/home/user/Desktop/ltslam-tutorial-kitti/data_ltslam_mulran/riverside0102rs/02_central_aft_intersession_loops.txt" 18 | 19 | # 20 | sequence_vfov: 50 # including upper and lower fovs, 21 | # for example, KITTI's HDL64E is 2 + 24.9 ~ 27 deg. (so 25 + 25 = 50 is recommended because it is enough) 22 | # , MulRan's Ouster OS1-64 has +22.5 to -22.5 d8eg. (so 22.5 + 22.5 = 45 < 50 is also enough and able to cover the all vfov) 23 | sequence_hfov: 360 # generally for targetting scanning LiDAR but a lidar having restricted hfov also just can use the 360 (because no point regions are considered in the algorithm) 24 | 25 | 26 | # @ Sequence's BaseToLidar info 27 | # @ If you use the lidar-itself odometry (e.g., from SC-LIO-SAM's pose saver, the pose is from lidar itself so use the identity) 28 | ExtrinsicLiDARtoPoseBase: [1.0, 0.0, 0.0, 0.0, 29 | 0.0, 1.0, 0.0, 0.0, 30 | 0.0, 0.0, 1.0, 0.0, 31 | 0.0, 0.0, 0.0, 1.0] 32 | 33 | 34 | # @ Sampling nodes 35 | use_keyframe_gap: true 36 | keyframe_gap: 1 # if you use SC-LIO-SAM's poses, the nodes are already keyframe (i.e., spatially apart enough) 37 | 38 | start_idx: 1100 # change this // for 0103 39 | end_idx: 1200 # change this // for 0103 40 | 41 | 42 | # @ auto repeat 43 | # means the removeter runs on all scans in a sequence 44 | clean_for_all_scan: false # default is false 45 | batch_size: 150 46 | valid_ratio_to_save: 0.75 # i.e., no save cleaned scans located at the boundaries of a batch 47 | 48 | 49 | # @ Range image resolution 50 | # the below is actually magnifier ratio (i.e., 5 means x5 resolution, the x1 means 1 deg x 1 deg per pixel) 51 | # - recommend to use the first removing resolution's magnifier ratio should meet the seonsor vertical fov / number of rays 52 | # - e.g., HDL 64E of KITTI dataset -> appx 25 deg / 64 ray ~ 0.4 deg per pixel -> the magnifier ratio = 1/0.4 = 2.5 53 | # - e.g., Ouster OS1-64 of MulRan dataset -> appx 45 deg / 64 ray ~ 0.7 deg per pixel -> the magnifier ratio = 1/0.7 = 1.4 54 | # - recommend to use the first reverting resolution's magnifier ratio should lied in 1.0 to 1.5 55 | remove_resolution_list: [2.5] # for Ouster OS1-64 of MulRan dataset 56 | revert_resolution_list: [2.2] # TODO 57 | 58 | 59 | # @ Removert params 60 | # about density 61 | downsample_voxel_size: 0.05 # user parameter but recommend to use 0.05 to make sure an enough density (this value is related to the removing resolution's expected performance) 62 | 63 | # about Static sensitivity (you need to tune these below two values, depends on the environement) 64 | # - if you use a raw scan (saved from the SC-A-LOAM or SC-LIO-SAM saver utility) 65 | num_nn_points_within: 2 # how many - using higher, more strict static 66 | dist_nn_points_within: 0.01 # meter - using smaller, more strict static (if you use a already downsampled feature cloud, use quite large value such as > 0.2 ) 67 | 68 | # @ For faster 69 | num_omp_cores: 16 # for faster map points projection (to make a map range image) 70 | 71 | # @ For visualization of range images (rviz -d removert_visualization.rviz) 72 | rimg_color_min: 0.0 # meter 73 | rimg_color_max: 20.0 # meter -------------------------------------------------------------------------------- /ltremovert/include/removert/Removerter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "removert/RosParamServer.h" 4 | #include "removert/Session.h" 5 | 6 | namespace ltremovert 7 | { 8 | 9 | class Removerter : public RosParamServer 10 | { 11 | private: 12 | 13 | ros::Subscriber subLaserCloud; // TODO, for real-time integration (later) 14 | 15 | Session central_sess_; 16 | Session query_sess_; 17 | 18 | std::string updated_scans_save_dir_; 19 | std::string updated_strong_scans_save_dir_; 20 | std::string pd_scans_save_dir; 21 | std::string strong_pd_scans_save_dir; 22 | std::string strong_nd_scans_save_dir; 23 | 24 | std::string central_map_static_save_dir_; 25 | std::string central_map_dynamic_save_dir_; 26 | 27 | pcl::IterativeClosestPoint icp_; 28 | 29 | // unuseds 30 | pcl::PointCloud::Ptr map_subset_global_curr_; // unused 31 | pcl::PointCloud::Ptr map_global_accumulated_static_; // TODO, the S_i after reverted 32 | pcl::PointCloud::Ptr map_global_accumulated_dynamic_; // TODO, the D_i after reverted 33 | 34 | std::vector::Ptr> static_map_global_history_; // TODO 35 | std::vector::Ptr> dynamic_map_global_history_; // TODO 36 | 37 | // configs 38 | std::pair curr_rimg_shape_; 39 | 40 | float curr_res_alpha_; // just for tracking current status 41 | 42 | const int base_node_idx_ = 0; 43 | 44 | unsigned long kPauseTimeForClearStaticScanVisualization = 1000; // microsec 45 | 46 | // NOT recommend to use for under 5 million points map input (becausing not-using is just faster) 47 | const bool kUseSubsetMapCloud = false; 48 | const float kBallSize = 80.0; // meter 49 | 50 | // for viz 51 | image_transport::ImageTransport ROSimg_transporter_; 52 | 53 | sensor_msgs::ImagePtr scan_rimg_msg_; 54 | image_transport::Publisher scan_rimg_msg_publisher_; 55 | 56 | sensor_msgs::ImagePtr map_rimg_msg_; 57 | image_transport::Publisher map_rimg_msg_publisher_; 58 | 59 | sensor_msgs::ImagePtr diff_rimg_msg_; 60 | image_transport::Publisher diff_rimg_msg_publisher_; 61 | 62 | sensor_msgs::ImagePtr map_rimg_ptidx_msg_; 63 | image_transport::Publisher map_rimg_ptidx_msg_publisher_; 64 | 65 | 66 | public: 67 | Removerter(); 68 | ~Removerter(); 69 | 70 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); // TODO (if removert run online) 71 | 72 | void allocateMemory(); 73 | 74 | void loadSessionInfo( void ); 75 | void parseKeyframes( void ); 76 | void loadKeyframes( void ); 77 | void precleaningKeyframes(float _radius); 78 | 79 | // void readValidScans(); 80 | 81 | void selfRemovert(const Session& _sess, int _repeat); 82 | void removeHighDynamicPoints(void); 83 | void detectLowDynamicPoints(void); 84 | 85 | void mergeScansWithinGlobalCoord( 86 | const std::vector::Ptr>& _scans, 87 | const std::vector& _scans_poses, 88 | pcl::PointCloud::Ptr& _ptcloud_to_save ); 89 | 90 | void makeGlobalMap(Session& _sess); 91 | void makeGlobalMap(); 92 | 93 | void updateCurrentMap(void); // the updated one is saved in the central session's class 94 | 95 | void run(void); 96 | 97 | void parseStaticScansViaProjection(Session& _sess); 98 | void parseStaticScansViaProjection(void); 99 | 100 | std::pair::Ptr, pcl::PointCloud::Ptr> 101 | partitionCurrentMap(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 102 | 103 | void removeOnce(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 104 | void removeOnce(float _res); 105 | 106 | void revertOnce( const Session& _target_sess, const Session& _source_sess, float _res_alpha ); 107 | void revertOnce(float _res); 108 | 109 | void resetCurrrentMapAsDynamic(const Session& _sess, bool _as_dynamic); 110 | void resetCurrrentMapAsDynamic(const Session& _sess); 111 | void resetCurrrentMapAsStatic(const Session& _sess); 112 | 113 | void saveCurrentStaticMapHistory(void); // the 0th element is a noisy (original input) (actually not static) map. 114 | void saveCurrentDynamicMapHistory(void); 115 | 116 | void takeGlobalMapSubsetWithinBall( int _center_scan_idx ); 117 | void transformGlobalMapSubsetToLocal(int _base_scan_idx); 118 | 119 | cv::Mat scan2RangeImg(const pcl::PointCloud::Ptr& _scan, 120 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 121 | const std::pair _rimg_size); 122 | // std::pair map2RangeImg(const pcl::PointCloud::Ptr& _scan, 123 | // const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 124 | // const std::pair _rimg_size); 125 | pcl::PointCloud::Ptr parseProjectedPoints(const pcl::PointCloud::Ptr& _scan, 126 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 127 | const std::pair _rimg_size); 128 | 129 | std::vector calcDescrepancyAndParseDynamicPointIdx( 130 | const cv::Mat& _scan_rimg, const cv::Mat& _diff_rimg, const cv::Mat& _map_rimg_ptidx, float _diff_thres = 0.1); 131 | 132 | std::vector calcDescrepancyAndParseDynamicPointIdxForEachScan( std::pair _rimg_shape ); 133 | std::vector calcDescrepancyAndParseDynamicPointIdxForEachScan( 134 | const Session& _target_sess, const Session& _source_sess, std::pair _rimg_shape); 135 | 136 | std::vector getStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes, int _num_all_points); 137 | std::vector getGlobalMapStaticIdxFromDynamicIdx(const std::vector& _dynamic_point_indexes); 138 | std::vector getGlobalMapStaticIdxFromDynamicIdx(const pcl::PointCloud::Ptr& _map_global_curr, 139 | std::vector &_dynamic_point_indexes); 140 | 141 | void parsePointcloudSubsetUsingPtIdx( const pcl::PointCloud::Ptr& _ptcloud_orig, 142 | std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ); 143 | void parseMapPointcloudSubsetUsingPtIdx( std::vector& _point_indexes, pcl::PointCloud::Ptr& _ptcloud_to_save ); 144 | void parseStaticMapPointcloudUsingPtIdx( std::vector& _point_indexes ); 145 | void parseDynamicMapPointcloudUsingPtIdx( std::vector& _point_indexes ); 146 | 147 | void saveCurrentStaticAndDynamicPointCloudGlobal( void ); 148 | void saveCurrentStaticAndDynamicPointCloudLocal( int _base_pose_idx = 0); 149 | void saveCurrentStaticAndDynamicPointCloudGlobal(const Session& _sess, std::string _postfix); 150 | 151 | pcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr& _scan_local, const Eigen::Matrix4d& _scan_pose); 152 | pcl::PointCloud::Ptr global2local(const pcl::PointCloud::Ptr& _scan_global, const Eigen::Matrix4d& _scan_pose_inverse); 153 | 154 | // scan-side removal 155 | std::pair::Ptr, pcl::PointCloud::Ptr> removeDynamicPointsOfScanByKnn ( int _scan_idx ); 156 | std::pair::Ptr, pcl::PointCloud::Ptr> removeDynamicPointsOfScanByKnn( int _scan_idx, 157 | const Session& _sess, const pcl::KdTreeFLANN::Ptr& _kdtree_map_global_curr); 158 | std::pair::Ptr, pcl::PointCloud::Ptr> removeDynamicPointsOfScanByKnnBtnSession ( int _scan_idx ); 159 | void removeDynamicPointsAndSaveStaticScanForEachScan( void ); 160 | 161 | void scansideRemovalForEachScan(void); 162 | void scansideRemovalForEachScan(const Session& _map_sess, const Session& _scan_sess); 163 | void scansideRemovalViaMapReprojectionForEachScan(void); 164 | void saveCleanedScans(void); 165 | void saveCleanedScans(const Session& _sess); 166 | // void saveMapPointcloudByMergingCleanedScans(void); 167 | void saveMapPointcloudByMergingCleanedScans( const Session& _sess, std::string _postfix ); 168 | void scansideRemovalForEachScanAndSaveThem(void); 169 | void scansideRemovalViaMapReprojectionForEachScanAndSaveThem(void); 170 | 171 | void saveStaticScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ); 172 | void saveDynamicScan( int _scan_idx, const pcl::PointCloud::Ptr& _ptcloud ); 173 | 174 | void filterStrongND(Session& _sess_src, Session& _sess_cleaner); 175 | void iremoveOnceForND(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 176 | std::pair partitionCurrentMapForND(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 177 | std::vector calcDescrepancyAndParseDynamicPointIdxForEachScanForND( 178 | const Session& _target_sess, const Session& _source_sess, std::pair _rimg_shape); 179 | 180 | void filterStrongPD(Session& _sess_src, Session& _sess_cleaner); 181 | void removeOnceForPD(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 182 | std::pair partitionCurrentMapForPD(const Session& _target_sess, const Session& _source_sess, float _res_alpha); 183 | std::vector calcDescrepancyAndParseDynamicPointIdxForEachScanForPD( 184 | const Session& _target_sess, const Session& _source_sess, std::pair _rimg_shape); 185 | 186 | void parseUpdatedStaticScansViaProjection( void ); 187 | void parseUpdatedStaticScansViaProjection(Session& _sess); 188 | 189 | void parseLDScansViaProjection(); 190 | void parseLDScansViaProjection(Session& _sess); 191 | 192 | void updateScansScanwise(Session& _sess); 193 | void updateScansScanwise(); 194 | 195 | void saveAllTypeOfScans(); 196 | void saveUpdatedScans( Session& _sess ); 197 | void saveLDScans( Session& _sess ); 198 | void savePDScans( Session& _sess ); 199 | void saveStrongPDScans( Session& _sess ); 200 | void saveStrongNDScans( Session& _sess ); 201 | void saveScans(Session& _sess, std::vector::Ptr> _scans, std::string _save_dir); 202 | 203 | 204 | }; // Removerter 205 | 206 | } // namespace ltremovert 207 | -------------------------------------------------------------------------------- /ltremovert/include/removert/RosParamServer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "removert/utility.h" 4 | 5 | class RosNodeHandle 6 | { 7 | public: 8 | ros::NodeHandle nh_super; 9 | }; // class: RosNodeHandle 10 | 11 | 12 | class RosParamServer: public RosNodeHandle 13 | { 14 | public: 15 | // 16 | ros::NodeHandle & nh; 17 | 18 | // 19 | std::string pointcloud_topic; 20 | 21 | // removert params 22 | float kVFOV; 23 | float kHFOV; 24 | std::pair kFOV; 25 | 26 | float kDownsampleVoxelSize; 27 | 28 | // pose base where 29 | std::vector kVecExtrinsicLiDARtoPoseBase; 30 | Eigen::Matrix4d kSE3MatExtrinsicLiDARtoPoseBase; 31 | // Base is where of the pose writtened (e.g., for KITTI, poses is usually in camera) 32 | // if the pose file is obtained via lidar odometry itself, then kMatExtrinsicLiDARtoBase is eye(4) 33 | Eigen::Matrix4d kSE3MatExtrinsicPoseBasetoLiDAR; 34 | 35 | // config 36 | bool isScanFileKITTIFormat_; 37 | 38 | // knn 39 | int kNumKnnPointsToCompare;// static sensitivity (increase this value, less static structure will be removed at the scan-side removal stage) 40 | float kScanKnnAndMapKnnAvgDiffThreshold; // static sensitivity (decrease this value, less static structure will be removed at the scan-side removal stage) 41 | 42 | // sessions' paths 43 | std::string central_sess_scan_dir_; 44 | std::string central_sess_pose_path_; 45 | std::string query_sess_scan_dir_; 46 | std::string query_sess_pose_path_; 47 | 48 | // target region to removerting 49 | int start_idx_; 50 | int end_idx_; 51 | 52 | bool use_keyframe_gap_; 53 | bool use_keyframe_meter_; 54 | int keyframe_gap_; 55 | float keyframe_gap_meter_; 56 | 57 | int repeat_removert_iter_; 58 | 59 | // 60 | std::vector remove_resolution_list_; 61 | std::vector revert_resolution_list_; 62 | 63 | // 64 | int kNumOmpCores; 65 | 66 | // 67 | pcl::PointCloud::Ptr single_scan; 68 | pcl::PointCloud::Ptr projected_scan; 69 | 70 | // ros pub 71 | ros::Publisher scan_publisher_; 72 | ros::Publisher global_scan_publisher_; 73 | 74 | ros::Publisher original_map_local_publisher_; 75 | 76 | ros::Publisher curr_map_local_publisher_; 77 | ros::Publisher static_map_local_publisher_; 78 | ros::Publisher dynamic_map_local_publisher_; 79 | 80 | ros::Publisher static_curr_scan_publisher_; 81 | ros::Publisher dynamic_curr_scan_publisher_; 82 | 83 | float rimg_color_min_; 84 | float rimg_color_max_; 85 | std::pair kRangeColorAxis; // meter 86 | std::pair kRangeColorAxisForDiff; // meter 87 | 88 | 89 | // 90 | bool kFlagSaveMapPointcloud; 91 | bool kFlagSaveCleanScans; 92 | std::string save_pcd_directory_; 93 | 94 | public: 95 | RosParamServer(); 96 | 97 | }; // class: RosParamServer 98 | -------------------------------------------------------------------------------- /ltremovert/include/removert/Session.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "removert/RosParamServer.h" 4 | #include "removert/utility.h" 5 | 6 | namespace ltremovert 7 | { 8 | 9 | class Session : public RosParamServer 10 | { 11 | public: 12 | 13 | const float kReprojectionAlpha = 3.0; 14 | 15 | std::string sess_type_; 16 | 17 | // config 18 | float kDownsampleVoxelSize; 19 | 20 | // all 21 | std::string scan_dir_; 22 | std::string pose_path_; 23 | 24 | // including all scans inforamtion in the session 25 | std::vector scan_names_; 26 | std::vector scan_paths_; 27 | std::vector scan_poses_; // including all scans inforamtion in the session 28 | std::vector scan_inverse_poses_; 29 | int num_scans_; 30 | 31 | // keyframes (for efficient processing) 32 | int keyframe_gap_; 33 | std::vector keyframe_names_; 34 | std::vector keyframe_paths_; 35 | std::vector keyframe_poses_; 36 | std::vector keyframe_inverse_poses_; 37 | int num_keyframes_; 38 | 39 | std::vector::Ptr> keyframe_scans_; 40 | std::vector::Ptr> keyframe_scans_static_; 41 | 42 | std::vector::Ptr> keyframe_scans_static_projected_; //keyframes 43 | std::vector::Ptr> keyframe_scans_dynamic_; 44 | 45 | pcl::PointCloud::Ptr target_map_down_for_knn_; 46 | std::vector::Ptr> scans_knn_coexist_; // keyframes, between session 47 | std::vector::Ptr> scans_knn_diff_; // keyframes, between session 48 | 49 | std::vector::Ptr> keyframe_scans_updated_; //keyframes 50 | std::vector::Ptr> keyframe_scans_updated_strong_; //keyframes 51 | std::vector::Ptr> keyframe_scans_pd_; //keyframes 52 | std::vector::Ptr> keyframe_scans_strong_pd_; //keyframes 53 | std::vector::Ptr> keyframe_scans_strong_nd_; //keyframes 54 | std::vector::Ptr> keyframe_scans_weak_nd_; //keyframes 55 | 56 | // 57 | pcl::KdTreeFLANN::Ptr kdtree_map_global_curr_; 58 | 59 | pcl::KdTreeFLANN::Ptr kdtree_target_map_global_; 60 | pcl::KdTreeFLANN::Ptr kdtree_scan_global_curr_; 61 | 62 | pcl::KdTreeFLANN::Ptr kdtree_map_global_nd_; 63 | pcl::KdTreeFLANN::Ptr kdtree_map_global_pd_; 64 | 65 | pcl::PointCloud::Ptr map_global_orig_; 66 | pcl::PointCloud::Ptr map_global_curr_; // the M_i. i.e., removert is: M1 -> S1 + D1, D1 -> M2 , M2 -> S2 + D2 ... repeat ... 67 | pcl::PointCloud::Ptr map_local_curr_; 68 | pcl::PointCloud::Ptr map_global_curr_static_; // the S_i 69 | pcl::PointCloud::Ptr map_global_curr_dynamic_; // the D_i 70 | 71 | pcl::PointCloud::Ptr map_global_updated_; 72 | pcl::PointCloud::Ptr map_global_updated_strong_; 73 | 74 | pcl::PointCloud::Ptr map_global_curr_diff_positive_; // the S_i 75 | pcl::PointCloud::Ptr map_global_curr_diff_negative_; // the D_i 76 | pcl::PointCloud::Ptr map_global_curr_down_for_icp_; 77 | 78 | pcl::PointCloud::Ptr map_global_nd_; 79 | pcl::PointCloud::Ptr map_local_nd_; 80 | pcl::PointCloud::Ptr map_global_nd_strong_; 81 | pcl::PointCloud::Ptr map_global_nd_weak_; // == ambigous 82 | 83 | pcl::PointCloud::Ptr map_global_pd_; 84 | pcl::PointCloud::Ptr map_global_pd_orig_; 85 | pcl::PointCloud::Ptr map_local_pd_; 86 | pcl::PointCloud::Ptr map_global_pd_strong_; 87 | pcl::PointCloud::Ptr map_global_pd_weak_; 88 | 89 | pcl::IterativeClosestPoint icp_for_knn_; 90 | 91 | std::mutex mtx; 92 | 93 | public: 94 | Session( void ); 95 | 96 | void allocateMemory(); 97 | 98 | void loadSessionInfo( std::string _sess_type, std::string _scan_dir, std::string _pose_path ); 99 | void setDownsampleSize( float _voxel_size ); 100 | 101 | void clearKeyframes( void ); 102 | 103 | void parseKeyframes( int _gap = 1 ); 104 | void parseKeyframes( std::pair _range, int _gap = 1 ); 105 | void parseKeyframesInROI( const std::vector& _roi_poses, int _gap = 1 ); 106 | 107 | void loadKeyframes( void ); 108 | void precleaningKeyframes ( float _radius ); 109 | void mergeScansWithinGlobalCoord(void); 110 | 111 | void parseStaticScansViaProjection(void); 112 | void parseUpdatedStaticScansViaProjection(void); 113 | void parseUpdatedStrongStaticScansViaProjection(void); 114 | void parsePDScansViaProjection(void); 115 | void parseStrongPDScansViaProjection(void); 116 | void parseWeakNDScansViaProjection(void); 117 | void parseStrongNDScansViaProjection(void); 118 | void parseScansViaProjection( 119 | pcl::PointCloud::Ptr _map, // to_be_parsed 120 | std::vector::Ptr>& _vec_to_store ); 121 | 122 | void updateScansScanwise(); 123 | 124 | void extractLowDynPointsViaKnnDiff(const pcl::PointCloud::Ptr _target_map); 125 | std::pair partitionLowDynamicPointsOfScanByKnn(int _scan_idx); 126 | 127 | void extractHighDynPointsViaKnnDiff(const pcl::PointCloud::Ptr _target_map); 128 | std::pair partitionHighDynamicPointsOfScanByKnn(int _scan_idx); 129 | 130 | void constructGlobalNDMap(); 131 | void removeWeakNDMapPointsHavingStrongNDInNear(); 132 | 133 | void constructGlobalPDMap(); 134 | void revertStrongPDMapPointsHavingWeakPDInNear(); 135 | 136 | }; 137 | 138 | 139 | 140 | } // namespace ltremovert -------------------------------------------------------------------------------- /ltremovert/include/removert/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _UTILITY_REMOVERT_H_ 3 | #define _UTILITY_REMOVERT_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include // requires gcc version >= 8 71 | 72 | namespace fs = std::filesystem; 73 | using std::ios; 74 | using std::cout; 75 | using std::cerr; 76 | using std::endl; 77 | 78 | // struct PointXYZIS 79 | // { 80 | // PCL_ADD_POINT4D 81 | // float intensity; 82 | // float score; 83 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 84 | // } EIGEN_ALIGN16; 85 | 86 | // POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIS, 87 | // (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, score, score) 88 | // ) 89 | 90 | using PointType = pcl::PointXYZI; 91 | using pcPtr = pcl::PointCloud::Ptr; 92 | 93 | const float kFlagNoPOINT = 10000.0; // no point constant, 10000 has no meaning, but must be larger than the maximum scan range (e.g., 200 meters) 94 | const float kValidDiffUpperBound = 200.0; // must smaller than kFlagNoPOINT 95 | 96 | struct SphericalPoint 97 | { 98 | float az; // azimuth 99 | float el; // elevation 100 | float r; // radius 101 | }; 102 | 103 | float rad2deg(float radians); 104 | float deg2rad(float degrees); 105 | 106 | void readBin(std::string _bin_path, pcl::PointCloud::Ptr _pcd_ptr); 107 | 108 | std::vector splitPoseLine(std::string _str_line, char _delimiter); 109 | 110 | SphericalPoint cart2sph(const PointType & _cp); 111 | 112 | std::pair resetRimgSize(const std::pair _fov, const float _resize_ratio); 113 | 114 | template 115 | cv::Mat convertColorMappedImg (const cv::Mat &_src, std::pair _caxis) 116 | { 117 | T min_color_val = _caxis.first; 118 | T max_color_val = _caxis.second; 119 | 120 | cv::Mat image_dst; 121 | image_dst = 255 * (_src - min_color_val) / (max_color_val - min_color_val); 122 | image_dst.convertTo(image_dst, CV_8UC1); 123 | 124 | cv::applyColorMap(image_dst, image_dst, cv::COLORMAP_JET); 125 | 126 | return image_dst; 127 | } 128 | 129 | pcl::PointCloud::Ptr parseProjectedPoints(const pcl::PointCloud::Ptr &_scan, 130 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 131 | const std::pair _rimg_size); 132 | 133 | std::pair map2RangeImg(const pcl::PointCloud::Ptr &_scan, 134 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 135 | const std::pair _rimg_size); 136 | 137 | void transformGlobalMapToLocal( 138 | const pcl::PointCloud::Ptr &_map_global, 139 | Eigen::Matrix4d _base_pose_inverse, Eigen::Matrix4d _base2lidar, 140 | pcl::PointCloud::Ptr &_map_local); 141 | 142 | void octreeDownsampling(const pcl::PointCloud::Ptr &_src, pcl::PointCloud::Ptr &_to_save); 143 | void octreeDownsampling(const pcl::PointCloud::Ptr &_src, pcl::PointCloud::Ptr &_to_save, 144 | const float _kDownsampleVoxelSize); 145 | 146 | std::set convertIntVecToSet(const std::vector & v); 147 | 148 | pcl::PointCloud::Ptr mergeScansWithinGlobalCoordUtil( 149 | const std::vector::Ptr> &_scans, 150 | const std::vector &_scans_poses, Eigen::Matrix4d _lidar2base); 151 | 152 | pcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr &_scan_local, 153 | const Eigen::Matrix4d &_scan_pose, const Eigen::Matrix4d &_base2lidar); 154 | 155 | pcl::PointCloud::Ptr global2local(const pcl::PointCloud::Ptr &_scan_global, 156 | const Eigen::Matrix4d &_scan_pose_inverse, const Eigen::Matrix4d &_base2lidar); 157 | 158 | template 159 | std::vector linspace(T a, T b, size_t N) { 160 | T h = (b - a) / static_cast(N-1); 161 | std::vector xs(N); 162 | typename std::vector::iterator x; 163 | T val; 164 | for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h) 165 | *x = val; 166 | return xs; 167 | } 168 | 169 | sensor_msgs::ImagePtr cvmat2msg(const cv::Mat &_img); 170 | 171 | void pubRangeImg(cv::Mat& _rimg, sensor_msgs::ImagePtr& _msg, image_transport::Publisher& _publiser, std::pair _caxis); 172 | void publishPointcloud2FromPCLptr(const ros::Publisher& _scan_publisher, const pcl::PointCloud::Ptr _scan); 173 | sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame); 174 | 175 | template 176 | double ROS_TIME(T msg) 177 | { 178 | return msg->header.stamp.toSec(); 179 | } 180 | 181 | float pointDistance(PointType p); 182 | float pointDistance(PointType p1, PointType p2); 183 | 184 | #endif -------------------------------------------------------------------------------- /ltremovert/launch/removert_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Image1 8 | - /Image2 9 | - /Image3 10 | Splitter Ratio: 0.4148681163787842 11 | Tree Height: 308 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Image 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Autocompute Intensity Bounds: true 40 | Autocompute Value Bounds: 41 | Max Value: 10 42 | Min Value: -10 43 | Value: true 44 | Axis: Z 45 | Channel Name: intensity 46 | Class: rviz/PointCloud2 47 | Color: 239; 41; 41 48 | Color Transformer: FlatColor 49 | Decay Time: 0 50 | Enabled: true 51 | Invert Rainbow: false 52 | Max Color: 255; 255; 255 53 | Max Intensity: 0.6600000262260437 54 | Min Color: 0; 0; 0 55 | Min Intensity: 0 56 | Name: PointCloud2 57 | Position Transformer: XYZ 58 | Queue Size: 10 59 | Selectable: true 60 | Size (Pixels): 3 61 | Size (m): 0.5 62 | Style: Spheres 63 | Topic: /removert/scan_single_local_dynamic 64 | Unreliable: false 65 | Use Fixed Frame: true 66 | Use rainbow: true 67 | Value: true 68 | - Alpha: 1 69 | Autocompute Intensity Bounds: true 70 | Autocompute Value Bounds: 71 | Max Value: 10 72 | Min Value: -10 73 | Value: true 74 | Axis: Z 75 | Channel Name: intensity 76 | Class: rviz/PointCloud2 77 | Color: 138; 226; 52 78 | Color Transformer: FlatColor 79 | Decay Time: 0 80 | Enabled: true 81 | Invert Rainbow: false 82 | Max Color: 255; 255; 255 83 | Max Intensity: 0.9900000095367432 84 | Min Color: 0; 0; 0 85 | Min Intensity: 0 86 | Name: PointCloud2 87 | Position Transformer: XYZ 88 | Queue Size: 10 89 | Selectable: true 90 | Size (Pixels): 1 91 | Size (m): 0.10000000149011612 92 | Style: Points 93 | Topic: /removert/scan_single_local_static 94 | Unreliable: false 95 | Use Fixed Frame: true 96 | Use rainbow: true 97 | Value: true 98 | - Class: rviz/Image 99 | Enabled: true 100 | Image Topic: /scan_rimg_single 101 | Max Value: 1 102 | Median window: 5 103 | Min Value: 0 104 | Name: Image 105 | Normalize Range: true 106 | Queue Size: 2 107 | Transport Hint: raw 108 | Unreliable: false 109 | Value: true 110 | - Class: rviz/Image 111 | Enabled: true 112 | Image Topic: /map_rimg_single 113 | Max Value: 1 114 | Median window: 5 115 | Min Value: 0 116 | Name: Image 117 | Normalize Range: true 118 | Queue Size: 2 119 | Transport Hint: raw 120 | Unreliable: false 121 | Value: true 122 | - Class: rviz/Image 123 | Enabled: true 124 | Image Topic: /diff_rimg_single 125 | Max Value: 1 126 | Median window: 5 127 | Min Value: 0 128 | Name: Image 129 | Normalize Range: true 130 | Queue Size: 2 131 | Transport Hint: raw 132 | Unreliable: false 133 | Value: true 134 | Enabled: true 135 | Global Options: 136 | Background Color: 0; 0; 0 137 | Default Light: true 138 | Fixed Frame: removert 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/FocusCamera 147 | - Class: rviz/Measure 148 | - Class: rviz/SetInitialPose 149 | Theta std deviation: 0.2617993950843811 150 | Topic: /initialpose 151 | X std deviation: 0.5 152 | Y std deviation: 0.5 153 | - Class: rviz/SetGoal 154 | Topic: /move_base_simple/goal 155 | - Class: rviz/PublishPoint 156 | Single click: true 157 | Topic: /clicked_point 158 | Value: true 159 | Views: 160 | Current: 161 | Class: rviz/Orbit 162 | Distance: 101.88595581054688 163 | Enable Stereo Rendering: 164 | Stereo Eye Separation: 0.05999999865889549 165 | Stereo Focal Distance: 1 166 | Swap Stereo Eyes: false 167 | Value: false 168 | Focal Point: 169 | X: -0.6624842882156372 170 | Y: 3.6606950759887695 171 | Z: 0.8095508813858032 172 | Focal Shape Fixed Size: true 173 | Focal Shape Size: 0.05000000074505806 174 | Invert Z Axis: false 175 | Name: Current View 176 | Near Clip Distance: 0.009999999776482582 177 | Pitch: 0.9153987169265747 178 | Target Frame: 179 | Value: Orbit (rviz) 180 | Yaw: 4.498590469360352 181 | Saved: ~ 182 | Window Geometry: 183 | Displays: 184 | collapsed: false 185 | Height: 752 186 | Hide Left Dock: false 187 | Hide Right Dock: true 188 | Image: 189 | collapsed: false 190 | QMainWindow State: 000000ff00000000fd0000000400000000000001a1000001bffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bf000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000751000000d1fc0100000006fb0000000a0049006d0061006700650100000000000002610000005e00fffffffb0000000800540069006d006500000002b4000002eb000002eb00fffffffb0000000a0049006d0061006700650100000267000002800000005e00fffffffb0000000a0049006d00610067006501000004ed000002640000005e00fffffffb0000000a0049006d00610067006501000004ef000002620000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005aa000001bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 191 | Selection: 192 | collapsed: false 193 | Time: 194 | collapsed: false 195 | Tool Properties: 196 | collapsed: false 197 | Views: 198 | collapsed: true 199 | Width: 1873 200 | X: 1199 201 | Y: 694 202 | -------------------------------------------------------------------------------- /ltremovert/launch/run_ltmapper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ltremovert/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | removert 4 | 1.0.0 5 | Remove-then-revert-based static point cloud map construction 6 | 7 | 8 | Giseop Kim 9 | 10 | TODO 11 | 12 | 13 | Giseop Kim 14 | 15 | catkin 16 | 17 | roscpp 18 | roscpp 19 | rospy 20 | rospy 21 | 22 | tf 23 | tf 24 | 25 | cv_bridge 26 | cv_bridge 27 | 28 | pcl_conversions 29 | pcl_conversions 30 | 31 | std_msgs 32 | std_msgs 33 | sensor_msgs 34 | sensor_msgs 35 | geometry_msgs 36 | geometry_msgs 37 | nav_msgs 38 | nav_msgs 39 | 40 | message_generation 41 | message_generation 42 | message_runtime 43 | message_runtime 44 | 45 | image_transport 46 | image_transport 47 | 48 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /ltremovert/src/RosParamServer.cpp: -------------------------------------------------------------------------------- 1 | #include "removert/RosParamServer.h" 2 | 3 | 4 | RosParamServer::RosParamServer() 5 | : nh(nh_super) 6 | { 7 | nh.param("removert/isScanFileKITTIFormat", isScanFileKITTIFormat_, true); 8 | 9 | nh.param("removert/rimg_color_min", rimg_color_min_, 0.0); 10 | nh.param("removert/rimg_color_max", rimg_color_max_, 10.0); 11 | kRangeColorAxis = std::pair {rimg_color_min_, rimg_color_max_}; // meter 12 | kRangeColorAxisForDiff = std::pair{0.0, 0.5}; // meter 13 | 14 | // fov 15 | nh.param("removert/sequence_vfov", kVFOV, 50.0); 16 | nh.param("removert/sequence_hfov", kHFOV, 360.0); 17 | kFOV = std::pair(kVFOV, kHFOV); 18 | 19 | // resolution 20 | nh.param>("removert/remove_resolution_list", remove_resolution_list_, std::vector()); 21 | nh.param>("removert/revert_resolution_list", revert_resolution_list_, std::vector()); 22 | 23 | // knn 24 | nh.param("removert/num_nn_points_within", kNumKnnPointsToCompare, 3); // using higher, more strict static 25 | nh.param("removert/dist_nn_points_within", kScanKnnAndMapKnnAvgDiffThreshold, 0.1); // using smaller, more strict static 26 | 27 | // sequcne system info 28 | nh.param>("removert/ExtrinsicLiDARtoPoseBase", kVecExtrinsicLiDARtoPoseBase, std::vector()); 29 | kSE3MatExtrinsicLiDARtoPoseBase = Eigen::Map>(kVecExtrinsicLiDARtoPoseBase.data(), 4, 4); 30 | kSE3MatExtrinsicPoseBasetoLiDAR = kSE3MatExtrinsicLiDARtoPoseBase.inverse(); 31 | 32 | // config: point cloud pre-processing 33 | nh.param("removert/downsample_voxel_size", kDownsampleVoxelSize, 0.05); 34 | 35 | // sessions' paths 36 | nh.param("removert/central_sess_scan_dir", central_sess_scan_dir_, "/use/your/directory/having/*.bin"); 37 | nh.param("removert/central_sess_pose_path", central_sess_pose_path_, "/use/your/path/having/pose.txt"); 38 | nh.param("removert/query_sess_scan_dir", query_sess_scan_dir_, "/use/your/directory/having/*.bin"); 39 | nh.param("removert/query_sess_pose_path", query_sess_pose_path_, "/use/your/path/having/pose.txt"); 40 | 41 | 42 | // target scan index range (used in Removert.cpp) 43 | nh.param("removert/start_idx", start_idx_, 1); 44 | nh.param("removert/end_idx", end_idx_, 100); 45 | 46 | nh.param("removert/use_keyframe_gap", use_keyframe_gap_, true); 47 | nh.param("removert/use_keyframe_meter", use_keyframe_meter_, false); 48 | nh.param("removert/keyframe_gap", keyframe_gap_, 10); 49 | nh.param("removert/keyframe_meter", keyframe_gap_meter_, 2.0); 50 | 51 | nh.param("removert/repeat_removert_iter", repeat_removert_iter_, 1); 52 | 53 | // faster 54 | nh.param("removert/num_omp_cores", kNumOmpCores, 4); 55 | 56 | // save info 57 | nh.param("removert/saveMapPCD", kFlagSaveMapPointcloud, false); 58 | nh.param("removert/saveCleanScansPCD", kFlagSaveCleanScans, false); 59 | nh.param("removert/save_pcd_directory", save_pcd_directory_, "/"); 60 | 61 | 62 | usleep(100); 63 | } // ctor RosParamServer 64 | 65 | -------------------------------------------------------------------------------- /ltremovert/src/Session.cpp: -------------------------------------------------------------------------------- 1 | #include "removert/Session.h" 2 | 3 | using std::cout; 4 | using std::endl; 5 | 6 | namespace ltremovert 7 | { 8 | 9 | Session::Session( void ) 10 | { 11 | allocateMemory(); 12 | } 13 | 14 | 15 | void Session::allocateMemory(void) 16 | { 17 | // 18 | kdtree_map_global_curr_.reset(new pcl::KdTreeFLANN()); 19 | kdtree_target_map_global_.reset(new pcl::KdTreeFLANN()); 20 | kdtree_scan_global_curr_.reset(new pcl::KdTreeFLANN()); 21 | 22 | kdtree_map_global_nd_.reset(new pcl::KdTreeFLANN()); 23 | kdtree_map_global_pd_.reset(new pcl::KdTreeFLANN()); 24 | 25 | map_global_orig_.reset(new pcl::PointCloud()); 26 | map_global_curr_.reset(new pcl::PointCloud()); 27 | map_local_curr_.reset(new pcl::PointCloud()); 28 | 29 | map_global_updated_.reset(new pcl::PointCloud()); 30 | map_global_updated_strong_.reset(new pcl::PointCloud()); 31 | 32 | map_global_curr_static_.reset(new pcl::PointCloud()); 33 | map_global_curr_dynamic_.reset(new pcl::PointCloud()); 34 | map_global_curr_down_for_icp_.reset(new pcl::PointCloud()); 35 | map_global_curr_diff_positive_.reset(new pcl::PointCloud()); 36 | map_global_curr_diff_negative_.reset(new pcl::PointCloud()); 37 | 38 | map_global_nd_.reset(new pcl::PointCloud()); 39 | map_local_nd_.reset(new pcl::PointCloud()); 40 | map_global_nd_strong_.reset(new pcl::PointCloud()); 41 | map_global_nd_weak_.reset(new pcl::PointCloud()); 42 | 43 | map_global_pd_.reset(new pcl::PointCloud()); 44 | map_global_pd_orig_.reset(new pcl::PointCloud()); 45 | map_local_pd_.reset(new pcl::PointCloud()); 46 | map_global_pd_strong_.reset(new pcl::PointCloud()); 47 | map_global_pd_weak_.reset(new pcl::PointCloud()); 48 | 49 | // 50 | target_map_down_for_knn_.reset(new pcl::PointCloud()); 51 | 52 | map_global_orig_->clear(); 53 | map_global_curr_->clear(); 54 | map_local_curr_->clear(); 55 | map_global_curr_static_->clear(); 56 | map_global_curr_dynamic_->clear(); 57 | 58 | map_global_updated_->clear(); 59 | map_global_updated_strong_->clear(); 60 | 61 | map_global_curr_down_for_icp_->clear(); 62 | map_global_curr_diff_positive_->clear(); 63 | map_global_curr_diff_negative_->clear(); 64 | 65 | map_global_nd_->clear(); 66 | map_local_nd_->clear(); 67 | map_global_nd_strong_->clear(); 68 | map_global_nd_weak_->clear(); 69 | 70 | map_global_pd_->clear(); 71 | map_global_pd_orig_->clear(); 72 | map_local_pd_->clear(); 73 | map_global_pd_strong_->clear(); 74 | map_global_pd_weak_->clear(); 75 | 76 | target_map_down_for_knn_->clear(); 77 | } // allocateMemory 78 | 79 | 80 | void Session::loadSessionInfo( std::string _sess_type, std::string _scan_dir, std::string _pose_path ) 81 | { 82 | sess_type_ = _sess_type; 83 | scan_dir_ = _scan_dir; 84 | pose_path_ = _pose_path; 85 | 86 | // load scan paths 87 | for(auto& _entry : fs::directory_iterator(_scan_dir)) { 88 | scan_names_.emplace_back(_entry.path().filename()); 89 | scan_paths_.emplace_back(_entry.path()); 90 | } 91 | std::sort(scan_names_.begin(), scan_names_.end()); 92 | std::sort(scan_paths_.begin(), scan_paths_.end()); 93 | 94 | num_scans_ = scan_paths_.size(); 95 | ROS_INFO_STREAM("\033[1;32m Total : " << num_scans_ << " scans in the directory.\033[0m"); 96 | 97 | // debug 98 | // for(auto& _elm: scan_paths_) 99 | // cout << _elm << endl; 100 | 101 | // load the trajectory poses 102 | std::ifstream pose_file_handle (_pose_path); 103 | std::string strOneLine; 104 | while (getline(pose_file_handle, strOneLine)) { 105 | std::vector ith_pose_vec = splitPoseLine(strOneLine, ' '); // str to vec 106 | if(ith_pose_vec.size() == 12) { // append a row [0,0,0,1] 107 | ith_pose_vec.emplace_back(double(0.0)); ith_pose_vec.emplace_back(double(0.0)); ith_pose_vec.emplace_back(double(0.0)); ith_pose_vec.emplace_back(double(1.0)); 108 | } 109 | Eigen::Matrix4d ith_pose = Eigen::Map>(ith_pose_vec.data(), 4, 4); 110 | Eigen::Matrix4d ith_pose_inverse = ith_pose.inverse(); 111 | 112 | scan_poses_.emplace_back(ith_pose); 113 | scan_inverse_poses_.emplace_back(ith_pose_inverse); 114 | } 115 | 116 | // check the number of scans and the number of poses are equivalent 117 | assert(scan_paths_.size() == scan_poses_.size()); 118 | } // loadSessionInfo 119 | 120 | 121 | void Session::setDownsampleSize( float _voxel_size ) 122 | { 123 | kDownsampleVoxelSize = _voxel_size; 124 | } // setDownsampleSize 125 | 126 | 127 | void Session::clearKeyframes( void ) 128 | { 129 | int num_keyframes_ = 0; 130 | 131 | keyframe_names_.clear(); 132 | keyframe_paths_.clear(); 133 | keyframe_poses_.clear(); 134 | keyframe_inverse_poses_.clear(); 135 | } // clearKeyframes 136 | 137 | 138 | void Session::parseKeyframes( std::pair _range, int _gap ) 139 | { 140 | clearKeyframes(); 141 | 142 | auto start_idx = _range.first; 143 | auto end_idx = _range.second; 144 | 145 | int num_valid_parsed {0}; 146 | for(int curr_idx=0; curr_idx < int(scan_paths_.size()); curr_idx++) 147 | { 148 | // check the scan idx within the target idx range 149 | if(curr_idx > end_idx || curr_idx < start_idx) { 150 | curr_idx++; 151 | continue; 152 | } 153 | 154 | if( remainder(num_valid_parsed, _gap) != 0 ) { 155 | num_valid_parsed++; 156 | continue; 157 | } 158 | 159 | // save the info (reading scan bin is in makeGlobalMap) 160 | keyframe_paths_.emplace_back(scan_paths_.at(curr_idx)); 161 | keyframe_names_.emplace_back(scan_names_.at(curr_idx)); 162 | 163 | keyframe_poses_.emplace_back(scan_poses_.at(curr_idx)); // used for local2global 164 | keyframe_inverse_poses_.emplace_back(scan_inverse_poses_.at(curr_idx)); // used for global2local 165 | 166 | // 167 | num_valid_parsed++; 168 | } 169 | 170 | ROS_INFO_STREAM("\033[1;32m Total " << keyframe_paths_.size() 171 | << " nodes are used from the index range [" << start_idx << ", " << end_idx << "]" 172 | << " (every " << _gap << " frames parsed)\033[0m"); 173 | 174 | } // parseKeyframes 175 | 176 | 177 | void Session::parseKeyframes( int _gap ) 178 | { 179 | clearKeyframes(); 180 | 181 | std::pair _range { 0, int(scan_paths_.size()) }; 182 | parseKeyframes(_range, _gap); 183 | } // parseKeyframes for all range 184 | 185 | 186 | void Session::mergeScansWithinGlobalCoord(void) 187 | { 188 | // NOTE: _scans must be in local coord 189 | for (std::size_t scan_idx = 0; scan_idx < keyframe_scans_.size(); scan_idx++) 190 | { 191 | auto ii_scan = keyframe_scans_.at(scan_idx); // pcl::PointCloud::Ptr 192 | auto ii_pose = keyframe_poses_.at(scan_idx); // Eigen::Matrix4d 193 | 194 | // local to global (local2global) 195 | pcl::PointCloud::Ptr scan_global_coord(new pcl::PointCloud()); 196 | pcl::transformPointCloud(*ii_scan, *scan_global_coord, kSE3MatExtrinsicLiDARtoPoseBase); 197 | pcl::transformPointCloud(*scan_global_coord, *scan_global_coord, ii_pose); 198 | 199 | // merge the scan into the global map 200 | *map_global_orig_ += *scan_global_coord; 201 | } 202 | } // mergeScansWithinGlobalCoord 203 | 204 | 205 | double xyzDist (const Eigen::Matrix4d& _pose1, const Eigen::Matrix4d& _pose2) 206 | { 207 | // cout << _pose1(0,0) << ", " << _pose1(0,1) << ", " << _pose1(0,2) << ", " << _pose1(0,3) << endl; 208 | // cout << _pose1(1,0) << ", " << _pose1(1,1) << ", " << _pose1(1,2) << ", " << _pose1(1,3) << endl; 209 | // cout << _pose1(2,0) << ", " << _pose1(2,1) << ", " << _pose1(2,2) << ", " << _pose1(2,3) << endl; 210 | // cout << _pose1(3,0) << ", " << _pose1(3,1) << ", " << _pose1(3,2) << ", " << _pose1(3,3) << endl; 211 | 212 | return std::sqrt( (_pose1(0,3)-_pose2(0,3))*(_pose1(0,3)-_pose2(0,3)) 213 | + (_pose1(1,3)-_pose2(1,3))*(_pose1(1,3)-_pose2(1,3)) 214 | + (_pose1(2,3)-_pose2(2,3))*(_pose1(2,3)-_pose2(2,3)) ); 215 | } // xyzDist 216 | 217 | double nnDist( const Eigen::Matrix4d& _pose, const std::vector& _roi_poses) 218 | { 219 | double nn_dist = 10000000000.0; 220 | for (auto _roi_pose: _roi_poses) { 221 | double dist = xyzDist(_pose, _roi_pose); 222 | if( dist < nn_dist ) 223 | nn_dist = dist; 224 | } 225 | 226 | return nn_dist; 227 | } // nnDist 228 | 229 | 230 | void Session::parseKeyframesInROI( const std::vector& _roi_poses, int _gap ) 231 | { 232 | clearKeyframes(); 233 | 234 | double inplace_thres = 10.0; // e.g., 10, 30 meter, user param 235 | 236 | int num_valid_parsed {0}; 237 | for(int curr_idx=0; curr_idx < int(scan_paths_.size()); curr_idx++) 238 | { 239 | auto curr_pose = scan_poses_.at(curr_idx); 240 | double dist = nnDist(curr_pose, _roi_poses); 241 | if(dist > inplace_thres) 242 | continue; 243 | 244 | if( remainder(num_valid_parsed, _gap) != 0 ) { 245 | num_valid_parsed++; 246 | continue; 247 | } 248 | 249 | // save the info (reading scan bin is in makeGlobalMap) 250 | keyframe_paths_.emplace_back(scan_paths_.at(curr_idx)); 251 | keyframe_names_.emplace_back(scan_names_.at(curr_idx)); 252 | 253 | keyframe_poses_.emplace_back(scan_poses_.at(curr_idx)); // used for local2global 254 | keyframe_inverse_poses_.emplace_back(scan_inverse_poses_.at(curr_idx)); // used for global2local 255 | 256 | // 257 | num_valid_parsed++; 258 | } 259 | 260 | ROS_INFO_STREAM("\033[1;32m Total " << keyframe_paths_.size() 261 | << " keyframes parsed in the map's ROI\033[0m"); 262 | 263 | } // parseKeyframesInROI 264 | 265 | 266 | void Session::loadKeyframes( void ) 267 | { 268 | const int cout_interval {10}; 269 | int cout_counter {0}; 270 | cout << endl; 271 | cout << " ... (display every " << cout_interval << " readings) ..." << endl; 272 | for(auto& _scan_path : keyframe_paths_) { 273 | // read bin files and save 274 | pcl::PointCloud::Ptr points (new pcl::PointCloud); // pcl::PointCloud Ptr is a shared ptr so this points will be automatically destroyed after this function block (because no others ref it). 275 | pcl::io::loadPCDFile (_scan_path, *points); // saved from SC-LIO-SAM's pcd binary (.pcd) 276 | // Deprecated: PLEASE use the binary pcd format scans saved by using SC-LIO-SAM's save utility 277 | // if( isScanFileKITTIFormat_ ) { 278 | // readBin(_scan_path, points); // For KITTI (.bin) 279 | // } else { 280 | // pcl::io::loadPCDFile (_scan_path, *points); // saved from SC-LIO-SAM's pcd binary (.pcd) 281 | // } 282 | 283 | // pcdown 284 | pcl::VoxelGrid downsize_filter; 285 | downsize_filter.setLeafSize(kDownsampleVoxelSize, kDownsampleVoxelSize, kDownsampleVoxelSize); 286 | downsize_filter.setInputCloud(points); 287 | 288 | pcl::PointCloud::Ptr downsampled_points (new pcl::PointCloud); 289 | downsize_filter.filter(*downsampled_points); 290 | 291 | // save downsampled pointcloud 292 | keyframe_scans_.emplace_back(downsampled_points); 293 | 294 | // cout for debug 295 | cout_counter++; 296 | if (remainder(cout_counter, cout_interval) == 0) { 297 | cout << _scan_path << endl; 298 | cout << "Read a pointcloud with " << points->points.size() << " points (" 299 | << "downsampled size: " << downsampled_points->points.size() << " points)" << endl; 300 | } 301 | } 302 | } // loadKeyframes 303 | 304 | 305 | void Session::parseStaticScansViaProjection(void) 306 | { 307 | parseScansViaProjection(map_global_curr_, keyframe_scans_static_projected_); 308 | 309 | // keyframe_scans_static_projected_.clear(); 310 | // for (std::size_t idx_scan = 0; idx_scan < keyframe_scans_.size(); idx_scan++) { 311 | // Eigen::Matrix4d base_pose_inverse = keyframe_inverse_poses_.at(idx_scan); 312 | // transformGlobalMapToLocal(map_global_curr_, base_pose_inverse, kSE3MatExtrinsicPoseBasetoLiDAR, map_local_curr_); 313 | // auto this_scan_static = parseProjectedPoints(map_local_curr_, kFOV, resetRimgSize(kFOV, kReprojectionAlpha)); // the most time comsuming part 2 -> so openMP applied inside 314 | // keyframe_scans_static_projected_.emplace_back(this_scan_static); 315 | // } 316 | } // parseStaticScansViaProjection 317 | 318 | void Session::parseUpdatedStaticScansViaProjection(void) 319 | { 320 | parseScansViaProjection(map_global_updated_, keyframe_scans_updated_); 321 | } 322 | 323 | void Session::parseUpdatedStrongStaticScansViaProjection(void) 324 | { 325 | parseScansViaProjection(map_global_updated_strong_, keyframe_scans_updated_strong_); 326 | } 327 | 328 | void Session::parsePDScansViaProjection(void) 329 | { 330 | parseScansViaProjection(map_global_pd_orig_, keyframe_scans_pd_); 331 | } 332 | 333 | void Session::parseStrongPDScansViaProjection(void) 334 | { 335 | parseScansViaProjection(map_global_pd_strong_, keyframe_scans_strong_pd_); // ? 336 | } 337 | 338 | void Session::parseWeakNDScansViaProjection(void) 339 | { 340 | parseScansViaProjection(map_global_nd_weak_, keyframe_scans_weak_nd_); 341 | } 342 | 343 | void Session::parseStrongNDScansViaProjection(void) 344 | { 345 | parseScansViaProjection(map_global_nd_strong_, keyframe_scans_strong_nd_); 346 | } 347 | 348 | void Session::parseScansViaProjection( 349 | pcl::PointCloud::Ptr _map, // to_be_parsed 350 | std::vector::Ptr>& _vec_to_store ) 351 | { 352 | _vec_to_store.clear(); 353 | pcl::PointCloud::Ptr map_local(new pcl::PointCloud()); 354 | for (std::size_t idx_scan = 0; idx_scan < keyframe_scans_.size(); idx_scan++) { 355 | Eigen::Matrix4d base_pose_inverse = keyframe_inverse_poses_.at(idx_scan); 356 | transformGlobalMapToLocal(_map, base_pose_inverse, kSE3MatExtrinsicPoseBasetoLiDAR, map_local); 357 | auto this_scan_static = parseProjectedPoints(map_local, kFOV, resetRimgSize(kFOV, kReprojectionAlpha)); // the most time comsuming part 2 -> so openMP applied inside 358 | _vec_to_store.emplace_back(this_scan_static); 359 | } 360 | } // parseStaticScansViaProjection 361 | 362 | void Session::updateScansScanwise() 363 | { 364 | for(std::size_t ii = 0; ii < keyframe_scans_updated_.size(); ii++) 365 | { 366 | // init 367 | pcl::PointCloud::Ptr final_updated_scan(new pcl::PointCloud()); 368 | *final_updated_scan = *keyframe_scans_updated_.at(ii); 369 | 370 | // append 371 | *final_updated_scan += *keyframe_scans_weak_nd_.at(ii); 372 | *final_updated_scan += *keyframe_scans_pd_.at(ii); 373 | 374 | // downsampling (to remove repeated points) 375 | octreeDownsampling(final_updated_scan, final_updated_scan, 0.05); 376 | 377 | // renewal 378 | *keyframe_scans_updated_[ii] = *final_updated_scan; 379 | } 380 | } // updateScansScanwise 381 | 382 | // void Session::parseDynamicScansViaProjection(void) 383 | // { 384 | // scans_dynamic_projected_.clear(); 385 | // for (std::size_t idx_scan = 0; idx_scan < keyframe_scans_.size(); idx_scan++) { 386 | // Eigen::Matrix4d base_pose_inverse = keyframe_inverse_poses_.at(idx_scan); 387 | // transformGlobalMapToLocal(map_global_curr_, base_pose_inverse, kSE3MatExtrinsicPoseBasetoLiDAR, map_local_curr_); 388 | // auto this_scan_static = parseProjectedPoints(map_local_curr_, kFOV, resetRimgSize(kFOV, kReprojectionAlpha)); // the most time comsuming part 2 -> so openMP applied inside 389 | // scans_dynamic_projected_.emplace_back(this_scan_static); 390 | // } 391 | // } // parseDynamicScansViaProjection 392 | 393 | void Session::extractLowDynPointsViaKnnDiff(const pcl::PointCloud::Ptr _target_map) 394 | { 395 | float icpVoxelSize = 0.4; // meter 396 | icp_for_knn_.setMaxCorrespondenceDistance(150); // giseop , use a value can cover 2*historyKeyframeSearchNum range in meter 397 | icp_for_knn_.setMaximumIterations(100); 398 | icp_for_knn_.setTransformationEpsilon(1e-6); 399 | icp_for_knn_.setEuclideanFitnessEpsilon(1e-6); 400 | icp_for_knn_.setRANSACIterations(0); 401 | octreeDownsampling(_target_map, target_map_down_for_knn_, icpVoxelSize); 402 | icp_for_knn_.setInputTarget(target_map_down_for_knn_); 403 | 404 | kdtree_target_map_global_->setInputCloud(_target_map); 405 | 406 | std::map::Ptr> scans_knn_coexist_map; 407 | std::map::Ptr> scans_knn_diff_map; 408 | #pragma omp parallel for num_threads(kNumOmpCores) // if using icp refinement (in current version, not recommended), do not use the this omp 409 | for (std::size_t idx_scan = 0; idx_scan < keyframe_scans_static_projected_.size(); idx_scan++) { 410 | auto [this_scan_coexist, this_scan_diff] = partitionLowDynamicPointsOfScanByKnn(idx_scan); 411 | mtx.lock(); 412 | scans_knn_coexist_map[int(idx_scan)] = this_scan_coexist; 413 | scans_knn_diff_map[int(idx_scan)] = this_scan_diff; 414 | mtx.unlock(); 415 | } 416 | 417 | scans_knn_coexist_.clear(); 418 | scans_knn_diff_.clear(); 419 | scans_knn_coexist_.reserve(keyframe_scans_static_projected_.size()); 420 | scans_knn_diff_.reserve(keyframe_scans_static_projected_.size()); 421 | 422 | for(auto& _elm: scans_knn_coexist_map) 423 | scans_knn_coexist_.emplace_back(_elm.second); 424 | 425 | for(auto& _elm: scans_knn_diff_map) 426 | scans_knn_diff_.emplace_back(_elm.second); 427 | } 428 | 429 | 430 | void Session::constructGlobalNDMap() 431 | { 432 | auto map_global_nd = mergeScansWithinGlobalCoordUtil(scans_knn_diff_, keyframe_poses_, kSE3MatExtrinsicLiDARtoPoseBase); 433 | *map_global_nd_ = *map_global_nd; 434 | octreeDownsampling(map_global_nd_, map_global_nd_, 0.05); 435 | } 436 | 437 | void Session::constructGlobalPDMap() 438 | { 439 | auto map_global_pd = mergeScansWithinGlobalCoordUtil(scans_knn_diff_, keyframe_poses_, kSE3MatExtrinsicLiDARtoPoseBase); 440 | *map_global_pd_ = *map_global_pd; 441 | octreeDownsampling(map_global_pd_, map_global_pd_, 0.05); 442 | 443 | // backup (for save) 444 | *map_global_pd_orig_ = *map_global_pd_; 445 | } 446 | 447 | void Session::revertStrongPDMapPointsHavingWeakPDInNear() 448 | { 449 | // TODO 450 | } 451 | 452 | void Session::removeWeakNDMapPointsHavingStrongNDInNear() 453 | { 454 | if( map_global_nd_strong_->points.size() == 0 ) 455 | return; 456 | 457 | kdtree_map_global_nd_->setInputCloud(map_global_nd_strong_); 458 | 459 | int num_points_of_weak_nd = map_global_nd_weak_->points.size(); 460 | pcl::PointCloud::Ptr points_added_to_strong(new pcl::PointCloud); 461 | pcl::PointCloud::Ptr points_to_be_new_weak(new pcl::PointCloud); 462 | for (std::size_t pt_idx = 0; pt_idx < num_points_of_weak_nd; pt_idx++) 463 | { 464 | std::vector topk_indexes_map; 465 | std::vector topk_L2dists_map; 466 | 467 | // hard coding for fast test 468 | int kNumKnnPointsToCompare = 2; 469 | float kScanKnnAndMapKnnAvgDiffThreshold = 1.0; // meter, should be larger than the knn diff's threshold 470 | 471 | kdtree_map_global_nd_->nearestKSearch(map_global_nd_weak_->points[pt_idx], kNumKnnPointsToCompare, topk_indexes_map, topk_L2dists_map); 472 | float sum_topknn_dists_in_map = accumulate(topk_L2dists_map.begin(), topk_L2dists_map.end(), 0.0); 473 | float avg_topknn_dists_in_map = sum_topknn_dists_in_map / float(kNumKnnPointsToCompare); 474 | 475 | if (std::abs(avg_topknn_dists_in_map) < kScanKnnAndMapKnnAvgDiffThreshold) // if this weak point have strong friend 476 | points_added_to_strong->push_back(map_global_nd_weak_->points[pt_idx]); 477 | else 478 | points_to_be_new_weak->push_back(map_global_nd_weak_->points[pt_idx]); 479 | } 480 | 481 | // update 482 | *map_global_nd_strong_ += *points_added_to_strong; // note that not = but += (append) 483 | *map_global_nd_weak_ = *points_to_be_new_weak; // note that not += but = (remake) 484 | } 485 | 486 | 487 | void Session::extractHighDynPointsViaKnnDiff(const pcl::PointCloud::Ptr _target_map) 488 | { 489 | kdtree_target_map_global_->setInputCloud(_target_map); 490 | std::map::Ptr> keyframe_scans_dynamic_map; 491 | #pragma omp parallel for num_threads(kNumOmpCores) 492 | for (std::size_t idx_scan = 0; idx_scan < keyframe_scans_.size(); idx_scan++) { 493 | auto [unused, this_scan_diff] = partitionHighDynamicPointsOfScanByKnn(idx_scan); 494 | mtx.lock(); 495 | keyframe_scans_dynamic_map[int(idx_scan)] = this_scan_diff; 496 | mtx.unlock(); 497 | } 498 | 499 | keyframe_scans_dynamic_.clear(); 500 | keyframe_scans_dynamic_.reserve(keyframe_scans_.size()); 501 | 502 | for(auto& _elm: keyframe_scans_dynamic_map) 503 | keyframe_scans_dynamic_.emplace_back(_elm.second); 504 | } 505 | 506 | void Session::precleaningKeyframes ( float _radius ) 507 | { 508 | for (std::size_t idx_kf = 0; idx_kf < keyframe_scans_.size(); idx_kf++) 509 | { 510 | auto scan = keyframe_scans_.at(idx_kf); // pcl::PointCloud::Ptr 511 | 512 | int num_points = scan->points.size(); 513 | pcl::PointCloud::Ptr scan_cleaned(new pcl::PointCloud()); 514 | for (int pt_idx = 0; pt_idx < num_points; ++pt_idx) 515 | { 516 | 517 | PointType this_point = scan->points[pt_idx]; 518 | SphericalPoint sph_point = cart2sph(this_point); 519 | float this_point_range = sph_point.r; 520 | float this_point_z = this_point.z; 521 | 522 | if( this_point_range < _radius 523 | & this_point_z < 0.5 524 | & -0.5 < this_point_z ) { 525 | continue; 526 | } 527 | 528 | scan_cleaned->push_back(this_point); 529 | } 530 | 531 | *scan = *scan_cleaned; 532 | } 533 | } // precleaningKeyframes 534 | 535 | 536 | // note: using pcPtr = pcl::PointCloud::Ptr; // in utility.h 537 | std::pair Session::partitionLowDynamicPointsOfScanByKnn(int _scan_idx) 538 | { 539 | // curr scan (in local coord) 540 | // pcl::PointCloud::Ptr scan = keyframe_scans_static_projected_.at(_scan_idx); 541 | auto scan = keyframe_scans_static_projected_.at(_scan_idx); 542 | auto scan_pose = keyframe_poses_.at(_scan_idx); 543 | 544 | // curr scan (in global coord) 545 | pcl::PointCloud::Ptr scan_global = local2global(scan, scan_pose, kSE3MatExtrinsicPoseBasetoLiDAR); 546 | // kdtree_scan_global_curr_->setInputCloud(scan_global); 547 | int num_points_of_a_scan = scan_global->points.size(); 548 | 549 | pcl::PointCloud::Ptr scan_eff_for_knn_comp(new pcl::PointCloud()); 550 | pcl::PointCloud::Ptr scan_eff_to_parse_in_cetral_frame(new pcl::PointCloud()); 551 | bool useICPrefinement { false }; // TODO: need to be tested more ... in current version not using it is better. 552 | if(useICPrefinement) { 553 | mtx.lock(); 554 | cout << "Knn diff of " << keyframe_names_.at(_scan_idx) << " of " << sess_type_ << " (with ICP refinement) ..." << endl; 555 | mtx.unlock(); 556 | 557 | pcl::PointCloud::Ptr scan_global_down(new pcl::PointCloud()); 558 | float icpVoxelSize = 0.4; // meter 559 | octreeDownsampling(scan_global, scan_global_down, icpVoxelSize); 560 | icp_for_knn_.setInputSource(scan_global_down); 561 | pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); 562 | icp_for_knn_.align(*unused_result); 563 | 564 | pcl::PointCloud::Ptr scan_global_reg(new pcl::PointCloud()); 565 | pcl::transformPointCloud(*scan_global, *scan_global_reg, icp_for_knn_.getFinalTransformation()); 566 | 567 | // pcl::io::savePCDFileBinary("/home/user/Desktop/data/rss21-ltmapper/mulran/dcc/01/removert/0102refactor/map.pcd", *target_map_down_for_knn_); 568 | // pcl::io::savePCDFileBinary("/home/user/Desktop/data/rss21-ltmapper/mulran/dcc/01/removert/0102refactor/scan.pcd", *scan_global); 569 | 570 | cout << icp_for_knn_.getFitnessScore() << endl; 571 | if (icp_for_knn_.getFitnessScore() < 1.0) { 572 | *scan_eff_for_knn_comp = *scan_global_reg; 573 | *scan_eff_to_parse_in_cetral_frame = *scan_global_reg; 574 | } else { 575 | *scan_eff_for_knn_comp = *scan_global; 576 | *scan_eff_to_parse_in_cetral_frame = *scan_global; 577 | } 578 | } else { 579 | mtx.lock(); 580 | cout << "Knn diff of " << keyframe_names_.at(_scan_idx) << " of " << sess_type_ << " (without ICP refinement) ..." << endl; 581 | mtx.unlock(); 582 | *scan_eff_for_knn_comp = *scan_global; 583 | *scan_eff_to_parse_in_cetral_frame = *scan_global; 584 | } 585 | 586 | pcl::PointCloud::Ptr scan_knn_coexist(new pcl::PointCloud); 587 | pcl::PointCloud::Ptr scan_knn_diff(new pcl::PointCloud); 588 | for (std::size_t pt_idx = 0; pt_idx < num_points_of_a_scan; pt_idx++) 589 | { 590 | std::vector topk_indexes_map; 591 | std::vector topk_L2dists_map; 592 | kdtree_target_map_global_->nearestKSearch(scan_eff_for_knn_comp->points[pt_idx], kNumKnnPointsToCompare, topk_indexes_map, topk_L2dists_map); 593 | float sum_topknn_dists_in_map = accumulate(topk_L2dists_map.begin(), topk_L2dists_map.end(), 0.0); 594 | float avg_topknn_dists_in_map = sum_topknn_dists_in_map / float(kNumKnnPointsToCompare); 595 | 596 | if (std::abs(avg_topknn_dists_in_map) < kScanKnnAndMapKnnAvgDiffThreshold) 597 | scan_knn_coexist->push_back(scan_eff_to_parse_in_cetral_frame->points[pt_idx]); 598 | else 599 | scan_knn_diff->push_back(scan_eff_to_parse_in_cetral_frame->points[pt_idx]); 600 | } 601 | 602 | // again global2local because later in the merging global map function, which requires scans within each local coord. 603 | pcl::PointCloud::Ptr scan_knn_coexist_local = global2local(scan_knn_coexist, keyframe_inverse_poses_.at(_scan_idx), kSE3MatExtrinsicPoseBasetoLiDAR); 604 | pcl::PointCloud::Ptr scan_knn_diff_local = global2local(scan_knn_diff, keyframe_inverse_poses_.at(_scan_idx), kSE3MatExtrinsicPoseBasetoLiDAR); 605 | 606 | return std::pair::Ptr, pcl::PointCloud::Ptr>(scan_knn_coexist_local, scan_knn_diff_local); 607 | } // partitionLowDynamicPointsOfScanByKnn 608 | 609 | 610 | std::pair Session::partitionHighDynamicPointsOfScanByKnn(int _scan_idx) 611 | { 612 | // curr scan (in local coord) 613 | // pcl::PointCloud::Ptr scan = keyframe_scans_static_projected_.at(_scan_idx); 614 | auto scan = keyframe_scans_.at(_scan_idx); 615 | auto scan_pose = keyframe_poses_.at(_scan_idx); 616 | 617 | // curr scan (in global coord) 618 | pcl::PointCloud::Ptr scan_global = local2global(scan, scan_pose, kSE3MatExtrinsicPoseBasetoLiDAR); 619 | // kdtree_scan_global_curr_->setInputCloud(scan_global); 620 | int num_points_of_a_scan = scan_global->points.size(); 621 | pcl::PointCloud::Ptr scan_knn_coexist(new pcl::PointCloud); 622 | pcl::PointCloud::Ptr scan_knn_diff(new pcl::PointCloud); 623 | for (std::size_t pt_idx = 0; pt_idx < num_points_of_a_scan; pt_idx++) 624 | { 625 | std::vector topk_indexes_map; 626 | std::vector topk_L2dists_map; 627 | kdtree_target_map_global_->nearestKSearch(scan_global->points[pt_idx], kNumKnnPointsToCompare, topk_indexes_map, topk_L2dists_map); 628 | float sum_topknn_dists_in_map = accumulate(topk_L2dists_map.begin(), topk_L2dists_map.end(), 0.0); 629 | float avg_topknn_dists_in_map = sum_topknn_dists_in_map / float(kNumKnnPointsToCompare); 630 | 631 | if (std::abs(avg_topknn_dists_in_map) < kScanKnnAndMapKnnAvgDiffThreshold) 632 | scan_knn_coexist->push_back(scan_global->points[pt_idx]); 633 | else 634 | scan_knn_diff->push_back(scan_global->points[pt_idx]); 635 | } 636 | 637 | // again global2local because later in the merging global map function, which requires scans within each local coord. 638 | pcl::PointCloud::Ptr scan_knn_coexist_local = global2local(scan_knn_coexist, keyframe_inverse_poses_.at(_scan_idx), kSE3MatExtrinsicPoseBasetoLiDAR); 639 | pcl::PointCloud::Ptr scan_knn_diff_local = global2local(scan_knn_diff, keyframe_inverse_poses_.at(_scan_idx), kSE3MatExtrinsicPoseBasetoLiDAR); 640 | 641 | return std::pair(scan_knn_coexist_local, scan_knn_diff_local); 642 | } // partitionHighDynamicPointsOfScanByKnn 643 | 644 | 645 | 646 | 647 | } // namespace ltremovert -------------------------------------------------------------------------------- /ltremovert/src/removert_main.cpp: -------------------------------------------------------------------------------- 1 | #include "removert/Removerter.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "removert"); 6 | ROS_INFO("\033[1;32m----> Removert Main Started.\033[0m"); 7 | 8 | ltremovert::Removerter RMV; 9 | RMV.run(); 10 | 11 | ros::spin(); 12 | 13 | return 0; 14 | } -------------------------------------------------------------------------------- /ltremovert/src/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "removert/utility.h" 2 | 3 | bool cout_debug = false; 4 | 5 | 6 | void readBin(std::string _bin_path, pcl::PointCloud::Ptr _pcd_ptr) 7 | { 8 | std::fstream input(_bin_path.c_str(), ios::in | ios::binary); 9 | if(!input.good()){ 10 | cerr << "Could not read file: " << _bin_path << endl; 11 | exit(EXIT_FAILURE); 12 | } 13 | input.seekg(0, ios::beg); 14 | 15 | for (int ii=0; input.good() && !input.eof(); ii++) { 16 | PointType point; 17 | 18 | input.read((char *) &point.x, sizeof(float)); 19 | input.read((char *) &point.y, sizeof(float)); 20 | input.read((char *) &point.z, sizeof(float)); 21 | input.read((char *) &point.intensity, sizeof(float)); 22 | 23 | _pcd_ptr->push_back(point); 24 | } 25 | input.close(); 26 | } 27 | 28 | std::vector splitPoseLine(std::string _str_line, char _delimiter) { 29 | std::vector parsed; 30 | std::stringstream ss(_str_line); 31 | std::string temp; 32 | while (getline(ss, temp, _delimiter)) { 33 | parsed.push_back(std::stod(temp)); // convert string to "double" 34 | } 35 | return parsed; 36 | } 37 | 38 | SphericalPoint cart2sph(const PointType & _cp) 39 | { // _cp means cartesian point 40 | 41 | if(cout_debug){ 42 | cout << "Cartesian Point [x, y, z]: [" << _cp.x << ", " << _cp.y << ", " << _cp.z << endl; 43 | } 44 | 45 | SphericalPoint sph_point { 46 | std::atan2(_cp.y, _cp.x), 47 | std::atan2(_cp.z, std::sqrt(_cp.x*_cp.x + _cp.y*_cp.y)), 48 | std::sqrt(_cp.x*_cp.x + _cp.y*_cp.y + _cp.z*_cp.z) 49 | }; 50 | return sph_point; 51 | } 52 | 53 | float rad2deg(float radians) 54 | { 55 | return radians * 180.0 / M_PI; 56 | } 57 | 58 | float deg2rad(float degrees) 59 | { 60 | return degrees * M_PI / 180.0; 61 | } 62 | 63 | 64 | void transformGlobalMapToLocal( 65 | const pcl::PointCloud::Ptr &_map_global, 66 | Eigen::Matrix4d _base_pose_inverse, Eigen::Matrix4d _base2lidar, pcl::PointCloud::Ptr &_map_local) 67 | { 68 | // global to local (global2local) 69 | _map_local->clear(); 70 | pcl::transformPointCloud(*_map_global, *_map_local, _base_pose_inverse); 71 | pcl::transformPointCloud(*_map_local, *_map_local, _base2lidar); // _base2lidar == kSE3MatExtrinsicPoseBasetoLiDAR 72 | } // transformGlobalMapToLocal 73 | 74 | pcl::PointCloud::Ptr parseProjectedPoints(const pcl::PointCloud::Ptr &_scan, 75 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 76 | const std::pair _rimg_size) 77 | { 78 | auto [map_rimg, map_rimg_ptidx] = map2RangeImg(_scan, _fov, _rimg_size); // the most time comsuming part 2 -> so openMP applied inside 79 | pcl::PointCloud::Ptr scan_projected(new pcl::PointCloud); 80 | for (int row_idx = 0; row_idx < map_rimg_ptidx.rows; ++row_idx) { 81 | for (int col_idx = 0; col_idx < map_rimg_ptidx.cols; ++col_idx) { 82 | if(map_rimg_ptidx.at(row_idx, col_idx) == 0) // 0 means no point (see map2RangeImg) 83 | continue; 84 | 85 | scan_projected->push_back(_scan->points[map_rimg_ptidx.at(row_idx, col_idx)]); 86 | } 87 | } 88 | return scan_projected; 89 | } // map2RangeImg 90 | 91 | 92 | std::pair map2RangeImg(const pcl::PointCloud::Ptr &_scan, 93 | const std::pair _fov, /* e.g., [vfov = 50 (upper 25, lower 25), hfov = 360] */ 94 | const std::pair _rimg_size) 95 | { 96 | const float kVFOV = _fov.first; 97 | const float kHFOV = _fov.second; 98 | 99 | const int kNumRimgRow = _rimg_size.first; 100 | const int kNumRimgCol = _rimg_size.second; 101 | 102 | // @ range image initizliation 103 | cv::Mat rimg = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32FC1, cv::Scalar::all(kFlagNoPOINT)); // float matrix, save range value 104 | cv::Mat rimg_ptidx = cv::Mat(kNumRimgRow, kNumRimgCol, CV_32SC1, cv::Scalar::all(0)); // int matrix, save point (of global map) index 105 | 106 | // @ points to range img 107 | int num_points = _scan->points.size(); 108 | 109 | const int kNumOmpCores = 16; // hard coding for fast dev 110 | #pragma omp parallel for num_threads(kNumOmpCores) 111 | for (int pt_idx = 0; pt_idx < num_points; ++pt_idx) 112 | { 113 | PointType this_point = _scan->points[pt_idx]; 114 | SphericalPoint sph_point = cart2sph(this_point); 115 | 116 | // @ note about vfov: e.g., (+ V_FOV/2) to adjust [-15, 15] to [0, 30] 117 | // @ min and max is just for the easier (naive) boundary checks. 118 | int lower_bound_row_idx{0}; 119 | int lower_bound_col_idx{0}; 120 | int upper_bound_row_idx{kNumRimgRow - 1}; 121 | int upper_bound_col_idx{kNumRimgCol - 1}; 122 | int pixel_idx_row = int(std::min(std::max(std::round(kNumRimgRow * (1 - (rad2deg(sph_point.el) + (kVFOV / float(2.0))) / (kVFOV - float(0.0)))), float(lower_bound_row_idx)), float(upper_bound_row_idx))); 123 | int pixel_idx_col = int(std::min(std::max(std::round(kNumRimgCol * ((rad2deg(sph_point.az) + (kHFOV / float(2.0))) / (kHFOV - float(0.0)))), float(lower_bound_col_idx)), float(upper_bound_col_idx))); 124 | 125 | float curr_range = sph_point.r; 126 | 127 | // @ Theoretically, this if-block would have race condition (i.e., this is a critical section), 128 | // @ But, the resulting range image is acceptable (watching via Rviz), 129 | // @ so I just naively applied omp pragma for this whole for-block (2020.10.28) 130 | // @ Reason: because this for loop is splited by the omp, points in a single splited for range do not race among them, 131 | // @ also, a point A and B lied in different for-segments do not tend to correspond to the same pixel, 132 | // # so we can assume practically there are few race conditions. 133 | // @ P.S. some explicit mutexing directive makes the code even slower ref: https://stackoverflow.com/questions/2396430/how-to-use-lock-in-openmp 134 | if (curr_range < rimg.at(pixel_idx_row, pixel_idx_col)) 135 | { 136 | rimg.at(pixel_idx_row, pixel_idx_col) = curr_range; 137 | rimg_ptidx.at(pixel_idx_row, pixel_idx_col) = pt_idx; 138 | } 139 | } 140 | 141 | return std::pair(rimg, rimg_ptidx); 142 | } // map2RangeImg 143 | 144 | // void octreeDownsampling(const pcl::PointCloud::Ptr &_src, pcl::PointCloud::Ptr &_to_save) 145 | // { 146 | // pcl::octree::OctreePointCloudVoxelCentroid octree(kDownsampleVoxelSize); 147 | // octree.setInputCloud(_src); 148 | // octree.defineBoundingBox(); 149 | // octree.addPointsFromInputCloud(); 150 | // pcl::octree::OctreePointCloudVoxelCentroid::AlignedPointTVector centroids; 151 | // octree.getVoxelCentroids(centroids); 152 | 153 | // // init current map with the downsampled full cloud 154 | // _to_save->points.assign(centroids.begin(), centroids.end()); 155 | // _to_save->width = 1; 156 | // _to_save->height = _to_save->points.size(); // make sure again the format of the downsampled point cloud 157 | // ROS_INFO_STREAM("\033[1;32m Octree Downsampled pointcloud have: " << _to_save->points.size() << " points.\033[0m"); 158 | // } // octreeDownsampling 159 | 160 | pcl::PointCloud::Ptr local2global(const pcl::PointCloud::Ptr &_scan_local, 161 | const Eigen::Matrix4d &_scan_pose, const Eigen::Matrix4d &_lidar2base) 162 | { 163 | pcl::PointCloud::Ptr scan_global(new pcl::PointCloud()); 164 | pcl::transformPointCloud(*_scan_local, *scan_global, _lidar2base); 165 | pcl::transformPointCloud(*scan_global, *scan_global, _scan_pose); 166 | 167 | return scan_global; 168 | } 169 | 170 | pcl::PointCloud::Ptr mergeScansWithinGlobalCoordUtil( 171 | const std::vector::Ptr> &_scans, 172 | const std::vector &_scans_poses, Eigen::Matrix4d _lidar2base) 173 | { 174 | pcl::PointCloud::Ptr ptcloud_merged(new pcl::PointCloud()); 175 | 176 | // NOTE: _scans must be in local coord 177 | for (std::size_t scan_idx = 0; scan_idx < _scans.size(); scan_idx++) 178 | { 179 | auto ii_scan = _scans.at(scan_idx); // pcl::PointCloud::Ptr 180 | auto ii_pose = _scans_poses.at(scan_idx); // Eigen::Matrix4d 181 | 182 | // local to global (local2global) 183 | pcl::PointCloud::Ptr scan_global_coord(new pcl::PointCloud()); 184 | pcl::transformPointCloud(*ii_scan, *scan_global_coord, _lidar2base); // kSE3MatExtrinsicLiDARtoPoseBase 185 | pcl::transformPointCloud(*scan_global_coord, *scan_global_coord, ii_pose); 186 | 187 | // merge the scan into the global map 188 | *ptcloud_merged += *scan_global_coord; 189 | } 190 | 191 | return ptcloud_merged; 192 | } // mergeScansWithinGlobalCoord 193 | 194 | pcl::PointCloud::Ptr global2local(const pcl::PointCloud::Ptr &_scan_global, 195 | const Eigen::Matrix4d &_scan_pose_inverse, const Eigen::Matrix4d &_base2lidar) 196 | { 197 | pcl::PointCloud::Ptr scan_local(new pcl::PointCloud()); 198 | pcl::transformPointCloud(*_scan_global, *scan_local, _scan_pose_inverse); 199 | pcl::transformPointCloud(*scan_local, *scan_local, _base2lidar); // kSE3MatExtrinsicPoseBasetoLiDAR 200 | 201 | return scan_local; 202 | } 203 | 204 | void octreeDownsampling(const pcl::PointCloud::Ptr &_src, 205 | pcl::PointCloud::Ptr &_to_save, 206 | const float _kDownsampleVoxelSize) 207 | { 208 | pcl::octree::OctreePointCloudVoxelCentroid octree(_kDownsampleVoxelSize); 209 | octree.setInputCloud(_src); 210 | octree.defineBoundingBox(); 211 | octree.addPointsFromInputCloud(); 212 | pcl::octree::OctreePointCloudVoxelCentroid::AlignedPointTVector centroids; 213 | octree.getVoxelCentroids(centroids); 214 | 215 | // init current map with the downsampled full cloud 216 | _to_save->points.assign(centroids.begin(), centroids.end()); 217 | _to_save->width = 1; 218 | _to_save->height = _to_save->points.size(); // make sure again the format of the downsampled point cloud 219 | } // octreeDownsampling 220 | 221 | 222 | std::pair resetRimgSize(const std::pair _fov, const float _resize_ratio) 223 | { 224 | // default is 1 deg x 1 deg 225 | float alpha_vfov = _resize_ratio; 226 | float alpha_hfov = _resize_ratio; 227 | 228 | float V_FOV = _fov.first; 229 | float H_FOV = _fov.second; 230 | 231 | int NUM_RANGE_IMG_ROW = std::round(V_FOV*alpha_vfov); 232 | int NUM_RANGE_IMG_COL = std::round(H_FOV*alpha_hfov); 233 | 234 | std::pair rimg {NUM_RANGE_IMG_ROW, NUM_RANGE_IMG_COL}; 235 | return rimg; 236 | } 237 | 238 | 239 | std::set convertIntVecToSet(const std::vector