diff --git a/cpp/bindings/CMakeLists.txt b/cpp/bindings/CMakeLists.txt index 4124af5..45c55ca 100644 --- a/cpp/bindings/CMakeLists.txt +++ b/cpp/bindings/CMakeLists.txt @@ -66,13 +66,14 @@ set(RESULTS "") # for printing results message("################### Searching for pipelines ###################") # find_pipeline(directory target_name definition_name) -find_pipeline(kiss-icp/cpp/kiss_icp kiss_icp_pipeline EVALIO_KISS_ICP) -find_pipeline(LIO-SAM lio_sam EVALIO_LIO_SAM) -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) +find_pipeline(kiss-icp/cpp/kiss_icp kiss_icp_pipeline EVALIO_KISS_ICP) +find_pipeline(LIO-SAM lio_sam EVALIO_LIO_SAM) +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(direct_lidar_inertial_odometry dlio_odom_node EVALIO_DLIO) +find_pipeline(form FORM EVALIO_FORM) message("################### Pipeline Results ###################") if(NOT "${RESULTS}" STREQUAL "") string(STRIP "${RESULTS}" RESULTS) @@ -93,8 +94,8 @@ FetchContent_Declare( nanobind GIT_REPOSITORY https://github.com/wjakob/nanobind GIT_TAG v2.9.2 - FIND_PACKAGE_ARGS - CONFIG # uses find_package first, git if it fails + # FIND_PACKAGE_ARGS + # CONFIG # uses find_package first, git if it fails ) FetchContent_MakeAvailable(nanobind) diff --git a/cpp/bindings/pipelines/bindings.h b/cpp/bindings/pipelines/bindings.h index 4efb78d..97729b7 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_DLIO + #include "bindings/pipelines/dlio.h" +#endif + #ifdef EVALIO_FORM #include "bindings/pipelines/form.h" #endif @@ -117,6 +121,19 @@ inline void make_pipelines(nb::module_& m) { "This is the version based on the 2022-ICRA paper."; #endif +#ifdef EVALIO_DLIO + nb::class_(m, "DLIO") + .def(nb::init<>()) + .def_static("name", &DLIO::name) + .def_static("default_params", &DLIO::default_params) + .def_static("url", &DLIO::url) + .def_static("version", &DLIO::version) + .doc() = + "Direct LiDAR-Inertial Odometry (DLIO) pipeline. DLIO is a lightweight " + "LiDAR-inertial odometry algorithm that uses a coarse-to-fine approach " + "with a geometric observer for real-time state estimation."; +#endif + #ifdef EVALIO_FORM nb::class_(m, "FORM") .def(nb::init<>()) diff --git a/cpp/bindings/pipelines/dlio.h b/cpp/bindings/pipelines/dlio.h new file mode 100644 index 0000000..b5f542f --- /dev/null +++ b/cpp/bindings/pipelines/dlio.h @@ -0,0 +1,187 @@ +#pragma once + +#include + +#include +#include +#include + +#include "dlio/odom.h" +#include "evalio/convert/base.h" +#include "evalio/pipeline.h" +#include "evalio/types.h" + +namespace ev = evalio; + +// ------------------------- Fill out some converters for custom types ------------------------- // +namespace evalio { +// Point conversions +template<> +inline Point convert(const dlio::Point& in) { + return Point { + .x = in.x, + .y = in.y, + .z = in.z, + .intensity = in.intensity, + .t = ev::Duration::from_nsec(in.t) + }; +} + +template<> +inline dlio::Point convert(const ev::Point& in) { + dlio::Point out; + out.x = static_cast(in.x); + out.y = static_cast(in.y); + out.z = static_cast(in.z); + out.intensity = static_cast(in.intensity); + out.t = static_cast(in.t.to_nsec()); + return out; +} + +// IMU conversions +template<> +inline dlio::OdomNode::ImuMeas convert(const ev::ImuMeasurement& in) { + return dlio::OdomNode::ImuMeas { + .stamp = in.stamp.to_sec(), + .dt = 0.0, // Will be computed by DLIO internally + .ang_vel = in.gyro.cast(), + .lin_accel = in.accel.cast() + }; +} + +// SE3 conversion from DLIO State +template<> +inline ev::SE3 convert(const dlio::OdomNode::State& in) { + const auto& q = in.q; + const auto rot = ev::SO3 {.qx = q.x(), .qy = q.y(), .qz = q.z(), .qw = q.w()}; + return ev::SE3(rot, in.p.cast()); +} + +} // namespace evalio + +// ------------------------- The pipeline impl ------------------------- // +class DLIO: public ev::Pipeline { +public: + DLIO() : config_(), lidar_T_imu_(ev::SE3::identity()) {} + + // Info + static std::string version() { + return XSTR(EVALIO_DLIO); + } + + static std::string name() { + return "dlio"; + } + + static std::string url() { + return "https://github.com/vectr-ucla/direct_lidar_inertial_odometry"; + } + + // Defaults as found in cfg/params.yaml in the DLIO repo + // clang-format off + EVALIO_SETUP_PARAMS( + (bool, verbose, false, config_.verbose), + + (bool, deskew, true, config_.deskew), + (double, gravity, 9.80665, config_.gravity), + (bool, time_offset, true, config_.time_offset), + + (double, keyframe_thresh_dist, 1.0, config_.keyframe_thresh_dist), + (double, keyframe_thresh_rot, 45.0, config_.keyframe_thresh_rot), + + (int, submap_knn, 10, config_.submap_knn), + (int, submap_kcv, 10, config_.submap_kcv), + (int, submap_kcc, 10, config_.submap_kcc), + + (bool, densemap_filtered, false, config_.densemap_filtered), + (bool, wait_until_move, true, config_.wait_until_move), + + (double, crop_size, 1.0, config_.crop_size), + + (bool, vf_use, true, config_.vf_use), + (double, vf_res, 0.25, config_.vf_res), + + (bool, adaptive_params, true, config_.adaptive_params), + + (bool, calibrate_gyro, true, config_.calibrate_gyro), + (bool, calibrate_accel, true, config_.calibrate_accel), + (double, imu_calib_time, 3.0, config_.imu_calib_time), + (int, imu_buffer_size, 5000, config_.imu_buffer_size), + + (bool, gravity_align, true, config_.gravity_align), + (bool, imu_calibrate, true, config_.imu_calibrate), + + (int, gicp_min_num_points, 64, config_.gicp_min_num_points), + (int, gicp_k_correspondences, 16, config_.gicp_k_correspondences), + (double, gicp_max_corr_dist, 0.5, config_.gicp_max_corr_dist), + (int, gicp_max_iter, 32, config_.gicp_max_iter), + (double, gicp_transformation_ep, 0.01, config_.gicp_transformation_ep), + (double, gicp_rotation_ep, 0.01, config_.gicp_rotation_ep), + (double, gicp_init_lambda_factor, 1e-9, config_.gicp_init_lambda_factor), + + (double, geo_Kp, 4.5, config_.geo_Kp), + (double, geo_Kv, 11.25, config_.geo_Kv), + (double, geo_Kq, 4.0, config_.geo_Kq), + (double, geo_Kab, 2.25, config_.geo_Kab), + (double, geo_Kgb, 1.0, config_.geo_Kgb), + (double, geo_abias_max, 5.0, config_.geo_abias_max), + (double, geo_gbias_max, 0.5, config_.geo_gbias_max) + ); + // clang-format on + + // Getters + const std::map> map() override { + return ev::make_map("point", *dlio_->getMap()); + } + + // Setters + void set_imu_params(ev::ImuParams params) override { + config_.gravity = params.gravity.norm(); + } + + void set_lidar_params(ev::LidarParams params) override { + // DLIO doesn't have explicit lidar params in its config + } + + void set_imu_T_lidar(ev::SE3 T) override { + // DLIO uses baselink2imu and baselink2lidar extrinsics + // Assuming baselink = imu frame + config_.extrinsics_baselink2lidar_t = T.trans.cast(); + config_.extrinsics_baselink2lidar_R = T.rot.to_mat().cast(); + // baselink2lidar is identity since baselink = lidar + config_.extrinsics_baselink2imu_t = Eigen::Vector3f::Zero(); + config_.extrinsics_baselink2imu_R = Eigen::Matrix3f::Identity(); + } + + // Doers + void initialize() override { + dlio_ = std::make_unique(config_); + dlio_->start(); + } + + void add_imu(ev::ImuMeasurement mm) override { + dlio_->callbackImu(ev::convert(mm)); + } + + void add_lidar(ev::LidarMeasurement mm) override { + // Convert to PCL point cloud + auto cloud = + ev::convert_iter>(mm.points).makeShared(); + + // Run through pipeline + dlio_->callbackPointCloud(cloud, mm.stamp.to_sec()); + + // Get pose from DLIO state and save + const auto state = dlio_->getState(); + // Results are in the baselink = imu frame + this->save(mm.stamp, state); + + // Save the current cloud used by DLIO + this->save(mm.stamp, "point", *dlio_->getCurrentScan()); + } + +private: + std::unique_ptr dlio_; + dlio::OdomNode::Params config_; + ev::SE3 lidar_T_imu_; +}; diff --git a/cpp/setup_pipelines.sh b/cpp/setup_pipelines.sh index 20d29f9..77b33ec 100755 --- a/cpp/setup_pipelines.sh +++ b/cpp/setup_pipelines.sh @@ -64,7 +64,14 @@ git switch --detach icra_2022 git apply ../../pipelines/ct_icp.patch cd .. -# FORM +if [ ! -d "direct_lidar_inertial_odometry" ]; then + git clone https://github.com/contagon/direct_lidar_inertial_odometry.git +fi +cd direct_lidar_inertial_odometry +git stash +git checkout master +cd .. + if [ ! -d "form" ]; then git clone https://github.com/rpl-cmu/form.git fi diff --git a/python/evalio/rerun.py b/python/evalio/rerun.py index 0a61401..1bb6ffa 100644 --- a/python/evalio/rerun.py +++ b/python/evalio/rerun.py @@ -200,7 +200,8 @@ def log_pose(self, stamp: Stamp, pose: SE3): # Find transform between ground truth and imu origins if self.gt_o_T_imu_o is None: - if stamp < self.gt.stamps[0]: + # If we haven't hit ground truth data or the pose isn't being updated, wait + if stamp < self.gt.stamps[0] or pose == SE3.identity(): pass else: imu_o_T_imu_0 = pose