[feat] ROS2 Node!

.gitattributes

@@ -0,0 +1,3 @@
+# SCM syntax highlighting & preventing 3-way merges
+pixi.lock merge=binary linguist-language=YAML linguist-generated=true -diff
+uv.lock merge=binary linguist-language=TOML linguist-generated=true -diff

.gitignore

@@ -5,9 +5,14 @@ build*
 *.egg-info
 compile_commands.json
 
+# ros env
+install
+log
+
 # vscode environments
 .vscode
 .envrc
+.clangd
 
 .helix
 
@@ -25,8 +30,8 @@ __pycache__
 evalio_results
 *.pyi
 
-*.png
 
 # final outputs
+*.png
 figures
-graphics
+graphics*

README.md

@@ -55,6 +55,19 @@ Datasets can be commented out in the config file if they haven't been downloaded
 evalio stats evalio_results/25.10.03_full/
 ```
 
+## ROS Support
+
+FORM has a simple ROS wrapper found in [ros/](ros/). Using it is as easy as cloning into your workspace,
+
+```sh
+cd ~/ros2_ws/src
+git clone https://github.com/rpl-cmu/form.git
+cd ..
+colcon build --packages-select form
+```
+
+More details (including setup using [pixi](https://pixi.prefix.dev/latest/) and [robostack](https://robostack.github.io/index.html) for the purists out there) can be found in the [ros README](ros/)
+
 ## Roadmap
 - [ ] ROS Node
 - [ ] Merge wrapper into upstream evalio

form/form.cpp

@@ -113,4 +113,14 @@ Estimator::register_scan(const std::vector<PointXYZf> &scan) noexcept {
   return keypoints;
 }
 
+std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
+Estimator::current_map() const noexcept {
+  std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>> map_points;
+  tuple::for_seq(std::make_index_sequence<2>{}, [&](auto I) {
+    std::get<I>(map_points) =
+        std::get<I>(m_keypoint_map).to_vector(m_constraints.get_values());
+  });
+  return map_points;
+}
+
 } // namespace form

form/form.hpp

@@ -78,6 +78,10 @@ struct Estimator {
   /// @brief Get the current lidar estimate
   gtsam::Pose3 current_lidar_estimate() { return m_constraints.get_current_pose(); }
 
+  /// @brief Get the current map point clouds for visualization
+  std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
+  current_map() const noexcept;
+
   /// @brief Register a new scan and return the extracted features
   std::tuple<std::vector<PlanarFeat>, std::vector<PointFeat>>
   register_scan(const std::vector<PointXYZf> &scan) noexcept;

form/mapping/map.hpp

@@ -137,6 +137,10 @@ template <typename Point> class KeypointMap {
   VoxelMap<Point> to_voxel_map(const gtsam::Values &values,
                                double voxel_width) const noexcept;
 
+  /// @brief Transform all keypoints to the global scan and insert them into a
+  /// vector
+  std::vector<Point> to_vector(const gtsam::Values &values) const noexcept;
+
   /// @brief Insert matches into the map, adding only those that are sufficiently
   /// far from existing points based on min_dist_map
   void insert_matches(const tbb::concurrent_vector<Match<Point>> &matches);

form/mapping/map.tpp

@@ -22,6 +22,7 @@
 #pragma once
 
 #include "form/mapping/map.hpp"
+#include <numeric>
 #include <tbb/concurrent_vector.h>
 
 namespace form {
@@ -145,6 +146,30 @@ VoxelMap<Point> KeypointMap<Point>::to_voxel_map(const gtsam::Values &values,
   return world_map;
 }
 
+template <typename Point>
+std::vector<Point>
+KeypointMap<Point>::to_vector(const gtsam::Values &values) const noexcept {
+  // Create a new map
+  std::vector<Point> world_map;
+  auto num_points = std::accumulate(
+      m_scan_keypoints.begin(), m_scan_keypoints.end(), size_t{0},
+      [](size_t sum, const auto &pair) { return sum + pair.second.size(); });
+  world_map.reserve(num_points);
+
+  // Iterate over all the keypoints in the map
+  for (const auto &[scan_index, keypoints] : m_scan_keypoints) {
+    // Get the pose of the scan
+    const auto &world_T_scan = values.at<gtsam::Pose3>(X(scan_index));
+
+    // Transform each keypoint into the world scan and add it to the map
+    for (const auto &keypoint : keypoints) {
+      world_map.push_back(keypoint.transform(world_T_scan));
+    }
+  }
+
+  return world_map;
+}
+
 template <typename Point>
 void KeypointMap<Point>::insert_matches(
     const tbb::concurrent_vector<Match<Point>> &matches) {
@@ -155,10 +180,10 @@ void KeypointMap<Point>::insert_matches(
   const ScanIndex scan_j = matches.front().query.scan;
   auto &keypoints = get(scan_j);
 
-  double max_dist_map_sqrd = m_params.min_dist_map * m_params.min_dist_map;
+  double min_dist_map_sqrd = m_params.min_dist_map * m_params.min_dist_map;
 
   for (const auto &match : matches) {
-    if (match.dist_sqrd > max_dist_map_sqrd) {
+    if (match.dist_sqrd > min_dist_map_sqrd) {
       keypoints.push_back(match.query);
     }
   }

python/bindings.cpp

@@ -82,7 +82,7 @@ class FORM : public evalio::Pipeline {
     (int,     max_num_recent_scans,  10, params_.scans.max_num_recent_scans),
     (int, max_steps_unused_keyscan,  10, params_.scans.max_steps_unused_keyscan),
     (double,    keyscan_match_ratio, 0.1, params_.scans.keyscan_match_ratio),
-    (double,           max_dist_map, 0.1, params_.map.min_dist_map),
+    (double,           min_dist_map, 0.1, params_.map.min_dist_map),
     // misc
     (int, num_threads, 0, params_.num_threads)
   );

ros/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.16...3.26)
+project(form VERSION 0.1.0 LANGUAGES CXX)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# Build the core FORM library from the parent directory
+# Need PIC since FORM (static) gets linked into a shared library (form_odometry_node)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_BINARY_DIR}/form_core)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+# ROS 2 node
+add_library(form_odometry_node SHARED src/node.cpp)
+target_compile_features(form_odometry_node PUBLIC cxx_std_17)
+target_include_directories(form_odometry_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(form_odometry_node FORM)
+ament_target_dependencies(
+  form_odometry_node
+  geometry_msgs
+  nav_msgs
+  rclcpp
+  rclcpp_components
+  rcutils
+  sensor_msgs
+  std_msgs
+  tf2_ros)
+
+rclcpp_components_register_node(form_odometry_node PLUGIN "form_ros::EstimatorNode" EXECUTABLE form_node)
+
+install(TARGETS form_odometry_node LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/)
+
+ament_package()

ros/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 Easton Potokar, Taylor Pool, and Michael Kaess
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

ros/README.md

@@ -0,0 +1,93 @@
+# FORM ROS 2 Wrapper
+
+This ROS 2 node is based on the [KISS-ICP](https://github.com/PRBonn/kiss-icp) ROS 2 wrapper.
+Thanks to the KISS-ICP developers for their clean and well-structured ROS node implementation.
+
+### Building
+
+You will need the same dependencies as FORM (most of which are available as ROS packages),
+- [eigen3](https://libeigen.gitlab.io/eigen/docs-nightly/)
+- [gtsam](https://github.com/borglab/gtsam/)
+- [tbb](https://github.com/uxlfoundation/oneTBB)
+- [tsl::robin_map](https://github.com/Tessil/robin-map) (will be pulled from source if not found)
+
+Once those are installed, simply clone FORM to your workspace and build,
+
+```sh
+git clone https://github.com/contagon/form
+colcon build
+source ./install/setup.bash
+```
+
+### Running
+
+The main launch file is `odometry.launch.py` which will launch the odometry node. FORM has three required inputs, **topic name**, **number of scanlines/rings/columns**, and **number of rows/circular count**. This last two are required for FORM's feature extraction.
+
+To view all the available arguments, you can run:
+
+```sh
+ros2 launch form odometry.launch.py --show-args
+```
+which will output the following:
+
+| Parameter          | Default      | Description                          |
+|--------------------|--------------|--------------------------------------|
+| `topic`            | `None`       | Input point cloud topic              |
+| `num_columns`      | `None`       | LiDAR image width (columns)          |
+| `num_rows`         | `None`       | LiDAR image height (rows)            |
+| `min_range`        | `1.0`        | Minimum LiDAR range                  |
+| `max_range`        | `100.0`      | Maximum LiDAR range                  |
+| `visualize`        | `true`       | Launch RViz and publish point clouds |
+| `bagfile`          | `''`         | Optional rosbag file/folder to play  |
+| `base_frame`       | `''`         | Base frame id                        |
+| `lidar_odom_frame` | `odom_lidar` | Odometry frame id                    |
+| `publish_odom_tf`  | `True`       | Publish odom->base TF                |
+| `invert_odom_tf`   | `True`       | Invert published odom TF             |
+| `use_sim_time`     | `True`       | Use simulation time                  |
+
+If a bagfile is provided, the node will play the bagfile and process the point clouds. If not, it will just subscribe to the topic and process incoming point clouds in real-time.
+
+Thus as an example
+
+```sh
+ros2 launch form odometry.launch.py bagfile:=<path_to_rosbag> topic:=<topic_name> num_columns:=<num_columns> num_rows:=<num_rows>
+```
+
+### Pointcloud Format
+
+FORM requires point clouds to be in row-major order with no dropped points for its feature extraction method. It does it's best to infer the ordering and density of the point cloud using the following heuristics,
+
+| Format         | Heuristic to Infer                                       |
+|----------------|----------------------------------------------------------|
+| all points     | Point cloud size equaling `num_columns` * `num_rows`     |
+| dropped points | Point cloud size not equaling `num_columns` * `num_rows` |
+| row-major      | Stationary ring number for first few points              |
+| column-major   | Increasing ring number for first few points              |
+
+IMPORTANT: If you're point cloud *does not* is not in row or column major format, FORM will crash! Please open an issue and we can add a flag to handle you're appropriate cloud formatting. (For example, I've seen some velodyne point clouds have ring order returned as 0, 8, 1, 9, 2, 10, ... This will break things)
+
+NOTE: If your point cloud is row major and has dropped invalid points, there is usually no way to figure out where along the scan line the dropped points belong. FORM places all of them at the end. This may have an impact on feature extraction, but things generally *should* still work.
+
+
+### Development
+
+We're not huge fans of installing ROS2 system wide, so this folder is setup to install all dependencies using [pixi](https://pixi.prefix.dev/latest/) and [robostack](https://robostack.github.io/index.html).
+
+Install pixi, then building is all done in a single command from the `ros/` directory
+```sh
+pixi run build
+source install/setup.sh
+```
+
+If you need to utilize ROS2 commands, you can do either of the following,
+```sh
+pixi run ros2 <your command>
+# enter into a pixi shell
+pixi shell
+ros2 <your command>
+```
+
+If you have the oxford spires dataset installed using [evalio](https://github.com/contagon/evalio/tree/master), you can use the following commands to launch and run a trajectory from it,
+```sh
+pixi run oxford
+```