diff --git a/.gitignore b/.gitignore index 0369c469..fa89dd4c 100644 --- a/.gitignore +++ b/.gitignore @@ -29,4 +29,7 @@ data compile_commands.json # cloned pipelines -cpp/bindings/pipelines-src \ No newline at end of file +cpp/bindings/pipelines-src + +# beads +.beads/ diff --git a/cpp/bindings/CMakeLists.txt b/cpp/bindings/CMakeLists.txt index 4ec26584..4124af5b 100644 --- a/cpp/bindings/CMakeLists.txt +++ b/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) diff --git a/cpp/bindings/pipelines/bindings.h b/cpp/bindings/pipelines/bindings.h index 508016a7..4efb78df 100644 --- a/cpp/bindings/pipelines/bindings.h +++ b/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_(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 diff --git a/cpp/bindings/pipelines/form.h b/cpp/bindings/pipelines/form.h new file mode 100644 index 00000000..af73ae6b --- /dev/null +++ b/cpp/bindings/pipelines/form.h @@ -0,0 +1,149 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#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(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(in.scan), + }; +} + +template<> +inline form::PointXYZf convert(const Point& in) { + return { + static_cast(in.x), + static_cast(in.y), + static_cast(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> 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(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>(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(); +}; diff --git a/cpp/bindings/pipelines/form.patch b/cpp/bindings/pipelines/form.patch new file mode 100644 index 00000000..6c7bf300 --- /dev/null +++ b/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 &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 + /// From kiss-icp: + /// https://github.com/PRBonn/kiss-icp/blob/main/cpp/kiss_icp/core/VoxelUtils.hpp#L45-L51 +-template <> struct std::hash> { ++struct VoxelHasher { + std::size_t operator()(const Eigen::Matrix &voxel) const { + const uint32_t *vec = reinterpret_cast(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 struct Match { +@@ -69,7 +69,8 @@ private: + double m_voxel_width = 0.5; + + /// @brief The data structure storing the points +- tsl::robin_map, std::vector> m_data; ++ tsl::robin_map, std::vector, VoxelHasher> ++ m_data; + + /// @brief Compute the voxel coordinates for a given point + [[nodiscard]] Eigen::Matrix diff --git a/cpp/evalio/convert/gtsam.h b/cpp/evalio/convert/gtsam.h new file mode 100644 index 00000000..15028a15 --- /dev/null +++ b/cpp/evalio/convert/gtsam.h @@ -0,0 +1,24 @@ +// Handle GTSAM conversions. +#pragma once + +#include + +#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 diff --git a/cpp/setup_pipelines.sh b/cpp/setup_pipelines.sh index 2bf5e96c..20d29f94 100755 --- a/cpp/setup_pipelines.sh +++ b/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 \ No newline at end of file +./.vcpkg/bootstrap-vcpkg.sh diff --git a/docs/ref/pipelines.md b/docs/ref/pipelines.md index 42900e32..0238cb69 100644 --- a/docs/ref/pipelines.md +++ b/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 \ No newline at end of file + - validate_params