# FASTLIO2_ROS2 adds loop closure and relocalization to a ROS2 port of FAST-LIO2 > This repository restructures FAST-LIO2 for ROS 2 and adds three nodes on top of it: a GTSAM pose graph loop closure node, a two stage ICP relocalizer, and consistent map optimization using BLAM for small scenes and HBA for large ones. The documented performance ceiling is the single threaded ROS 2 callback model. **liangheming/FASTLIO2_ROS2** — ROS2 / FAST_LIO / SAM / PGO / Online Re-Localization / Consistent Map with BA or HBA - Repository: https://github.com/liangheming/FASTLIO2_ROS2 - Stars: 772 · Forks: 100 - Language: C++ - License: not declared - Published: 2026-10-21 · Updated: 2026-10-21 · Language: en - Canonical page: https://hysenlabs.com/projects/liangheming-fastlio2-ros2 ## Four pieces of work, and only the first is the port The repository lists its main work in four items, and the first one is a rewrite rather than a new capability. The first is the restructuring of FAST_LIO, the original repository from the hku-mars group, to fit ROS 2. The second is a loop closure node that performs loop detection based on position priors plus ICP and then optimises a pose graph with GTSAM. The third is a relocalization node based on a coarse to fine two stage ICP. The fourth is consistent map optimization, built on BLAM for small scene maps and on HBA for large scene maps. So the positioning is clear: this is not a competitor to FAST-LIO2 itself, it is a ROS 2 distribution of it with the parts that FAST-LIO2 leaves out added alongside. That framing explains the acknowledgements, which thank FASTLIO2, BLAM and HBA, the three upstream projects the code is built on. The audience is a robotics developer with a Livox lidar who is working inside ROS 2 and needs more than a local odometry estimate. A single run of FAST-LIO2 gives you a locally consistent trajectory. Loop closure gives you a globally consistent map. Relocalization lets you place a new session into that map. Consistent map optimization fixes the residual deformation that both of those leave behind. The licence is not stated for the project, so redistribution terms are unclear, which is worth resolving before this code ends up in a product. ## The package layout mirrors the four capabilities The repository root contains five directories, and the naming tells you how the system is decomposed. The fastlio2/ directory holds the ported odometry. The pgo/ directory holds the loop closure node, which the README launches with a launch file named pgo_launch.py. The localizer/ directory holds the relocalization node, launched with localizer_launch.py. The hba/ directory holds the consistent map optimization node, launched with hba_launch.py. The interface/ directory holds the service definitions that the other packages use. That last one is the piece that makes the system composable. The services are named with an interface/srv prefix rather than a package specific one, so the map saving service, the relocalization service and the map refinement service are defined in one place and consumed from several nodes. The service types visible in the README are SaveMaps, Relocalize, IsValid and RefineMap. Separating the interface into its own package is the decision that makes the rest of the design work. The odometry node, the loop closure node and the map optimizer can be launched independently, in different combinations, and still agree on the shape of the data they exchange. In practice that means you can run the LIO node alone while collecting data, then add the pose graph node later on the same recorded bag, and use the optimizer afterwards on the saved maps, without a single node needing to know the whole pipeline. ## Loop closure is position priors plus ICP, then a GTSAM pose graph The loop closure node is described in one line, and the line contains two separate ideas that are easy to conflate. Loop detection is based on position priors plus ICP. The position prior is the cheap filter: a candidate is only considered if the current pose is close enough to a previously visited place to be plausible, which prunes the search space before any point cloud work happens. ICP then does the actual matching between the current local map and the stored submap, which is the expensive part and the part that produces a geometrically meaningful constraint rather than a guess. The second idea is the optimisation. Once a loop is detected, the constraint is fed into a pose graph solved with GTSAM, and the result is a globally consistent set of poses instead of a trajectory that has drifted. Running the node is two commands, one to start it and one to replay a bag: ```shell ros2 launch pgo pgo_launch.py ros2 bag play your_bag_file ``` Saving the resulting map goes through a service on the pgo package, and the request includes a flag for saving patches: ```shell ros2 service call /pgo/save_maps interface/srv/SaveMaps "{file_path: 'your_save_dir', save_patches: true}" ``` That flag is not decorative. The README states later that calling the optimization service requires the map to have been saved with save_patches set to true, so a map saved without it cannot be refined afterwards. A session where you forgot the flag produces a map you have to re-run the bag for. ## Relocalization takes a map and a pose guess, then validates the result The relocalization node is based on a coarse to fine two stage ICP, which is the standard answer to the problem of matching a live scan into a stored map where the initial pose is only roughly known. The coarse stage takes a broad initial guess and finds the approximate alignment, and the fine stage refines it against the point data. The node is launched the same way as the others: ```shell ros2 launch localizer localizer_launch.py ros2 bag play your_bag_file ``` The initial value is then supplied through a service, and the request carries the path to the point cloud map plus a full pose with x, y, z, yaw, pitch and roll: ```shell ros2 service call /localizer/relocalize interface/srv/Relocalize "{"pcd_path": "your_map.pcd", "x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0, "pitch": 0.0, "roll": 0.0}" ``` The design point worth noticing is that the guess is a parameter, not a hope. A relocalization system that searches the whole map is slow and can converge to the wrong place, so this one asks the operator or the calling system for a starting pose and then improves it. The cost is that the caller has to know roughly where it is, which is a reasonable assumption for a robot returning to a known site but not for one that has been carried somewhere unknown. The result is checked separately, through a service that takes a code: ```shell ros2 service call /localizer/relocalize_check interface/srv/IsValid "{"code": 0}" ``` Separating the check from the call means the caller can decide what to do with a failed relocalization rather than the node deciding for it. ## Consistent map optimization splits small scenes and large scenes The fourth capability is consistent map optimization, and it is where the project borrows two different upstream algorithms for two different problem sizes. For small scene maps the choice is BLAM, and for large scene maps it is HBA, both from the hku-mars group and both acknowledged in the README. The distinction matters because a globally optimised map of a large environment is a different computational problem from a globally optimised map of a small one, and the project does not try to make one algorithm do both jobs. The node is launched with hba_launch.py, and the optimisation is triggered by a service that takes the path of a directory of maps: ```shell ros2 launch hba hba_launch.py ``` ```shell ros2 service call /hba/refine_map interface/srv/RefineMap "{"maps_path": "your maps directory"}" ``` The dependency between the pipeline stages is explicit. The README notes that to call the optimisation service, the map must have been saved with save_patches set to true. The patches are the per-submap corrections that the optimiser consumes, so the map has to be saved in the form the optimiser expects before the optimisation can run. That is the least intuitive part of the whole flow. A user who saves a map with the default options, tries the refinement service and gets nothing useful, has to re-record or re-play the bag with the flag set. The requirement is stated in the README, but it appears after the launch instructions rather than as part of them. ## Building means Ubuntu 22.04, ROS 2 Humble, and three source builds The environment requirements are stated as Ubuntu 22.04 and ROS 2 Humble, and the build dependencies are pcl, Eigen, sophus, gtsam and livox_ros_driver2. Three of those need building from source in a specific order, and the README is explicit about the order. The Livox SDK comes first: ```shell git clone https://github.com/Livox-SDK/Livox-SDK2.git cd ./Livox-SDK2/ mkdir build cd build cmake .. && make -j sudo make install ``` Then the ROS 2 driver, built inside a workspace with the Humble setup sourced: ```shell mkdir -r ws_livox/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git ws_livox/src/livox_ros_driver2 cd ws_livox/src/livox_ros_driver2 source /opt/ros/humble/setup.sh ./build.sh humble ``` Then Sophus, and here the version matters, because the README checks out a specific tag: ```shell git clone https://github.com/strasdat/Sophus.git cd Sophus git checkout 1.22.10 mkdir build && cd build cmake .. -DSOPHUS_USE_BASIC_LOGGING=ON make sudo make install ``` The README also warns about a newer Sophus dependency on fmt, and says that adding add_compile_definitions(SOPHUS_USE_BASIC_LOGGING) to CMakeLists.txt removes the need for it, otherwise the build errors out. The -DSOPHUS_USE_BASIC_LOGGING=ON flag in the cmake line above is the same switch passed on the command line instead. The first run is the odometry node with a recorded bag, which is the point at which you find out whether the build is sound: ```shell ros2 launch fastlio2 lio_launch.py ros2 bag play your_bag_file ``` The README also points at an example dataset hosted on Baidu Pan with an extraction code included. ## The callback model is the acknowledged performance limit The README has a section on performance questions, and it names the problem rather than hiding it. The code mainly uses timerCB as the trigger for the main function. Because in ROS 2 the timer, subscriber and service callbacks actually run on the same thread, on a slow machine calls can block each other. The suggested remedy is to move the time consuming callbacks onto their own threads, for example with a threaded timerCB, to improve performance. That is an architectural statement, not a bug report. The node was written on the assumption that a ROS 2 executor would serialise the work, which is convenient and simple, and it holds until the machine cannot keep up. The fix is a real code change rather than a configuration flag, which is what makes it a limit for anyone planning to run this on hardware with a tight latency budget. The second limitation is the platform. Ubuntu 22.04 and ROS 2 Humble are stated requirements, and the build chain includes a pinned Sophus and a from source Livox SDK, so moving to a different distribution or a different ROS 2 release means redoing the dependency work and hoping the C++ still compiles against a newer Sophus. On maintenance, the last push was on 2026-08-10 and the repository is not archived. The repository publishes no GitHub releases, so there is no tagged version to record what you are running, and no licence is stated for the project, which for a component that will end up inside a robot is a gap to close with the author. ## Conclusion FASTLIO2_ROS2 fits a robotics team already running ROS 2 Humble on Ubuntu 22.04 with a Livox sensor, that needs odometry plus loop closure, plus the ability to relocalise into a previously saved map, plus a map refinement step, and that is prepared to build the Livox SDK and driver and a pinned Sophus from source. It does not fit a team on a different ROS 2 distribution or on a non Livox sensor without extra work, and it is a poor match for a real time platform where the callback blocking described in the README cannot be tolerated. Before you adopt it, build the Livox SDK, livox_ros_driver2 and Sophus 1.22.10 in that order, run ros2 launch fastlio2 lio_launch.py against a recorded bag, and check whether your target platform can afford the single threaded callback design or whether you need the threaded callback the README recommends. ## FAQ ### What does FASTLIO2_ROS2 add on top of FAST-LIO2? Besides porting FAST_LIO to ROS 2, it adds a loop closure node that detects loops with position priors plus ICP and optimises a pose graph with GTSAM, a relocalization node based on a coarse to fine two stage ICP, and consistent map optimization using BLAM for small scenes and HBA for large scenes. ### What are the environment requirements for FASTLIO2_ROS2? Ubuntu 22.04 and ROS 2 Humble, with pcl, Eigen, sophus, gtsam and livox_ros_driver2 as build dependencies. The Livox SDK, the ROS 2 driver and Sophus 1.22.10 are built from source in that order. ### How do I relocalize into a previously saved map? Start the node with ros2 launch localizer localizer_launch.py, then call the /localizer/relocalize service with the path to the map file and a starting pose in x, y, z, yaw, pitch and roll. The /localizer/relocalize_check service with a code reports whether the result is valid. ### Why does the map refinement service not work on my saved map? The map has to be saved with save_patches set to true. The README states that calling the optimisation service requires the map to have been saved with that flag, and the save service is ros2 service call /pgo/save_maps interface/srv/SaveMaps. ### What is the known performance limitation of FASTLIO2_ROS2? The main function is triggered from timerCB, and in ROS 2 the timer, subscriber and service callbacks run on the same thread, so calls can block each other on a slow machine. The README suggests running the time consuming callbacks on separate threads to improve performance. ## Sources - [Issues](https://github.com/liangheming/FASTLIO2_ROS2/issues) - [liangheming/FASTLIO2_ROS2 on GitHub](https://github.com/liangheming/FASTLIO2_ROS2) - [README](https://github.com/liangheming/FASTLIO2_ROS2/blob/main/README.md) --- Hysen Labs editorial analysis, written from the project's own repository and release notes. Cite the canonical page: https://hysenlabs.com/projects/liangheming-fastlio2-ros2