From 544dbf355354fb30b9953a211f61b5672dc38f5c Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 22:18:18 -0400 Subject: [PATCH 1/6] feat(pl): add FORM pipeline support --- cpp/bindings/CMakeLists.txt | 1 + cpp/bindings/pipelines/bindings.h | 16 +++ cpp/bindings/pipelines/form.h | 176 ++++++++++++++++++++++++++++++ cpp/bindings/pipelines/form.patch | 36 ++++++ cpp/setup_pipelines.sh | 12 +- docs/ref/pipelines.md | 3 +- 6 files changed, 242 insertions(+), 2 deletions(-) create mode 100644 cpp/bindings/pipelines/form.h create mode 100644 cpp/bindings/pipelines/form.patch 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..30d18781 --- /dev/null +++ b/cpp/bindings/pipelines/form.h @@ -0,0 +1,176 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "evalio/pipeline.h" +#include "evalio/types.h" +#include "form/feature/extraction.hpp" +#include "form/form.hpp" +#include "form/utils.hpp" + +namespace ev = evalio; + +inline gtsam::Pose3 pose_to_gtsam(const ev::SE3& pose) { + return gtsam::Pose3(gtsam::Rot3(pose.rot.to_mat()), gtsam::Point3(pose.trans)); +} + +inline ev::SE3 pose_to_evalio(const gtsam::Pose3& pose) { + return ev::SE3(ev::SO3::from_mat(pose.rotation().matrix()), pose.translation()); +} + +template +inline ev::Point point_to_evalio(const PointT& point) { + return { + .x = point.x, + .y = point.y, + .z = point.z, + .intensity = 0.0, + .t = ev::Duration::from_sec(0), + .row = 0, + .col = static_cast(point.scan), + }; +} + +inline form::PointXYZf point_to_form(const ev::Point& point) { + return form::PointXYZf( + static_cast(point.x), + static_cast(point.y), + static_cast(point.z) + ); +} + +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 world_map = + form::tuple::transform(estimator_.m_keypoint_map, [&](auto& map) { + return map.to_voxel_map( + estimator_.m_constraints.get_values(), + estimator_.m_params.map.min_dist_map + ); + }); + + std::tuple map_names = {"planar", "point"}; + std::map> points; + + form::tuple::for_seq(std::make_index_sequence<2> {}, [&](auto I) { + const auto name = std::get(map_names); + points.insert({name, {}}); + auto& vec = points[name]; + + for (const auto& [_, voxel] : std::get(world_map)) { + for (const auto& point : voxel) { + vec.push_back(point_to_evalio(point)); + } + } + }); + + return points; + } + + // 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_ = pose_to_gtsam(T).inverse(); + } + + // Doers + void initialize() override { + estimator_ = form::Estimator(params_); + } + + void add_imu(ev::ImuMeasurement mm) override {} + + void add_lidar(ev::LidarMeasurement mm) override { + std::vector scan; + scan.reserve(mm.points.size()); + for (const auto& point : mm.points) { + scan.push_back(point_to_form(point)); + } + + auto [planar_kp, point_kp] = estimator_.register_scan(scan); + current_pose_ = pose_to_evalio(estimator_.current_lidar_estimate() * lidar_T_imu_); + + std::map> points = { + {"planar", {}}, + {"point", {}}, + }; + auto& all_planar = points["planar"]; + auto& all_point = points["point"]; + all_planar.reserve(planar_kp.size()); + all_point.reserve(point_kp.size()); + + for (const auto& point : planar_kp) { + all_planar.push_back(point_to_evalio(point)); + } + for (const auto& point : point_kp) { + all_point.push_back(point_to_evalio(point)); + } + + this->save(mm.stamp, current_pose_); + this->save(mm.stamp, points); + } + +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..a39e78eb --- /dev/null +++ b/cpp/bindings/pipelines/form.patch @@ -0,0 +1,36 @@ +diff --git a/form/mapping/map.hpp b/form/mapping/map.hpp +index 321e54d..86cf051 100644 +--- a/form/mapping/map.hpp ++++ b/form/mapping/map.hpp +@@ -31,16 +31,17 @@ + + 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 +70,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/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 From 35e49edd165d5479269a692be24bb61e66c7fe78 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 09:10:05 -0400 Subject: [PATCH 2/6] fix(pl): patch FORM constraints binding for clang --- cpp/bindings/pipelines/form.patch | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/cpp/bindings/pipelines/form.patch b/cpp/bindings/pipelines/form.patch index a39e78eb..6c7bf300 100644 --- a/cpp/bindings/pipelines/form.patch +++ b/cpp/bindings/pipelines/form.patch @@ -1,8 +1,23 @@ +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..86cf051 100644 +index 321e54d..b7c8b28 100644 --- a/form/mapping/map.hpp +++ b/form/mapping/map.hpp -@@ -31,16 +31,17 @@ +@@ -31,18 +31,18 @@ using gtsam::symbol_shorthand::X; @@ -24,7 +39,7 @@ index 321e54d..86cf051 100644 /// @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 +70,8 @@ private: +@@ -69,7 +69,8 @@ private: double m_voxel_width = 0.5; /// @brief The data structure storing the points From bf6783b8349b7a44be584e8a64fa823f877f318d Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 09:21:24 -0400 Subject: [PATCH 3/6] style(pl): format FORM pipeline bindings --- cpp/bindings/pipelines/form.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/cpp/bindings/pipelines/form.h b/cpp/bindings/pipelines/form.h index 30d18781..453cf0a4 100644 --- a/cpp/bindings/pipelines/form.h +++ b/cpp/bindings/pipelines/form.h @@ -17,11 +17,17 @@ namespace ev = evalio; inline gtsam::Pose3 pose_to_gtsam(const ev::SE3& pose) { - return gtsam::Pose3(gtsam::Rot3(pose.rot.to_mat()), gtsam::Point3(pose.trans)); + return gtsam::Pose3( + gtsam::Rot3(pose.rot.to_mat()), + gtsam::Point3(pose.trans) + ); } inline ev::SE3 pose_to_evalio(const gtsam::Pose3& pose) { - return ev::SE3(ev::SO3::from_mat(pose.rotation().matrix()), pose.translation()); + return ev::SE3( + ev::SO3::from_mat(pose.rotation().matrix()), + pose.translation() + ); } template @@ -145,7 +151,8 @@ class FORM: public ev::Pipeline { } auto [planar_kp, point_kp] = estimator_.register_scan(scan); - current_pose_ = pose_to_evalio(estimator_.current_lidar_estimate() * lidar_T_imu_); + current_pose_ = + pose_to_evalio(estimator_.current_lidar_estimate() * lidar_T_imu_); std::map> points = { {"planar", {}}, From f87b323f81c4a0820f9e5150c42fb8681642ed7f Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 09:29:57 -0400 Subject: [PATCH 4/6] chore: ignore beads workspace directory --- .gitignore | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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/ From 6e80ac93c86fd398e55f6d08bfaef4a8d21affa1 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 09:35:13 -0400 Subject: [PATCH 5/6] refactor(pl): use evalio converters in FORM pipeline --- cpp/bindings/pipelines/form.h | 95 +++++++++++++++++------------------ cpp/evalio/convert/gtsam.h | 27 ++++++++++ 2 files changed, 74 insertions(+), 48 deletions(-) create mode 100644 cpp/evalio/convert/gtsam.h diff --git a/cpp/bindings/pipelines/form.h b/cpp/bindings/pipelines/form.h index 453cf0a4..ff462a83 100644 --- a/cpp/bindings/pipelines/form.h +++ b/cpp/bindings/pipelines/form.h @@ -8,6 +8,8 @@ #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" @@ -16,41 +18,58 @@ namespace ev = evalio; -inline gtsam::Pose3 pose_to_gtsam(const ev::SE3& pose) { - return gtsam::Pose3( - gtsam::Rot3(pose.rot.to_mat()), - gtsam::Point3(pose.trans) - ); +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), + }; } -inline ev::SE3 pose_to_evalio(const gtsam::Pose3& pose) { - return ev::SE3( - ev::SO3::from_mat(pose.rotation().matrix()), - pose.translation() - ); +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 ev::Point point_to_evalio(const PointT& point) { +template<> +inline Point convert(const form::PointXYZf& in) { return { - .x = point.x, - .y = point.y, - .z = point.z, + .x = in.x, + .y = in.y, + .z = in.z, .intensity = 0.0, - .t = ev::Duration::from_sec(0), + .t = Duration::from_sec(0), .row = 0, - .col = static_cast(point.scan), + .col = 0, }; } -inline form::PointXYZf point_to_form(const ev::Point& point) { - return form::PointXYZf( - static_cast(point.x), - static_cast(point.y), - static_cast(point.z) - ); +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_() {} @@ -114,7 +133,7 @@ class FORM: public ev::Pipeline { for (const auto& [_, voxel] : std::get(world_map)) { for (const auto& point : voxel) { - vec.push_back(point_to_evalio(point)); + vec.push_back(ev::convert(point)); } } }); @@ -133,7 +152,7 @@ class FORM: public ev::Pipeline { } void set_imu_T_lidar(ev::SE3 T) override { - lidar_T_imu_ = pose_to_gtsam(T).inverse(); + lidar_T_imu_ = ev::convert(T).inverse(); } // Doers @@ -144,34 +163,14 @@ class FORM: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} void add_lidar(ev::LidarMeasurement mm) override { - std::vector scan; - scan.reserve(mm.points.size()); - for (const auto& point : mm.points) { - scan.push_back(point_to_form(point)); - } + const auto scan = ev::convert_iter>(mm.points); auto [planar_kp, point_kp] = estimator_.register_scan(scan); current_pose_ = - pose_to_evalio(estimator_.current_lidar_estimate() * lidar_T_imu_); - - std::map> points = { - {"planar", {}}, - {"point", {}}, - }; - auto& all_planar = points["planar"]; - auto& all_point = points["point"]; - all_planar.reserve(planar_kp.size()); - all_point.reserve(point_kp.size()); - - for (const auto& point : planar_kp) { - all_planar.push_back(point_to_evalio(point)); - } - for (const auto& point : point_kp) { - all_point.push_back(point_to_evalio(point)); - } + ev::convert(estimator_.current_lidar_estimate() * lidar_T_imu_); this->save(mm.stamp, current_pose_); - this->save(mm.stamp, points); + this->save(mm.stamp, ev::make_map("planar", planar_kp, "point", point_kp)); } private: diff --git a/cpp/evalio/convert/gtsam.h b/cpp/evalio/convert/gtsam.h new file mode 100644 index 00000000..6c032174 --- /dev/null +++ b/cpp/evalio/convert/gtsam.h @@ -0,0 +1,27 @@ +// 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 SE3( + SO3::from_mat(in.rotation().matrix()), + in.translation() + ); +} + +template<> +inline gtsam::Pose3 convert(const SE3& in) { + return gtsam::Pose3( + gtsam::Rot3(in.rot.to_mat()), + gtsam::Point3(in.trans) + ); +} + +} // namespace evalio From 0bf613f04cc20f4864fcdc212ea2a98c4933bdea Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 10:41:20 -0400 Subject: [PATCH 6/6] Clean up form binding a smidge --- cpp/bindings/pipelines/form.h | 87 +++++++++++------------------------ cpp/evalio/convert/gtsam.h | 9 ++-- 2 files changed, 30 insertions(+), 66 deletions(-) diff --git a/cpp/bindings/pipelines/form.h b/cpp/bindings/pipelines/form.h index ff462a83..af73ae6b 100644 --- a/cpp/bindings/pipelines/form.h +++ b/cpp/bindings/pipelines/form.h @@ -46,19 +46,6 @@ inline Point convert(const form::PlanarFeat& in) { }; } -template<> -inline Point convert(const form::PointXYZf& in) { - return { - .x = in.x, - .y = in.y, - .z = in.z, - .intensity = 0.0, - .t = Duration::from_sec(0), - .row = 0, - .col = 0, - }; -} - template<> inline form::PointXYZf convert(const Point& in) { return { @@ -89,25 +76,25 @@ class FORM: public ev::Pipeline { // 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), + // 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) ); @@ -115,30 +102,12 @@ class FORM: public ev::Pipeline { // Getters const std::map> map() override { - const auto world_map = - form::tuple::transform(estimator_.m_keypoint_map, [&](auto& map) { - return map.to_voxel_map( - estimator_.m_constraints.get_values(), - estimator_.m_params.map.min_dist_map - ); - }); - - std::tuple map_names = {"planar", "point"}; - std::map> points; - - form::tuple::for_seq(std::make_index_sequence<2> {}, [&](auto I) { - const auto name = std::get(map_names); - points.insert({name, {}}); - auto& vec = points[name]; - - for (const auto& [_, voxel] : std::get(world_map)) { - for (const auto& point : voxel) { - vec.push_back(ev::convert(point)); - } - } - }); - - return points; + 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 @@ -166,11 +135,9 @@ class FORM: public ev::Pipeline { const auto scan = ev::convert_iter>(mm.points); auto [planar_kp, point_kp] = estimator_.register_scan(scan); - current_pose_ = - ev::convert(estimator_.current_lidar_estimate() * lidar_T_imu_); - this->save(mm.stamp, current_pose_); - this->save(mm.stamp, ev::make_map("planar", planar_kp, "point", point_kp)); + this->save(mm.stamp, estimator_.current_lidar_estimate() * lidar_T_imu_); + this->save(mm.stamp, "planar", planar_kp, "point", point_kp); } private: diff --git a/cpp/evalio/convert/gtsam.h b/cpp/evalio/convert/gtsam.h index 6c032174..15028a15 100644 --- a/cpp/evalio/convert/gtsam.h +++ b/cpp/evalio/convert/gtsam.h @@ -10,18 +10,15 @@ namespace evalio { template<> inline SE3 convert(const gtsam::Pose3& in) { - return SE3( - SO3::from_mat(in.rotation().matrix()), + 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_mat()), - gtsam::Point3(in.trans) - ); + return gtsam::Pose3(gtsam::Rot3(in.rot.to_eigen()), gtsam::Point3(in.trans)); } } // namespace evalio