From 19648091c77ce7e6eb2f63aeeb6f458610d623d9 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Sat, 12 Jul 2025 13:48:38 -0400 Subject: [PATCH 1/2] Add rustfmt-like clang-format --- .clang-format | 99 ++++ cpp/bindings/main.cpp | 19 +- cpp/bindings/pipeline.h | 119 +++-- cpp/bindings/pipelines/bindings.h | 44 +- cpp/bindings/pipelines/kiss_icp.h | 77 +-- cpp/bindings/pipelines/lio_sam.h | 131 ++--- cpp/bindings/ros_pc2.h | 344 +++++++------ cpp/bindings/types.h | 781 +++++++++++++++++++----------- cpp/evalio/pipeline.h | 29 +- cpp/evalio/types.h | 195 +++++--- 10 files changed, 1172 insertions(+), 666 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..a688ad08 --- /dev/null +++ b/.clang-format @@ -0,0 +1,99 @@ +# https://gist.github.com/YodaEmbedding/c2c77dc693d11f3734d78489f9a6eea4 +AccessModifierOffset: -2 +AlignAfterOpenBracket: BlockIndent # New in v14. For earlier clang-format versions, use AlwaysBreak instead. +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: DontAlign +AlignOperands: false +AlignTrailingComments: false +AllowAllArgumentsOnNextLine: false +# AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Empty +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Empty +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: AfterColon +BreakInheritanceList: AfterColon +BreakStringLiterals: false +ColumnLimit: 80 +CompactNamespaces: false +# ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +FixNamespaceComments: true +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: BeforeHash +IndentWidth: 2 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: false +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: Inner +PointerAlignment: Left +ReferenceAlignment: Left # New in v13. int &name ==> int& name +ReflowComments: false +SeparateDefinitionBlocks: Always # New in v14. +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: true +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: false +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +# SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 2 +UseTab: Never + +# extras +BreakAfterAttributes: Always # v16. [[attribute]] ⏎ void f(); +EmptyLineAfterAccessModifier: Never # v13. +EmptyLineBeforeAccessModifier: Always # v12. +InsertBraces: true # v15. if (true) return; ==> if (true) { return; } +InsertNewlineAtEOF: true # v16. +LineEnding: LF # v16. +QualifierAlignment: Left # v14. const int ==> int const +# RemoveParentheses: Return # v17. return(0) ==> return 0 +RemoveSemicolon: true # v16. \ No newline at end of file diff --git a/cpp/bindings/main.cpp b/cpp/bindings/main.cpp index 83b7f0bd..89548511 100644 --- a/cpp/bindings/main.cpp +++ b/cpp/bindings/main.cpp @@ -9,20 +9,23 @@ namespace nb = nanobind; NB_MODULE(_cpp, m) { m.def( - "abi_tag", []() { return nb::detail::abi_tag(); }, - "Get the ABI tag of the current module. Useful for debugging when adding " - "external pipelines."); + "abi_tag", + []() { return nb::detail::abi_tag(); }, + "Get the ABI tag of the current module. Useful for debugging when adding " + "external pipelines." + ); auto m_types = m.def_submodule( - "types", - "Common types used for conversion between datasets and pipelines."); + "types", + "Common types used for conversion between datasets and pipelines." + ); evalio::makeTypes(m_types); - auto m_helpers = m.def_submodule( - "_helpers", "Helper functions for internal evalio usage."); + auto m_helpers = + m.def_submodule("_helpers", "Helper functions for internal evalio usage."); evalio::makeConversions(m_helpers); auto m_pipelines = m.def_submodule("pipelines", "Pipelines used in evalio."); evalio::makeBasePipeline(m_pipelines); evalio::makePipelines(m_pipelines); -} \ No newline at end of file +} diff --git a/cpp/bindings/pipeline.h b/cpp/bindings/pipeline.h index 3ee25c23..27908160 100644 --- a/cpp/bindings/pipeline.h +++ b/cpp/bindings/pipeline.h @@ -16,69 +16,118 @@ using namespace nb::literals; namespace evalio { -class PyPipeline : public evalio::Pipeline { +class PyPipeline: public evalio::Pipeline { public: NB_TRAMPOLINE(Pipeline, 9); // Getters - const evalio::SE3 pose() override { NB_OVERRIDE_PURE(pose); } - const std::vector map() override { NB_OVERRIDE_PURE(map); } + evalio::SE3 const pose() override { + NB_OVERRIDE_PURE(pose); + } + + const std::vector map() override { + NB_OVERRIDE_PURE(map); + } // Setters void set_imu_params(evalio::ImuParams params) override { NB_OVERRIDE_PURE(set_imu_params, params); } + void set_lidar_params(evalio::LidarParams params) override { NB_OVERRIDE_PURE(set_lidar_params, params); } + void set_imu_T_lidar(evalio::SE3 T) override { NB_OVERRIDE_PURE(set_imu_T_lidar, T); } + void set_params(std::map params) override { NB_OVERRIDE_PURE(set_params, params); } // Doers - void initialize() override { NB_OVERRIDE_PURE(initialize); } + void initialize() override { + NB_OVERRIDE_PURE(initialize); + } + void add_imu(evalio::ImuMeasurement mm) override { NB_OVERRIDE_PURE(add_imu, mm); } + std::vector add_lidar(evalio::LidarMeasurement mm) override { NB_OVERRIDE_PURE(add_lidar, mm); } }; -inline void makeBasePipeline(nb::module_ &m) { +inline void makeBasePipeline(nb::module_& m) { nb::class_(m, "Pipeline") - .def(nb::init<>(), "Construct a new pipeline.") - .def_static("name", &evalio::Pipeline::name, "Name of the pipeline.") - .def_static("default_params", &evalio::Pipeline::default_params, - "Default parameters for the pipeline.") - .def_static("url", &evalio::Pipeline::url, - "URL for more information about the pipeline.") - .def_static("version", &evalio::Pipeline::version, - "Version of the pipeline.") - .def("pose", &evalio::Pipeline::pose, "Most recent pose estimate.") - .def("map", &evalio::Pipeline::map, "Map of the environment.") - .def("initialize", &evalio::Pipeline::initialize, - "Initialize the pipeline. Must be called after constructing the " - "object and before setting parameters.") - .def("add_imu", &evalio::Pipeline::add_imu, "mm"_a, - "Register an IMU measurement.") - .def("add_lidar", &evalio::Pipeline::add_lidar, "mm"_a, - "Register a LiDAR measurement.") - .def("set_params", &evalio::Pipeline::set_params, "params"_a, - "Set parameters for the pipeline. This will override any default " - "parameters.") - .def("set_imu_params", &evalio::Pipeline::set_imu_params, "params"_a, - "Set IMU parameters for the pipeline.") - .def("set_lidar_params", &evalio::Pipeline::set_lidar_params, "params"_a, - "Set LiDAR parameters for the pipeline.") - .def("set_imu_T_lidar", &evalio::Pipeline::set_imu_T_lidar, "T"_a, - "Set the transformation from IMU to LiDAR frame.") - .doc() = "Base class for all pipelines. This class defines the interface " - "for interacting with pipelines, and is intended to be " - "subclassed by specific implementations."; + .def(nb::init<>(), "Construct a new pipeline.") + .def_static("name", &evalio::Pipeline::name, "Name of the pipeline.") + .def_static( + "default_params", + &evalio::Pipeline::default_params, + "Default parameters for the pipeline." + ) + .def_static( + "url", + &evalio::Pipeline::url, + "URL for more information about the pipeline." + ) + .def_static( + "version", + &evalio::Pipeline::version, + "Version of the pipeline." + ) + .def("pose", &evalio::Pipeline::pose, "Most recent pose estimate.") + .def("map", &evalio::Pipeline::map, "Map of the environment.") + .def( + "initialize", + &evalio::Pipeline::initialize, + "Initialize the pipeline. Must be called after constructing the " + "object and before setting parameters." + ) + .def( + "add_imu", + &evalio::Pipeline::add_imu, + "mm"_a, + "Register an IMU measurement." + ) + .def( + "add_lidar", + &evalio::Pipeline::add_lidar, + "mm"_a, + "Register a LiDAR measurement." + ) + .def( + "set_params", + &evalio::Pipeline::set_params, + "params"_a, + "Set parameters for the pipeline. This will override any default " + "parameters." + ) + .def( + "set_imu_params", + &evalio::Pipeline::set_imu_params, + "params"_a, + "Set IMU parameters for the pipeline." + ) + .def( + "set_lidar_params", + &evalio::Pipeline::set_lidar_params, + "params"_a, + "Set LiDAR parameters for the pipeline." + ) + .def( + "set_imu_T_lidar", + &evalio::Pipeline::set_imu_T_lidar, + "T"_a, + "Set the transformation from IMU to LiDAR frame." + ) + .doc() = + "Base class for all pipelines. This class defines the interface " + "for interacting with pipelines, and is intended to be " + "subclassed by specific implementations."; } -} // namespace evalio \ No newline at end of file +} // namespace evalio diff --git a/cpp/bindings/pipelines/bindings.h b/cpp/bindings/pipelines/bindings.h index 81dc8d1f..587871ee 100644 --- a/cpp/bindings/pipelines/bindings.h +++ b/cpp/bindings/pipelines/bindings.h @@ -8,40 +8,40 @@ namespace nb = nanobind; using namespace nb::literals; #ifdef EVALIO_KISS_ICP -#include "bindings/pipelines/kiss_icp.h" + #include "bindings/pipelines/kiss_icp.h" #endif #ifdef EVALIO_LIO_SAM -#include "bindings/pipelines/lio_sam.h" + #include "bindings/pipelines/lio_sam.h" #endif namespace evalio { -inline void makePipelines(nb::module_ &m) { +inline void makePipelines(nb::module_& m) { // List all the pipelines here #ifdef EVALIO_KISS_ICP nb::class_(m, "KissICP") - .def(nb::init<>()) - .def_static("name", &KissICP::name) - .def_static("default_params", &KissICP::default_params) - .def_static("url", &KissICP::url) - .def_static("version", &KissICP::version) - .doc() = - "KissICP LiDAR-only pipeline for point cloud registration. KissICP is " - "designed to be simple and easy to use, while still providing good " - "performance with minimal parameter tuning required across datasets."; + .def(nb::init<>()) + .def_static("name", &KissICP::name) + .def_static("default_params", &KissICP::default_params) + .def_static("url", &KissICP::url) + .def_static("version", &KissICP::version) + .doc() = + "KissICP LiDAR-only pipeline for point cloud registration. KissICP is " + "designed to be simple and easy to use, while still providing good " + "performance with minimal parameter tuning required across datasets."; #endif #ifdef EVALIO_LIO_SAM nb::class_(m, "LioSAM") - .def(nb::init<>()) - .def_static("name", &LioSam::name) - .def_static("default_params", &LioSam::default_params) - .def_static("url", &LioSam::url) - .def_static("version", &LioSam::version) - .doc() = - "Lidar-Inertial Smoothing and Mapping (LioSAM) pipeline. LioSAM is an " - "extension of LOAM (=> uses planar and edge features) that additionally " - "utilizes an IMU for initializing ICP steps and for dewarping points"; + .def(nb::init<>()) + .def_static("name", &LioSam::name) + .def_static("default_params", &LioSam::default_params) + .def_static("url", &LioSam::url) + .def_static("version", &LioSam::version) + .doc() = + "Lidar-Inertial Smoothing and Mapping (LioSAM) pipeline. LioSAM is an " + "extension of LOAM (=> uses planar and edge features) that additionally " + "utilizes an IMU for initializing ICP steps and for dewarping points"; #endif } -} // namespace evalio \ No newline at end of file +} // namespace evalio diff --git a/cpp/bindings/pipelines/kiss_icp.h b/cpp/bindings/pipelines/kiss_icp.h index 14f2f808..8df82d8f 100644 --- a/cpp/bindings/pipelines/kiss_icp.h +++ b/cpp/bindings/pipelines/kiss_icp.h @@ -8,13 +8,15 @@ #include "kiss_icp/pipeline/KissICP.hpp" inline evalio::Point to_evalio_point(Eigen::Vector3d point) { - return {.x = point[0], - .y = point[1], - .z = point[2], - .intensity = 0.0, - .t = evalio::Duration::from_sec(0), - .row = 0, - .col = 0}; + return { + .x = point[0], + .y = point[1], + .z = point[2], + .intensity = 0.0, + .t = evalio::Duration::from_sec(0), + .row = 0, + .col = 0 + }; } inline Eigen::Vector3d to_eigen_point(evalio::Point point) { @@ -25,7 +27,7 @@ inline evalio::SE3 to_evalio_se3(Sophus::SE3d pose) { const auto t = pose.translation(); const auto q = pose.unit_quaternion(); const auto rot = - evalio::SO3{.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; + evalio::SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; return evalio::SE3(rot, t); } @@ -33,20 +35,33 @@ inline Sophus::SE3d to_sophus_se3(evalio::SE3 pose) { return Sophus::SE3d(Sophus::SO3d(pose.rot.toEigen()), pose.trans); } -class KissICP : public evalio::Pipeline { +class KissICP: public evalio::Pipeline { public: - KissICP() : config_() {}; + KissICP() : config_() {} // Info - static std::string version() { return XSTR(EVALIO_KISS_ICP); } - static std::string name() { return "kiss"; } - static std::string url() { return "https://github.com/PRBonn/kiss-icp"; } + static std::string version() { + return XSTR(EVALIO_KISS_ICP); + } + + static std::string name() { + return "kiss"; + } + + static std::string url() { + return "https://github.com/PRBonn/kiss-icp"; + } + static std::map default_params() { return { - {"voxel_size", 1.0}, {"min_motion_th", 0.1}, - {"initial_threshold", 2.0}, {"convergence_criterion", 0.0001}, - {"max_num_iterations", 500}, {"max_num_threads", 0}, - {"max_points_per_voxel", 20}, {"deskew", false}, + {"voxel_size", 1.0}, + {"min_motion_th", 0.1}, + {"initial_threshold", 2.0}, + {"convergence_criterion", 0.0001}, + {"max_num_iterations", 500}, + {"max_num_threads", 0}, + {"max_points_per_voxel", 20}, + {"deskew", false}, }; } @@ -67,23 +82,26 @@ class KissICP : public evalio::Pipeline { } // Setters - void set_imu_params(evalio::ImuParams params) override {}; + void set_imu_params(evalio::ImuParams params) override {} + void set_lidar_params(evalio::LidarParams params) override { config_.max_range = params.max_range; config_.min_range = params.min_range; - }; + } + void set_imu_T_lidar(evalio::SE3 T) override { lidar_T_imu_ = to_sophus_se3(T).inverse(); - }; + } void set_params(std::map params) override { - for (auto &[key, value] : params) { + for (auto& [key, value] : params) { if (std::holds_alternative(value)) { if (key == "deskew") { config_.deskew = std::get(value); } else { throw std::invalid_argument( - "Invalid parameter, KissICP doesn't have bool param " + key); + "Invalid parameter, KissICP doesn't have bool param " + key + ); } } else if (std::holds_alternative(value)) { if (key == "max_points_per_voxel") { @@ -94,7 +112,8 @@ class KissICP : public evalio::Pipeline { config_.max_num_threads = std::get(value); } else { throw std::invalid_argument( - "Invalid parameter, KissICP doesn't have int param " + key); + "Invalid parameter, KissICP doesn't have int param " + key + ); } } else if (std::holds_alternative(value)) { if (key == "voxel_size") { @@ -107,11 +126,13 @@ class KissICP : public evalio::Pipeline { config_.convergence_criterion = std::get(value); } else { throw std::invalid_argument( - "Invalid parameter, KissICP doesn't have double param " + key); + "Invalid parameter, KissICP doesn't have double param " + key + ); } } else if (std::holds_alternative(value)) { throw std::invalid_argument( - "Invalid parameter, KissICP doesn't have string param " + key); + "Invalid parameter, KissICP doesn't have string param " + key + ); } else { throw std::invalid_argument("Invalid parameter type"); } @@ -123,7 +144,7 @@ class KissICP : public evalio::Pipeline { kiss_icp_ = std::make_unique(config_); } - void add_imu(evalio::ImuMeasurement mm) override {}; + void add_imu(evalio::ImuMeasurement mm) override {} std::vector add_lidar(evalio::LidarMeasurement mm) override { // Set everything up @@ -139,7 +160,7 @@ class KissICP : public evalio::Pipeline { } // Run through pipeline - const auto &[_, used_points] = kiss_icp_->RegisterFrame(points, timestamps); + const auto& [_, used_points] = kiss_icp_->RegisterFrame(points, timestamps); std::vector result; result.reserve(used_points.size()); for (auto point : used_points) { @@ -152,4 +173,4 @@ class KissICP : public evalio::Pipeline { std::unique_ptr kiss_icp_; kiss_icp::pipeline::KISSConfig config_; Sophus::SE3d lidar_T_imu_; -}; \ No newline at end of file +}; diff --git a/cpp/bindings/pipelines/lio_sam.h b/cpp/bindings/pipelines/lio_sam.h index fdff2250..98336e06 100644 --- a/cpp/bindings/pipelines/lio_sam.h +++ b/cpp/bindings/pipelines/lio_sam.h @@ -1,7 +1,8 @@ #pragma once -#include #include + +#include #include #include @@ -10,8 +11,8 @@ #include "evalio/pipeline.h" #include "evalio/types.h" -inline void to_evalio_point(evalio::Point &ev_point, - const lio_sam::PointXYZIRT &ls_point) { +inline void +to_evalio_point(evalio::Point& ev_point, const lio_sam::PointXYZIRT& ls_point) { ev_point.x = ls_point.x; ev_point.y = ls_point.y; ev_point.z = ls_point.z; @@ -20,16 +21,16 @@ inline void to_evalio_point(evalio::Point &ev_point, ev_point.row = ls_point.ring; } -inline void to_evalio_point(evalio::Point &ev_point, - const lio_sam::PointType &ls_point) { +inline void +to_evalio_point(evalio::Point& ev_point, const lio_sam::PointType& ls_point) { ev_point.x = ls_point.x; ev_point.y = ls_point.y; ev_point.z = ls_point.z; ev_point.intensity = ls_point.intensity; } -inline void to_pcl_point(lio_sam::PointXYZIRT &ls_point, - const evalio::Point &ev_point) { +inline void +to_pcl_point(lio_sam::PointXYZIRT& ls_point, const evalio::Point& ev_point) { ls_point.x = ev_point.x; ls_point.y = ev_point.y; ls_point.z = ev_point.z; @@ -42,53 +43,62 @@ inline evalio::SE3 to_evalio_se3(lio_sam::Odometry pose) { const auto t = pose.position; const auto q = pose.orientation; const auto rot = - evalio::SO3{.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; + evalio::SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; return evalio::SE3(rot, t); } -class LioSam : public evalio::Pipeline { +class LioSam: public evalio::Pipeline { public: - LioSam() : config_(), lidar_T_imu_(evalio::SE3::identity()) {}; + LioSam() : config_(), lidar_T_imu_(evalio::SE3::identity()) {} // Info - static std::string version() { return XSTR(EVALIO_LIO_SAM); } - static std::string name() { return "liosam"; } - static std::string url() { return "https://github.com/contagon/LIO-SAM"; } + static std::string version() { + return XSTR(EVALIO_LIO_SAM); + } + + static std::string name() { + return "liosam"; + } + + static std::string url() { + return "https://github.com/contagon/LIO-SAM"; + } + static std::map default_params() { return { - {"downsampleRate", 1}, - {"edgeThreshold", 1.0}, - {"surfThreshold", 0.1}, - {"edgeFeatureMinValidNum", 10}, - {"surfFeatureMinValidNum", 100}, - - // voxel filter paprams - {"odometrySurfLeafSize", 0.4}, - {"mappingCornerLeafSize", 0.2}, - {"mappingSurfLeafSize", 0.4}, - - {"z_tolerance", 1000.0}, - {"rotation_tolerance", 1000.0}, - - // CPU Params - {"numberOfCores", 4}, - {"mappingProcessInterval", 0.15}, - - // Surrounding map - {"surroundingkeyframeAddingDistThreshold", 1.0}, - {"surroundingkeyframeAddingAngleThreshold", 0.2}, - {"surroundingKeyframeDensity", 2.0}, - {"surroundingKeyframeSearchRadius", 50.0}, - - // global map visualization radius - {"globalMapVisualizationSearchRadius", 1000.0}, - {"globalMapVisualizationPoseDensity", 10.0}, - {"globalMapVisualizationLeafSize", 1.0}, + {"downsampleRate", 1}, + {"edgeThreshold", 1.0}, + {"surfThreshold", 0.1}, + {"edgeFeatureMinValidNum", 10}, + {"surfFeatureMinValidNum", 100}, + + // voxel filter paprams + {"odometrySurfLeafSize", 0.4}, + {"mappingCornerLeafSize", 0.2}, + {"mappingSurfLeafSize", 0.4}, + + {"z_tolerance", 1000.0}, + {"rotation_tolerance", 1000.0}, + + // CPU Params + {"numberOfCores", 4}, + {"mappingProcessInterval", 0.15}, + + // Surrounding map + {"surroundingkeyframeAddingDistThreshold", 1.0}, + {"surroundingkeyframeAddingAngleThreshold", 0.2}, + {"surroundingKeyframeDensity", 2.0}, + {"surroundingKeyframeSearchRadius", 50.0}, + + // global map visualization radius + {"globalMapVisualizationSearchRadius", 1000.0}, + {"globalMapVisualizationPoseDensity", 10.0}, + {"globalMapVisualizationLeafSize", 1.0}, }; } // Getters - const evalio::SE3 pose() override { + evalio::SE3 const pose() override { return to_evalio_se3(lio_sam_->getPose()) * lidar_T_imu_; } @@ -108,24 +118,27 @@ class LioSam : public evalio::Pipeline { config_.imuGyrNoise = params.gyro; config_.imuGyrBiasN = params.gyro_bias; config_.imuGravity = params.gravity[2]; - }; + } + void set_lidar_params(evalio::LidarParams params) override { config_.N_SCAN = params.num_rows; config_.Horizon_SCAN = params.num_columns; config_.lidarMaxRange = params.max_range; config_.lidarMinRange = params.min_range; - }; + } + void set_imu_T_lidar(evalio::SE3 T) override { lidar_T_imu_ = T.inverse(); config_.lidar_P_imu = lidar_T_imu_.trans; config_.lidar_R_imu = lidar_T_imu_.rot.toEigen(); - }; + } void set_params(std::map params) override { - for (auto &[key, value] : params) { + for (auto& [key, value] : params) { if (std::holds_alternative(value)) { throw std::invalid_argument( - "Invalid parameter, KissICP doesn't have bool param " + key); + "Invalid parameter, KissICP doesn't have bool param " + key + ); } else if (std::holds_alternative(value)) { if (key == "edgeFeatureMinValidNum") { config_.edgeFeatureMinValidNum = std::get(value); @@ -137,7 +150,8 @@ class LioSam : public evalio::Pipeline { config_.downsampleRate = std::get(value); } else { throw std::invalid_argument( - "Invalid parameter, LioSAM doesn't have int param " + key); + "Invalid parameter, LioSAM doesn't have int param " + key + ); } } else if (std::holds_alternative(value)) { if (key == "edgeThreshold") { @@ -158,10 +172,10 @@ class LioSam : public evalio::Pipeline { config_.mappingProcessInterval = std::get(value); } else if (key == "surroundingkeyframeAddingDistThreshold") { config_.surroundingkeyframeAddingDistThreshold = - std::get(value); + std::get(value); } else if (key == "surroundingkeyframeAddingAngleThreshold") { config_.surroundingkeyframeAddingAngleThreshold = - std::get(value); + std::get(value); } else if (key == "surroundingKeyframeDensity") { config_.surroundingKeyframeDensity = std::get(value); } else if (key == "surroundingKeyframeSearchRadius") { @@ -174,11 +188,13 @@ class LioSam : public evalio::Pipeline { config_.globalMapVisualizationLeafSize = std::get(value); } else { throw std::invalid_argument( - "Invalid parameter, LioSAM doesn't have double param " + key); + "Invalid parameter, LioSAM doesn't have double param " + key + ); } } else if (std::holds_alternative(value)) { throw std::invalid_argument( - "Invalid parameter, LioSAM doesn't have string param " + key); + "Invalid parameter, LioSAM doesn't have string param " + key + ); } else { throw std::invalid_argument("Invalid parameter type"); } @@ -191,10 +207,13 @@ class LioSam : public evalio::Pipeline { } void add_imu(evalio::ImuMeasurement mm) override { - lio_sam::Imu imuMsg{ - .stamp = mm.stamp.to_sec(), .gyro = mm.gyro, .acc = mm.accel}; + lio_sam::Imu imuMsg { + .stamp = mm.stamp.to_sec(), + .gyro = mm.gyro, + .acc = mm.accel + }; lio_sam_->addImuMeasurement(imuMsg); - }; + } std::vector add_lidar(evalio::LidarMeasurement mm) override { // Set everything up @@ -223,4 +242,4 @@ class LioSam : public evalio::Pipeline { std::unique_ptr lio_sam_; lio_sam::LioSamParams config_; evalio::SE3 lidar_T_imu_; -}; \ No newline at end of file +}; diff --git a/cpp/bindings/ros_pc2.h b/cpp/bindings/ros_pc2.h index 2a86d79f..a6743939 100644 --- a/cpp/bindings/ros_pc2.h +++ b/cpp/bindings/ros_pc2.h @@ -1,12 +1,12 @@ #pragma once -#include -#include #include #include #include #include #include +#include +#include #include #include "evalio/types.h" @@ -44,101 +44,107 @@ struct PointCloudMetadata { }; // ------------------------- Point loading lambdas ------------------------- // -template -std::function data_getter(DataType datatype, - const uint32_t offset) { +template +std::function +data_getter(DataType datatype, const uint32_t offset) { switch (datatype) { - case UINT8: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - case INT8: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - case UINT16: { - return [offset](T &value, const uint8_t *data) noexcept { - value = - static_cast(*reinterpret_cast(data + offset)); - }; - } - case UINT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = - static_cast(*reinterpret_cast(data + offset)); - }; - } - case INT16: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - case INT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - case FLOAT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - case FLOAT64: { - return [offset](T &value, const uint8_t *data) noexcept { - value = static_cast(*reinterpret_cast(data + offset)); - }; - } - default: { - throw std::runtime_error("Unsupported datatype"); - } + case UINT8: { + return [offset](T& value, const uint8_t* data) noexcept { + value = + static_cast(*reinterpret_cast(data + offset)); + }; + } + case INT8: { + return [offset](T& value, const uint8_t* data) noexcept { + value = static_cast(*reinterpret_cast(data + offset)); + }; + } + case UINT16: { + return [offset](T& value, const uint8_t* data) noexcept { + value = + static_cast(*reinterpret_cast(data + offset)); + }; + } + case UINT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = + static_cast(*reinterpret_cast(data + offset)); + }; + } + case INT16: { + return [offset](T& value, const uint8_t* data) noexcept { + value = + static_cast(*reinterpret_cast(data + offset)); + }; + } + case INT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = + static_cast(*reinterpret_cast(data + offset)); + }; + } + case FLOAT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = static_cast(*reinterpret_cast(data + offset)); + }; + } + case FLOAT64: { + return [offset](T& value, const uint8_t* data) noexcept { + value = static_cast(*reinterpret_cast(data + offset)); + }; + } + default: { + throw std::runtime_error("Unsupported datatype"); + } } } // Specialization for Stamp & Duration -template -std::function time_getter(DataType datatype, - const uint32_t offset) { +template +std::function +time_getter(DataType datatype, const uint32_t offset) { switch (datatype) { - case UINT16: { - return [offset](T &value, const uint8_t *data) noexcept { - value = T::from_nsec(*reinterpret_cast(data + offset)); - }; - } - case UINT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = T::from_nsec(*reinterpret_cast(data + offset)); - }; - } - case INT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = T::from_nsec(*reinterpret_cast(data + offset)); - }; - } - case FLOAT32: { - return [offset](T &value, const uint8_t *data) noexcept { - value = T::from_sec(*reinterpret_cast(data + offset)); - }; - } - case FLOAT64: { - return [offset](T &value, const uint8_t *data) noexcept { - value = T::from_sec(*reinterpret_cast(data + offset)); - }; - } - default: { - throw std::runtime_error("Unsupported datatype for time"); - } + case UINT16: { + return [offset](T& value, const uint8_t* data) noexcept { + value = T::from_nsec(*reinterpret_cast(data + offset)); + }; + } + case UINT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = T::from_nsec(*reinterpret_cast(data + offset)); + }; + } + case INT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = T::from_nsec(*reinterpret_cast(data + offset)); + }; + } + case FLOAT32: { + return [offset](T& value, const uint8_t* data) noexcept { + value = T::from_sec(*reinterpret_cast(data + offset)); + }; + } + case FLOAT64: { + return [offset](T& value, const uint8_t* data) noexcept { + value = T::from_sec(*reinterpret_cast(data + offset)); + }; + } + default: { + throw std::runtime_error("Unsupported datatype for time"); + } } } -template std::function blank() { - return [](T &, const uint8_t *) noexcept {}; +template +std::function blank() { + return [](T&, const uint8_t*) noexcept {}; } -inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, - const std::vector &fields, - const uint8_t *data) { +inline evalio::LidarMeasurement pc2_to_evalio( + const PointCloudMetadata& msg, + const std::vector& fields, + const uint8_t* data +) { if (msg.is_bigendian) { throw std::runtime_error("Big endian not supported yet"); } @@ -155,7 +161,7 @@ inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, std::function func_range = blank(); std::function func_row = blank(); - for (const auto &field : fields) { + for (const auto& field : fields) { if (field.name == "x") { func_x = data_getter(field.datatype, field.offset); } else if (field.name == "y") { @@ -164,15 +170,15 @@ inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, func_z = data_getter(field.datatype, field.offset); } else if (field.name == "intensity") { func_intensity = data_getter(field.datatype, field.offset); - } else if (field.name == "t" || field.name == "time" || - field.name == "stamp" || field.name == "time_offset" || - field.name == "timeOffset" || field.name == "timestamp") { + } else if (field.name == "t" || field.name == "time" + || field.name == "stamp" || field.name == "time_offset" + || field.name == "timeOffset" || field.name == "timestamp") { func_duration = time_getter(field.datatype, field.offset); func_stamp = time_getter(field.datatype, field.offset); } else if (field.name == "range") { func_range = data_getter(field.datatype, field.offset); - } else if (field.name == "row" || field.name == "ring" || - field.name == "channel") { + } else if (field.name == "row" || field.name == "ring" + || field.name == "channel") { func_row = data_getter(field.datatype, field.offset); } } @@ -181,13 +187,13 @@ inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, // relative. We'll handle relative to start/end of scan later Duration t; func_duration(t, data); - std::function func_t = func_duration; + std::function func_t = func_duration; // Convert from absolute time if (t.to_sec() > 100.0) { Stamp scan_stamp = mm.stamp; - func_t = [func_stamp, scan_stamp](Duration &duration, - const uint8_t *data) noexcept { + func_t = [func_stamp, + scan_stamp](Duration& duration, const uint8_t* data) noexcept { Stamp absolute_stamp; func_stamp(absolute_stamp, data); duration = absolute_stamp - scan_stamp; @@ -197,7 +203,7 @@ inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, } size_t index = 0; - for (evalio::Point &point : mm.points) { + for (evalio::Point& point : mm.points) { const auto pointStart = data + static_cast(index * msg.point_step); func_x(point.x, pointStart); func_y(point.y, pointStart); @@ -214,11 +220,15 @@ inline evalio::LidarMeasurement pc2_to_evalio(const PointCloudMetadata &msg, // -------------------- Helpers to fill out column index -------------------- // // Iterates through points to fill in columns -inline void -_fill_col(LidarMeasurement &mm, - std::function - func_col) { +inline void _fill_col( + LidarMeasurement& mm, + std::function func_col +) { // fill out the first one to kickstart uint16_t prev_col = 0; uint8_t prev_row = mm.points[0].row; @@ -230,9 +240,13 @@ _fill_col(LidarMeasurement &mm, } // Fills in column index for row major order -inline void fill_col_row_major(LidarMeasurement &mm) { - auto func_col = [](uint16_t &col, const uint16_t &prev_col, - const uint8_t &prev_row, const uint8_t &curr_row) { +inline void fill_col_row_major(LidarMeasurement& mm) { + auto func_col = []( + uint16_t& col, + const uint16_t& prev_col, + const uint8_t& prev_row, + const uint8_t& curr_row + ) { if (prev_row != curr_row) { col = 0; } else { @@ -244,9 +258,13 @@ inline void fill_col_row_major(LidarMeasurement &mm) { } // Fills in column index for column major order -inline void fill_col_col_major(LidarMeasurement &mm) { - auto func_col = [](uint16_t &col, const uint16_t &prev_col, - const uint8_t &prev_row, const uint8_t &curr_row) { +inline void fill_col_col_major(LidarMeasurement& mm) { + auto func_col = []( + uint16_t& col, + const uint16_t& prev_col, + const uint8_t& prev_row, + const uint8_t& curr_row + ) { if (curr_row < prev_row) { col = prev_col + 1; } else { @@ -258,7 +276,7 @@ inline void fill_col_col_major(LidarMeasurement &mm) { } // point cloud loader where rows come in in 0, 8, 1, 9, ... order -inline void fill_col_split_row_velodyne(LidarMeasurement &mm) { +inline void fill_col_split_row_velodyne(LidarMeasurement& mm) { auto func_row_idx_to_row_seq = [](uint8_t row_idx) { if (row_idx < 8) { return row_idx * 2; @@ -268,8 +286,11 @@ inline void fill_col_split_row_velodyne(LidarMeasurement &mm) { }; auto func_col = [&func_row_idx_to_row_seq]( - uint16_t &col, const uint16_t &prev_col, - const uint8_t &prev_row, const uint8_t &curr_row) { + uint16_t& col, + const uint16_t& prev_col, + const uint8_t& prev_row, + const uint8_t& curr_row + ) { if (func_row_idx_to_row_seq(curr_row) < func_row_idx_to_row_seq(prev_row)) { col = prev_col + 1; } else { @@ -281,8 +302,8 @@ inline void fill_col_split_row_velodyne(LidarMeasurement &mm) { } // ------------------------- Helpers for reordering ------------------------- // -inline void reorder_points(LidarMeasurement &mm, size_t num_rows, - size_t num_cols) { +inline void +reorder_points(LidarMeasurement& mm, size_t num_rows, size_t num_cols) { std::vector points_original = mm.points; mm.points = std::vector(num_rows * num_cols); @@ -299,8 +320,8 @@ inline void reorder_points(LidarMeasurement &mm, size_t num_rows, } // ------------------------- Shift point stamps ------------------------- // -inline void shift_point_stamps(LidarMeasurement &mm, const Duration &shift) { - for (auto &p : mm.points) { +inline void shift_point_stamps(LidarMeasurement& mm, const Duration& shift) { + for (auto& p : mm.points) { p.t = p.t + shift; } } @@ -311,12 +332,14 @@ inline void shift_point_stamps(LidarMeasurement &mm, const Duration &shift) { // major, its nigh impossible to infer where these were along a scan line for // now we just tack them on the end Largely borrowed from // https://github.com/minwoo0611/HeLiPR-File-Player/blob/501b338c4be1070fc61a438177c3c0e22b628b30/src/ROSThread.cpp#L444-L454 -inline LidarMeasurement helipr_bin_to_evalio(const std::string &filename, - Stamp stamp, - const LidarParams ¶ms) { +inline LidarMeasurement helipr_bin_to_evalio( + const std::string& filename, + Stamp stamp, + const LidarParams& params +) { LidarMeasurement mm(stamp); mm.points.resize(params.num_columns * params.num_rows); - for (auto &p : mm.points) { + for (auto& p : mm.points) { p = Point(); } @@ -364,42 +387,63 @@ inline LidarMeasurement helipr_bin_to_evalio(const std::string &filename, } // ---------------------- Create python bindings ---------------------- // -inline void makeConversions(nb::module_ &m) { +inline void makeConversions(nb::module_& m) { nb::enum_(m, "DataType") - .value("UINT8", DataType::UINT8) - .value("INT8", DataType::INT8) - .value("UINT16", DataType::UINT16) - .value("UINT32", DataType::UINT32) - .value("INT16", DataType::INT16) - .value("INT32", DataType::INT32) - .value("FLOAT32", DataType::FLOAT32) - .value("FLOAT64", DataType::FLOAT64); + .value("UINT8", DataType::UINT8) + .value("INT8", DataType::INT8) + .value("UINT16", DataType::UINT16) + .value("UINT32", DataType::UINT32) + .value("INT16", DataType::INT16) + .value("INT32", DataType::INT32) + .value("FLOAT32", DataType::FLOAT32) + .value("FLOAT64", DataType::FLOAT64); nb::class_(m, "Field") - .def(nb::init(), nb::kw_only(), "name"_a, - "datatype"_a, "offset"_a) - .def_rw("name", &Field::name) - .def_rw("datatype", &Field::datatype) - .def_rw("offset", &Field::offset); + .def( + nb::init(), + nb::kw_only(), + "name"_a, + "datatype"_a, + "offset"_a + ) + .def_rw("name", &Field::name) + .def_rw("datatype", &Field::datatype) + .def_rw("offset", &Field::offset); nb::class_(m, "PointCloudMetadata") - .def(nb::init(), - nb::kw_only(), "stamp"_a, "width"_a, "height"_a, "point_step"_a, - "row_step"_a, "is_bigendian"_a, "is_dense"_a) - .def_rw("stamp", &PointCloudMetadata::stamp) - .def_rw("width", &PointCloudMetadata::width) - .def_rw("height", &PointCloudMetadata::height) - .def_rw("point_step", &PointCloudMetadata::point_step) - .def_rw("row_step", &PointCloudMetadata::row_step) - .def_rw("is_bigendian", &PointCloudMetadata::is_bigendian) - .def_rw("is_dense", &PointCloudMetadata::is_dense); - - m.def("pc2_to_evalio", - [](const PointCloudMetadata &msg, const std::vector &fields, - nb::bytes c) -> evalio::LidarMeasurement { - return pc2_to_evalio(msg, fields, - reinterpret_cast(c.c_str())); - }); + .def( + nb::init(), + nb::kw_only(), + "stamp"_a, + "width"_a, + "height"_a, + "point_step"_a, + "row_step"_a, + "is_bigendian"_a, + "is_dense"_a + ) + .def_rw("stamp", &PointCloudMetadata::stamp) + .def_rw("width", &PointCloudMetadata::width) + .def_rw("height", &PointCloudMetadata::height) + .def_rw("point_step", &PointCloudMetadata::point_step) + .def_rw("row_step", &PointCloudMetadata::row_step) + .def_rw("is_bigendian", &PointCloudMetadata::is_bigendian) + .def_rw("is_dense", &PointCloudMetadata::is_dense); + + m.def( + "pc2_to_evalio", + []( + const PointCloudMetadata& msg, + const std::vector& fields, + nb::bytes c + ) -> evalio::LidarMeasurement { + return pc2_to_evalio( + msg, + fields, + reinterpret_cast(c.c_str()) + ); + } + ); m.def("fill_col_row_major", &fill_col_row_major); m.def("fill_col_col_major", &fill_col_col_major); @@ -412,4 +456,4 @@ inline void makeConversions(nb::module_ &m) { m.def("fill_col_split_row_velodyne", &fill_col_split_row_velodyne); } -} // namespace evalio \ No newline at end of file +} // namespace evalio diff --git a/cpp/bindings/types.h b/cpp/bindings/types.h index ec5940b2..0b5b8b02 100644 --- a/cpp/bindings/types.h +++ b/cpp/bindings/types.h @@ -1,5 +1,4 @@ #pragma once -#include #include #include #include @@ -7,6 +6,8 @@ #include #include +#include + #include "evalio/types.h" namespace nb = nanobind; @@ -16,305 +17,525 @@ namespace evalio { // TODO: Check if copy/deepcopy works or not -inline void makeTypes(nb::module_ &m) { +inline void makeTypes(nb::module_& m) { nb::class_(m, "Duration") - .def_static("from_sec", &Duration::from_sec, "sec"_a, - "Create a Duration from seconds") - .def_static("from_nsec", &Duration::from_nsec, "nsec"_a, - "Create a Duration from nanoseconds") - .def("to_sec", &Duration::to_sec, "Convert to seconds") - .def("to_nsec", &Duration::to_nsec, "Convert to nanoseconds") - .def_ro("nsec", &Duration::nsec, "Underlying nanoseconds representation") - .def(nb::self < nb::self, "Compare two Durations") - .def(nb::self > nb::self, "Compare two Durations") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def(nb::self - nb::self, "Compute the difference between two Durations") - .def(nb::self + nb::self, "Add two Durations") - .def("__repr__", &Duration::toString) - .def("__copy__", [](const Duration &self) { return Duration(self); }) - .def( - "__deepcopy__", - [](const Duration &self, nb::dict) { return Duration(self); }, - "memo"_a) - .def("__getstate__", - [](const Duration &p) { return nb::make_tuple(p.nsec); }) - .def("__setstate__", - [](Duration &p, std::tuple t) { - new (&p) Duration{.nsec = std::get<0>(t)}; - }) - .doc() = - "Duration class for representing a positive or negative delta time, uses " - "int64 as the underlying data storage for nanoseconds."; + .def_static( + "from_sec", + &Duration::from_sec, + "sec"_a, + "Create a Duration from seconds" + ) + .def_static( + "from_nsec", + &Duration::from_nsec, + "nsec"_a, + "Create a Duration from nanoseconds" + ) + .def("to_sec", &Duration::to_sec, "Convert to seconds") + .def("to_nsec", &Duration::to_nsec, "Convert to nanoseconds") + .def_ro("nsec", &Duration::nsec, "Underlying nanoseconds representation") + .def(nb::self < nb::self, "Compare two Durations") + .def(nb::self > nb::self, "Compare two Durations") + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def(nb::self - nb::self, "Compute the difference between two Durations") + .def(nb::self + nb::self, "Add two Durations") + .def("__repr__", &Duration::toString) + .def("__copy__", [](const Duration& self) { return Duration(self); }) + .def( + "__deepcopy__", + [](const Duration& self, nb::dict) { return Duration(self); }, + "memo"_a + ) + .def( + "__getstate__", + [](const Duration& p) { return nb::make_tuple(p.nsec); } + ) + .def( + "__setstate__", + [](Duration& p, std::tuple t) { + new (&p) Duration {.nsec = std::get<0>(t)}; + } + ) + .doc() = + "Duration class for representing a positive or negative delta time, uses " + "int64 as the underlying data storage for nanoseconds."; nb::class_(m, "Stamp") - .def(nb::init(), nb::kw_only(), "sec"_a, "nsec"_a, - "Create a Stamp from seconds and nanoseconds") - .def_static("from_sec", &Stamp::from_sec, "sec"_a, - "Create a Stamp from seconds") - .def_static("from_nsec", &Stamp::from_nsec, "nsec"_a, - "Create a Stamp from nanoseconds") - .def("to_sec", &Stamp::to_sec, "Convert to seconds") - .def("to_nsec", &Stamp::to_nsec, "Convert to nanoseconds") - .def_ro("sec", &Stamp::sec, "Underlying seconds storage") - .def_ro("nsec", &Stamp::nsec, "Underlying nanoseconds storage") - .def(nb::self < nb::self, - "Compare two Stamps to see which happened first") - .def(nb::self > nb::self, - "Compare two Stamps to see which happened first") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for equality") - .def(nb::self - nb::self, - "Compute the difference between two Stamps, returning a duration") - .def(nb::self + Duration(), "Add a Duration to a Stamp") - .def(nb::self - Duration(), "Subtract a Duration from a Stamp") - .def("__repr__", &Stamp::toString) - .def("__copy__", [](const Stamp &self) { return Stamp(self); }) - .def( - "__deepcopy__", - [](const Stamp &self, nb::dict) { return Stamp(self); }, "memo"_a) - .def("__getstate__", - [](const Stamp &p) { return nb::make_tuple(p.sec, p.nsec); }) - .def("__setstate__", - [](Stamp &p, std::tuple t) { - new (&p) Stamp{.sec = std::get<0>(t), .nsec = std::get<1>(t)}; - }) - .doc() = - "Stamp class for representing an absolute point in time, uses uint32 as " - "the underlying data storage for seconds and nanoseconds."; + .def( + nb::init(), + nb::kw_only(), + "sec"_a, + "nsec"_a, + "Create a Stamp from seconds and nanoseconds" + ) + .def_static( + "from_sec", + &Stamp::from_sec, + "sec"_a, + "Create a Stamp from seconds" + ) + .def_static( + "from_nsec", + &Stamp::from_nsec, + "nsec"_a, + "Create a Stamp from nanoseconds" + ) + .def("to_sec", &Stamp::to_sec, "Convert to seconds") + .def("to_nsec", &Stamp::to_nsec, "Convert to nanoseconds") + .def_ro("sec", &Stamp::sec, "Underlying seconds storage") + .def_ro("nsec", &Stamp::nsec, "Underlying nanoseconds storage") + .def(nb::self < nb::self, "Compare two Stamps to see which happened first") + .def(nb::self > nb::self, "Compare two Stamps to see which happened first") + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for equality") + .def( + nb::self - nb::self, + "Compute the difference between two Stamps, returning a duration" + ) + .def(nb::self + Duration(), "Add a Duration to a Stamp") + .def(nb::self - Duration(), "Subtract a Duration from a Stamp") + .def("__repr__", &Stamp::toString) + .def("__copy__", [](const Stamp& self) { return Stamp(self); }) + .def( + "__deepcopy__", + [](const Stamp& self, nb::dict) { return Stamp(self); }, + "memo"_a + ) + .def( + "__getstate__", + [](const Stamp& p) { return nb::make_tuple(p.sec, p.nsec); } + ) + .def( + "__setstate__", + [](Stamp& p, std::tuple t) { + new (&p) Stamp {.sec = std::get<0>(t), .nsec = std::get<1>(t)}; + } + ) + .doc() = + "Stamp class for representing an absolute point in time, uses uint32 as " + "the underlying data storage for seconds and nanoseconds."; ; // Lidar nb::class_(m, "Point") - .def(nb::init(), - nb::kw_only(), "x"_a = 0, "y"_a = 0, "z"_a = 0, "intensity"_a = 0, - "t"_a = Duration::from_sec(0.0), "range"_a = 0, "row"_a = 0, - "col"_a = 0, - "Create a Point from x, y, z, intensity, t, range, row, col") - .def_rw("x", &Point::x, "X coordinate") - .def_rw("y", &Point::y, "Y coordinate") - .def_rw("z", &Point::z, "Z coordinate") - .def_rw("intensity", &Point::intensity, "Intensity value as a float.") - .def_rw("range", &Point::range, "Range value as a uint32.") - .def_rw("t", &Point::t, - "Timestamp of the point as a Duration. In evalio, this is always " - "relative to the point cloud stamp, which occurs at the start of " - "the scan.") - .def_rw("row", &Point::row, - "Row index of the point in the point cloud. Also known as the " - "scanline index.") - .def_rw("col", &Point::col, - "Column index of the point in the point cloud.") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def("__repr__", &Point::toString) - .def("__getstate__", - [](const Point &p) { - return std::make_tuple(p.x, p.y, p.z, p.intensity, p.t, p.range, - p.row, p.col); - }) - .def("__setstate__", - [](Point &p, std::tuple - t) { - new (&p) Point{.x = std::get<0>(t), - .y = std::get<1>(t), - .z = std::get<2>(t), - .intensity = std::get<3>(t), - .t = std::get<4>(t), - .range = std::get<5>(t), - .row = std::get<6>(t), - .col = std::get<7>(t)}; - }) - .doc() = "Point is a general point structure in evalio, with common " - "point cloud attributes included."; + .def( + nb::init< + double, + double, + double, + double, + Duration, + uint32_t, + uint8_t, + uint16_t>(), + nb::kw_only(), + "x"_a = 0, + "y"_a = 0, + "z"_a = 0, + "intensity"_a = 0, + "t"_a = Duration::from_sec(0.0), + "range"_a = 0, + "row"_a = 0, + "col"_a = 0, + "Create a Point from x, y, z, intensity, t, range, row, col" + ) + .def_rw("x", &Point::x, "X coordinate") + .def_rw("y", &Point::y, "Y coordinate") + .def_rw("z", &Point::z, "Z coordinate") + .def_rw("intensity", &Point::intensity, "Intensity value as a float.") + .def_rw("range", &Point::range, "Range value as a uint32.") + .def_rw( + "t", + &Point::t, + "Timestamp of the point as a Duration. In evalio, this is always " + "relative to the point cloud stamp, which occurs at the start of " + "the scan." + ) + .def_rw( + "row", + &Point::row, + "Row index of the point in the point cloud. Also known as the " + "scanline index." + ) + .def_rw("col", &Point::col, "Column index of the point in the point cloud.") + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def("__repr__", &Point::toString) + .def( + "__getstate__", + [](const Point& p) { + return std::make_tuple( + p.x, + p.y, + p.z, + p.intensity, + p.t, + p.range, + p.row, + p.col + ); + } + ) + .def( + "__setstate__", + []( + Point& p, + std::tuple< + double, + double, + double, + double, + Duration, + uint32_t, + uint8_t, + uint16_t> t + ) { + new (&p) Point { + .x = std::get<0>(t), + .y = std::get<1>(t), + .z = std::get<2>(t), + .intensity = std::get<3>(t), + .t = std::get<4>(t), + .range = std::get<5>(t), + .row = std::get<6>(t), + .col = std::get<7>(t) + }; + } + ) + .doc() = + "Point is a general point structure in evalio, with common " + "point cloud attributes included."; nb::class_(m, "LidarMeasurement") - .def(nb::init(), "stamp"_a) - .def(nb::init>(), "stamp"_a, "points"_a) - .def_rw("stamp", &LidarMeasurement::stamp, - "Timestamp of the point cloud, always at the start of the scan.") - .def_rw("points", &LidarMeasurement::points, - "List of points in the " - "point cloud. Note, this is always in row major format.") - .def("to_vec_positions", &LidarMeasurement::to_vec_positions, - "Convert the point cloud to a (n,3) numpy array.") - .def("to_vec_stamps", &LidarMeasurement::to_vec_stamps, - "Convert the point stamps to a list of durations.") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def("__repr__", &LidarMeasurement::toString) - .def("__getstate__", - [](const LidarMeasurement &p) { - return std::make_tuple(p.stamp, p.points); - }) - .def("__setstate__", - [](LidarMeasurement &p, std::tuple> t) { - new (&p) LidarMeasurement(std::get<0>(t), std::get<1>(t)); - }) - .doc() = "LidarMeasurement is a structure for storing a point cloud " - "measurement, with a timestamp and a vector of points. Note, " - "the stamp always represents the _start_ of the scan. " - "Additionally, the points are always in row major format."; + .def(nb::init(), "stamp"_a) + .def(nb::init>(), "stamp"_a, "points"_a) + .def_rw( + "stamp", + &LidarMeasurement::stamp, + "Timestamp of the point cloud, always at the start of the scan." + ) + .def_rw( + "points", + &LidarMeasurement::points, + "List of points in the " + "point cloud. Note, this is always in row major format." + ) + .def( + "to_vec_positions", + &LidarMeasurement::to_vec_positions, + "Convert the point cloud to a (n,3) numpy array." + ) + .def( + "to_vec_stamps", + &LidarMeasurement::to_vec_stamps, + "Convert the point stamps to a list of durations." + ) + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def("__repr__", &LidarMeasurement::toString) + .def( + "__getstate__", + [](const LidarMeasurement& p) { + return std::make_tuple(p.stamp, p.points); + } + ) + .def( + "__setstate__", + [](LidarMeasurement& p, std::tuple> t) { + new (&p) LidarMeasurement(std::get<0>(t), std::get<1>(t)); + } + ) + .doc() = + "LidarMeasurement is a structure for storing a point cloud " + "measurement, with a timestamp and a vector of points. Note, " + "the stamp always represents the _start_ of the scan. " + "Additionally, the points are always in row major format."; nb::class_(m, "LidarParams") - .def(nb::init(), - nb::kw_only(), "num_rows"_a, "num_columns"_a, "min_range"_a, - "max_range"_a, "rate"_a = 10.0, "brand"_a = "-", "model"_a = "-") - .def_ro("num_rows", &LidarParams::num_rows, - "Number of rows in the point cloud, also known as the scanlines.") - .def_ro("num_columns", &LidarParams::num_columns, - "Number of columns in the point cloud, also known as the number " - "of points per scanline.") - .def_ro("min_range", &LidarParams::min_range, - "Minimum range of the lidar sensor, in meters.") - .def_ro("max_range", &LidarParams::max_range, - "Maximum range of the lidar sensor, in meters.") - .def_ro("rate", &LidarParams::rate, "Rate of the lidar sensor, in Hz.") - .def_ro("brand", &LidarParams::brand, "Brand of the lidar sensor.") - .def_ro("model", &LidarParams::model, "Model of the lidar sensor.") - .def("delta_time", &LidarParams::delta_time, - "Get the time between two consecutive scans as a Duration. Inverse " - "of the rate.") - .def("__repr__", &LidarParams::toString) - .doc() = "LidarParams is a structure for storing the parameters of a " - "lidar sensor."; + .def( + nb::init(), + nb::kw_only(), + "num_rows"_a, + "num_columns"_a, + "min_range"_a, + "max_range"_a, + "rate"_a = 10.0, + "brand"_a = "-", + "model"_a = "-" + ) + .def_ro( + "num_rows", + &LidarParams::num_rows, + "Number of rows in the point cloud, also known as the scanlines." + ) + .def_ro( + "num_columns", + &LidarParams::num_columns, + "Number of columns in the point cloud, also known as the number " + "of points per scanline." + ) + .def_ro( + "min_range", + &LidarParams::min_range, + "Minimum range of the lidar sensor, in meters." + ) + .def_ro( + "max_range", + &LidarParams::max_range, + "Maximum range of the lidar sensor, in meters." + ) + .def_ro("rate", &LidarParams::rate, "Rate of the lidar sensor, in Hz.") + .def_ro("brand", &LidarParams::brand, "Brand of the lidar sensor.") + .def_ro("model", &LidarParams::model, "Model of the lidar sensor.") + .def( + "delta_time", + &LidarParams::delta_time, + "Get the time between two consecutive scans as a Duration. Inverse " + "of the rate." + ) + .def("__repr__", &LidarParams::toString) + .doc() = + "LidarParams is a structure for storing the parameters of a " + "lidar sensor."; // Imu nb::class_(m, "ImuMeasurement") - .def(nb::init(), "stamp"_a, - "gyro"_a, "accel"_a) - .def_ro("stamp", &ImuMeasurement::stamp, - "Timestamp of the IMU measurement.") - .def_ro("gyro", &ImuMeasurement::gyro, - "Gyroscope measurement as a 3D vector.") - .def_ro("accel", &ImuMeasurement::accel, - "Accelerometer measurement as a 3D vector.") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def("__repr__", &ImuMeasurement::toString) - .def("__getstate__", - [](const ImuMeasurement &p) { - return std::make_tuple(p.stamp, p.gyro, p.accel); - }) - .def("__setstate__", - [](ImuMeasurement &p, - std::tuple t) { - new (&p) ImuMeasurement{.stamp = std::get<0>(t), - .gyro = std::get<1>(t), - .accel = std::get<2>(t)}; - }) + .def( + nb::init(), + "stamp"_a, + "gyro"_a, + "accel"_a + ) + .def_ro( + "stamp", + &ImuMeasurement::stamp, + "Timestamp of the IMU measurement." + ) + .def_ro( + "gyro", + &ImuMeasurement::gyro, + "Gyroscope measurement as a 3D vector." + ) + .def_ro( + "accel", + &ImuMeasurement::accel, + "Accelerometer measurement as a 3D vector." + ) + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def("__repr__", &ImuMeasurement::toString) + .def( + "__getstate__", + [](const ImuMeasurement& p) { + return std::make_tuple(p.stamp, p.gyro, p.accel); + } + ) + .def( + "__setstate__", + []( + ImuMeasurement& p, + std::tuple t + ) { + new (&p) ImuMeasurement { + .stamp = std::get<0>(t), + .gyro = std::get<1>(t), + .accel = std::get<2>(t) + }; + } + ) - .doc() = - "ImuMeasurement is a simple structure for storing an IMU measurement."; + .doc() = + "ImuMeasurement is a simple structure for storing an IMU measurement."; nb::class_(m, "ImuParams") - .def(nb::init(), - nb::kw_only(), "gyro"_a = 1e-5, "accel"_a = 1e-5, - "gyro_bias"_a = 1e-6, "accel_bias"_a = 1e-6, "bias_init"_a = 1e-7, - "integration"_a = 1e-7, "gravity"_a = Eigen::Vector3d(0, 0, 9.81), - "brand"_a = "-", "model"_a = "-") - .def_static("up", &ImuParams::up, - "Simple helper for initializing with an `up` gravity vector.") - .def_static( - "down", &ImuParams::down, - "Simple helper for initializing with a `down` gravity vector.") - .def_ro("gyro", &ImuParams::gyro, - "Gyroscope standard deviation, in rad/s/sqrt(Hz).") - .def_ro("accel", &ImuParams::accel, - "Accelerometer standard deviation, in m/s^2/sqrt(Hz).") - .def_ro("gyro_bias", &ImuParams::gyro_bias, - "Gyroscope bias standard deviation, in rad/s^2/sqrt(Hz).") - .def_ro("accel_bias", &ImuParams::accel_bias, - "Accelerometer bias standard deviation, in m/s^3/sqrt(Hz).") - .def_ro("bias_init", &ImuParams::bias_init, - "Initial bias standard deviation.") - .def_ro("integration", &ImuParams::integration, - "Integration standard deviation.") - .def_ro("gravity", &ImuParams::gravity, "Gravity vector as a 3D vector.") - .def_ro("brand", &ImuParams::brand, "Brand of the IMU sensor.") - .def_ro("model", &ImuParams::model, "Model of the IMU sensor.") - .def("__repr__", &ImuParams::toString) - .doc() = "ImuParams is a structure for storing the parameters of an IMU"; + .def( + nb::init< + double, + double, + double, + double, + double, + double, + Eigen::Vector3d, + std::string, + std::string>(), + nb::kw_only(), + "gyro"_a = 1e-5, + "accel"_a = 1e-5, + "gyro_bias"_a = 1e-6, + "accel_bias"_a = 1e-6, + "bias_init"_a = 1e-7, + "integration"_a = 1e-7, + "gravity"_a = Eigen::Vector3d(0, 0, 9.81), + "brand"_a = "-", + "model"_a = "-" + ) + .def_static( + "up", + &ImuParams::up, + "Simple helper for initializing with an `up` gravity vector." + ) + .def_static( + "down", + &ImuParams::down, + "Simple helper for initializing with a `down` gravity vector." + ) + .def_ro( + "gyro", + &ImuParams::gyro, + "Gyroscope standard deviation, in rad/s/sqrt(Hz)." + ) + .def_ro( + "accel", + &ImuParams::accel, + "Accelerometer standard deviation, in m/s^2/sqrt(Hz)." + ) + .def_ro( + "gyro_bias", + &ImuParams::gyro_bias, + "Gyroscope bias standard deviation, in rad/s^2/sqrt(Hz)." + ) + .def_ro( + "accel_bias", + &ImuParams::accel_bias, + "Accelerometer bias standard deviation, in m/s^3/sqrt(Hz)." + ) + .def_ro( + "bias_init", + &ImuParams::bias_init, + "Initial bias standard deviation." + ) + .def_ro( + "integration", + &ImuParams::integration, + "Integration standard deviation." + ) + .def_ro("gravity", &ImuParams::gravity, "Gravity vector as a 3D vector.") + .def_ro("brand", &ImuParams::brand, "Brand of the IMU sensor.") + .def_ro("model", &ImuParams::model, "Model of the IMU sensor.") + .def("__repr__", &ImuParams::toString) + .doc() = "ImuParams is a structure for storing the parameters of an IMU"; nb::class_(m, "SO3") - .def(nb::init(), nb::kw_only(), "qx"_a, - "qy"_a, "qz"_a, "qw"_a) - .def_ro("qx", &SO3::qx, "X component of the quaternion.") - .def_ro("qy", &SO3::qy, "Y component of the quaternion.") - .def_ro("qz", &SO3::qz, "Z component of the quaternion.") - .def_ro("qw", &SO3::qw, "Scalar component of the quaternion.") - .def_static("identity", &SO3::identity, "Create an identity rotation.") - .def_static("fromMat", &SO3::fromMat, "mat"_a, - "Create a rotation from a 3x3 rotation matrix.") - .def_static("exp", &SO3::exp, "v"_a, - "Create a rotation from a 3D vector.") - .def("inverse", &SO3::inverse, "Compute the inverse of the rotation.") - .def("log", &SO3::log, "Compute the logarithm of the rotation.") - .def("toMat", &SO3::toMat, "Convert the rotation to a 3x3 matrix.") - .def("rotate", &SO3::rotate, "v"_a, "Rotate a 3D vector by the rotation.") - .def(nb::self * nb::self, "Compose two rotations.") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def("__repr__", &SO3::toString) - .def("__copy__", [](const SO3 &self) { return SO3(self); }) - .def( - "__deepcopy__", [](const SO3 &self, nb::dict) { return SO3(self); }, - "memo"_a) - .def("__getstate__", - [](const SO3 &p) { return nb::make_tuple(p.qx, p.qy, p.qz, p.qw); }) - .def("__setstate__", - [](SO3 &p, std::tuple t) { - new (&p) SO3{.qx = std::get<0>(t), - .qy = std::get<1>(t), - .qz = std::get<2>(t), - .qw = std::get<3>(t)}; - }) - .doc() = "SO3 class for representing a 3D rotation using a quaternion. " - "This is outfitted with some basic functionality, but mostly " - "intended for storage and converting between types."; + .def( + nb::init(), + nb::kw_only(), + "qx"_a, + "qy"_a, + "qz"_a, + "qw"_a + ) + .def_ro("qx", &SO3::qx, "X component of the quaternion.") + .def_ro("qy", &SO3::qy, "Y component of the quaternion.") + .def_ro("qz", &SO3::qz, "Z component of the quaternion.") + .def_ro("qw", &SO3::qw, "Scalar component of the quaternion.") + .def_static("identity", &SO3::identity, "Create an identity rotation.") + .def_static( + "fromMat", + &SO3::fromMat, + "mat"_a, + "Create a rotation from a 3x3 rotation matrix." + ) + .def_static("exp", &SO3::exp, "v"_a, "Create a rotation from a 3D vector.") + .def("inverse", &SO3::inverse, "Compute the inverse of the rotation.") + .def("log", &SO3::log, "Compute the logarithm of the rotation.") + .def("toMat", &SO3::toMat, "Convert the rotation to a 3x3 matrix.") + .def("rotate", &SO3::rotate, "v"_a, "Rotate a 3D vector by the rotation.") + .def(nb::self * nb::self, "Compose two rotations.") + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def("__repr__", &SO3::toString) + .def("__copy__", [](const SO3& self) { return SO3(self); }) + .def( + "__deepcopy__", + [](const SO3& self, nb::dict) { return SO3(self); }, + "memo"_a + ) + .def( + "__getstate__", + [](const SO3& p) { return nb::make_tuple(p.qx, p.qy, p.qz, p.qw); } + ) + .def( + "__setstate__", + [](SO3& p, std::tuple t) { + new (&p) SO3 { + .qx = std::get<0>(t), + .qy = std::get<1>(t), + .qz = std::get<2>(t), + .qw = std::get<3>(t) + }; + } + ) + .doc() = + "SO3 class for representing a 3D rotation using a quaternion. " + "This is outfitted with some basic functionality, but mostly " + "intended for storage and converting between types."; nb::class_(m, "SE3") - .def(nb::init(), "rot"_a, "trans"_a, - "Create a SE3 from a rotation and translation.") - .def_static("identity", &SE3::identity, "Create an identity SE3.") - .def_static("fromMat", &SE3::fromMat, "mat"_a, - "Create a SE3 from a 4x4 transformation matrix.") - .def_ro("rot", &SE3::rot, "Rotation as a SO3 object.") - .def_ro("trans", &SE3::trans, "Translation as a 3D vector.") - .def("toMat", &SE3::toMat, "Convert to a 4x4 matrix.") - .def("inverse", &SE3::inverse, "Compute the inverse.") - .def(nb::self * nb::self, "Compose two rigid body transformations.") - .def(nb::self == nb::self, "Check for equality") - .def(nb::self != nb::self, "Check for inequality") - .def("__repr__", &SE3::toString) - .def("__copy__", [](const SE3 &self) { return SE3(self); }) - .def( - "__deepcopy__", [](const SE3 &self, nb::dict) { return SE3(self); }, - "memo"_a) - .def("__getstate__", - [](const SE3 &p) { - return nb::make_tuple(p.rot.qx, p.rot.qy, p.rot.qz, p.rot.qw, - p.trans[0], p.trans[1], p.trans[2]); - }) - .def("__setstate__", - [](SE3 &p, - std::tuple - t) { - new (&p) SE3(SO3{.qx = std::get<0>(t), - .qy = std::get<1>(t), - .qz = std::get<2>(t), - .qw = std::get<3>(t)}, - Eigen::Vector3d{std::get<4>(t), std::get<5>(t), - std::get<6>(t)}); - }) - .doc() = "SE3 class for representing a 3D rigid body transformation " - "using a quaternion and a translation vector. This is outfitted " - "with some basic functionality, but mostly intended for storage " - "and converting between types."; + .def( + nb::init(), + "rot"_a, + "trans"_a, + "Create a SE3 from a rotation and translation." + ) + .def_static("identity", &SE3::identity, "Create an identity SE3.") + .def_static( + "fromMat", + &SE3::fromMat, + "mat"_a, + "Create a SE3 from a 4x4 transformation matrix." + ) + .def_ro("rot", &SE3::rot, "Rotation as a SO3 object.") + .def_ro("trans", &SE3::trans, "Translation as a 3D vector.") + .def("toMat", &SE3::toMat, "Convert to a 4x4 matrix.") + .def("inverse", &SE3::inverse, "Compute the inverse.") + .def(nb::self * nb::self, "Compose two rigid body transformations.") + .def(nb::self == nb::self, "Check for equality") + .def(nb::self != nb::self, "Check for inequality") + .def("__repr__", &SE3::toString) + .def("__copy__", [](const SE3& self) { return SE3(self); }) + .def( + "__deepcopy__", + [](const SE3& self, nb::dict) { return SE3(self); }, + "memo"_a + ) + .def( + "__getstate__", + [](const SE3& p) { + return nb::make_tuple( + p.rot.qx, + p.rot.qy, + p.rot.qz, + p.rot.qw, + p.trans[0], + p.trans[1], + p.trans[2] + ); + } + ) + .def( + "__setstate__", + []( + SE3& p, + std::tuple t + ) { + new (&p) SE3( + SO3 { + .qx = std::get<0>(t), + .qy = std::get<1>(t), + .qz = std::get<2>(t), + .qw = std::get<3>(t) + }, + Eigen::Vector3d {std::get<4>(t), std::get<5>(t), std::get<6>(t)} + ); + } + ) + .doc() = + "SE3 class for representing a 3D rigid body transformation " + "using a quaternion and a translation vector. This is outfitted " + "with some basic functionality, but mostly intended for storage " + "and converting between types."; } -} // namespace evalio \ No newline at end of file +} // namespace evalio diff --git a/cpp/evalio/pipeline.h b/cpp/evalio/pipeline.h index 7bb6b83a..d6cd4b2a 100644 --- a/cpp/evalio/pipeline.h +++ b/cpp/evalio/pipeline.h @@ -11,11 +11,11 @@ // Helps evalio bindings be found in other modules // Licensed via BSD-3-Clause #if !defined(PYBIND11_EXPORT) -#if defined(WIN32) || defined(_WIN32) -#define PYBIND11_EXPORT __declspec(dllexport) -#else -#define PYBIND11_EXPORT __attribute__((visibility("default"))) -#endif + #if defined(WIN32) || defined(_WIN32) + #define PYBIND11_EXPORT __declspec(dllexport) + #else + #define PYBIND11_EXPORT __attribute__((visibility("default"))) + #endif #endif // For converting version definitions to string @@ -29,12 +29,21 @@ using Param = std::variant; class PYBIND11_EXPORT Pipeline { public: - virtual ~Pipeline() {}; + virtual ~Pipeline() {} // Info - static std::string version() { return "0.0.0"; } - static std::string url() { return "url-not-set"; } - static std::string name() { throw std::runtime_error("Not implemented"); } + static std::string version() { + return "0.0.0"; + } + + static std::string url() { + return "url-not-set"; + } + + static std::string name() { + throw std::runtime_error("Not implemented"); + } + static std::map default_params() { throw std::runtime_error("Not implemented"); } @@ -55,4 +64,4 @@ class PYBIND11_EXPORT Pipeline { virtual std::vector add_lidar(LidarMeasurement mm) = 0; }; -} // namespace evalio \ No newline at end of file +} // namespace evalio diff --git a/cpp/evalio/types.h b/cpp/evalio/types.h index dba84d10..357c05ac 100644 --- a/cpp/evalio/types.h +++ b/cpp/evalio/types.h @@ -14,32 +14,50 @@ struct Duration { int64_t nsec; static Duration from_sec(double sec) { - return Duration{.nsec = int64_t(sec * 1e9)}; + return Duration {.nsec = int64_t(sec * 1e9)}; } - static Duration from_nsec(int64_t nsec) { return Duration{.nsec = nsec}; } + static Duration from_nsec(int64_t nsec) { + return Duration {.nsec = nsec}; + } - double to_sec() const { return double(nsec) / 1e9; } + double to_sec() const { + return double(nsec) / 1e9; + } - int64_t to_nsec() const { return nsec; } + int64_t to_nsec() const { + return nsec; + } - std::string toString() const { return "Duration(" + toStringBrief() + ")"; } + std::string toString() const { + return "Duration(" + toStringBrief() + ")"; + } - std::string toStringBrief() const { return std::to_string(to_sec()); }; + std::string toStringBrief() const { + return std::to_string(to_sec()); + } - bool operator<(const Duration &other) const { return nsec < other.nsec; } + bool operator<(const Duration& other) const { + return nsec < other.nsec; + } - bool operator>(const Duration &other) const { return nsec > other.nsec; } + bool operator>(const Duration& other) const { + return nsec > other.nsec; + } - bool operator==(const Duration &other) const { return nsec == other.nsec; } + bool operator==(const Duration& other) const { + return nsec == other.nsec; + } - bool operator!=(const Duration &other) const { return !(*this == other); } + bool operator!=(const Duration& other) const { + return !(*this == other); + } - Duration operator-(const Duration &other) const { + Duration operator-(const Duration& other) const { return Duration::from_nsec(nsec - other.nsec); } - Duration operator+(const Duration &other) const { + Duration operator+(const Duration& other) const { return Duration::from_nsec(nsec + other.nsec); } }; @@ -49,52 +67,64 @@ struct Stamp { uint32_t nsec; static Stamp from_sec(double sec) { - return Stamp{.sec = uint32_t(sec), - .nsec = uint32_t((sec - uint32_t(sec)) * 1e9)}; + return Stamp { + .sec = uint32_t(sec), + .nsec = uint32_t((sec - uint32_t(sec)) * 1e9) + }; } static Stamp from_nsec(uint64_t nsec) { - return Stamp{.sec = uint32_t(nsec / 1e9), - .nsec = uint32_t(nsec % uint64_t(1e9))}; + return Stamp { + .sec = uint32_t(nsec / 1e9), + .nsec = uint32_t(nsec % uint64_t(1e9)) + }; } - uint64_t to_nsec() const { return uint64_t(sec) * uint64_t(1e9) + nsec; } + uint64_t to_nsec() const { + return uint64_t(sec) * uint64_t(1e9) + nsec; + } - double to_sec() const { return double(sec) + double(nsec) * 1e-9; } + double to_sec() const { + return double(sec) + double(nsec) * 1e-9; + } - std::string toString() const { return "Stamp(" + toStringBrief() + ")"; } + std::string toString() const { + return "Stamp(" + toStringBrief() + ")"; + } std::string toStringBrief() const { size_t n_zeros = 9; auto nsec_str = std::to_string(nsec); auto nsec_str_leading = - std::string(9 - std::min(n_zeros, nsec_str.length()), '0') + nsec_str; + std::string(9 - std::min(n_zeros, nsec_str.length()), '0') + nsec_str; return std::to_string(sec) + "." + nsec_str_leading; - }; + } - bool operator<(const Stamp &other) const { + bool operator<(const Stamp& other) const { return sec < other.sec || (sec == other.sec && nsec < other.nsec); } - bool operator>(const Stamp &other) const { + bool operator>(const Stamp& other) const { return sec > other.sec || (sec == other.sec && nsec > other.nsec); } - bool operator==(const Stamp &other) const { + bool operator==(const Stamp& other) const { return sec == other.sec && nsec == other.nsec; } - bool operator!=(const Stamp &other) const { return !(*this == other); } + bool operator!=(const Stamp& other) const { + return !(*this == other); + } - Stamp operator-(const Duration &other) const { + Stamp operator-(const Duration& other) const { return Stamp::from_nsec(to_nsec() - other.nsec); } - Stamp operator+(const Duration &other) const { + Stamp operator+(const Duration& other) const { return Stamp::from_nsec(to_nsec() + other.nsec); } - Duration operator-(const Stamp &other) const { + Duration operator-(const Stamp& other) const { return Duration::from_sec(to_sec() - other.to_sec()); } }; @@ -104,26 +134,26 @@ struct Point { double y = 0.0; double z = 0.0; double intensity = 0.0; - Duration t = Duration{.nsec = 0}; + Duration t = Duration {.nsec = 0}; uint32_t range = 0; uint8_t row = 0; uint16_t col = 0; std::string toString() const { - return "Point(x: " + std::to_string(x) + ", y: " + std::to_string(y) + - ", z: " + std::to_string(z) + - ", intensity: " + std::to_string(intensity) + - ", t: " + std::to_string(t.to_sec()) + - ", row: " + std::to_string(row) + ", col: " + std::to_string(col) + - ")"; + return "Point(x: " + std::to_string(x) + ", y: " + std::to_string(y) + + ", z: " + std::to_string(z) + ", intensity: " + + std::to_string(intensity) + ", t: " + std::to_string(t.to_sec()) + + ", row: " + std::to_string(row) + ", col: " + std::to_string(col) + ")"; } - bool operator!=(const Point &other) const { return !(*this == other); } + bool operator!=(const Point& other) const { + return !(*this == other); + } - bool operator==(const Point &other) const { - return x == other.x && y == other.y && z == other.z && - intensity == other.intensity && t == other.t && - range == other.range && row == other.row && col == other.col; + bool operator==(const Point& other) const { + return x == other.x && y == other.y && z == other.z + && intensity == other.intensity && t == other.t && range == other.range + && row == other.row && col == other.col; } }; @@ -133,8 +163,8 @@ struct LidarMeasurement { LidarMeasurement(Stamp stamp) : stamp(stamp) {} - LidarMeasurement(Stamp stamp, std::vector points) - : stamp(stamp), points(points) {} + LidarMeasurement(Stamp stamp, std::vector points) : + stamp(stamp), points(points) {} std::string toString() const { std::ostringstream oss; @@ -146,7 +176,7 @@ struct LidarMeasurement { std::vector to_vec_positions() const { std::vector eigen_points; eigen_points.reserve(points.size()); - for (const auto &point : points) { + for (const auto& point : points) { eigen_points.push_back(Eigen::Vector3d(point.x, point.y, point.z)); } return eigen_points; @@ -155,17 +185,17 @@ struct LidarMeasurement { std::vector to_vec_stamps() const { std::vector vec_stamps; vec_stamps.reserve(points.size()); - for (const auto &point : points) { + for (const auto& point : points) { vec_stamps.push_back(point.t.to_sec()); } return vec_stamps; } - bool operator!=(const LidarMeasurement &other) const { + bool operator!=(const LidarMeasurement& other) const { return !(*this == other); } - bool operator==(const LidarMeasurement &other) const { + bool operator==(const LidarMeasurement& other) const { if (stamp != other.stamp || points.size() != other.points.size()) { return false; } @@ -191,14 +221,15 @@ struct LidarParams { std::string model = "-"; std::string toString() const { - return "LidarParams(rows: " + std::to_string(num_rows) + - ", cols: " + std::to_string(num_columns) + - ", min_range: " + std::to_string(min_range) + - ", max_range: " + std::to_string(max_range) + - ", rate: " + std::to_string(rate) + ")"; - }; - - Duration delta_time() const { return Duration::from_sec(1.0 / rate); } + return "LidarParams(rows: " + std::to_string(num_rows) + + ", cols: " + std::to_string(num_columns) + ", min_range: " + + std::to_string(min_range) + ", max_range: " + std::to_string(max_range) + + ", rate: " + std::to_string(rate) + ")"; + } + + Duration delta_time() const { + return Duration::from_sec(1.0 / rate); + } }; struct ImuMeasurement { @@ -214,11 +245,11 @@ struct ImuMeasurement { return oss.str(); } - bool operator!=(const ImuMeasurement &other) const { + bool operator!=(const ImuMeasurement& other) const { return !(*this == other); } - bool operator==(const ImuMeasurement &other) const { + bool operator==(const ImuMeasurement& other) const { return stamp == other.stamp && gyro == other.gyro && accel == other.accel; } }; @@ -266,27 +297,31 @@ struct SO3 { return Eigen::Quaterniond(qw, qx, qy, qz); } - static SO3 fromEigen(const Eigen::Quaterniond &q) { - return SO3{.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; + static SO3 fromEigen(const Eigen::Quaterniond& q) { + return SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; } - static SO3 identity() { return SO3{.qx = 0, .qy = 0, .qz = 0, .qw = 1}; } + static SO3 identity() { + return SO3 {.qx = 0, .qy = 0, .qz = 0, .qw = 1}; + } - static SO3 fromMat(const Eigen::Matrix3d &R) { + static SO3 fromMat(const Eigen::Matrix3d& R) { return fromEigen(Eigen::Quaterniond(R)); } - SO3 inverse() const { return SO3{.qx = -qx, .qy = -qy, .qz = -qz, .qw = qw}; } + SO3 inverse() const { + return SO3 {.qx = -qx, .qy = -qy, .qz = -qz, .qw = qw}; + } - SO3 operator*(const SO3 &other) const { + SO3 operator*(const SO3& other) const { return fromEigen(toEigen() * other.toEigen()); } - Eigen::Vector3d rotate(const Eigen::Vector3d &v) const { + Eigen::Vector3d rotate(const Eigen::Vector3d& v) const { return toEigen() * v; } - static SO3 exp(const Eigen::Vector3d &v) { + static SO3 exp(const Eigen::Vector3d& v) { Eigen::AngleAxisd axis(v.norm(), v.normalized()); Eigen::Quaterniond q(axis); return fromEigen(q); @@ -298,23 +333,27 @@ struct SO3 { return axis.angle() * axis.axis(); } - Eigen::Matrix3d toMat() const { return toEigen().toRotationMatrix(); } + Eigen::Matrix3d toMat() const { + return toEigen().toRotationMatrix(); + } std::string toString() const { - return "SO3(x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + - ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw) + ")"; + return "SO3(x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + + ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw) + ")"; } std::string toStringBrief() const { - return "x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + - ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw); + return "x: " + std::to_string(qx) + ", y: " + std::to_string(qy) + + ", z: " + std::to_string(qz) + ", w: " + std::to_string(qw); } - bool operator==(const SO3 &other) const { + bool operator==(const SO3& other) const { return qx == other.qx && qy == other.qy && qz == other.qz && qw == other.qw; } - bool operator!=(const SO3 &other) const { return !(*this == other); } + bool operator!=(const SO3& other) const { + return !(*this == other); + } }; struct SE3 { @@ -327,7 +366,7 @@ struct SE3 { return SE3(SO3::identity(), Eigen::Vector3d::Zero()); } - static SE3 fromMat(const Eigen::Matrix4d &T) { + static SE3 fromMat(const Eigen::Matrix4d& T) { return SE3(SO3::fromMat(T.block<3, 3>(0, 0)), T.block<3, 1>(0, 3)); } @@ -343,7 +382,7 @@ struct SE3 { return SE3(inv_rot, inv_rot.rotate(-trans)); } - SE3 operator*(const SE3 &other) const { + SE3 operator*(const SE3& other) const { return SE3(rot * other.rot, rot.rotate(other.trans) + trans); } @@ -354,11 +393,13 @@ struct SE3 { return oss.str(); } - bool operator==(const SE3 &other) const { + bool operator==(const SE3& other) const { return rot == other.rot && trans == other.trans; - }; + } - bool operator!=(const SE3 &other) const { return !(*this == other); } + bool operator!=(const SE3& other) const { + return !(*this == other); + } }; -} // namespace evalio \ No newline at end of file +} // namespace evalio From dc6c315c93efde06d24255cb5162fe1143b9cc13 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Sat, 12 Jul 2025 13:50:24 -0400 Subject: [PATCH 2/2] Missed one! --- cpp/bindings/pipelines/lio_sam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cpp/bindings/pipelines/lio_sam.h b/cpp/bindings/pipelines/lio_sam.h index 98336e06..14ca24af 100644 --- a/cpp/bindings/pipelines/lio_sam.h +++ b/cpp/bindings/pipelines/lio_sam.h @@ -98,7 +98,7 @@ class LioSam: public evalio::Pipeline { } // Getters - evalio::SE3 const pose() override { + const evalio::SE3 pose() override { return to_evalio_se3(lio_sam_->getPose()) * lidar_T_imu_; }