feat(pl): add FORM pipeline support

.gitignore

@@ -29,4 +29,7 @@ data
 compile_commands.json
 
 # cloned pipelines
-cpp/bindings/pipelines-src
+cpp/bindings/pipelines-src
+
+# beads
+.beads/

cpp/bindings/CMakeLists.txt

@@ -72,6 +72,7 @@ find_pipeline(loam                   loam               EVALIO_LOAM)
 find_pipeline(genz-icp/cpp/genz_icp  genz_icp::pipeline EVALIO_GENZ_ICP)
 find_pipeline(mad-icp/mad_icp        mad_odometry       EVALIO_MAD_ICP)
 find_pipeline(ct_icp                 CT_ICP             EVALIO_CT_ICP)
+find_pipeline(form                   FORM               EVALIO_FORM)
 message("###################    Pipeline Results     ###################")
 if(NOT "${RESULTS}" STREQUAL "")
     string(STRIP "${RESULTS}" RESULTS)

cpp/bindings/pipelines/bindings.h

@@ -31,6 +31,10 @@ using namespace nb::literals;
   #include "bindings/pipelines/ct_icp.h"
 #endif
 
+#ifdef EVALIO_FORM
+  #include "bindings/pipelines/form.h"
+#endif
+
 namespace evalio {
 inline void make_pipelines(nb::module_& m) {
   // List all the pipelines here
@@ -112,5 +116,17 @@ inline void make_pipelines(nb::module_& m) {
     "a small window of scans to perform more accurate dewarping performance. "
     "This is the version based on the 2022-ICRA paper.";
 #endif
+
+#ifdef EVALIO_FORM
+  nb::class_<FORM, evalio::Pipeline>(m, "FORM")
+    .def(nb::init<>())
+    .def_static("name", &FORM::name)
+    .def_static("default_params", &FORM::default_params)
+    .def_static("url", &FORM::url)
+    .def_static("version", &FORM::version)
+    .doc() =
+    "FORM LiDAR odometry pipeline with feature extraction, factor-graph "
+    "optimization, and sparse keyscan map management.";
+#endif
 }
 } // namespace evalio

cpp/bindings/pipelines/form.h

@@ -0,0 +1,149 @@
+#pragma once
+
+#include <gtsam/geometry/Pose3.h>
+
+#include <map>
+#include <string>
+#include <tuple>
+#include <utility>
+#include <vector>
+
+#include "evalio/convert/base.h"
+#include "evalio/convert/gtsam.h"
+#include "evalio/pipeline.h"
+#include "evalio/types.h"
+#include "form/feature/extraction.hpp"
+#include "form/form.hpp"
+#include "form/utils.hpp"
+
+namespace ev = evalio;
+
+namespace evalio {
+
+template<>
+inline Point convert(const form::PointFeat& in) {
+  return {
+    .x = in.x,
+    .y = in.y,
+    .z = in.z,
+    .intensity = 0.0,
+    .t = Duration::from_sec(0),
+    .row = 0,
+    .col = static_cast<uint16_t>(in.scan),
+  };
+}
+
+template<>
+inline Point convert(const form::PlanarFeat& in) {
+  return {
+    .x = in.x,
+    .y = in.y,
+    .z = in.z,
+    .intensity = 0.0,
+    .t = Duration::from_sec(0),
+    .row = 0,
+    .col = static_cast<uint16_t>(in.scan),
+  };
+}
+
+template<>
+inline form::PointXYZf convert(const Point& in) {
+  return {
+    static_cast<float>(in.x),
+    static_cast<float>(in.y),
+    static_cast<float>(in.z),
+  };
+}
+
+} // namespace evalio
+
+class FORM: public ev::Pipeline {
+public:
+  FORM() : estimator_(), params_() {}
+
+  // Info
+  static std::string version() {
+    return XSTR(EVALIO_FORM);
+  }
+
+  static std::string name() {
+    return "form";
+  }
+
+  static std::string url() {
+    return "https://github.com/rpl-cmu/form";
+  }
+
+  // clang-format off
+  EVALIO_SETUP_PARAMS(
+    // FEATURE EXTRACTION
+    (int,         neighbor_points,   5, params_.extraction.neighbor_points),
+    (int,             num_sectors,   6, params_.extraction.num_sectors),
+    (double,     planar_threshold, 1.0, params_.extraction.planar_threshold),
+    (int, planar_feats_per_sector,  50, params_.extraction.planar_feats_per_sector),
+    (int,  point_feats_per_sector,   3, params_.extraction.point_feats_per_sector),
+    (double,               radius, 1.0, params_.extraction.radius),
+    (int,              min_points,   5, params_.extraction.min_points),
+    // OPTIMIZATION
+    (double,  max_dist_matching,   0.8, params_.matcher.max_dist_matching),
+    (double, new_pose_threshold,  1e-4, params_.matcher.new_pose_threshold),
+    (int,     max_num_rematches,    30, params_.matcher.max_num_rematches),
+    (bool,    disable_smoothing, false, params_.constraints.disable_smoothing),
+    // MAPPING
+    (int,         max_num_keyscans,  50, params_.scans.max_num_keyscans),
+    (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,           min_dist_map, 0.1, params_.map.min_dist_map),
+    // misc
+    (int, num_threads, 0, params_.num_threads)
+  );
+  // clang-format on
+
+  // Getters
+  const std::map<std::string, std::vector<ev::Point>> map() override {
+    const auto planar = std::get<0>(estimator_.m_keypoint_map)
+                          .to_vector(estimator_.m_constraints.get_values());
+    const auto point = std::get<1>(estimator_.m_keypoint_map)
+                         .to_vector(estimator_.m_constraints.get_values());
+
+    return ev::make_map("planar", planar, "point", point);
+  }
+
+  // Setters
+  void set_imu_params(ev::ImuParams params) override {}
+
+  void set_lidar_params(ev::LidarParams params) override {
+    params_.extraction.min_norm_squared = params.min_range * params.min_range;
+    params_.extraction.max_norm_squared = params.max_range * params.max_range;
+    params_.extraction.num_columns = params.num_columns;
+    params_.extraction.num_rows = params.num_rows;
+  }
+
+  void set_imu_T_lidar(ev::SE3 T) override {
+    lidar_T_imu_ = ev::convert<gtsam::Pose3>(T).inverse();
+  }
+
+  // Doers
+  void initialize() override {
+    estimator_ = form::Estimator(params_);
+  }
+
+  void add_imu(ev::ImuMeasurement mm) override {}
+
+  void add_lidar(ev::LidarMeasurement mm) override {
+    const auto scan = ev::convert_iter<std::vector<form::PointXYZf>>(mm.points);
+
+    auto [planar_kp, point_kp] = estimator_.register_scan(scan);
+
+    this->save(mm.stamp, estimator_.current_lidar_estimate() * lidar_T_imu_);
+    this->save(mm.stamp, "planar", planar_kp, "point", point_kp);
+  }
+
+private:
+  form::Estimator estimator_;
+  form::Estimator::Params params_;
+
+  gtsam::Pose3 lidar_T_imu_ = gtsam::Pose3::Identity();
+  ev::SE3 current_pose_ = ev::SE3::identity();
+};

cpp/bindings/pipelines/form.patch

@@ -0,0 +1,51 @@
+diff --git a/form/form.cpp b/form/form.cpp
+index d69f197..8a6a93b 100644
+--- a/form/form.cpp
++++ b/form/form.cpp
+@@ -47,7 +47,9 @@ Estimator::register_scan(const std::vector<PointXYZf> &scan) noexcept {
+   // ------------------------------ Initialization ------------------------------ //
+   // This needs to go first to get the scan index
+   Pose3 prediction = m_constraints.predict_next();
+-  auto [scan_idx, scan_constraints] = m_constraints.step(prediction);
++  auto scan_step = m_constraints.step(prediction);
++  const auto scan_idx = std::get<0>(scan_step);
++  auto &scan_constraints = std::get<1>(scan_step);
+
+   // ----------------------------- Extract Features ----------------------------- //
+   const auto keypoints = m_extractor.extract(scan, scan_idx);
+diff --git a/form/mapping/map.hpp b/form/mapping/map.hpp
+index 321e54d..b7c8b28 100644
+--- a/form/mapping/map.hpp
++++ b/form/mapping/map.hpp
+@@ -31,18 +31,18 @@
+
+ using gtsam::symbol_shorthand::X;
+
++namespace form {
++
+ /// @brief Hash function for Eigen::Matrix<int, 3, 1>
+ /// From kiss-icp:
+ /// https://github.com/PRBonn/kiss-icp/blob/main/cpp/kiss_icp/core/VoxelUtils.hpp#L45-L51
+-template <> struct std::hash<Eigen::Matrix<int, 3, 1>> {
++struct VoxelHasher {
+   std::size_t operator()(const Eigen::Matrix<int, 3, 1> &voxel) const {
+     const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+     return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+   }
+ };
+
+-namespace form {
+-
+ /// @brief A match between a query point and a point in the map
+ /// If no match is found, dist_sqrd is set to max double
+ template <typename Point> struct Match {
+@@ -69,7 +69,8 @@ private:
+   double m_voxel_width = 0.5;
+
+   /// @brief The data structure storing the points
+-  tsl::robin_map<Eigen::Matrix<int, 3, 1>, std::vector<Point>> m_data;
++  tsl::robin_map<Eigen::Matrix<int, 3, 1>, std::vector<Point>, VoxelHasher>
++      m_data;
+
+   /// @brief Compute the voxel coordinates for a given point
+   [[nodiscard]] Eigen::Matrix<int, 3, 1>

cpp/evalio/convert/gtsam.h

@@ -0,0 +1,24 @@
+// Handle GTSAM conversions.
+#pragma once
+
+#include <gtsam/geometry/Pose3.h>
+
+#include "evalio/convert/base.h"
+#include "evalio/types.h"
+
+namespace evalio {
+
+template<>
+inline SE3 convert(const gtsam::Pose3& in) {
+  return evalio::SE3(
+    evalio::SO3::from_eigen(in.rotation().toQuaternion()),
+    in.translation()
+  );
+}
+
+template<>
+inline gtsam::Pose3 convert(const SE3& in) {
+  return gtsam::Pose3(gtsam::Rot3(in.rot.to_eigen()), gtsam::Point3(in.trans));
+}
+
+} // namespace evalio

cpp/setup_pipelines.sh

@@ -64,6 +64,16 @@ git switch --detach icra_2022
 git apply ../../pipelines/ct_icp.patch
 cd ..
 
+# FORM
+if [ ! -d "form" ]; then
+    git clone https://github.com/rpl-cmu/form.git
+fi
+cd form
+git stash
+git switch --detach main
+git apply ../../pipelines/form.patch
+cd ..
+
 # ------------------------- Dependencies ------------------------- #
 cd $topdir
 if [ ! -d ".vcpkg/" ]; then
@@ -72,4 +82,4 @@ fi
 cd .vcpkg
 git switch --detach 2025.08.27
 cd ..
-./.vcpkg/bootstrap-vcpkg.sh
+./.vcpkg/bootstrap-vcpkg.sh

docs/ref/pipelines.md

@@ -10,10 +10,11 @@ For more information about the pipelines included in evalio, see the [included p
             - LOAM
             - LioSAM
             - MadICP
+            - FORM
             - PipelineNotFound
             - UnusedPipelineParam
             - InvalidPipelineParamType
             - all_pipelines
             - get_pipeline
             - register_pipeline
-            - validate_params
+            - validate_params