From 496b51200d469a33994b4787d9153514f4251a01 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 14:06:39 -0400 Subject: [PATCH 01/11] Starting with dlio --- cpp/bindings/CMakeLists.txt | 11 +- cpp/bindings/pipelines/bindings.h | 17 + cpp/bindings/pipelines/dlio.h | 184 ++++ cpp/bindings/pipelines/dlio.patch | 1425 +++++++++++++++++++++++++++++ 4 files changed, 1632 insertions(+), 5 deletions(-) create mode 100644 cpp/bindings/pipelines/dlio.h create mode 100644 cpp/bindings/pipelines/dlio.patch diff --git a/cpp/bindings/CMakeLists.txt b/cpp/bindings/CMakeLists.txt index 4ec26584..d7c465fe 100644 --- a/cpp/bindings/CMakeLists.txt +++ b/cpp/bindings/CMakeLists.txt @@ -67,11 +67,12 @@ 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(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) 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..2ff5216d 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 + namespace evalio { inline void make_pipelines(nb::module_& m) { // List all the pipelines here @@ -112,5 +116,18 @@ 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_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 } } // namespace evalio diff --git a/cpp/bindings/pipelines/dlio.h b/cpp/bindings/pipelines/dlio.h new file mode 100644 index 00000000..4587513b --- /dev/null +++ b/cpp/bindings/pipelines/dlio.h @@ -0,0 +1,184 @@ +#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"; + } + + // clang-format off + EVALIO_SETUP_PARAMS( + (bool, verbose, true, config_.verbose), + + (bool, deskew, true, config_.deskew), + (double, gravity, 9.80665, config_.gravity), + (bool, time_offset, false, config_.time_offset), + + (double, keyframe_thresh_dist, 0.1, config_.keyframe_thresh_dist), + (double, keyframe_thresh_rot, 1.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, true, config_.densemap_filtered), + (bool, wait_until_move, false, config_.wait_until_move), + + (double, crop_size, 1.0, config_.crop_size), + + (bool, vf_use, true, config_.vf_use), + (double, vf_res, 0.05, 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, 2000, config_.imu_buffer_size), + + (bool, gravity_align, true, config_.gravity_align), + (bool, imu_calibrate, true, config_.imu_calibrate), + + (int, gicp_min_num_points, 100, config_.gicp_min_num_points), + (int, gicp_k_correspondences, 20, config_.gicp_k_correspondences), + (double, gicp_max_corr_dist, 10000.0, config_.gicp_max_corr_dist), + (int, gicp_max_iter, 64, config_.gicp_max_iter), + (double, gicp_transformation_ep, 0.0005, config_.gicp_transformation_ep), + (double, gicp_rotation_ep, 0.0005, config_.gicp_rotation_ep), + (double, gicp_init_lambda_factor, 1e-9, config_.gicp_init_lambda_factor), + + (double, geo_Kp, 1.0, config_.geo_Kp), + (double, geo_Kv, 1.0, config_.geo_Kv), + (double, geo_Kq, 1.0, config_.geo_Kq), + (double, geo_Kab, 1.0, config_.geo_Kab), + (double, geo_Kgb, 1.0, config_.geo_Kgb), + (double, geo_abias_max, 1.0, config_.geo_abias_max), + (double, geo_gbias_max, 1.0, config_.geo_gbias_max) + ); + // clang-format on + + // Getters + const std::map> map() override { + // DLIO doesn't expose map retrieval in the same way, return empty for now + return std::map>(); + } + + // 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); + } + +private: + std::unique_ptr dlio_; + dlio::OdomNode::Params config_; + ev::SE3 lidar_T_imu_; +}; diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch new file mode 100644 index 00000000..67af9f04 --- /dev/null +++ b/cpp/bindings/pipelines/dlio.patch @@ -0,0 +1,1425 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 91e47a0..4aa2bdf 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -11,10 +11,11 @@ + ########################################################### + + cmake_minimum_required(VERSION 3.12.4) +-project(direct_lidar_inertial_odometry) ++project(direct_lidar_inertial_odometry VERSION 1.1.1) + + add_compile_options(-std=c++17) + set(CMAKE_BUILD_TYPE "Release") ++set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + + find_package( PCL REQUIRED ) + include_directories(${PCL_INCLUDE_DIRS}) +@@ -33,50 +34,7 @@ else(OPENMP_FOUND) + message("ERROR: OpenMP could not be found.") + endif(OPENMP_FOUND) + +-set(CMAKE_THREAD_PREFER_PTHREAD TRUE) +-set(THREADS_PREFER_PTHREAD_FLAG TRUE) +-find_package(Threads REQUIRED) +- +-find_package(catkin REQUIRED COMPONENTS +- roscpp +- std_msgs +- sensor_msgs +- geometry_msgs +- nav_msgs +- pcl_ros +- message_generation +-) +- +-add_service_files( +- DIRECTORY srv +- FILES +- save_pcd.srv +-) +- +-generate_messages( +- DEPENDENCIES +- sensor_msgs +- std_msgs +- geometry_msgs +-) +- +-catkin_package( +- CATKIN_DEPENDS +- roscpp +- std_msgs +- sensor_msgs +- geometry_msgs +- pcl_ros +- INCLUDE_DIRS +- include +- LIBRARIES +- ${PROJECT_NAME} +- nano_gicp +- nanoflann +-) +- + include_directories(include) +-include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) + + # Not all machines have available + set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) +@@ -91,29 +49,24 @@ endif() + add_library(nanoflann STATIC + src/nano_gicp/nanoflann.cc + ) +-target_link_libraries(nanoflann ${PCL_LIBRARIES}) ++target_link_libraries(nanoflann PUBLIC ${PCL_LIBRARIES}) ++target_include_directories(nanoflann PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + + # NanoGICP + add_library(nano_gicp STATIC + src/nano_gicp/lsq_registration.cc + src/nano_gicp/nano_gicp.cc + ) +-target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) ++target_link_libraries(nano_gicp PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) ++target_include_directories(nano_gicp PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + + # Odometry Node +-add_executable(dlio_odom_node src/dlio/odom_node.cc src/dlio/odom.cc) +-add_dependencies(dlio_odom_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) ++add_library(dlio_odom_node src/dlio/odom.cc) + target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS}) +-target_link_libraries(dlio_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp) ++target_link_libraries(dlio_odom_node PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nano_gicp) ++target_include_directories(dlio_odom_node PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + + # Mapping Node +-add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc) +-add_dependencies(dlio_map_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +-target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) +-target_link_libraries(dlio_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) +- +-# Binaries +-install( TARGETS dlio_odom_node dlio_map_node +- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +-install( DIRECTORY cfg launch +- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ++# add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc) ++# target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) ++# target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) +diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h +index 47386ff..0bda740 100644 +--- a/include/dlio/dlio.h ++++ b/include/dlio/dlio.h +@@ -44,16 +44,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) + return out.str(); + } + +-// ROS +-#include +-#include +-#include +-#include +-#include +-#include +-#include +-#include +- + // BOOST + #include + #include +@@ -69,14 +59,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) + #include + #include + #include +-#include +-#include +-#include +-#include + + // DLIO + #include +-#include + + namespace dlio { + enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN }; +@@ -84,7 +69,7 @@ namespace dlio { + class OdomNode; + class MapNode; + +- struct Point { ++ struct EIGEN_ALIGN16 Point { + Point(): data{0.f, 0.f, 0.f, 1.f} {} + + PCL_ADD_POINT4D; +@@ -96,7 +81,7 @@ namespace dlio { + // (Livox) absolute timestamp in (seconds * 10e9) + }; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +- } EIGEN_ALIGN16; ++ }; + } + + POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, +diff --git a/include/dlio/odom.h b/include/dlio/odom.h +index c79ebd7..2c91f67 100644 +--- a/include/dlio/odom.h ++++ b/include/dlio/odom.h +@@ -10,35 +10,111 @@ + * * + ***********************************************************/ + ++#include ++ + #include "dlio/dlio.h" + + class dlio::OdomNode { + + public: ++ struct Params { ++ bool verbose = false; ++ ++ bool deskew = true; ++ double gravity = 9.80665; ++ bool time_offset = false; ++ ++ double keyframe_thresh_dist = 0.1; ++ double keyframe_thresh_rot = 1.0; ++ ++ int submap_knn = 10; ++ int submap_kcv = 10; ++ int submap_kcc = 10; ++ ++ bool densemap_filtered = true; ++ bool wait_until_move = false; ++ ++ double crop_size = 1.0; ++ ++ bool vf_use = true; ++ double vf_res = 0.05; ++ ++ bool adaptive_params = true; ++ ++ Eigen::Vector3f extrinsics_baselink2imu_t = Eigen::Vector3f::Zero(); ++ Eigen::Matrix3f extrinsics_baselink2imu_R = Eigen::Matrix3f::Identity(); ++ Eigen::Vector3f extrinsics_baselink2lidar_t = Eigen::Vector3f::Zero(); ++ Eigen::Matrix3f extrinsics_baselink2lidar_R = Eigen::Matrix3f::Identity(); ++ ++ bool calibrate_gyro = true; ++ bool calibrate_accel = true; ++ double imu_calib_time = 3.0; ++ int imu_buffer_size = 2000; ++ ++ bool gravity_align = true; ++ bool imu_calibrate = true; ++ ++ int gicp_min_num_points = 100; ++ int gicp_k_correspondences = 20; ++ double gicp_max_corr_dist = std::sqrt(std::numeric_limits::max()); ++ int gicp_max_iter = 64; ++ double gicp_transformation_ep = 0.0005; ++ double gicp_rotation_ep = 0.0005; ++ double gicp_init_lambda_factor = 1e-9; ++ ++ double geo_Kp = 1.0; ++ double geo_Kv = 1.0; ++ double geo_Kq = 1.0; ++ double geo_Kab = 1.0; ++ double geo_Kgb = 1.0; ++ double geo_abias_max = 1.0; ++ double geo_gbias_max = 1.0; ++ }; + +- OdomNode(ros::NodeHandle node_handle); +- ~OdomNode(); ++ struct ImuMeas { ++ double stamp; ++ double dt; // defined as the difference between the current and the previous ++ // measurement ++ Eigen::Vector3f ang_vel; ++ Eigen::Vector3f lin_accel; ++ }; + +- void start(); ++ struct ImuBias { ++ Eigen::Vector3f gyro; ++ Eigen::Vector3f accel; ++ }; + +-private: ++ struct Frames { ++ Eigen::Vector3f b; ++ Eigen::Vector3f w; ++ }; + +- struct State; +- struct ImuMeas; ++ struct Velocity { ++ Frames lin; ++ Frames ang; ++ }; + +- void getParams(); ++ struct State { ++ Eigen::Vector3f p; // position in world frame ++ Eigen::Quaternionf q; // orientation in world frame ++ Velocity v; ++ ImuBias b; // imu biases in body frame ++ }; + +- void callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc); +- void callbackImu(const sensor_msgs::Imu::ConstPtr& imu); ++ OdomNode(const Params ¶ms); ++ ~OdomNode(); + +- void publishPose(const ros::TimerEvent& e); ++ void start(); ++ State getState() const { return state; }; ++ ++ void callbackPointCloud(const pcl::PointCloud::ConstPtr &pc, ++ double stamp); ++ void callbackImu(const ImuMeas &imu); + +- void publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); +- void publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); +- void publishKeyframe(std::pair, +- pcl::PointCloud::ConstPtr> kf, ros::Time timestamp); ++private: ++ void getParams(const Params ¶ms); + +- void getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc); ++ void getScanFromROS(const pcl::PointCloud::ConstPtr &pc, double stamp); + void preprocessPoints(); + void deskewPointcloud(); + void initializeInputTarget(); +@@ -70,7 +146,7 @@ private: + void computeSpaciousness(); + void computeDensity(); + +- sensor_msgs::Imu::Ptr transformImu(const sensor_msgs::Imu::ConstPtr& imu); ++ ImuMeas transformImu(const ImuMeas &imu); + + void updateKeyframes(); + void computeConvexHull(); +@@ -82,27 +158,6 @@ private: + + void debug(); + +- ros::NodeHandle nh; +- ros::Timer publish_timer; +- +- // Subscribers +- ros::Subscriber lidar_sub; +- ros::Subscriber imu_sub; +- +- // Publishers +- ros::Publisher odom_pub; +- ros::Publisher pose_pub; +- ros::Publisher path_pub; +- ros::Publisher kf_pose_pub; +- ros::Publisher kf_cloud_pub; +- ros::Publisher deskewed_pub; +- +- // ROS Msgs +- nav_msgs::Odometry odom_ros; +- geometry_msgs::PoseStamped pose_ros; +- nav_msgs::Path path_ros; +- geometry_msgs::PoseArray kf_pose_ros; +- + // Flags + std::atomic dlio_initialized; + std::atomic first_valid_scan; +@@ -113,12 +168,6 @@ private: + std::atomic deskew_status; + std::atomic deskew_size; + +- // Threads +- std::thread publish_thread; +- std::thread publish_keyframe_thread; +- std::thread metrics_thread; +- std::thread debug_thread; +- + // Trajectory + std::vector> trajectory; + double length_traversed; +@@ -126,7 +175,7 @@ private: + // Keyframes + std::vector, + pcl::PointCloud::ConstPtr>> keyframes; +- std::vector keyframe_timestamps; ++ std::vector keyframe_timestamps; + std::vector> keyframe_normals; + std::vector> keyframe_transformations; + std::mutex keyframes_mutex; +@@ -134,12 +183,6 @@ private: + // Sensor Type + dlio::SensorType sensor; + +- // Frames +- std::string odom_frame; +- std::string baselink_frame; +- std::string lidar_frame; +- std::string imu_frame; +- + // Preprocessing + pcl::CropBox crop; + pcl::VoxelGrid voxel; +@@ -173,7 +216,7 @@ private: + std::mutex main_loop_running_mutex; + + // Timestamps +- ros::Time scan_header_stamp; ++ double scan_header_stamp; + double scan_stamp; + double prev_scan_stamp; + double scan_dt; +@@ -206,17 +249,12 @@ private: + }; Extrinsics extrinsics; + + // IMU +- ros::Time imu_stamp; ++ double imu_stamp; + double first_imu_stamp; + double prev_imu_stamp; + double imu_dp, imu_dq_deg; + +- struct ImuMeas { +- double stamp; +- double dt; // defined as the difference between the current and the previous measurement +- Eigen::Vector3f ang_vel; +- Eigen::Vector3f lin_accel; +- }; ImuMeas imu_meas; ++ ImuMeas imu_meas; + + boost::circular_buffer imu_buffer; + std::mutex mtx_imu; +@@ -238,27 +276,7 @@ private: + }; Geo geo; + + // State Vector +- struct ImuBias { +- Eigen::Vector3f gyro; +- Eigen::Vector3f accel; +- }; +- +- struct Frames { +- Eigen::Vector3f b; +- Eigen::Vector3f w; +- }; +- +- struct Velocity { +- Frames lin; +- Frames ang; +- }; +- +- struct State { +- Eigen::Vector3f p; // position in world frame +- Eigen::Quaternionf q; // orientation in world frame +- Velocity v; +- ImuBias b; // imu biases in body frame +- }; State state; ++ State state; + + struct Pose { + Eigen::Vector3f p; // position in world frame +@@ -279,7 +297,7 @@ private: + int numProcessors; + + // Parameters +- std::string version_; ++ std::string version_ = "1.1.1"; + int num_threads_; + bool verbose; + +diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc +index bceae37..4cd75fc 100644 +--- a/src/dlio/odom.cc ++++ b/src/dlio/odom.cc +@@ -12,9 +12,9 @@ + + #include "dlio/odom.h" + +-dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { ++dlio::OdomNode::OdomNode(const Params ¶ms) { + +- this->getParams(); ++ this->getParams(params); + + this->num_threads_ = omp_get_max_threads(); + +@@ -26,20 +26,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + this->deskew_status = false; + this->deskew_size = 0; + +- this->lidar_sub = this->nh.subscribe("pointcloud", 1, +- &dlio::OdomNode::callbackPointCloud, this, ros::TransportHints().tcpNoDelay()); +- this->imu_sub = this->nh.subscribe("imu", 1000, +- &dlio::OdomNode::callbackImu, this, ros::TransportHints().tcpNoDelay()); +- +- this->odom_pub = this->nh.advertise("odom", 1, true); +- this->pose_pub = this->nh.advertise("pose", 1, true); +- this->path_pub = this->nh.advertise("path", 1, true); +- this->kf_pose_pub = this->nh.advertise("kf_pose", 1, true); +- this->kf_cloud_pub = this->nh.advertise("kf_cloud", 1, true); +- this->deskewed_pub = this->nh.advertise("deskewed", 1, true); +- +- this->publish_timer = this->nh.createTimer(ros::Duration(0.01), &dlio::OdomNode::publishPose, this); +- + this->T = Eigen::Matrix4f::Identity(); + this->T_prior = Eigen::Matrix4f::Identity(); + this->T_corr = Eigen::Matrix4f::Identity(); +@@ -67,10 +53,10 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + this->first_imu_stamp = 0.; + this->prev_imu_stamp = 0.; + +- this->original_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); +- this->deskewed_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); +- this->current_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); +- this->submap_cloud = pcl::PointCloud::ConstPtr (boost::make_shared>()); ++ this->original_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->deskewed_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->current_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->submap_cloud = pcl::PointCloud::ConstPtr (std::make_shared>()); + + this->num_processed_keyframes = 0; + +@@ -79,7 +65,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + + this->first_scan_stamp = 0.; + this->elapsed_time = 0.; +- this->length_traversed; + + this->convex_hull.setDimension(3); + this->concave_hull.setDimension(3); +@@ -157,151 +142,81 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + if (strncmp(line, "processor", 9) == 0) this->numProcessors++; + } + fclose(file); +- + } + + dlio::OdomNode::~OdomNode() {} + +-void dlio::OdomNode::getParams() { +- +- // Version +- ros::param::param("~dlio/version", this->version_, "0.0.0"); +- +- // Frames +- ros::param::param("~dlio/frames/odom", this->odom_frame, "odom"); +- ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); +- ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); +- ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); ++void dlio::OdomNode::getParams(const Params ¶ms) { ++ this->verbose = params.verbose; + +- // Get Node NS and Remove Leading Character +- std::string ns = ros::this_node::getNamespace(); +- ns.erase(0,1); ++ this->deskew_ = params.deskew; ++ this->gravity_ = params.gravity; ++ this->time_offset_ = params.time_offset; + +- // Concatenate Frame Name Strings +- this->odom_frame = ns + "/" + this->odom_frame; +- this->baselink_frame = ns + "/" + this->baselink_frame; +- this->lidar_frame = ns + "/" + this->lidar_frame; +- this->imu_frame = ns + "/" + this->imu_frame; ++ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; ++ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; + +- // Deskew FLag +- ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); ++ this->submap_knn_ = params.submap_knn; ++ this->submap_kcv_ = params.submap_kcv; ++ this->submap_kcc_ = params.submap_kcc; + +- // Gravity +- ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); ++ this->densemap_filtered_ = params.densemap_filtered; + +- // Compute time offset between lidar and imu +- ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); ++ this->wait_until_move_ = params.wait_until_move; + +- // Keyframe Threshold +- ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); +- ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); ++ this->crop_size_ = params.crop_size; + +- // Submap +- ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); +- ros::param::param("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10); +- ros::param::param("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10); ++ this->vf_use_ = params.vf_use; ++ this->vf_res_ = params.vf_res; + +- // Dense map resolution +- ros::param::param("~dlio/map/dense/filtered", this->densemap_filtered_, true); +- +- // Wait until movement to publish map +- ros::param::param("~dlio/map/waitUntilMove", this->wait_until_move_, false); +- +- // Crop Box Filter +- ros::param::param("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); +- +- // Voxel Grid Filter +- ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); +- ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); ++ this->adaptive_params_ = params.adaptive_params; + +- // Adaptive Parameters +- ros::param::param("~dlio/adaptive", this->adaptive_params_, true); +- +- // Extrinsics +- std::vector t_default{0., 0., 0.}; +- std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; +- +- // center of gravity to imu +- std::vector baselink2imu_t, baselink2imu_R; +- ros::param::param>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default); +- ros::param::param>("~dlio/extrinsics/baselink2imu/R", baselink2imu_R, R_default); +- this->extrinsics.baselink2imu.t = +- Eigen::Vector3f(baselink2imu_t[0], baselink2imu_t[1], baselink2imu_t[2]); +- this->extrinsics.baselink2imu.R = +- Eigen::Map>(baselink2imu_R.data(), 3, 3); ++ this->extrinsics.baselink2imu.t = params.extrinsics_baselink2imu_t; ++ this->extrinsics.baselink2imu.R = params.extrinsics_baselink2imu_R; ++ this->extrinsics.baselink2lidar.t = params.extrinsics_baselink2lidar_t; ++ this->extrinsics.baselink2lidar.R = params.extrinsics_baselink2lidar_R; + ++ // Build 4x4 transformation matrices + this->extrinsics.baselink2imu_T = Eigen::Matrix4f::Identity(); +- this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = this->extrinsics.baselink2imu.t; +- this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = this->extrinsics.baselink2imu.R; +- +- // center of gravity to lidar +- std::vector baselink2lidar_t, baselink2lidar_R; +- ros::param::param>("~dlio/extrinsics/baselink2lidar/t", baselink2lidar_t, t_default); +- ros::param::param>("~dlio/extrinsics/baselink2lidar/R", baselink2lidar_R, R_default); +- +- this->extrinsics.baselink2lidar.t = +- Eigen::Vector3f(baselink2lidar_t[0], baselink2lidar_t[1], baselink2lidar_t[2]); +- this->extrinsics.baselink2lidar.R = +- Eigen::Map>(baselink2lidar_R.data(), 3, 3); ++ this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = ++ this->extrinsics.baselink2imu.t; ++ this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = ++ this->extrinsics.baselink2imu.R; + + this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity(); +- this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t; +- this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = this->extrinsics.baselink2lidar.R; +- +- // IMU +- ros::param::param("~dlio/odom/imu/calibration/accel", this->calibrate_accel_, true); +- ros::param::param("~dlio/odom/imu/calibration/gyro", this->calibrate_gyro_, true); +- ros::param::param("~dlio/odom/imu/calibration/time", this->imu_calib_time_, 3.0); +- ros::param::param("~dlio/odom/imu/bufferSize", this->imu_buffer_size_, 2000); +- +- std::vector accel_default{0., 0., 0.}; std::vector prior_accel_bias; +- std::vector gyro_default{0., 0., 0.}; std::vector prior_gyro_bias; +- +- ros::param::param("~dlio/odom/imu/approximateGravity", this->gravity_align_, true); +- ros::param::param("~dlio/imu/calibration", this->imu_calibrate_, true); +- ros::param::param>("~dlio/imu/intrinsics/accel/bias", prior_accel_bias, accel_default); +- ros::param::param>("~dlio/imu/intrinsics/gyro/bias", prior_gyro_bias, gyro_default); +- +- // scale-misalignment matrix +- std::vector imu_sm_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; +- std::vector imu_sm; +- +- ros::param::param>("~dlio/imu/intrinsics/accel/sm", imu_sm, imu_sm_default); +- +- if (!this->imu_calibrate_) { +- this->state.b.accel[0] = prior_accel_bias[0]; +- this->state.b.accel[1] = prior_accel_bias[1]; +- this->state.b.accel[2] = prior_accel_bias[2]; +- this->state.b.gyro[0] = prior_gyro_bias[0]; +- this->state.b.gyro[1] = prior_gyro_bias[1]; +- this->state.b.gyro[2] = prior_gyro_bias[2]; +- this->imu_accel_sm_ = Eigen::Map>(imu_sm.data(), 3, 3); +- } else { +- this->state.b.accel = Eigen::Vector3f(0., 0., 0.); +- this->state.b.gyro = Eigen::Vector3f(0., 0., 0.); +- this->imu_accel_sm_ = Eigen::Matrix3f::Identity(); +- } +- +- // GICP +- ros::param::param("~dlio/odom/gicp/minNumPoints", this->gicp_min_num_points_, 100); +- ros::param::param("~dlio/odom/gicp/kCorrespondences", this->gicp_k_correspondences_, 20); +- ros::param::param("~dlio/odom/gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_, +- std::sqrt(std::numeric_limits::max())); +- ros::param::param("~dlio/odom/gicp/maxIterations", this->gicp_max_iter_, 64); +- ros::param::param("~dlio/odom/gicp/transformationEpsilon", this->gicp_transformation_ep_, 0.0005); +- ros::param::param("~dlio/odom/gicp/rotationEpsilon", this->gicp_rotation_ep_, 0.0005); +- ros::param::param("~dlio/odom/gicp/initLambdaFactor", this->gicp_init_lambda_factor_, 1e-9); +- +- // Geometric Observer +- ros::param::param("~dlio/odom/geo/Kp", this->geo_Kp_, 1.0); +- ros::param::param("~dlio/odom/geo/Kv", this->geo_Kv_, 1.0); +- ros::param::param("~dlio/odom/geo/Kq", this->geo_Kq_, 1.0); +- ros::param::param("~dlio/odom/geo/Kab", this->geo_Kab_, 1.0); +- ros::param::param("~dlio/odom/geo/Kgb", this->geo_Kgb_, 1.0); +- ros::param::param("~dlio/odom/geo/abias_max", this->geo_abias_max_, 1.0); +- ros::param::param("~dlio/odom/geo/gbias_max", this->geo_gbias_max_, 1.0); +- +- ros::param::param("~dlio/verbose", this->verbose, true); ++ this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = ++ this->extrinsics.baselink2lidar.t; ++ this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = ++ this->extrinsics.baselink2lidar.R; ++ ++ this->calibrate_gyro_ = params.calibrate_gyro; ++ this->calibrate_accel_ = params.calibrate_accel; ++ this->imu_calib_time_ = params.imu_calib_time; ++ this->imu_buffer_size_ = params.imu_buffer_size; ++ ++ this->gravity_align_ = params.gravity_align; ++ this->imu_calibrate_ = params.imu_calibrate; ++ ++ // Initialize IMU accelerometer scale-misalignment matrix ++ this->state.b.accel = Eigen::Vector3f::Zero(); ++ this->state.b.gyro = Eigen::Vector3f::Zero(); ++ this->imu_accel_sm_ = Eigen::Matrix3f::Identity(); ++ ++ this->gicp_min_num_points_ = params.gicp_min_num_points; ++ this->gicp_k_correspondences_ = params.gicp_k_correspondences; ++ this->gicp_max_corr_dist_ = params.gicp_max_corr_dist; ++ this->gicp_max_iter_ = params.gicp_max_iter; ++ this->gicp_transformation_ep_ = params.gicp_transformation_ep; ++ this->gicp_rotation_ep_ = params.gicp_rotation_ep; ++ this->gicp_init_lambda_factor_ = params.gicp_init_lambda_factor; ++ ++ this->geo_Kp_ = params.geo_Kp; ++ this->geo_Kv_ = params.geo_Kv; ++ this->geo_Kq_ = params.geo_Kq; ++ this->geo_Kab_ = params.geo_Kab; ++ this->geo_Kgb_ = params.geo_Kgb; ++ this->geo_abias_max_ = params.geo_abias_max; ++ this->geo_gbias_max_ = params.geo_gbias_max; + } + + void dlio::OdomNode::start() { +@@ -318,185 +233,12 @@ void dlio::OdomNode::start() { + + } + +-void dlio::OdomNode::publishPose(const ros::TimerEvent& e) { +- +- // nav_msgs::Odometry +- this->odom_ros.header.stamp = this->imu_stamp; +- this->odom_ros.header.frame_id = this->odom_frame; +- this->odom_ros.child_frame_id = this->baselink_frame; +- +- this->odom_ros.pose.pose.position.x = this->state.p[0]; +- this->odom_ros.pose.pose.position.y = this->state.p[1]; +- this->odom_ros.pose.pose.position.z = this->state.p[2]; +- +- this->odom_ros.pose.pose.orientation.w = this->state.q.w(); +- this->odom_ros.pose.pose.orientation.x = this->state.q.x(); +- this->odom_ros.pose.pose.orientation.y = this->state.q.y(); +- this->odom_ros.pose.pose.orientation.z = this->state.q.z(); +- +- this->odom_ros.twist.twist.linear.x = this->state.v.lin.w[0]; +- this->odom_ros.twist.twist.linear.y = this->state.v.lin.w[1]; +- this->odom_ros.twist.twist.linear.z = this->state.v.lin.w[2]; +- +- this->odom_ros.twist.twist.angular.x = this->state.v.ang.b[0]; +- this->odom_ros.twist.twist.angular.y = this->state.v.ang.b[1]; +- this->odom_ros.twist.twist.angular.z = this->state.v.ang.b[2]; +- +- this->odom_pub.publish(this->odom_ros); +- +- // geometry_msgs::PoseStamped +- this->pose_ros.header.stamp = this->imu_stamp; +- this->pose_ros.header.frame_id = this->odom_frame; +- +- this->pose_ros.pose.position.x = this->state.p[0]; +- this->pose_ros.pose.position.y = this->state.p[1]; +- this->pose_ros.pose.position.z = this->state.p[2]; +- +- this->pose_ros.pose.orientation.w = this->state.q.w(); +- this->pose_ros.pose.orientation.x = this->state.q.x(); +- this->pose_ros.pose.orientation.y = this->state.q.y(); +- this->pose_ros.pose.orientation.z = this->state.q.z(); +- +- this->pose_pub.publish(this->pose_ros); +- +-} +- +-void dlio::OdomNode::publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { +- this->publishCloud(published_cloud, T_cloud); +- +- // nav_msgs::Path +- this->path_ros.header.stamp = this->imu_stamp; +- this->path_ros.header.frame_id = this->odom_frame; +- +- geometry_msgs::PoseStamped p; +- p.header.stamp = this->imu_stamp; +- p.header.frame_id = this->odom_frame; +- p.pose.position.x = this->state.p[0]; +- p.pose.position.y = this->state.p[1]; +- p.pose.position.z = this->state.p[2]; +- p.pose.orientation.w = this->state.q.w(); +- p.pose.orientation.x = this->state.q.x(); +- p.pose.orientation.y = this->state.q.y(); +- p.pose.orientation.z = this->state.q.z(); +- +- this->path_ros.poses.push_back(p); +- this->path_pub.publish(this->path_ros); +- +- // transform: odom to baselink +- static tf2_ros::TransformBroadcaster br; +- geometry_msgs::TransformStamped transformStamped; +- +- transformStamped.header.stamp = this->imu_stamp; +- transformStamped.header.frame_id = this->odom_frame; +- transformStamped.child_frame_id = this->baselink_frame; +- +- transformStamped.transform.translation.x = this->state.p[0]; +- transformStamped.transform.translation.y = this->state.p[1]; +- transformStamped.transform.translation.z = this->state.p[2]; +- +- transformStamped.transform.rotation.w = this->state.q.w(); +- transformStamped.transform.rotation.x = this->state.q.x(); +- transformStamped.transform.rotation.y = this->state.q.y(); +- transformStamped.transform.rotation.z = this->state.q.z(); +- +- br.sendTransform(transformStamped); +- +- // transform: baselink to imu +- transformStamped.header.stamp = this->imu_stamp; +- transformStamped.header.frame_id = this->baselink_frame; +- transformStamped.child_frame_id = this->imu_frame; +- +- transformStamped.transform.translation.x = this->extrinsics.baselink2imu.t[0]; +- transformStamped.transform.translation.y = this->extrinsics.baselink2imu.t[1]; +- transformStamped.transform.translation.z = this->extrinsics.baselink2imu.t[2]; +- +- Eigen::Quaternionf q(this->extrinsics.baselink2imu.R); +- transformStamped.transform.rotation.w = q.w(); +- transformStamped.transform.rotation.x = q.x(); +- transformStamped.transform.rotation.y = q.y(); +- transformStamped.transform.rotation.z = q.z(); ++void dlio::OdomNode::getScanFromROS( ++ const pcl::PointCloud::ConstPtr &pc, double stamp) { + +- br.sendTransform(transformStamped); +- +- // transform: baselink to lidar +- transformStamped.header.stamp = this->imu_stamp; +- transformStamped.header.frame_id = this->baselink_frame; +- transformStamped.child_frame_id = this->lidar_frame; +- +- transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; +- transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; +- transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; +- +- Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); +- transformStamped.transform.rotation.w = qq.w(); +- transformStamped.transform.rotation.x = qq.x(); +- transformStamped.transform.rotation.y = qq.y(); +- transformStamped.transform.rotation.z = qq.z(); +- +- br.sendTransform(transformStamped); +- +-} +- +-void dlio::OdomNode::publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { +- +- if (this->wait_until_move_) { +- if (this->length_traversed < 0.1) { return; } +- } +- +- pcl::PointCloud::Ptr deskewed_scan_t_ (boost::make_shared>()); +- +- pcl::transformPointCloud (*published_cloud, *deskewed_scan_t_, T_cloud); +- +- // published deskewed cloud +- sensor_msgs::PointCloud2 deskewed_ros; +- pcl::toROSMsg(*deskewed_scan_t_, deskewed_ros); +- deskewed_ros.header.stamp = this->scan_header_stamp; +- deskewed_ros.header.frame_id = this->odom_frame; +- this->deskewed_pub.publish(deskewed_ros); +- +-} +- +-void dlio::OdomNode::publishKeyframe(std::pair, pcl::PointCloud::ConstPtr> kf, ros::Time timestamp) { +- +- // Push back +- geometry_msgs::Pose p; +- p.position.x = kf.first.first[0]; +- p.position.y = kf.first.first[1]; +- p.position.z = kf.first.first[2]; +- p.orientation.w = kf.first.second.w(); +- p.orientation.x = kf.first.second.x(); +- p.orientation.y = kf.first.second.y(); +- p.orientation.z = kf.first.second.z(); +- this->kf_pose_ros.poses.push_back(p); +- +- // Publish +- this->kf_pose_ros.header.stamp = timestamp; +- this->kf_pose_ros.header.frame_id = this->odom_frame; +- this->kf_pose_pub.publish(this->kf_pose_ros); +- +- // publish keyframe scan for map +- if (this->vf_use_) { +- if (kf.second->points.size() == kf.second->width * kf.second->height) { +- sensor_msgs::PointCloud2 keyframe_cloud_ros; +- pcl::toROSMsg(*kf.second, keyframe_cloud_ros); +- keyframe_cloud_ros.header.stamp = timestamp; +- keyframe_cloud_ros.header.frame_id = this->odom_frame; +- this->kf_cloud_pub.publish(keyframe_cloud_ros); +- } +- } else { +- sensor_msgs::PointCloud2 keyframe_cloud_ros; +- pcl::toROSMsg(*kf.second, keyframe_cloud_ros); +- keyframe_cloud_ros.header.stamp = timestamp; +- keyframe_cloud_ros.header.frame_id = this->odom_frame; +- this->kf_cloud_pub.publish(keyframe_cloud_ros); +- } +- +-} +- +-void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) { +- +- pcl::PointCloud::Ptr original_scan_ (boost::make_shared>()); +- pcl::fromROSMsg(*pc, *original_scan_); ++ pcl::PointCloud::Ptr original_scan_( ++ std::make_shared>()); ++ *original_scan_ = *pc; + + // Remove NaNs + std::vector idx; +@@ -508,28 +250,14 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) + this->crop.filter(*original_scan_); + + // automatically detect sensor type +- this->sensor = dlio::SensorType::UNKNOWN; +- for (auto &field : pc->fields) { +- if (field.name == "t") { +- this->sensor = dlio::SensorType::OUSTER; +- break; +- } else if (field.name == "time") { +- this->sensor = dlio::SensorType::VELODYNE; +- break; +- } else if (field.name == "timestamp" && original_scan_->points[0].timestamp < 1e14) { +- this->sensor = dlio::SensorType::HESAI; +- break; +- } else if (field.name == "timestamp" && original_scan_->points[0].timestamp > 1e14) { +- this->sensor = dlio::SensorType::LIVOX; +- break; +- } +- } +- ++ // NOTE: Ouster used here as it utilizes time since start of scan in ++ // nanoseconds like evalio ++ this->sensor = dlio::SensorType::OUSTER; + if (this->sensor == dlio::SensorType::UNKNOWN) { + this->deskew_ = false; + } + +- this->scan_header_stamp = pc->header.stamp; ++ this->scan_header_stamp = stamp; + this->original_scan = original_scan_; + + } +@@ -547,7 +275,7 @@ void dlio::OdomNode::preprocessPoints() { + + } else { + +- this->scan_stamp = this->scan_header_stamp.toSec(); ++ this->scan_stamp = this->scan_header_stamp; + + // don't process scans until IMU data is present + if (!this->first_valid_scan) { +@@ -574,7 +302,7 @@ void dlio::OdomNode::preprocessPoints() { + + } + +- pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); ++ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); + pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, + this->T_prior * this->extrinsics.baselink2lidar_T); + this->deskewed_scan = deskewed_scan_; +@@ -584,7 +312,7 @@ void dlio::OdomNode::preprocessPoints() { + // Voxel Grid Filter + if (this->vf_use_) { + pcl::PointCloud::Ptr current_scan_ +- (boost::make_shared>(*this->deskewed_scan)); ++ (std::make_shared>(*this->deskewed_scan)); + this->voxel.setInputCloud(current_scan_); + this->voxel.filter(*current_scan_); + this->current_scan = current_scan_; +@@ -596,11 +324,11 @@ void dlio::OdomNode::preprocessPoints() { + + void dlio::OdomNode::deskewPointcloud() { + +- pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); ++ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); + deskewed_scan_->points.resize(this->original_scan->points.size()); + + // individual point timestamps should be relative to this time +- double sweep_ref_time = this->scan_header_stamp.toSec(); ++ double sweep_ref_time = this->scan_header_stamp; + + // sort points by timestamp and build list of timestamps + std::function point_time_cmp; +@@ -700,8 +428,10 @@ void dlio::OdomNode::deskewPointcloud() { + // if there are no frames between the start and end of the sweep + // that probably means that there's a sync issue + if (frames.size() != timestamps.size()) { +- ROS_FATAL("Bad time sync between LiDAR and IMU!"); +- ++ printf("FATAL: "); ++ printf("Bad time sync between LiDAR and IMU!"); ++ printf("\n"); ++ + this->T_prior = this->T; + pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); + this->deskewed_scan = deskewed_scan_; +@@ -755,20 +485,25 @@ void dlio::OdomNode::initializeDLIO() { + } + + this->dlio_initialized = true; +- std::cout << std::endl << " DLIO initialized!" << std::endl; ++ if (verbose) { ++ std::cout << std::endl << " DLIO initialized!" << std::endl; ++ } + + } + +-void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) { ++void dlio::OdomNode::callbackPointCloud( ++ const pcl::PointCloud::ConstPtr &pc, double stamp) { + + std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); + this->main_loop_running = true; + lock.unlock(); + +- double then = ros::Time::now().toSec(); ++ double then = std::chrono::duration_cast>( ++ std::chrono::system_clock::now().time_since_epoch()) ++ .count(); + + if (this->first_scan_stamp == 0.) { +- this->first_scan_stamp = pc->header.stamp.toSec(); ++ this->first_scan_stamp = stamp; + } + + // DLIO Initialization procedures (IMU calib, gravity align) +@@ -777,7 +512,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + } + + // Convert incoming scan into DLIO format +- this->getScanFromROS(pc); ++ this->getScanFromROS(pc, stamp); + + // Preprocess points + this->preprocessPoints(); +@@ -787,13 +522,14 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + } + + if (this->current_scan->points.size() <= this->gicp_min_num_points_) { +- ROS_FATAL("Low number of points in the cloud!"); ++ printf("FATAL: "); ++ printf("Low number of points in the cloud!"); ++ printf("\n"); + return; + } + + // Compute Metrics +- this->metrics_thread = std::thread( &dlio::OdomNode::computeMetrics, this ); +- this->metrics_thread.detach(); ++ this->computeMetrics(); + + // Set Adaptive Parameters + if (this->adaptive_params_) { +@@ -839,50 +575,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + this->prev_scan_stamp = this->scan_stamp; + this->elapsed_time = this->scan_stamp - this->first_scan_stamp; + +- // Publish stuff to ROS +- pcl::PointCloud::ConstPtr published_cloud; +- if (this->densemap_filtered_) { +- published_cloud = this->current_scan; +- } else { +- published_cloud = this->deskewed_scan; +- } +- this->publish_thread = std::thread( &dlio::OdomNode::publishToROS, this, published_cloud, this->T_corr ); +- this->publish_thread.detach(); +- + // Update some statistics +- this->comp_times.push_back(ros::Time::now().toSec() - then); ++ this->comp_times.push_back( ++ std::chrono::duration_cast>( ++ std::chrono::system_clock::now().time_since_epoch()) ++ .count() - ++ then); + this->gicp_hasConverged = this->gicp.hasConverged(); + + // Debug statements and publish custom DLIO message + if (this->verbose) { +- this->debug_thread = std::thread( &dlio::OdomNode::debug, this ); +- this->debug_thread.detach(); ++ this->debug(); + } + + this->geo.first_opt_done = true; + } + +-void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { ++void dlio::OdomNode::callbackImu(const ImuMeas &imu_raw) { + + this->first_imu_received = true; + +- sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw ); +- this->imu_stamp = imu->header.stamp; ++ ImuMeas imu = this->transformImu(imu_raw); ++ this->imu_stamp = imu.stamp; + + Eigen::Vector3f lin_accel; + Eigen::Vector3f ang_vel; + + // Get IMU samples +- ang_vel[0] = imu->angular_velocity.x; +- ang_vel[1] = imu->angular_velocity.y; +- ang_vel[2] = imu->angular_velocity.z; ++ ang_vel[0] = imu.ang_vel[0]; ++ ang_vel[1] = imu.ang_vel[1]; ++ ang_vel[2] = imu.ang_vel[2]; + +- lin_accel[0] = imu->linear_acceleration.x; +- lin_accel[1] = imu->linear_acceleration.y; +- lin_accel[2] = imu->linear_acceleration.z; ++ lin_accel[0] = imu.lin_accel[0]; ++ lin_accel[1] = imu.lin_accel[1]; ++ lin_accel[2] = imu.lin_accel[2]; + + if (this->first_imu_stamp == 0.) { +- this->first_imu_stamp = imu->header.stamp.toSec(); ++ this->first_imu_stamp = imu.stamp; + } + + // IMU calibration procedure - do for three seconds +@@ -893,7 +622,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + static Eigen::Vector3f accel_avg (0., 0., 0.); + static bool print = true; + +- if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) { ++ if ((imu.stamp - this->first_imu_stamp) < this->imu_calib_time_) { + + num_samples++; + +@@ -913,7 +642,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + + } else { + +- std::cout << "done" << std::endl << std::endl; ++ if(verbose) { ++ std::cout << "done" << std::endl << std::endl; ++ } + + gyro_avg /= num_samples; + accel_avg /= num_samples; +@@ -943,44 +674,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + pitch = remainder(180.0 - pitch, 360.0); + roll = remainder(roll + 180.0, 360.0); + } +- std::cout << " Estimated initial attitude:" << std::endl; +- std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; +- std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; +- std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; +- std::cout << std::endl; ++ if (verbose){ ++ std::cout << " Estimated initial attitude:" << std::endl; ++ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; ++ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; ++ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; ++ std::cout << std::endl; ++ } + } + + if (this->calibrate_accel_) { + + // subtract gravity from avg accel to get bias + this->state.b.accel = accel_avg - grav_vec; +- +- std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " +- << to_string_with_precision(this->state.b.accel[1], 8) << ", " +- << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; ++ if (verbose) { ++ std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " ++ << to_string_with_precision(this->state.b.accel[1], 8) << ", " ++ << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; ++ } + } + + if (this->calibrate_gyro_) { + + this->state.b.gyro = gyro_avg; +- +- std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " +- << to_string_with_precision(this->state.b.gyro[1], 8) << ", " +- << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; ++ if(verbose) { ++ std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " ++ << to_string_with_precision(this->state.b.gyro[1], 8) << ", " ++ << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; ++ } + } + + this->imu_calibrated = true; +- + } + + } else { + +- double dt = imu->header.stamp.toSec() - this->prev_imu_stamp; ++ double dt = imu.stamp - this->prev_imu_stamp; + if (dt == 0) { dt = 1.0/200.0; } + this->imu_rates.push_back( 1./dt ); + + // Apply the calibrated bias to the new IMU measurements +- this->imu_meas.stamp = imu->header.stamp.toSec(); ++ this->imu_meas.stamp = imu.stamp; + this->imu_meas.dt = dt; + this->prev_imu_stamp = this->imu_meas.stamp; + +@@ -990,7 +724,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + this->imu_meas.lin_accel = lin_accel_corrected; + this->imu_meas.ang_vel = ang_vel_corrected; + +- // Store calibrated IMU measurements into imu buffer for manual integration later. ++ // Store calibrated IMU measurements into imu buffer for manual integration ++ // later. + this->mtx_imu.lock(); + this->imu_buffer.push_front(this->imu_meas); + this->mtx_imu.unlock(); +@@ -1027,7 +762,7 @@ void dlio::OdomNode::getNextPose() { + } + + // Align with current submap with global IMU transformation as initial guess +- pcl::PointCloud::Ptr aligned (boost::make_shared>()); ++ pcl::PointCloud::Ptr aligned (std::make_shared>()); + this->gicp.align(*aligned); + + // Get final transformation in global frame +@@ -1268,7 +1003,6 @@ void dlio::OdomNode::propagateGICP() { + double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); + q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; + this->lidarPose.q = q; +- + } + + void dlio::OdomNode::propagateState() { +@@ -1369,36 +1103,36 @@ void dlio::OdomNode::updateState() { + + } + +-sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { ++dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas &imu_raw) { + +- sensor_msgs::Imu::Ptr imu (new sensor_msgs::Imu); ++ ImuMeas imu; + + // Copy header +- imu->header = imu_raw->header; ++ imu.stamp = imu_raw.stamp; + +- static double prev_stamp = imu->header.stamp.toSec(); +- double dt = imu->header.stamp.toSec() - prev_stamp; +- prev_stamp = imu->header.stamp.toSec(); ++ static double prev_stamp = imu.stamp; ++ double dt = imu.stamp - prev_stamp; ++ prev_stamp = imu.stamp; + + if (dt == 0) { dt = 1.0/200.0; } + +- // Transform angular velocity (will be the same on a rigid body, so just rotate to ROS convention) +- Eigen::Vector3f ang_vel(imu_raw->angular_velocity.x, +- imu_raw->angular_velocity.y, +- imu_raw->angular_velocity.z); ++ // Transform angular velocity (will be the same on a rigid body, so just ++ // rotate to ROS convention) ++ Eigen::Vector3f ang_vel(imu_raw.ang_vel[0], imu_raw.ang_vel[1], ++ imu_raw.ang_vel[2]); + + Eigen::Vector3f ang_vel_cg = this->extrinsics.baselink2imu.R * ang_vel; + +- imu->angular_velocity.x = ang_vel_cg[0]; +- imu->angular_velocity.y = ang_vel_cg[1]; +- imu->angular_velocity.z = ang_vel_cg[2]; ++ imu.ang_vel[0] = ang_vel_cg[0]; ++ imu.ang_vel[1] = ang_vel_cg[1]; ++ imu.ang_vel[2] = ang_vel_cg[2]; + + static Eigen::Vector3f ang_vel_cg_prev = ang_vel_cg; + +- // Transform linear acceleration (need to account for component due to translational difference) +- Eigen::Vector3f lin_accel(imu_raw->linear_acceleration.x, +- imu_raw->linear_acceleration.y, +- imu_raw->linear_acceleration.z); ++ // Transform linear acceleration (need to account for component due to ++ // translational difference) ++ Eigen::Vector3f lin_accel(imu_raw.lin_accel[0], imu_raw.lin_accel[1], ++ imu_raw.lin_accel[2]); + + Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; + +@@ -1408,9 +1142,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const + + ang_vel_cg_prev = ang_vel_cg; + +- imu->linear_acceleration.x = lin_accel_cg[0]; +- imu->linear_acceleration.y = lin_accel_cg[1]; +- imu->linear_acceleration.z = lin_accel_cg[2]; ++ imu.lin_accel[0] = lin_accel_cg[0]; ++ imu.lin_accel[1] = lin_accel_cg[1]; ++ imu.lin_accel[2] = lin_accel_cg[2]; + + return imu; + +@@ -1471,7 +1205,7 @@ void dlio::OdomNode::computeConvexHull() { + + // create a pointcloud with points at keyframes + pcl::PointCloud::Ptr cloud = +- pcl::PointCloud::Ptr (boost::make_shared>()); ++ pcl::PointCloud::Ptr (std::make_shared>()); + + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + for (int i = 0; i < this->num_processed_keyframes; i++) { +@@ -1488,10 +1222,10 @@ void dlio::OdomNode::computeConvexHull() { + + // get the indices of the keyframes on the convex hull + pcl::PointCloud::Ptr convex_points = +- pcl::PointCloud::Ptr (boost::make_shared>()); ++ pcl::PointCloud::Ptr (std::make_shared>()); + this->convex_hull.reconstruct(*convex_points); + +- pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); ++ pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); + this->convex_hull.getHullPointIndices(*convex_hull_point_idx); + + this->keyframe_convex.clear(); +@@ -1510,7 +1244,7 @@ void dlio::OdomNode::computeConcaveHull() { + + // create a pointcloud with points at keyframes + pcl::PointCloud::Ptr cloud = +- pcl::PointCloud::Ptr (boost::make_shared>()); ++ pcl::PointCloud::Ptr (std::make_shared>()); + + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + for (int i = 0; i < this->num_processed_keyframes; i++) { +@@ -1527,10 +1261,10 @@ void dlio::OdomNode::computeConcaveHull() { + + // get the indices of the keyframes on the concave hull + pcl::PointCloud::Ptr concave_points = +- pcl::PointCloud::Ptr (boost::make_shared>()); ++ pcl::PointCloud::Ptr (std::make_shared>()); + this->concave_hull.reconstruct(*concave_points); + +- pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); ++ pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); + this->concave_hull.getHullPointIndices(*concave_hull_point_idx); + + this->keyframe_concave.clear(); +@@ -1739,7 +1473,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { + this->pauseSubmapBuildIfNeeded(); + + // reinitialize submap cloud and normals +- pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); ++ pcl::PointCloud::Ptr submap_cloud_ (std::make_shared>()); + std::shared_ptr submap_normals_ (std::make_shared()); + + for (auto k : this->submap_kf_idx_curr) { +@@ -1780,7 +1514,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { + + Eigen::Matrix4d Td = T.cast(); + +- pcl::PointCloud::Ptr transformed_keyframe (boost::make_shared>()); ++ pcl::PointCloud::Ptr transformed_keyframe (std::make_shared>()); + pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); + + std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); +@@ -1792,9 +1526,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { + lock.lock(); + this->keyframes[i].second = transformed_keyframe; + this->keyframe_normals[i] = transformed_covariances; +- +- this->publish_keyframe_thread = std::thread( &dlio::OdomNode::publishKeyframe, this, this->keyframes[i], this->keyframe_timestamps[i] ); +- this->publish_keyframe_thread.detach(); + } + + lock.unlock(); +@@ -2018,3 +1749,19 @@ void dlio::OdomNode::debug() { + std::cout << "+-------------------------------------------------------------------+" << std::endl; + + } ++ ++// Explicit instantiations of PCL templates for dlio::Point ++// This is needed because PCL doesn't automatically instantiate templates for ++// custom point types ++#include ++#include ++#include ++#include ++#include ++#include ++ ++template class pcl::PCLBase; ++template class pcl::search::Search; ++template class pcl::KdTreeFLANN>; ++template class pcl::search::KdTree< ++ dlio::Point, pcl::KdTreeFLANN>>; +\ No newline at end of file +diff --git a/src/nano_gicp/lsq_registration.cc b/src/nano_gicp/lsq_registration.cc +index 271cb03..b6c8f07 100644 +--- a/src/nano_gicp/lsq_registration.cc ++++ b/src/nano_gicp/lsq_registration.cc +@@ -42,10 +42,8 @@ + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +-#include "dlio/dlio.h" + #include "nano_gicp/lsq_registration.h" +- +-template class nano_gicp::LsqRegistration; ++#include "dlio/dlio.h" + + namespace nano_gicp { + +@@ -228,4 +226,7 @@ bool LsqRegistration::step_lm(Eigen::Isometry3d& x0, E + return false; + } + +-} // namespace nano_gicp ++} // namespace nano_gicp ++ ++// Explicit template instantiation must come after all member definitions ++template class nano_gicp::LsqRegistration; +diff --git a/src/nano_gicp/nano_gicp.cc b/src/nano_gicp/nano_gicp.cc +index 906e3c3..b9c67a4 100644 +--- a/src/nano_gicp/nano_gicp.cc ++++ b/src/nano_gicp/nano_gicp.cc +@@ -42,10 +42,8 @@ + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +-#include "dlio/dlio.h" + #include "nano_gicp/nano_gicp.h" +- +-template class nano_gicp::NanoGICP; ++#include "dlio/dlio.h" + + namespace nano_gicp { + +@@ -391,4 +389,7 @@ bool NanoGICP::calculate_covariances( + return true; + } + +-} // namespace nano_gicp ++} // namespace nano_gicp ++ ++// Explicit template instantiation must come after all member definitions ++template class nano_gicp::NanoGICP; From 096d8446803b9f57400c5217646745d8966ebe07 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 14:34:58 -0400 Subject: [PATCH 02/11] Start finalizing dlio --- cpp/bindings/pipelines/dlio.h | 41 ++++++++-------- cpp/bindings/pipelines/dlio.patch | 77 +++++++++++++++++++------------ python/evalio/rerun.py | 3 +- 3 files changed, 71 insertions(+), 50 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.h b/cpp/bindings/pipelines/dlio.h index 4587513b..9b1eb97a 100644 --- a/cpp/bindings/pipelines/dlio.h +++ b/cpp/bindings/pipelines/dlio.h @@ -77,54 +77,55 @@ class DLIO: public ev::Pipeline { 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, true, config_.verbose), + (bool, verbose, false, config_.verbose), (bool, deskew, true, config_.deskew), (double, gravity, 9.80665, config_.gravity), - (bool, time_offset, false, config_.time_offset), + (bool, time_offset, true, config_.time_offset), - (double, keyframe_thresh_dist, 0.1, config_.keyframe_thresh_dist), - (double, keyframe_thresh_rot, 1.0, config_.keyframe_thresh_rot), + (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, true, config_.densemap_filtered), - (bool, wait_until_move, false, config_.wait_until_move), + (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.05, config_.vf_res), + (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, 2000, config_.imu_buffer_size), + (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, 100, config_.gicp_min_num_points), - (int, gicp_k_correspondences, 20, config_.gicp_k_correspondences), - (double, gicp_max_corr_dist, 10000.0, config_.gicp_max_corr_dist), - (int, gicp_max_iter, 64, config_.gicp_max_iter), - (double, gicp_transformation_ep, 0.0005, config_.gicp_transformation_ep), - (double, gicp_rotation_ep, 0.0005, config_.gicp_rotation_ep), + (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, 1.0, config_.geo_Kp), - (double, geo_Kv, 1.0, config_.geo_Kv), - (double, geo_Kq, 1.0, config_.geo_Kq), - (double, geo_Kab, 1.0, config_.geo_Kab), + (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, 1.0, config_.geo_abias_max), - (double, geo_gbias_max, 1.0, config_.geo_gbias_max) + (double, geo_abias_max, 5.0, config_.geo_abias_max), + (double, geo_gbias_max, 0.5, config_.geo_gbias_max) ); // clang-format on diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index 67af9f04..51e9c07a 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -431,7 +431,7 @@ index c79ebd7..2c91f67 100644 bool verbose; diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc -index bceae37..4cd75fc 100644 +index bceae37..51d7421 100644 --- a/src/dlio/odom.cc +++ b/src/dlio/odom.cc @@ -12,9 +12,9 @@ @@ -509,71 +509,71 @@ index bceae37..4cd75fc 100644 - ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); - ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); - ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); -+void dlio::OdomNode::getParams(const Params ¶ms) { -+ this->verbose = params.verbose; - +- - // Get Node NS and Remove Leading Character - std::string ns = ros::this_node::getNamespace(); - ns.erase(0,1); -+ this->deskew_ = params.deskew; -+ this->gravity_ = params.gravity; -+ this->time_offset_ = params.time_offset; - +- - // Concatenate Frame Name Strings - this->odom_frame = ns + "/" + this->odom_frame; - this->baselink_frame = ns + "/" + this->baselink_frame; - this->lidar_frame = ns + "/" + this->lidar_frame; - this->imu_frame = ns + "/" + this->imu_frame; -+ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; -+ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - +- - // Deskew FLag - ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); -+ this->submap_knn_ = params.submap_knn; -+ this->submap_kcv_ = params.submap_kcv; -+ this->submap_kcc_ = params.submap_kcc; - +- - // Gravity - ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); -+ this->densemap_filtered_ = params.densemap_filtered; - +- - // Compute time offset between lidar and imu - ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); -+ this->wait_until_move_ = params.wait_until_move; ++void dlio::OdomNode::getParams(const Params ¶ms) { ++ this->verbose = params.verbose; - // Keyframe Threshold - ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); - ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); -+ this->crop_size_ = params.crop_size; ++ this->deskew_ = params.deskew; ++ this->gravity_ = params.gravity; ++ this->time_offset_ = params.time_offset; - // Submap - ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); - ros::param::param("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10); - ros::param::param("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10); -+ this->vf_use_ = params.vf_use; -+ this->vf_res_ = params.vf_res; ++ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; ++ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - // Dense map resolution - ros::param::param("~dlio/map/dense/filtered", this->densemap_filtered_, true); -- ++ this->submap_knn_ = params.submap_knn; ++ this->submap_kcv_ = params.submap_kcv; ++ this->submap_kcc_ = params.submap_kcc; + - // Wait until movement to publish map - ros::param::param("~dlio/map/waitUntilMove", this->wait_until_move_, false); -- ++ this->densemap_filtered_ = params.densemap_filtered; + - // Crop Box Filter - ros::param::param("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); -- ++ this->wait_until_move_ = params.wait_until_move; + - // Voxel Grid Filter - ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); - ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); -+ this->adaptive_params_ = params.adaptive_params; ++ this->crop_size_ = params.crop_size; - // Adaptive Parameters - ros::param::param("~dlio/adaptive", this->adaptive_params_, true); -- ++ this->vf_use_ = params.vf_use; ++ this->vf_res_ = params.vf_res; + - // Extrinsics - std::vector t_default{0., 0., 0.}; - std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; -- ++ this->adaptive_params_ = params.adaptive_params; + - // center of gravity to imu - std::vector baselink2imu_t, baselink2imu_R; - ros::param::param>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default); @@ -801,11 +801,11 @@ index bceae37..4cd75fc 100644 - transformStamped.transform.rotation.x = q.x(); - transformStamped.transform.rotation.y = q.y(); - transformStamped.transform.rotation.z = q.z(); +- +- br.sendTransform(transformStamped); +void dlio::OdomNode::getScanFromROS( + const pcl::PointCloud::ConstPtr &pc, double stamp) { -- br.sendTransform(transformStamped); -- - // transform: baselink to lidar - transformStamped.header.stamp = this->imu_stamp; - transformStamped.header.frame_id = this->baselink_frame; @@ -1205,6 +1205,25 @@ index bceae37..4cd75fc 100644 this->gicp.align(*aligned); // Get final transformation in global frame +@@ -1046,12 +781,12 @@ void dlio::OdomNode::getNextPose() { + bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, + boost::circular_buffer::reverse_iterator& begin_imu_it, + boost::circular_buffer::reverse_iterator& end_imu_it) { +- +- if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { +- // Wait for the latest IMU data +- std::unique_lockmtx_imu)> lock(this->mtx_imu); +- this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); +- } ++ // Evalio run deterministically, nothing will ever appear here! ++ // if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { ++ // // Wait for the latest IMU data ++ // std::unique_lockmtx_imu)> lock(this->mtx_imu); ++ // this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); ++ // } + + auto imu_it = this->imu_buffer.begin(); + @@ -1268,7 +1003,6 @@ void dlio::OdomNode::propagateGICP() { double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; diff --git a/python/evalio/rerun.py b/python/evalio/rerun.py index 0a614014..1bb6ffa6 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 From 15599ab3a02a902581aaf1f7c6412330d3b2450e Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 14:56:41 -0400 Subject: [PATCH 03/11] Last change! --- cpp/bindings/pipelines/dlio.patch | 150 ++++++++++++++++-------------- cpp/setup_pipelines.sh | 10 ++ 2 files changed, 91 insertions(+), 69 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index 51e9c07a..c1dfdb82 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -431,7 +431,7 @@ index c79ebd7..2c91f67 100644 bool verbose; diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc -index bceae37..51d7421 100644 +index bceae37..c0c625f 100644 --- a/src/dlio/odom.cc +++ b/src/dlio/odom.cc @@ -12,9 +12,9 @@ @@ -509,71 +509,71 @@ index bceae37..51d7421 100644 - ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); - ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); - ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); -- ++void dlio::OdomNode::getParams(const Params ¶ms) { ++ this->verbose = params.verbose; + - // Get Node NS and Remove Leading Character - std::string ns = ros::this_node::getNamespace(); - ns.erase(0,1); -- ++ this->deskew_ = params.deskew; ++ this->gravity_ = params.gravity; ++ this->time_offset_ = params.time_offset; + - // Concatenate Frame Name Strings - this->odom_frame = ns + "/" + this->odom_frame; - this->baselink_frame = ns + "/" + this->baselink_frame; - this->lidar_frame = ns + "/" + this->lidar_frame; - this->imu_frame = ns + "/" + this->imu_frame; -- ++ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; ++ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; + - // Deskew FLag - ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); -- ++ this->submap_knn_ = params.submap_knn; ++ this->submap_kcv_ = params.submap_kcv; ++ this->submap_kcc_ = params.submap_kcc; + - // Gravity - ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); -- ++ this->densemap_filtered_ = params.densemap_filtered; + - // Compute time offset between lidar and imu - ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); -+void dlio::OdomNode::getParams(const Params ¶ms) { -+ this->verbose = params.verbose; ++ this->wait_until_move_ = params.wait_until_move; - // Keyframe Threshold - ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); - ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); -+ this->deskew_ = params.deskew; -+ this->gravity_ = params.gravity; -+ this->time_offset_ = params.time_offset; ++ this->crop_size_ = params.crop_size; - // Submap - ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); - ros::param::param("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10); - ros::param::param("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10); -+ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; -+ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - +- - // Dense map resolution - ros::param::param("~dlio/map/dense/filtered", this->densemap_filtered_, true); -+ this->submap_knn_ = params.submap_knn; -+ this->submap_kcv_ = params.submap_kcv; -+ this->submap_kcc_ = params.submap_kcc; - +- - // Wait until movement to publish map - ros::param::param("~dlio/map/waitUntilMove", this->wait_until_move_, false); -+ this->densemap_filtered_ = params.densemap_filtered; - +- - // Crop Box Filter - ros::param::param("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); -+ this->wait_until_move_ = params.wait_until_move; - +- - // Voxel Grid Filter - ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); - ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); -+ this->crop_size_ = params.crop_size; ++ this->vf_use_ = params.vf_use; ++ this->vf_res_ = params.vf_res; - // Adaptive Parameters - ros::param::param("~dlio/adaptive", this->adaptive_params_, true); -+ this->vf_use_ = params.vf_use; -+ this->vf_res_ = params.vf_res; ++ this->adaptive_params_ = params.adaptive_params; - // Extrinsics - std::vector t_default{0., 0., 0.}; - std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; -+ this->adaptive_params_ = params.adaptive_params; - +- - // center of gravity to imu - std::vector baselink2imu_t, baselink2imu_R; - ros::param::param>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default); @@ -803,9 +803,7 @@ index bceae37..51d7421 100644 - transformStamped.transform.rotation.z = q.z(); - - br.sendTransform(transformStamped); -+void dlio::OdomNode::getScanFromROS( -+ const pcl::PointCloud::ConstPtr &pc, double stamp) { - +- - // transform: baselink to lidar - transformStamped.header.stamp = this->imu_stamp; - transformStamped.header.frame_id = this->baselink_frame; @@ -814,7 +812,9 @@ index bceae37..51d7421 100644 - transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; - transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; - transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; -- ++void dlio::OdomNode::getScanFromROS( ++ const pcl::PointCloud::ConstPtr &pc, double stamp) { + - Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); - transformStamped.transform.rotation.w = qq.w(); - transformStamped.transform.rotation.x = qq.x(); @@ -965,20 +965,22 @@ index bceae37..51d7421 100644 // sort points by timestamp and build list of timestamps std::function point_time_cmp; -@@ -700,8 +428,10 @@ void dlio::OdomNode::deskewPointcloud() { +@@ -700,8 +428,12 @@ void dlio::OdomNode::deskewPointcloud() { // if there are no frames between the start and end of the sweep // that probably means that there's a sync issue if (frames.size() != timestamps.size()) { - ROS_FATAL("Bad time sync between LiDAR and IMU!"); - -+ printf("FATAL: "); -+ printf("Bad time sync between LiDAR and IMU!"); -+ printf("\n"); ++ if(verbose) { ++ printf("FATAL: "); ++ printf("Bad time sync between LiDAR and IMU!"); ++ printf("\n"); ++ } + this->T_prior = this->T; pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); this->deskewed_scan = deskewed_scan_; -@@ -755,20 +485,25 @@ void dlio::OdomNode::initializeDLIO() { +@@ -755,20 +487,25 @@ void dlio::OdomNode::initializeDLIO() { } this->dlio_initialized = true; @@ -1008,7 +1010,7 @@ index bceae37..51d7421 100644 } // DLIO Initialization procedures (IMU calib, gravity align) -@@ -777,7 +512,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -777,7 +514,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } // Convert incoming scan into DLIO format @@ -1017,15 +1019,18 @@ index bceae37..51d7421 100644 // Preprocess points this->preprocessPoints(); -@@ -787,13 +522,14 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -787,13 +524,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } if (this->current_scan->points.size() <= this->gicp_min_num_points_) { - ROS_FATAL("Low number of points in the cloud!"); -+ printf("FATAL: "); -+ printf("Low number of points in the cloud!"); -+ printf("\n"); - return; +- return; ++ if (verbose) { ++ printf("FATAL: "); ++ printf("Low number of points in the cloud!"); ++ printf("\n"); ++ return; ++ } } // Compute Metrics @@ -1035,7 +1040,7 @@ index bceae37..51d7421 100644 // Set Adaptive Parameters if (this->adaptive_params_) { -@@ -839,50 +575,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -839,50 +579,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& this->prev_scan_stamp = this->scan_stamp; this->elapsed_time = this->scan_stamp - this->first_scan_stamp; @@ -1102,7 +1107,7 @@ index bceae37..51d7421 100644 } // IMU calibration procedure - do for three seconds -@@ -893,7 +622,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +@@ -893,7 +626,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { static Eigen::Vector3f accel_avg (0., 0., 0.); static bool print = true; @@ -1111,7 +1116,16 @@ index bceae37..51d7421 100644 num_samples++; -@@ -913,7 +642,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +@@ -905,7 +638,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + accel_avg[1] += lin_accel[1]; + accel_avg[2] += lin_accel[2]; + +- if(print) { ++ if(verbose) { + std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; + std::cout.flush(); + print = false; +@@ -913,7 +646,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { } else { @@ -1122,7 +1136,7 @@ index bceae37..51d7421 100644 gyro_avg /= num_samples; accel_avg /= num_samples; -@@ -943,44 +674,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +@@ -943,44 +678,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { pitch = remainder(180.0 - pitch, 360.0); roll = remainder(roll + 180.0, 360.0); } @@ -1186,7 +1200,7 @@ index bceae37..51d7421 100644 this->imu_meas.dt = dt; this->prev_imu_stamp = this->imu_meas.stamp; -@@ -990,7 +724,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +@@ -990,7 +728,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { this->imu_meas.lin_accel = lin_accel_corrected; this->imu_meas.ang_vel = ang_vel_corrected; @@ -1196,7 +1210,7 @@ index bceae37..51d7421 100644 this->mtx_imu.lock(); this->imu_buffer.push_front(this->imu_meas); this->mtx_imu.unlock(); -@@ -1027,7 +762,7 @@ void dlio::OdomNode::getNextPose() { +@@ -1027,7 +766,7 @@ void dlio::OdomNode::getNextPose() { } // Align with current submap with global IMU transformation as initial guess @@ -1205,26 +1219,24 @@ index bceae37..51d7421 100644 this->gicp.align(*aligned); // Get final transformation in global frame -@@ -1046,12 +781,12 @@ void dlio::OdomNode::getNextPose() { +@@ -1046,11 +785,12 @@ void dlio::OdomNode::getNextPose() { bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, boost::circular_buffer::reverse_iterator& begin_imu_it, boost::circular_buffer::reverse_iterator& end_imu_it) { - -- if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { ++ // Evalio run deterministically, nothing will ever appear here! + if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { - // Wait for the latest IMU data - std::unique_lockmtx_imu)> lock(this->mtx_imu); - this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); -- } -+ // Evalio run deterministically, nothing will ever appear here! -+ // if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { -+ // // Wait for the latest IMU data -+ // std::unique_lockmtx_imu)> lock(this->mtx_imu); -+ // this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); -+ // } ++ // // Wait for the latest IMU data ++ // std::unique_lockmtx_imu)> lock(this->mtx_imu); ++ // this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); ++ return false; + } auto imu_it = this->imu_buffer.begin(); - -@@ -1268,7 +1003,6 @@ void dlio::OdomNode::propagateGICP() { +@@ -1268,7 +1008,6 @@ void dlio::OdomNode::propagateGICP() { double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; this->lidarPose.q = q; @@ -1232,7 +1244,7 @@ index bceae37..51d7421 100644 } void dlio::OdomNode::propagateState() { -@@ -1369,36 +1103,36 @@ void dlio::OdomNode::updateState() { +@@ -1369,36 +1108,36 @@ void dlio::OdomNode::updateState() { } @@ -1286,7 +1298,7 @@ index bceae37..51d7421 100644 Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; -@@ -1408,9 +1142,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const +@@ -1408,9 +1147,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const ang_vel_cg_prev = ang_vel_cg; @@ -1299,7 +1311,7 @@ index bceae37..51d7421 100644 return imu; -@@ -1471,7 +1205,7 @@ void dlio::OdomNode::computeConvexHull() { +@@ -1471,7 +1210,7 @@ void dlio::OdomNode::computeConvexHull() { // create a pointcloud with points at keyframes pcl::PointCloud::Ptr cloud = @@ -1308,7 +1320,7 @@ index bceae37..51d7421 100644 std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1488,10 +1222,10 @@ void dlio::OdomNode::computeConvexHull() { +@@ -1488,10 +1227,10 @@ void dlio::OdomNode::computeConvexHull() { // get the indices of the keyframes on the convex hull pcl::PointCloud::Ptr convex_points = @@ -1321,7 +1333,7 @@ index bceae37..51d7421 100644 this->convex_hull.getHullPointIndices(*convex_hull_point_idx); this->keyframe_convex.clear(); -@@ -1510,7 +1244,7 @@ void dlio::OdomNode::computeConcaveHull() { +@@ -1510,7 +1249,7 @@ void dlio::OdomNode::computeConcaveHull() { // create a pointcloud with points at keyframes pcl::PointCloud::Ptr cloud = @@ -1330,7 +1342,7 @@ index bceae37..51d7421 100644 std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1527,10 +1261,10 @@ void dlio::OdomNode::computeConcaveHull() { +@@ -1527,10 +1266,10 @@ void dlio::OdomNode::computeConcaveHull() { // get the indices of the keyframes on the concave hull pcl::PointCloud::Ptr concave_points = @@ -1343,7 +1355,7 @@ index bceae37..51d7421 100644 this->concave_hull.getHullPointIndices(*concave_hull_point_idx); this->keyframe_concave.clear(); -@@ -1739,7 +1473,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { +@@ -1739,7 +1478,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { this->pauseSubmapBuildIfNeeded(); // reinitialize submap cloud and normals @@ -1352,7 +1364,7 @@ index bceae37..51d7421 100644 std::shared_ptr submap_normals_ (std::make_shared()); for (auto k : this->submap_kf_idx_curr) { -@@ -1780,7 +1514,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { +@@ -1780,7 +1519,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { Eigen::Matrix4d Td = T.cast(); @@ -1361,7 +1373,7 @@ index bceae37..51d7421 100644 pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); -@@ -1792,9 +1526,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { +@@ -1792,9 +1531,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { lock.lock(); this->keyframes[i].second = transformed_keyframe; this->keyframe_normals[i] = transformed_covariances; @@ -1371,7 +1383,7 @@ index bceae37..51d7421 100644 } lock.unlock(); -@@ -2018,3 +1749,19 @@ void dlio::OdomNode::debug() { +@@ -2018,3 +1754,19 @@ void dlio::OdomNode::debug() { std::cout << "+-------------------------------------------------------------------+" << std::endl; } diff --git a/cpp/setup_pipelines.sh b/cpp/setup_pipelines.sh index 2bf5e96c..21e24249 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 .. +if [ ! -d "direct_lidar_inertial_odometry" ]; then + git clone https://github.com/vectr-ucla/direct_lidar_inertial_odometry.git +fi +cd direct_lidar_inertial_odometry +git stash +# master branch as of writing +git checkout fc8d183f18cdcfb9bb4fc754c6d373cedc4cbd04 +git apply ../../pipelines/dlio.patch +cd .. + # ------------------------- Dependencies ------------------------- # cd $topdir if [ ! -d ".vcpkg/" ]; then From 6a86b42bf73bd32674abd2ecdd44cb68ec9d7717 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 14:59:27 -0400 Subject: [PATCH 04/11] Clean up cmake --- cpp/bindings/CMakeLists.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/cpp/bindings/CMakeLists.txt b/cpp/bindings/CMakeLists.txt index d7c465fe..39a7b932 100644 --- a/cpp/bindings/CMakeLists.txt +++ b/cpp/bindings/CMakeLists.txt @@ -66,13 +66,13 @@ 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(direct_lidar_inertial_odometry dlio_odom_node EVALIO_DLIO) +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) message("################### Pipeline Results ###################") if(NOT "${RESULTS}" STREQUAL "") string(STRIP "${RESULTS}" RESULTS) @@ -93,8 +93,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) From e16f7a0fec4a3afa017755bd205d956e81aba131 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 15:14:44 -0400 Subject: [PATCH 05/11] Update some build errors --- cpp/bindings/pipelines/dlio.patch | 36 ++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index c1dfdb82..f651ef6b 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -1,5 +1,5 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt -index 91e47a0..4aa2bdf 100644 +index 91e47a0..e3e2ba0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,10 +11,11 @@ @@ -66,13 +66,15 @@ index 91e47a0..4aa2bdf 100644 # Not all machines have available set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) -@@ -91,29 +49,24 @@ endif() - add_library(nanoflann STATIC - src/nano_gicp/nanoflann.cc - ) +@@ -88,32 +46,23 @@ if(HAS_CPUID) + endif() + + # NanoFLANN +-add_library(nanoflann STATIC +- src/nano_gicp/nanoflann.cc +-) -target_link_libraries(nanoflann ${PCL_LIBRARIES}) -+target_link_libraries(nanoflann PUBLIC ${PCL_LIBRARIES}) -+target_include_directories(nanoflann PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) ++find_package(nanoflann CONFIG REQUIRED) # NanoGICP add_library(nano_gicp STATIC @@ -80,7 +82,7 @@ index 91e47a0..4aa2bdf 100644 src/nano_gicp/nano_gicp.cc ) -target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) -+target_link_libraries(nano_gicp PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) ++target_link_libraries(nano_gicp PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann::nanoflann) +target_include_directories(nano_gicp PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) # Odometry Node @@ -107,10 +109,18 @@ index 91e47a0..4aa2bdf 100644 +# target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) +# target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h -index 47386ff..0bda740 100644 +index 47386ff..0416918 100644 --- a/include/dlio/dlio.h +++ b/include/dlio/dlio.h -@@ -44,16 +44,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -31,7 +31,6 @@ + #include + #include + #include +-#include + #include + #include + +@@ -44,16 +43,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) return out.str(); } @@ -127,7 +137,7 @@ index 47386ff..0bda740 100644 // BOOST #include #include -@@ -69,14 +59,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -69,14 +58,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) #include #include #include @@ -142,7 +152,7 @@ index 47386ff..0bda740 100644 namespace dlio { enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN }; -@@ -84,7 +69,7 @@ namespace dlio { +@@ -84,7 +68,7 @@ namespace dlio { class OdomNode; class MapNode; @@ -151,7 +161,7 @@ index 47386ff..0bda740 100644 Point(): data{0.f, 0.f, 0.f, 1.f} {} PCL_ADD_POINT4D; -@@ -96,7 +81,7 @@ namespace dlio { +@@ -96,7 +80,7 @@ namespace dlio { // (Livox) absolute timestamp in (seconds * 10e9) }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW From edea4b66ee0cd30468ad6ba72d8fe36b2557a348 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 16:35:38 -0400 Subject: [PATCH 06/11] fix: align dlio patch with vcpkg nanoflann Switch the dlio patch to include nanoflann from vcpkg and update adaptor types for nanoflann 1.7.1 compatibility. Also make the malloc include macOS-safe so CI builds no longer fail on missing malloc.h. --- cpp/bindings/pipelines/dlio.patch | 34 +++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index f651ef6b..5f5613e2 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -112,13 +112,17 @@ diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h index 47386ff..0416918 100644 --- a/include/dlio/dlio.h +++ b/include/dlio/dlio.h -@@ -31,7 +31,6 @@ +@@ -31,7 +31,10 @@ #include #include #include -#include #include ++#ifdef __APPLE__ ++#include ++#else #include ++#endif @@ -44,16 +43,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) return out.str(); @@ -169,7 +173,33 @@ index 47386ff..0416918 100644 + }; } - POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, +POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, +diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h +index 2c6732b..d88de52 100644 +--- a/include/nano_gicp/nanoflann_adaptor.h ++++ b/include/nano_gicp/nanoflann_adaptor.h +@@ -49,7 +49,7 @@ + #include + #include + +-#include "nano_gicp/nanoflann.h" ++#include + + namespace nanoflann + { +@@ -86,7 +86,7 @@ protected: + +- nanoflann::SearchParams _params; ++ nanoflann::SearchParameters _params; + + struct PointCloud_Adaptor + { +@@ -159,7 +159,7 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, +- static std::vector > indices_dist; ++ static std::vector> indices_dist; + indices_dist.reserve( 128 ); + + RadiusResultSet resultSet(radius, indices_dist); diff --git a/include/dlio/odom.h b/include/dlio/odom.h index c79ebd7..2c91f67 100644 --- a/include/dlio/odom.h From aafd28f782320befb215111894c36a4b7743dc09 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 18:30:15 -0400 Subject: [PATCH 07/11] fix: repair dlio patch hunks for CI apply Correct malformed hunk context in dlio.patch introduced in the prior change so setup_pipelines.sh can apply the patch cleanly in CI. This unblocks all jobs before CMake configuration (including the catkin detection path). --- cpp/bindings/pipelines/dlio.patch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index 5f5613e2..9c53a9e6 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -173,7 +173,7 @@ index 47386ff..0416918 100644 + }; } -POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, + POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h index 2c6732b..d88de52 100644 --- a/include/nano_gicp/nanoflann_adaptor.h @@ -187,14 +187,14 @@ index 2c6732b..d88de52 100644 namespace nanoflann { -@@ -86,7 +86,7 @@ protected: +@@ -86,5 +86,5 @@ protected: - nanoflann::SearchParams _params; + nanoflann::SearchParameters _params; struct PointCloud_Adaptor { -@@ -159,7 +159,7 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, +@@ -159,4 +159,4 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, - static std::vector > indices_dist; + static std::vector> indices_dist; indices_dist.reserve( 128 ); From 7e4387fad4ebbea7cde49e67d3d23e24c0e2a0b2 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 19:17:05 -0400 Subject: [PATCH 08/11] fix: regenerate dlio patch with macOS iterator compatibility --- cpp/bindings/pipelines/dlio.patch | 2021 ++++++++++++++++++++++++++--- 1 file changed, 1834 insertions(+), 187 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index 9c53a9e6..dd347a95 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -109,10 +109,10 @@ index 91e47a0..e3e2ba0 100644 +# target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) +# target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h -index 47386ff..0416918 100644 +index 47386ff..d04c526 100644 --- a/include/dlio/dlio.h +++ b/include/dlio/dlio.h -@@ -31,7 +31,10 @@ +@@ -31,9 +31,12 @@ #include #include #include @@ -124,7 +124,9 @@ index 47386ff..0416918 100644 #include +#endif -@@ -44,16 +43,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) + template + std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -44,16 +47,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) return out.str(); } @@ -141,7 +143,7 @@ index 47386ff..0416918 100644 // BOOST #include #include -@@ -69,14 +58,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -69,14 +62,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) #include #include #include @@ -156,7 +158,7 @@ index 47386ff..0416918 100644 namespace dlio { enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN }; -@@ -84,7 +68,7 @@ namespace dlio { +@@ -84,7 +72,7 @@ namespace dlio { class OdomNode; class MapNode; @@ -165,7 +167,7 @@ index 47386ff..0416918 100644 Point(): data{0.f, 0.f, 0.f, 1.f} {} PCL_ADD_POINT4D; -@@ -96,7 +80,7 @@ namespace dlio { +@@ -96,7 +84,7 @@ namespace dlio { // (Livox) absolute timestamp in (seconds * 10e9) }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -174,32 +176,6 @@ index 47386ff..0416918 100644 } POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, -diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h -index 2c6732b..d88de52 100644 ---- a/include/nano_gicp/nanoflann_adaptor.h -+++ b/include/nano_gicp/nanoflann_adaptor.h -@@ -49,7 +49,7 @@ - #include - #include - --#include "nano_gicp/nanoflann.h" -+#include - - namespace nanoflann - { -@@ -86,5 +86,5 @@ protected: - -- nanoflann::SearchParams _params; -+ nanoflann::SearchParameters _params; - - struct PointCloud_Adaptor - { -@@ -159,4 +159,4 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, -- static std::vector > indices_dist; -+ static std::vector> indices_dist; - indices_dist.reserve( 128 ); - - RadiusResultSet resultSet(radius, indices_dist); diff --git a/include/dlio/odom.h b/include/dlio/odom.h index c79ebd7..2c91f67 100644 --- a/include/dlio/odom.h @@ -470,23 +446,63 @@ index c79ebd7..2c91f67 100644 int num_threads_; bool verbose; +diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h +index 0e2c2d7..eddd512 100644 +--- a/include/nano_gicp/nanoflann_adaptor.h ++++ b/include/nano_gicp/nanoflann_adaptor.h +@@ -49,7 +49,7 @@ + #include + #include + +-#include "nano_gicp/nanoflann.h" ++#include + + namespace nanoflann + { +@@ -86,7 +86,7 @@ public: + + protected: + +- nanoflann::SearchParams _params; ++ nanoflann::SearchParameters _params; + + struct PointCloud_Adaptor + { +@@ -156,7 +156,7 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, + std::vector &k_indices, + std::vector &k_sqr_distances) const + { +- static std::vector > indices_dist; ++ static std::vector> indices_dist; + indices_dist.reserve( 128 ); + + RadiusResultSet resultSet(radius, indices_dist); diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc -index bceae37..c0c625f 100644 +index bceae37..307ad52 100644 --- a/src/dlio/odom.cc +++ b/src/dlio/odom.cc -@@ -12,9 +12,9 @@ +@@ -12,34 +12,22 @@ #include "dlio/odom.h" -dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { -+dlio::OdomNode::OdomNode(const Params ¶ms) { - +- - this->getParams(); ++dlio::OdomNode::OdomNode(const Params& params) { + this->getParams(params); this->num_threads_ = omp_get_max_threads(); -@@ -26,20 +26,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + this->dlio_initialized = false; + this->first_valid_scan = false; + this->first_imu_received = false; +- if (this->imu_calibrate_) {this->imu_calibrated = false;} +- else {this->imu_calibrated = true;} ++ if (this->imu_calibrate_) { ++ this->imu_calibrated = false; ++ } else { ++ this->imu_calibrated = true; ++ } this->deskew_status = false; this->deskew_size = 0; @@ -507,7 +523,7 @@ index bceae37..c0c625f 100644 this->T = Eigen::Matrix4f::Identity(); this->T_prior = Eigen::Matrix4f::Identity(); this->T_corr = Eigen::Matrix4f::Identity(); -@@ -67,10 +53,10 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { +@@ -67,10 +55,18 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->first_imu_stamp = 0.; this->prev_imu_stamp = 0.; @@ -515,14 +531,22 @@ index bceae37..c0c625f 100644 - this->deskewed_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); - this->current_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); - this->submap_cloud = pcl::PointCloud::ConstPtr (boost::make_shared>()); -+ this->original_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->deskewed_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->current_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->submap_cloud = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->original_scan = pcl::PointCloud::ConstPtr( ++ std::make_shared>() ++ ); ++ this->deskewed_scan = pcl::PointCloud::ConstPtr( ++ std::make_shared>() ++ ); ++ this->current_scan = pcl::PointCloud::ConstPtr( ++ std::make_shared>() ++ ); ++ this->submap_cloud = pcl::PointCloud::ConstPtr( ++ std::make_shared>() ++ ); this->num_processed_keyframes = 0; -@@ -79,7 +65,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { +@@ -79,7 +75,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->first_scan_stamp = 0.; this->elapsed_time = 0.; @@ -530,8 +554,66 @@ index bceae37..c0c625f 100644 this->convex_hull.setDimension(3); this->concave_hull.setDimension(3); -@@ -157,151 +142,81 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - if (strncmp(line, "processor", 9) == 0) this->numProcessors++; +@@ -112,8 +107,17 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + pcl::console::setVerbosityLevel(pcl::console::L_ERROR); + + this->crop.setNegative(true); +- this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_, -this->crop_size_, 1.0)); +- this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0)); ++ this->crop.setMin( ++ Eigen::Vector4f( ++ -this->crop_size_, ++ -this->crop_size_, ++ -this->crop_size_, ++ 1.0 ++ ) ++ ); ++ this->crop.setMax( ++ Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0) ++ ); + + this->voxel.setLeafSize(this->vf_res_, this->vf_res_, this->vf_res_); + +@@ -126,22 +130,23 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + + this->cpu_type = ""; + +- #ifdef HAS_CPUID +- unsigned int CPUInfo[4] = {0,0,0,0}; ++#ifdef HAS_CPUID ++ unsigned int CPUInfo[4] = {0, 0, 0, 0}; + __cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); + unsigned int nExIds = CPUInfo[0]; + for (unsigned int i = 0x80000000; i <= nExIds; ++i) { + __cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); +- if (i == 0x80000002) ++ if (i == 0x80000002) { + memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo)); +- else if (i == 0x80000003) ++ } else if (i == 0x80000003) { + memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo)); +- else if (i == 0x80000004) ++ } else if (i == 0x80000004) { + memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo)); ++ } + } + this->cpu_type = CPUBrandString; + boost::trim(this->cpu_type); +- #endif ++#endif + + FILE* file; + struct tms timeSample; +@@ -153,155 +158,87 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + + file = fopen("/proc/cpuinfo", "r"); + this->numProcessors = 0; +- while(fgets(line, 128, file) != nullptr) { +- if (strncmp(line, "processor", 9) == 0) this->numProcessors++; ++ while (fgets(line, 128, file) != nullptr) { ++ if (strncmp(line, "processor", 9) == 0) { ++ this->numProcessors++; ++ } } fclose(file); - @@ -540,51 +622,54 @@ index bceae37..c0c625f 100644 dlio::OdomNode::~OdomNode() {} -void dlio::OdomNode::getParams() { -- ++void dlio::OdomNode::getParams(const Params& params) { ++ this->verbose = params.verbose; + - // Version - ros::param::param("~dlio/version", this->version_, "0.0.0"); -- ++ this->deskew_ = params.deskew; ++ this->gravity_ = params.gravity; ++ this->time_offset_ = params.time_offset; + - // Frames - ros::param::param("~dlio/frames/odom", this->odom_frame, "odom"); - ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); - ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); - ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); -+void dlio::OdomNode::getParams(const Params ¶ms) { -+ this->verbose = params.verbose; ++ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; ++ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - // Get Node NS and Remove Leading Character - std::string ns = ros::this_node::getNamespace(); - ns.erase(0,1); -+ this->deskew_ = params.deskew; -+ this->gravity_ = params.gravity; -+ this->time_offset_ = params.time_offset; ++ this->submap_knn_ = params.submap_knn; ++ this->submap_kcv_ = params.submap_kcv; ++ this->submap_kcc_ = params.submap_kcc; - // Concatenate Frame Name Strings - this->odom_frame = ns + "/" + this->odom_frame; - this->baselink_frame = ns + "/" + this->baselink_frame; - this->lidar_frame = ns + "/" + this->lidar_frame; - this->imu_frame = ns + "/" + this->imu_frame; -+ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; -+ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; ++ this->densemap_filtered_ = params.densemap_filtered; - // Deskew FLag - ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); -+ this->submap_knn_ = params.submap_knn; -+ this->submap_kcv_ = params.submap_kcv; -+ this->submap_kcc_ = params.submap_kcc; ++ this->wait_until_move_ = params.wait_until_move; - // Gravity - ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); -+ this->densemap_filtered_ = params.densemap_filtered; ++ this->crop_size_ = params.crop_size; - // Compute time offset between lidar and imu - ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); -+ this->wait_until_move_ = params.wait_until_move; ++ this->vf_use_ = params.vf_use; ++ this->vf_res_ = params.vf_res; - // Keyframe Threshold - ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); - ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); -+ this->crop_size_ = params.crop_size; ++ this->adaptive_params_ = params.adaptive_params; - // Submap - ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); @@ -603,13 +688,10 @@ index bceae37..c0c625f 100644 - // Voxel Grid Filter - ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); - ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); -+ this->vf_use_ = params.vf_use; -+ this->vf_res_ = params.vf_res; - +- - // Adaptive Parameters - ros::param::param("~dlio/adaptive", this->adaptive_params_, true); -+ this->adaptive_params_ = params.adaptive_params; - +- - // Extrinsics - std::vector t_default{0., 0., 0.}; - std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; @@ -642,9 +724,9 @@ index bceae37..c0c625f 100644 - this->extrinsics.baselink2lidar.R = - Eigen::Map>(baselink2lidar_R.data(), 3, 3); + this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2imu.t; ++ this->extrinsics.baselink2imu.t; + this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2imu.R; ++ this->extrinsics.baselink2imu.R; this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity(); - this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t; @@ -705,9 +787,9 @@ index bceae37..c0c625f 100644 - - ros::param::param("~dlio/verbose", this->verbose, true); + this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2lidar.t; ++ this->extrinsics.baselink2lidar.t; + this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2lidar.R; ++ this->extrinsics.baselink2lidar.R; + + this->calibrate_gyro_ = params.calibrate_gyro; + this->calibrate_accel_ = params.calibrate_accel; @@ -740,8 +822,25 @@ index bceae37..c0c625f 100644 } void dlio::OdomNode::start() { -@@ -318,185 +233,12 @@ void dlio::OdomNode::start() { +@@ -310,193 +247,25 @@ void dlio::OdomNode::start() { + } + printf("\033[2J\033[1;1H"); +- std::cout << std::endl +- << "+-------------------------------------------------------------------+" << std::endl; +- std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" +- << std::endl; +- std::cout << "+-------------------------------------------------------------------+" << std::endl; +- ++ std::cout ++ << std::endl ++ << "+-------------------------------------------------------------------+" ++ << std::endl; ++ std::cout << "| Direct LiDAR-Inertial Odometry v" ++ << this->version_ << " |" << std::endl; ++ std::cout ++ << "+-------------------------------------------------------------------+" ++ << std::endl; } -void dlio::OdomNode::publishPose(const ros::TimerEvent& e) { @@ -852,9 +951,7 @@ index bceae37..c0c625f 100644 - transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; - transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; - transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; -+void dlio::OdomNode::getScanFromROS( -+ const pcl::PointCloud::ConstPtr &pc, double stamp) { - +- - Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); - transformStamped.transform.rotation.w = qq.w(); - transformStamped.transform.rotation.x = qq.x(); @@ -925,13 +1022,18 @@ index bceae37..c0c625f 100644 - - pcl::PointCloud::Ptr original_scan_ (boost::make_shared>()); - pcl::fromROSMsg(*pc, *original_scan_); ++void dlio::OdomNode::getScanFromROS( ++ const pcl::PointCloud::ConstPtr& pc, ++ double stamp ++) { + pcl::PointCloud::Ptr original_scan_( -+ std::make_shared>()); ++ std::make_shared>() ++ ); + *original_scan_ = *pc; // Remove NaNs std::vector idx; -@@ -508,28 +250,14 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) +@@ -508,37 +277,20 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) this->crop.filter(*original_scan_); // automatically detect sensor type @@ -962,41 +1064,103 @@ index bceae37..c0c625f 100644 - this->scan_header_stamp = pc->header.stamp; + this->scan_header_stamp = stamp; this->original_scan = original_scan_; - +- } -@@ -547,7 +275,7 @@ void dlio::OdomNode::preprocessPoints() { - } else { + void dlio::OdomNode::preprocessPoints() { +- + // Deskew the original dlio-type scan + if (this->deskew_) { +- + this->deskewPointcloud(); + if (!this->first_valid_scan) { +@@ -546,13 +298,12 @@ void dlio::OdomNode::preprocessPoints() { + } + + } else { +- - this->scan_stamp = this->scan_header_stamp.toSec(); + this->scan_stamp = this->scan_header_stamp; // don't process scans until IMU data is present if (!this->first_valid_scan) { -@@ -574,7 +302,7 @@ void dlio::OdomNode::preprocessPoints() { +- +- if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { ++ if (this->imu_buffer.empty() ++ || this->scan_stamp <= this->imu_buffer.back().stamp) { + return; + } + +@@ -560,102 +311,130 @@ void dlio::OdomNode::preprocessPoints() { + this->T_prior = this->T; // assume no motion for the first scan + } else { +- + // IMU prior for second scan onwards +- std::vector> frames; +- frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, +- this->geo.prev_vel.cast(), {this->scan_stamp}); +- +- if (frames.size() > 0) { +- this->T_prior = frames.back(); +- } else { +- this->T_prior = this->T; +- } ++ std::vector> ++ frames; ++ frames = this->integrateImu( ++ this->prev_scan_stamp, ++ this->lidarPose.q, ++ this->lidarPose.p, ++ this->geo.prev_vel.cast(), ++ {this->scan_stamp} ++ ); + ++ if (frames.size() > 0) { ++ this->T_prior = frames.back(); ++ } else { ++ this->T_prior = this->T; ++ } } - pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); - pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, - this->T_prior * this->extrinsics.baselink2lidar_T); +- pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, +- this->T_prior * this->extrinsics.baselink2lidar_T); ++ pcl::PointCloud::Ptr deskewed_scan_( ++ std::make_shared>() ++ ); ++ pcl::transformPointCloud( ++ *this->original_scan, ++ *deskewed_scan_, ++ this->T_prior * this->extrinsics.baselink2lidar_T ++ ); this->deskewed_scan = deskewed_scan_; -@@ -584,7 +312,7 @@ void dlio::OdomNode::preprocessPoints() { + this->deskew_status = false; + } + // Voxel Grid Filter if (this->vf_use_) { - pcl::PointCloud::Ptr current_scan_ +- pcl::PointCloud::Ptr current_scan_ - (boost::make_shared>(*this->deskewed_scan)); -+ (std::make_shared>(*this->deskewed_scan)); ++ pcl::PointCloud::Ptr current_scan_( ++ std::make_shared>(*this->deskewed_scan) ++ ); this->voxel.setInputCloud(current_scan_); this->voxel.filter(*current_scan_); this->current_scan = current_scan_; -@@ -596,11 +324,11 @@ void dlio::OdomNode::preprocessPoints() { + } else { + this->current_scan = this->deskewed_scan; + } +- + } void dlio::OdomNode::deskewPointcloud() { - +- - pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); ++ pcl::PointCloud::Ptr deskewed_scan_( ++ std::make_shared>() ++ ); deskewed_scan_->points.resize(this->original_scan->points.size()); // individual point timestamps should be relative to this time @@ -1005,44 +1169,276 @@ index bceae37..c0c625f 100644 // sort points by timestamp and build list of timestamps std::function point_time_cmp; -@@ -700,8 +428,12 @@ void dlio::OdomNode::deskewPointcloud() { +- std::function, +- boost::range::index_value)> point_time_neq; +- std::function)> extract_point_time; ++ std::function, ++ boost::range::index_value ++ )> ++ point_time_neq; ++ std::function)> ++ extract_point_time; + + if (this->sensor == dlio::SensorType::OUSTER) { +- +- point_time_cmp = [](const PointType& p1, const PointType& p2) +- { return p1.t < p2.t; }; +- point_time_neq = [](boost::range::index_value p1, +- boost::range::index_value p2) +- { return p1.value().t != p2.value().t; }; +- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) +- { return sweep_ref_time + pt.value().t * 1e-9f; }; ++ point_time_cmp = [](const PointType& p1, const PointType& p2) { ++ return p1.t < p2.t; ++ }; ++ point_time_neq = []( ++ boost::range::index_value p1, ++ boost::range::index_value p2 ++ ) { return p1.value().t != p2.value().t; }; ++ extract_point_time = [&sweep_ref_time]( ++ boost::range::index_value pt ++ ) { return sweep_ref_time + pt.value().t * 1e-9f; }; + + } else if (this->sensor == dlio::SensorType::VELODYNE) { +- +- point_time_cmp = [](const PointType& p1, const PointType& p2) +- { return p1.time < p2.time; }; +- point_time_neq = [](boost::range::index_value p1, +- boost::range::index_value p2) +- { return p1.value().time != p2.value().time; }; +- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) +- { return sweep_ref_time + pt.value().time; }; ++ point_time_cmp = [](const PointType& p1, const PointType& p2) { ++ return p1.time < p2.time; ++ }; ++ point_time_neq = []( ++ boost::range::index_value p1, ++ boost::range::index_value p2 ++ ) { return p1.value().time != p2.value().time; }; ++ extract_point_time = [&sweep_ref_time]( ++ boost::range::index_value pt ++ ) { return sweep_ref_time + pt.value().time; }; + + } else if (this->sensor == dlio::SensorType::HESAI) { +- +- point_time_cmp = [](const PointType& p1, const PointType& p2) +- { return p1.timestamp < p2.timestamp; }; +- point_time_neq = [](boost::range::index_value p1, +- boost::range::index_value p2) +- { return p1.value().timestamp != p2.value().timestamp; }; +- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) +- { return pt.value().timestamp; }; ++ point_time_cmp = [](const PointType& p1, const PointType& p2) { ++ return p1.timestamp < p2.timestamp; ++ }; ++ point_time_neq = []( ++ boost::range::index_value p1, ++ boost::range::index_value p2 ++ ) { return p1.value().timestamp != p2.value().timestamp; }; ++ extract_point_time = [&sweep_ref_time]( ++ boost::range::index_value pt ++ ) { return pt.value().timestamp; }; + + } else if (this->sensor == dlio::SensorType::LIVOX) { +- point_time_cmp = [](const PointType& p1, const PointType& p2) +- { return p1.timestamp < p2.timestamp; }; +- point_time_neq = [](boost::range::index_value p1, +- boost::range::index_value p2) +- { return p1.value().timestamp != p2.value().timestamp; }; +- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) +- { return pt.value().timestamp * 1e-9f; }; ++ point_time_cmp = [](const PointType& p1, const PointType& p2) { ++ return p1.timestamp < p2.timestamp; ++ }; ++ point_time_neq = []( ++ boost::range::index_value p1, ++ boost::range::index_value p2 ++ ) { return p1.value().timestamp != p2.value().timestamp; }; ++ extract_point_time = [&sweep_ref_time]( ++ boost::range::index_value pt ++ ) { return pt.value().timestamp * 1e-9f; }; + } + + // copy points into deskewed_scan_ in order of timestamp +- std::partial_sort_copy(this->original_scan->points.begin(), this->original_scan->points.end(), +- deskewed_scan_->points.begin(), deskewed_scan_->points.end(), point_time_cmp); ++ std::partial_sort_copy( ++ this->original_scan->points.begin(), ++ this->original_scan->points.end(), ++ deskewed_scan_->points.begin(), ++ deskewed_scan_->points.end(), ++ point_time_cmp ++ ); + + // filter unique timestamps + auto points_unique_timestamps = deskewed_scan_->points +- | boost::adaptors::indexed() +- | boost::adaptors::adjacent_filtered(point_time_neq); ++ | boost::adaptors::indexed() ++ | boost::adaptors::adjacent_filtered(point_time_neq); + + // extract timestamps from points and put them in their own list + std::vector timestamps; +@@ -664,46 +443,68 @@ void dlio::OdomNode::deskewPointcloud() { + // compute offset between sweep reference time and first point timestamp + double offset = 0.0; + if (this->time_offset_) { +- offset = sweep_ref_time - extract_point_time(*points_unique_timestamps.begin()); ++ offset = ++ sweep_ref_time - extract_point_time(*points_unique_timestamps.begin()); + } + + // build list of unique timestamps and indices of first point with each timestamp +- for (auto it = points_unique_timestamps.begin(); it != points_unique_timestamps.end(); it++) { +- timestamps.push_back(extract_point_time(*it) + offset); +- unique_time_indices.push_back(it->index()); ++ for (const auto& indexed_pt : points_unique_timestamps) { ++ timestamps.push_back(extract_point_time(indexed_pt) + offset); ++ unique_time_indices.push_back(indexed_pt.index()); + } + unique_time_indices.push_back(deskewed_scan_->points.size()); + + int median_pt_index = timestamps.size() / 2; +- this->scan_stamp = timestamps[median_pt_index]; // set this->scan_stamp to the timestamp of the median point ++ this->scan_stamp = timestamps ++ [median_pt_index]; // set this->scan_stamp to the timestamp of the median point + + // don't process scans until IMU data is present + if (!this->first_valid_scan) { +- if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { ++ if (this->imu_buffer.empty() ++ || this->scan_stamp <= this->imu_buffer.back().stamp) { + return; + } + + this->first_valid_scan = true; + this->T_prior = this->T; // assume no motion for the first scan +- pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); ++ pcl::transformPointCloud( ++ *deskewed_scan_, ++ *deskewed_scan_, ++ this->T_prior * this->extrinsics.baselink2lidar_T ++ ); + this->deskewed_scan = deskewed_scan_; + this->deskew_status = true; + return; + } + + // IMU prior & deskewing for second scan onwards +- std::vector> frames; +- frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, +- this->geo.prev_vel.cast(), timestamps); +- this->deskew_size = frames.size(); // if integration successful, equal to timestamps.size() ++ std::vector> ++ frames; ++ frames = this->integrateImu( ++ this->prev_scan_stamp, ++ this->lidarPose.q, ++ this->lidarPose.p, ++ this->geo.prev_vel.cast(), ++ timestamps ++ ); ++ this->deskew_size = ++ frames.size(); // if integration successful, equal to timestamps.size() + // if there are no frames between the start and end of the sweep // that probably means that there's a sync issue if (frames.size() != timestamps.size()) { - ROS_FATAL("Bad time sync between LiDAR and IMU!"); -- -+ if(verbose) { ++ if (verbose) { + printf("FATAL: "); + printf("Bad time sync between LiDAR and IMU!"); + printf("\n"); + } -+ + this->T_prior = this->T; - pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); +- pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); ++ pcl::transformPointCloud( ++ *deskewed_scan_, ++ *deskewed_scan_, ++ this->T_prior * this->extrinsics.baselink2lidar_T ++ ); this->deskewed_scan = deskewed_scan_; -@@ -755,20 +487,25 @@ void dlio::OdomNode::initializeDLIO() { + this->deskew_status = false; + return; +@@ -714,12 +515,11 @@ void dlio::OdomNode::deskewPointcloud() { + + #pragma omp parallel for num_threads(this->num_threads_) + for (int i = 0; i < timestamps.size(); i++) { +- + Eigen::Matrix4f T = frames[i] * this->extrinsics.baselink2lidar_T; + + // transform point to world frame +- for (int k = unique_time_indices[i]; k < unique_time_indices[i+1]; k++) { +- auto &pt = deskewed_scan_->points[k]; ++ for (int k = unique_time_indices[i]; k < unique_time_indices[i + 1]; k++) { ++ auto& pt = deskewed_scan_->points[k]; + pt.getVector4fMap()[3] = 1.; + pt.getVector4fMap() = T * pt.getVector4fMap(); + } +@@ -727,19 +527,21 @@ void dlio::OdomNode::deskewPointcloud() { + + this->deskewed_scan = deskewed_scan_; + this->deskew_status = true; +- + } + + void dlio::OdomNode::initializeInputTarget() { +- + this->prev_scan_stamp = this->scan_stamp; + + // keep history of keyframes +- this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); ++ this->keyframes.push_back( ++ std::make_pair( ++ std::make_pair(this->lidarPose.p, this->lidarPose.q), ++ this->current_scan ++ ) ++ ); + this->keyframe_timestamps.push_back(this->scan_header_stamp); + this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); + this->keyframe_transformations.push_back(this->T_corr); +- + } + + void dlio::OdomNode::setInputSource() { +@@ -748,27 +550,34 @@ void dlio::OdomNode::setInputSource() { + } + + void dlio::OdomNode::initializeDLIO() { +- + // Wait for IMU + if (!this->first_imu_received || !this->imu_calibrated) { + return; } this->dlio_initialized = true; - std::cout << std::endl << " DLIO initialized!" << std::endl; +- + if (verbose) { + std::cout << std::endl << " DLIO initialized!" << std::endl; + } - } -void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) { +- +- std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); +void dlio::OdomNode::callbackPointCloud( -+ const pcl::PointCloud::ConstPtr &pc, double stamp) { - - std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); ++ const pcl::PointCloud::ConstPtr& pc, ++ double stamp ++) { ++ std::unique_lockmain_loop_running_mutex)> lock( ++ main_loop_running_mutex ++ ); this->main_loop_running = true; lock.unlock(); - double then = ros::Time::now().toSec(); + double then = std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch()) -+ .count(); ++ std::chrono::system_clock::now().time_since_epoch() ++ ) ++ .count(); if (this->first_scan_stamp == 0.) { - this->first_scan_stamp = pc->header.stamp.toSec(); @@ -1050,7 +1446,7 @@ index bceae37..c0c625f 100644 } // DLIO Initialization procedures (IMU calib, gravity align) -@@ -777,7 +514,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -777,7 +586,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } // Convert incoming scan into DLIO format @@ -1059,7 +1455,7 @@ index bceae37..c0c625f 100644 // Preprocess points this->preprocessPoints(); -@@ -787,13 +524,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -787,13 +596,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } if (this->current_scan->points.size() <= this->gicp_min_num_points_) { @@ -1080,7 +1476,46 @@ index bceae37..c0c625f 100644 // Set Adaptive Parameters if (this->adaptive_params_) { -@@ -839,50 +579,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -807,8 +619,12 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + if (this->keyframes.size() == 0) { + this->initializeInputTarget(); + this->main_loop_running = false; +- this->submap_future = +- std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); ++ this->submap_future = std::async( ++ std::launch::async, ++ &dlio::OdomNode::buildKeyframesAndSubmap, ++ this, ++ this->state ++ ); + this->submap_future.wait(); // wait until completion + return; + } +@@ -822,8 +638,12 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + // Build keyframe normals and submap if needed (and if we're not already waiting) + if (this->new_submap_is_ready) { + this->main_loop_running = false; +- this->submap_future = +- std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); ++ this->submap_future = std::async( ++ std::launch::async, ++ &dlio::OdomNode::buildKeyframesAndSubmap, ++ this, ++ this->state ++ ); + } else { + lock.lock(); + this->main_loop_running = false; +@@ -832,69 +652,61 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& + } + + // Update trajectory +- this->trajectory.push_back( std::make_pair(this->state.p, this->state.q) ); ++ this->trajectory.push_back(std::make_pair(this->state.p, this->state.q)); + + // Update time stamps +- this->lidar_rates.push_back( 1. / (this->scan_stamp - this->prev_scan_stamp) ); ++ this->lidar_rates.push_back(1. / (this->scan_stamp - this->prev_scan_stamp)); this->prev_scan_stamp = this->scan_stamp; this->elapsed_time = this->scan_stamp - this->first_scan_stamp; @@ -1097,10 +1532,12 @@ index bceae37..c0c625f 100644 // Update some statistics - this->comp_times.push_back(ros::Time::now().toSec() - then); + this->comp_times.push_back( -+ std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch()) -+ .count() - -+ then); ++ std::chrono::duration_cast>( ++ std::chrono::system_clock::now().time_since_epoch() ++ ) ++ .count() ++ - then ++ ); this->gicp_hasConverged = this->gicp.hasConverged(); // Debug statements and publish custom DLIO message @@ -1109,13 +1546,14 @@ index bceae37..c0c625f 100644 - this->debug_thread.detach(); + this->debug(); } - +- ++ this->geo.first_opt_done = true; } -void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -+void dlio::OdomNode::callbackImu(const ImuMeas &imu_raw) { - +- ++void dlio::OdomNode::callbackImu(const ImuMeas& imu_raw) { this->first_imu_received = true; - sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw ); @@ -1147,55 +1585,102 @@ index bceae37..c0c625f 100644 } // IMU calibration procedure - do for three seconds -@@ -893,7 +626,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - static Eigen::Vector3f accel_avg (0., 0., 0.); + if (!this->imu_calibrated) { +- + static int num_samples = 0; +- static Eigen::Vector3f gyro_avg (0., 0., 0.); +- static Eigen::Vector3f accel_avg (0., 0., 0.); ++ static Eigen::Vector3f gyro_avg(0., 0., 0.); ++ static Eigen::Vector3f accel_avg(0., 0., 0.); static bool print = true; - if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) { +- + if ((imu.stamp - this->first_imu_stamp) < this->imu_calib_time_) { - num_samples++; -@@ -905,7 +638,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + gyro_avg[0] += ang_vel[0]; +@@ -905,92 +717,113 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { accel_avg[1] += lin_accel[1]; accel_avg[2] += lin_accel[2]; - if(print) { -+ if(verbose) { - std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; +- std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; ++ if (verbose) { ++ std::cout << std::endl ++ << " Calibrating IMU for " << this->imu_calib_time_ ++ << " seconds... "; std::cout.flush(); print = false; -@@ -913,7 +646,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + } } else { - +- - std::cout << "done" << std::endl << std::endl; -+ if(verbose) { ++ if (verbose) { + std::cout << "done" << std::endl << std::endl; + } gyro_avg /= num_samples; accel_avg /= num_samples; -@@ -943,44 +678,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + +- Eigen::Vector3f grav_vec (0., 0., this->gravity_); ++ Eigen::Vector3f grav_vec(0., 0., this->gravity_); + + if (this->gravity_align_) { +- + // Estimate gravity vector - Only approximate if biases have not been pre-calibrated +- grav_vec = (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_); +- Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(grav_vec, Eigen::Vector3f(0., 0., this->gravity_)); ++ grav_vec = ++ (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_); ++ Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors( ++ grav_vec, ++ Eigen::Vector3f(0., 0., this->gravity_) ++ ); + + // set gravity aligned orientation + this->state.q = grav_q; +- this->T.block(0,0,3,3) = this->state.q.toRotationMatrix(); ++ this->T.block(0, 0, 3, 3) = this->state.q.toRotationMatrix(); + this->lidarPose.q = this->state.q; + + // rpy + auto euler = grav_q.toRotationMatrix().eulerAngles(2, 1, 0); +- double yaw = euler[0] * (180.0/M_PI); +- double pitch = euler[1] * (180.0/M_PI); +- double roll = euler[2] * (180.0/M_PI); ++ double yaw = euler[0] * (180.0 / M_PI); ++ double pitch = euler[1] * (180.0 / M_PI); ++ double roll = euler[2] * (180.0 / M_PI); + + // use alternate representation if the yaw is smaller + if (abs(remainder(yaw + 180.0, 360.0)) < abs(yaw)) { +- yaw = remainder(yaw + 180.0, 360.0); ++ yaw = remainder(yaw + 180.0, 360.0); pitch = remainder(180.0 - pitch, 360.0); - roll = remainder(roll + 180.0, 360.0); +- roll = remainder(roll + 180.0, 360.0); ++ roll = remainder(roll + 180.0, 360.0); ++ } ++ if (verbose) { ++ std::cout << " Estimated initial attitude:" << std::endl; ++ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) ++ << std::endl; ++ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) ++ << std::endl; ++ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) ++ << std::endl; ++ std::cout << std::endl; } - std::cout << " Estimated initial attitude:" << std::endl; - std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; - std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; - std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; - std::cout << std::endl; -+ if (verbose){ -+ std::cout << " Estimated initial attitude:" << std::endl; -+ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; -+ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; -+ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; -+ std::cout << std::endl; -+ } } if (this->calibrate_accel_) { - +- // subtract gravity from avg accel to get bias this->state.b.accel = accel_avg - grav_vec; - @@ -1203,23 +1688,31 @@ index bceae37..c0c625f 100644 - << to_string_with_precision(this->state.b.accel[1], 8) << ", " - << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; + if (verbose) { -+ std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " -+ << to_string_with_precision(this->state.b.accel[1], 8) << ", " -+ << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; ++ std::cout << " Accel biases [xyz]: " ++ << to_string_with_precision(this->state.b.accel[0], 8) ++ << ", " ++ << to_string_with_precision(this->state.b.accel[1], 8) ++ << ", " ++ << to_string_with_precision(this->state.b.accel[2], 8) ++ << std::endl; + } } if (this->calibrate_gyro_) { - +- this->state.b.gyro = gyro_avg; - - std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " - << to_string_with_precision(this->state.b.gyro[1], 8) << ", " - << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; -+ if(verbose) { -+ std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " -+ << to_string_with_precision(this->state.b.gyro[1], 8) << ", " -+ << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; ++ if (verbose) { ++ std::cout << " Gyro biases [xyz]: " ++ << to_string_with_precision(this->state.b.gyro[0], 8) ++ << ", " ++ << to_string_with_precision(this->state.b.gyro[1], 8) ++ << ", " ++ << to_string_with_precision(this->state.b.gyro[2], 8) ++ << std::endl; + } } @@ -1228,11 +1721,15 @@ index bceae37..c0c625f 100644 } } else { - +- - double dt = imu->header.stamp.toSec() - this->prev_imu_stamp; +- if (dt == 0) { dt = 1.0/200.0; } +- this->imu_rates.push_back( 1./dt ); + double dt = imu.stamp - this->prev_imu_stamp; - if (dt == 0) { dt = 1.0/200.0; } - this->imu_rates.push_back( 1./dt ); ++ if (dt == 0) { ++ dt = 1.0 / 200.0; ++ } ++ this->imu_rates.push_back(1. / dt); // Apply the calibrated bias to the new IMU measurements - this->imu_meas.stamp = imu->header.stamp.toSec(); @@ -1240,7 +1737,11 @@ index bceae37..c0c625f 100644 this->imu_meas.dt = dt; this->prev_imu_stamp = this->imu_meas.stamp; -@@ -990,7 +728,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +- Eigen::Vector3f lin_accel_corrected = (this->imu_accel_sm_ * lin_accel) - this->state.b.accel; ++ Eigen::Vector3f lin_accel_corrected = ++ (this->imu_accel_sm_ * lin_accel) - this->state.b.accel; + Eigen::Vector3f ang_vel_corrected = ang_vel - this->state.b.gyro; + this->imu_meas.lin_accel = lin_accel_corrected; this->imu_meas.ang_vel = ang_vel_corrected; @@ -1250,20 +1751,62 @@ index bceae37..c0c625f 100644 this->mtx_imu.lock(); this->imu_buffer.push_front(this->imu_meas); this->mtx_imu.unlock(); -@@ -1027,7 +766,7 @@ void dlio::OdomNode::getNextPose() { +@@ -1002,18 +835,16 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + // Geometric Observer: Propagate State + this->propagateState(); + } +- + } +- + } + + void dlio::OdomNode::getNextPose() { +- + // Check if the new submap is ready to be used +- this->new_submap_is_ready = (this->submap_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready); ++ this->new_submap_is_ready = ++ (this->submap_future.wait_for(std::chrono::seconds(0)) ++ == std::future_status::ready); + + if (this->new_submap_is_ready && this->submap_hasChanged) { +- + // Set the current global submap as the target cloud + this->gicp.registerInputTarget(this->submap_cloud); + +@@ -1027,11 +858,14 @@ void dlio::OdomNode::getNextPose() { } // Align with current submap with global IMU transformation as initial guess - pcl::PointCloud::Ptr aligned (boost::make_shared>()); -+ pcl::PointCloud::Ptr aligned (std::make_shared>()); ++ pcl::PointCloud::Ptr aligned( ++ std::make_shared>() ++ ); this->gicp.align(*aligned); // Get final transformation in global frame -@@ -1046,11 +785,12 @@ void dlio::OdomNode::getNextPose() { - bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, - boost::circular_buffer::reverse_iterator& begin_imu_it, - boost::circular_buffer::reverse_iterator& end_imu_it) { +- this->T_corr = this->gicp.getFinalTransformation(); // "correction" transformation ++ this->T_corr = ++ this->gicp.getFinalTransformation(); // "correction" transformation + this->T = this->T_corr * this->T_prior; + + // Update next global pose +@@ -1040,17 +874,20 @@ void dlio::OdomNode::getNextPose() { + + // Geometric observer update + this->updateState(); +- + } + +-bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, +- boost::circular_buffer::reverse_iterator& begin_imu_it, +- boost::circular_buffer::reverse_iterator& end_imu_it) { - ++bool dlio::OdomNode::imuMeasFromTimeRange( ++ double start_time, ++ double end_time, ++ boost::circular_buffer::reverse_iterator& begin_imu_it, ++ boost::circular_buffer::reverse_iterator& end_imu_it ++) { + // Evalio run deterministically, nothing will ever appear here! if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { - // Wait for the latest IMU data @@ -1276,22 +1819,368 @@ index bceae37..c0c625f 100644 } auto imu_it = this->imu_buffer.begin(); -@@ -1268,7 +1008,6 @@ void dlio::OdomNode::propagateGICP() { - double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); - q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; +@@ -1080,10 +917,15 @@ bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, + } + + std::vector> +-dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen::Vector3f p_init, +- Eigen::Vector3f v_init, const std::vector& sorted_timestamps) { +- +- const std::vector> empty; ++dlio::OdomNode::integrateImu( ++ double start_time, ++ Eigen::Quaternionf q_init, ++ Eigen::Vector3f p_init, ++ Eigen::Vector3f v_init, ++ const std::vector& sorted_timestamps ++) { ++ const std::vector> ++ empty; + + if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) { + // invalid input, return empty vector +@@ -1092,14 +934,20 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen + + boost::circular_buffer::reverse_iterator begin_imu_it; + boost::circular_buffer::reverse_iterator end_imu_it; +- if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(), begin_imu_it, end_imu_it) == false) { ++ if (this->imuMeasFromTimeRange( ++ start_time, ++ sorted_timestamps.back(), ++ begin_imu_it, ++ end_imu_it ++ ) ++ == false) { + // not enough IMU measurements, return empty vector + return empty; + } + + // Backwards integration to find pose at first IMU sample + const ImuMeas& f1 = *begin_imu_it; +- const ImuMeas& f2 = *(begin_imu_it+1); ++ const ImuMeas& f2 = *(begin_imu_it + 1); + + // Time between first two IMU samples + double dt = f2.dt; +@@ -1112,26 +960,50 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen + Eigen::Vector3f alpha = alpha_dt / dt; + + // Average angular velocity (reversed) between first IMU sample and start_time +- Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5*alpha*idt); ++ Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5 * alpha * idt); + + // Set q_init to orientation at first IMU sample +- q_init = Eigen::Quaternionf ( +- q_init.w() - 0.5*( q_init.x()*omega_i[0] + q_init.y()*omega_i[1] + q_init.z()*omega_i[2] ) * idt, +- q_init.x() + 0.5*( q_init.w()*omega_i[0] - q_init.z()*omega_i[1] + q_init.y()*omega_i[2] ) * idt, +- q_init.y() + 0.5*( q_init.z()*omega_i[0] + q_init.w()*omega_i[1] - q_init.x()*omega_i[2] ) * idt, +- q_init.z() + 0.5*( q_init.x()*omega_i[1] - q_init.y()*omega_i[0] + q_init.w()*omega_i[2] ) * idt ++ q_init = Eigen::Quaternionf( ++ q_init.w() ++ - 0.5 ++ * (q_init.x() * omega_i[0] + q_init.y() * omega_i[1] + q_init.z() * omega_i[2]) ++ * idt, ++ q_init.x() ++ + 0.5 ++ * (q_init.w() * omega_i[0] - q_init.z() * omega_i[1] + q_init.y() * omega_i[2]) ++ * idt, ++ q_init.y() ++ + 0.5 ++ * (q_init.z() * omega_i[0] + q_init.w() * omega_i[1] - q_init.x() * omega_i[2]) ++ * idt, ++ q_init.z() ++ + 0.5 ++ * (q_init.x() * omega_i[1] - q_init.y() * omega_i[0] + q_init.w() * omega_i[2]) ++ * idt + ); + q_init.normalize(); + + // Average angular velocity between first two IMU samples +- Eigen::Vector3f omega = f1.ang_vel + 0.5*alpha_dt; ++ Eigen::Vector3f omega = f1.ang_vel + 0.5 * alpha_dt; + + // Orientation at second IMU sample +- Eigen::Quaternionf q2 ( +- q_init.w() - 0.5*( q_init.x()*omega[0] + q_init.y()*omega[1] + q_init.z()*omega[2] ) * dt, +- q_init.x() + 0.5*( q_init.w()*omega[0] - q_init.z()*omega[1] + q_init.y()*omega[2] ) * dt, +- q_init.y() + 0.5*( q_init.z()*omega[0] + q_init.w()*omega[1] - q_init.x()*omega[2] ) * dt, +- q_init.z() + 0.5*( q_init.x()*omega[1] - q_init.y()*omega[0] + q_init.w()*omega[2] ) * dt ++ Eigen::Quaternionf q2( ++ q_init.w() ++ - 0.5 ++ * (q_init.x() * omega[0] + q_init.y() * omega[1] + q_init.z() * omega[2]) ++ * dt, ++ q_init.x() ++ + 0.5 ++ * (q_init.w() * omega[0] - q_init.z() * omega[1] + q_init.y() * omega[2]) ++ * dt, ++ q_init.y() ++ + 0.5 ++ * (q_init.z() * omega[0] + q_init.w() * omega[1] - q_init.x() * omega[2]) ++ * dt, ++ q_init.z() ++ + 0.5 ++ * (q_init.x() * omega[1] - q_init.y() * omega[0] + q_init.w() * omega[2]) ++ * dt + ); + q2.normalize(); + +@@ -1147,21 +1019,33 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen + Eigen::Vector3f j = (a2 - a1) / dt; + + // Set v_init to velocity at first IMU sample (go backwards from start_time) +- v_init -= a1*idt + 0.5*j*idt*idt; ++ v_init -= a1 * idt + 0.5 * j * idt * idt; + + // Set p_init to position at first IMU sample (go backwards from start_time) +- p_init -= v_init*idt + 0.5*a1*idt*idt + (1/6.)*j*idt*idt*idt; +- +- return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps, begin_imu_it, end_imu_it); ++ p_init -= ++ v_init * idt + 0.5 * a1 * idt * idt + (1 / 6.) * j * idt * idt * idt; ++ ++ return this->integrateImuInternal( ++ q_init, ++ p_init, ++ v_init, ++ sorted_timestamps, ++ begin_imu_it, ++ end_imu_it ++ ); + } + + std::vector> +-dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init, +- const std::vector& sorted_timestamps, +- boost::circular_buffer::reverse_iterator begin_imu_it, +- boost::circular_buffer::reverse_iterator end_imu_it) { +- +- std::vector> imu_se3; ++dlio::OdomNode::integrateImuInternal( ++ Eigen::Quaternionf q_init, ++ Eigen::Vector3f p_init, ++ Eigen::Vector3f v_init, ++ const std::vector& sorted_timestamps, ++ boost::circular_buffer::reverse_iterator begin_imu_it, ++ boost::circular_buffer::reverse_iterator end_imu_it ++) { ++ std::vector> ++ imu_se3; + + // Initialization + Eigen::Quaternionf q = q_init; +@@ -1177,7 +1061,6 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f + auto stamp_it = sorted_timestamps.begin(); + + for (; imu_it != end_imu_it; imu_it++) { +- + const ImuMeas& f0 = *prev_imu_it; + const ImuMeas& f = *imu_it; + +@@ -1189,14 +1072,18 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f + Eigen::Vector3f alpha = alpha_dt / dt; + + // Average angular velocity +- Eigen::Vector3f omega = f0.ang_vel + 0.5*alpha_dt; ++ Eigen::Vector3f omega = f0.ang_vel + 0.5 * alpha_dt; + + // Orientation +- q = Eigen::Quaternionf ( +- q.w() - 0.5*( q.x()*omega[0] + q.y()*omega[1] + q.z()*omega[2] ) * dt, +- q.x() + 0.5*( q.w()*omega[0] - q.z()*omega[1] + q.y()*omega[2] ) * dt, +- q.y() + 0.5*( q.z()*omega[0] + q.w()*omega[1] - q.x()*omega[2] ) * dt, +- q.z() + 0.5*( q.x()*omega[1] - q.y()*omega[0] + q.w()*omega[2] ) * dt ++ q = Eigen::Quaternionf( ++ q.w() ++ - 0.5 * (q.x() * omega[0] + q.y() * omega[1] + q.z() * omega[2]) * dt, ++ q.x() ++ + 0.5 * (q.w() * omega[0] - q.z() * omega[1] + q.y() * omega[2]) * dt, ++ q.y() ++ + 0.5 * (q.z() * omega[0] + q.w() * omega[1] - q.x() * omega[2]) * dt, ++ q.z() ++ + 0.5 * (q.x() * omega[1] - q.y() * omega[0] + q.w() * omega[2]) * dt + ); + q.normalize(); + +@@ -1215,19 +1102,28 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f + double idt = *stamp_it - f0.stamp; + + // Average angular velocity +- Eigen::Vector3f omega_i = f0.ang_vel + 0.5*alpha*idt; ++ Eigen::Vector3f omega_i = f0.ang_vel + 0.5 * alpha * idt; + + // Orientation +- Eigen::Quaternionf q_i ( +- q.w() - 0.5*( q.x()*omega_i[0] + q.y()*omega_i[1] + q.z()*omega_i[2] ) * idt, +- q.x() + 0.5*( q.w()*omega_i[0] - q.z()*omega_i[1] + q.y()*omega_i[2] ) * idt, +- q.y() + 0.5*( q.z()*omega_i[0] + q.w()*omega_i[1] - q.x()*omega_i[2] ) * idt, +- q.z() + 0.5*( q.x()*omega_i[1] - q.y()*omega_i[0] + q.w()*omega_i[2] ) * idt ++ Eigen::Quaternionf q_i( ++ q.w() ++ - 0.5 * (q.x() * omega_i[0] + q.y() * omega_i[1] + q.z() * omega_i[2]) ++ * idt, ++ q.x() ++ + 0.5 * (q.w() * omega_i[0] - q.z() * omega_i[1] + q.y() * omega_i[2]) ++ * idt, ++ q.y() ++ + 0.5 * (q.z() * omega_i[0] + q.w() * omega_i[1] - q.x() * omega_i[2]) ++ * idt, ++ q.z() ++ + 0.5 * (q.x() * omega_i[1] - q.y() * omega_i[0] + q.w() * omega_i[2]) ++ * idt + ); + q_i.normalize(); + + // Position +- Eigen::Vector3f p_i = p + v*idt + 0.5*a0*idt*idt + (1/6.)*j*idt*idt*idt; ++ Eigen::Vector3f p_i = ++ p + v * idt + 0.5 * a0 * idt * idt + (1 / 6.) * j * idt * idt * idt; + + // Transformation + Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); +@@ -1240,41 +1136,39 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f + } + + // Position +- p += v*dt + 0.5*a0*dt*dt + (1/6.)*j_dt*dt*dt; ++ p += v * dt + 0.5 * a0 * dt * dt + (1 / 6.) * j_dt * dt * dt; + + // Velocity +- v += a0*dt + 0.5*j_dt*dt; ++ v += a0 * dt + 0.5 * j_dt * dt; + + prev_imu_it = imu_it; +- + } + + return imu_se3; +- + } + + void dlio::OdomNode::propagateGICP() { +- +- this->lidarPose.p << this->T(0,3), this->T(1,3), this->T(2,3); ++ this->lidarPose.p << this->T(0, 3), this->T(1, 3), this->T(2, 3); + + Eigen::Matrix3f rotSO3; +- rotSO3 << this->T(0,0), this->T(0,1), this->T(0,2), +- this->T(1,0), this->T(1,1), this->T(1,2), +- this->T(2,0), this->T(2,1), this->T(2,2); ++ rotSO3 << this->T(0, 0), this->T(0, 1), this->T(0, 2), this->T(1, 0), ++ this->T(1, 1), this->T(1, 2), this->T(2, 0), this->T(2, 1), this->T(2, 2); + + Eigen::Quaternionf q(rotSO3); + + // Normalize quaternion +- double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); +- q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; ++ double norm = ++ sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); ++ q.w() /= norm; ++ q.x() /= norm; ++ q.y() /= norm; ++ q.z() /= norm; this->lidarPose.q = q; - } void dlio::OdomNode::propagateState() { -@@ -1369,36 +1108,36 @@ void dlio::OdomNode::updateState() { +- + // Lock thread to prevent state from being accessed by UpdateState +- std::lock_guard lock( this->geo.mtx ); ++ std::lock_guard lock(this->geo.mtx); + + double dt = this->imu_meas.dt; + +@@ -1285,14 +1179,18 @@ void dlio::OdomNode::propagateState() { + world_accel = qhat._transformVector(this->imu_meas.lin_accel); + + // Accel propogation +- this->state.p[0] += this->state.v.lin.w[0]*dt + 0.5*dt*dt*world_accel[0]; +- this->state.p[1] += this->state.v.lin.w[1]*dt + 0.5*dt*dt*world_accel[1]; +- this->state.p[2] += this->state.v.lin.w[2]*dt + 0.5*dt*dt*(world_accel[2] - this->gravity_); +- +- this->state.v.lin.w[0] += world_accel[0]*dt; +- this->state.v.lin.w[1] += world_accel[1]*dt; +- this->state.v.lin.w[2] += (world_accel[2] - this->gravity_)*dt; +- this->state.v.lin.b = this->state.q.toRotationMatrix().inverse() * this->state.v.lin.w; ++ this->state.p[0] += ++ this->state.v.lin.w[0] * dt + 0.5 * dt * dt * world_accel[0]; ++ this->state.p[1] += ++ this->state.v.lin.w[1] * dt + 0.5 * dt * dt * world_accel[1]; ++ this->state.p[2] += this->state.v.lin.w[2] * dt ++ + 0.5 * dt * dt * (world_accel[2] - this->gravity_); ++ ++ this->state.v.lin.w[0] += world_accel[0] * dt; ++ this->state.v.lin.w[1] += world_accel[1] * dt; ++ this->state.v.lin.w[2] += (world_accel[2] - this->gravity_) * dt; ++ this->state.v.lin.b = ++ this->state.q.toRotationMatrix().inverse() * this->state.v.lin.w; + // Gyro propogation + omega.w() = 0; +@@ -1306,13 +1204,11 @@ void dlio::OdomNode::propagateState() { + + this->state.v.ang.b = this->imu_meas.ang_vel; + this->state.v.ang.w = this->state.q.toRotationMatrix() * this->state.v.ang.b; +- } --sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -+dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas &imu_raw) { + void dlio::OdomNode::updateState() { +- + // Lock thread to prevent state from being accessed by PropagateState +- std::lock_guard lock( this->geo.mtx ); ++ std::lock_guard lock(this->geo.mtx); + + Eigen::Vector3f pin = this->lidarPose.p; + Eigen::Quaternionf qin = this->lidarPose.q; +@@ -1322,7 +1218,7 @@ void dlio::OdomNode::updateState() { + qhat = this->state.q; + + // Constuct error quaternion +- qe = qhat.conjugate()*qin; ++ qe = qhat.conjugate() * qin; + + double sgn = 1.; + if (qe.w() < 0) { +@@ -1331,7 +1227,7 @@ void dlio::OdomNode::updateState() { + + // Construct quaternion correction + qcorr.w() = 1 - abs(qe.w()); +- qcorr.vec() = sgn*qe.vec(); ++ qcorr.vec() = sgn * qe.vec(); + qcorr = qhat * qcorr; + + Eigen::Vector3f err = pin - this->state.p; +@@ -1344,13 +1240,15 @@ void dlio::OdomNode::updateState() { + + // Update accel bias + this->state.b.accel -= dt * this->geo_Kab_ * err_body; +- this->state.b.accel = this->state.b.accel.array().min(abias_max).max(-abias_max); ++ this->state.b.accel = ++ this->state.b.accel.array().min(abias_max).max(-abias_max); + + // Update gyro bias + this->state.b.gyro[0] -= dt * this->geo_Kgb_ * qe.w() * qe.x(); + this->state.b.gyro[1] -= dt * this->geo_Kgb_ * qe.w() * qe.y(); + this->state.b.gyro[2] -= dt * this->geo_Kgb_ * qe.w() * qe.z(); +- this->state.b.gyro = this->state.b.gyro.array().min(gbias_max).max(-gbias_max); ++ this->state.b.gyro = ++ this->state.b.gyro.array().min(gbias_max).max(-gbias_max); + + // Update state + this->state.p += dt * this->geo_Kp_ * err; +@@ -1366,54 +1264,60 @@ void dlio::OdomNode::updateState() { + this->geo.prev_p = this->state.p; + this->geo.prev_q = this->state.q; + this->geo.prev_vel = this->state.v.lin.w; +- + } +-sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +- - sensor_msgs::Imu::Ptr imu (new sensor_msgs::Imu); ++dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas& imu_raw) { + ImuMeas imu; // Copy header @@ -1301,20 +2190,27 @@ index bceae37..c0c625f 100644 - static double prev_stamp = imu->header.stamp.toSec(); - double dt = imu->header.stamp.toSec() - prev_stamp; - prev_stamp = imu->header.stamp.toSec(); +- +- if (dt == 0) { dt = 1.0/200.0; } + static double prev_stamp = imu.stamp; + double dt = imu.stamp - prev_stamp; + prev_stamp = imu.stamp; - - if (dt == 0) { dt = 1.0/200.0; } - // Transform angular velocity (will be the same on a rigid body, so just rotate to ROS convention) - Eigen::Vector3f ang_vel(imu_raw->angular_velocity.x, - imu_raw->angular_velocity.y, - imu_raw->angular_velocity.z); ++ if (dt == 0) { ++ dt = 1.0 / 200.0; ++ } ++ + // Transform angular velocity (will be the same on a rigid body, so just + // rotate to ROS convention) -+ Eigen::Vector3f ang_vel(imu_raw.ang_vel[0], imu_raw.ang_vel[1], -+ imu_raw.ang_vel[2]); ++ Eigen::Vector3f ang_vel( ++ imu_raw.ang_vel[0], ++ imu_raw.ang_vel[1], ++ imu_raw.ang_vel[2] ++ ); Eigen::Vector3f ang_vel_cg = this->extrinsics.baselink2imu.R * ang_vel; @@ -1333,12 +2229,20 @@ index bceae37..c0c625f 100644 - imu_raw->linear_acceleration.z); + // Transform linear acceleration (need to account for component due to + // translational difference) -+ Eigen::Vector3f lin_accel(imu_raw.lin_accel[0], imu_raw.lin_accel[1], -+ imu_raw.lin_accel[2]); ++ Eigen::Vector3f lin_accel( ++ imu_raw.lin_accel[0], ++ imu_raw.lin_accel[1], ++ imu_raw.lin_accel[2] ++ ); Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; -@@ -1408,9 +1147,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const + lin_accel_cg = lin_accel_cg +- + ((ang_vel_cg - ang_vel_cg_prev) / dt).cross(-this->extrinsics.baselink2imu.t) +- + ang_vel_cg.cross(ang_vel_cg.cross(-this->extrinsics.baselink2imu.t)); ++ + ((ang_vel_cg - ang_vel_cg_prev) / dt) ++ .cross(-this->extrinsics.baselink2imu.t) ++ + ang_vel_cg.cross(ang_vel_cg.cross(-this->extrinsics.baselink2imu.t)); ang_vel_cg_prev = ang_vel_cg; @@ -1350,70 +2254,442 @@ index bceae37..c0c625f 100644 + imu.lin_accel[2] = lin_accel_cg[2]; return imu; +- + } + + void dlio::OdomNode::computeMetrics() { +@@ -1422,30 +1326,29 @@ void dlio::OdomNode::computeMetrics() { + } + + void dlio::OdomNode::computeSpaciousness() { +- + // compute range of points + std::vector ds; + + for (int i = 0; i < this->original_scan->points.size(); i++) { +- float d = std::sqrt(pow(this->original_scan->points[i].x, 2) + +- pow(this->original_scan->points[i].y, 2)); ++ float d = std::sqrt( ++ pow(this->original_scan->points[i].x, 2) ++ + pow(this->original_scan->points[i].y, 2) ++ ); + ds.push_back(d); + } + + // median +- std::nth_element(ds.begin(), ds.begin() + ds.size()/2, ds.end()); +- float median_curr = ds[ds.size()/2]; ++ std::nth_element(ds.begin(), ds.begin() + ds.size() / 2, ds.end()); ++ float median_curr = ds[ds.size() / 2]; + static float median_prev = median_curr; +- float median_lpf = 0.95*median_prev + 0.05*median_curr; ++ float median_lpf = 0.95 * median_prev + 0.05 * median_curr; + median_prev = median_lpf; + + // push +- this->metrics.spaciousness.push_back( median_lpf ); +- ++ this->metrics.spaciousness.push_back(median_lpf); + } + + void dlio::OdomNode::computeDensity() { +- + float density; + + if (!this->geo.first_opt_done) { +@@ -1455,23 +1358,22 @@ void dlio::OdomNode::computeDensity() { + } -@@ -1471,7 +1210,7 @@ void dlio::OdomNode::computeConvexHull() { + static float density_prev = density; +- float density_lpf = 0.95*density_prev + 0.05*density; ++ float density_lpf = 0.95 * density_prev + 0.05 * density; + density_prev = density_lpf; + +- this->metrics.density.push_back( density_lpf ); +- ++ this->metrics.density.push_back(density_lpf); + } + + void dlio::OdomNode::computeConvexHull() { +- + // at least 4 keyframes for convex hull + if (this->num_processed_keyframes < 4) { + return; + } // create a pointcloud with points at keyframes - pcl::PointCloud::Ptr cloud = +- pcl::PointCloud::Ptr cloud = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); ++ pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr( ++ std::make_shared>() ++ ); std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1488,10 +1227,10 @@ void dlio::OdomNode::computeConvexHull() { +@@ -1488,29 +1390,31 @@ void dlio::OdomNode::computeConvexHull() { // get the indices of the keyframes on the convex hull pcl::PointCloud::Ptr convex_points = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); ++ pcl::PointCloud::Ptr( ++ std::make_shared>() ++ ); this->convex_hull.reconstruct(*convex_points); - pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); ++ pcl::PointIndices::Ptr convex_hull_point_idx = ++ pcl::PointIndices::Ptr(std::make_shared()); this->convex_hull.getHullPointIndices(*convex_hull_point_idx); this->keyframe_convex.clear(); -@@ -1510,7 +1249,7 @@ void dlio::OdomNode::computeConcaveHull() { +- for (int i=0; iindices.size(); ++i) { ++ for (int i = 0; i < convex_hull_point_idx->indices.size(); ++i) { + this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]); + } +- + } + + void dlio::OdomNode::computeConcaveHull() { +- + // at least 5 keyframes for concave hull + if (this->num_processed_keyframes < 5) { + return; + } // create a pointcloud with points at keyframes - pcl::PointCloud::Ptr cloud = +- pcl::PointCloud::Ptr cloud = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); ++ pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr( ++ std::make_shared>() ++ ); std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1527,10 +1266,10 @@ void dlio::OdomNode::computeConcaveHull() { +@@ -1527,21 +1431,22 @@ void dlio::OdomNode::computeConcaveHull() { // get the indices of the keyframes on the concave hull pcl::PointCloud::Ptr concave_points = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); ++ pcl::PointCloud::Ptr( ++ std::make_shared>() ++ ); this->concave_hull.reconstruct(*concave_points); - pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); ++ pcl::PointIndices::Ptr concave_hull_point_idx = ++ pcl::PointIndices::Ptr(std::make_shared()); this->concave_hull.getHullPointIndices(*concave_hull_point_idx); this->keyframe_concave.clear(); -@@ -1739,7 +1478,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { +- for (int i=0; iindices.size(); ++i) { ++ for (int i = 0; i < concave_hull_point_idx->indices.size(); ++i) { + this->keyframe_concave.push_back(concave_hull_point_idx->indices[i]); + } +- + } + + void dlio::OdomNode::updateKeyframes() { +- + // calculate difference in pose and rotation to all poses in trajectory + float closest_d = std::numeric_limits::infinity(); + int closest_idx = 0; +@@ -1550,14 +1455,15 @@ void dlio::OdomNode::updateKeyframes() { + int num_nearby = 0; + + for (const auto& k : this->keyframes) { +- + // calculate distance between current pose and pose in keyframes +- float delta_d = sqrt( pow(this->state.p[0] - k.first.first[0], 2) + +- pow(this->state.p[1] - k.first.first[1], 2) + +- pow(this->state.p[2] - k.first.first[2], 2) ); ++ float delta_d = sqrt( ++ pow(this->state.p[0] - k.first.first[0], 2) ++ + pow(this->state.p[1] - k.first.first[1], 2) ++ + pow(this->state.p[2] - k.first.first[2], 2) ++ ); + + // count the number nearby current pose +- if (delta_d <= this->keyframe_thresh_dist_ * 1.5){ ++ if (delta_d <= this->keyframe_thresh_dist_ * 1.5) { + ++num_nearby; + } + +@@ -1568,7 +1474,6 @@ void dlio::OdomNode::updateKeyframes() { + } + + keyframes_idx++; +- + } + + // get closest pose and corresponding rotation +@@ -1576,28 +1481,35 @@ void dlio::OdomNode::updateKeyframes() { + Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second; + + // calculate distance between current pose and closest pose from above +- float dd = sqrt( pow(this->state.p[0] - closest_pose[0], 2) + +- pow(this->state.p[1] - closest_pose[1], 2) + +- pow(this->state.p[2] - closest_pose[2], 2) ); ++ float dd = sqrt( ++ pow(this->state.p[0] - closest_pose[0], 2) ++ + pow(this->state.p[1] - closest_pose[1], 2) ++ + pow(this->state.p[2] - closest_pose[2], 2) ++ ); + + // calculate difference in orientation using SLERP + Eigen::Quaternionf dq; + + if (this->state.q.dot(closest_pose_r) < 0.) { + Eigen::Quaternionf lq = closest_pose_r; +- lq.w() *= -1.; lq.x() *= -1.; lq.y() *= -1.; lq.z() *= -1.; ++ lq.w() *= -1.; ++ lq.x() *= -1.; ++ lq.y() *= -1.; ++ lq.z() *= -1.; + dq = this->state.q * lq.inverse(); + } else { + dq = this->state.q * closest_pose_r.inverse(); + } + +- double theta_rad = 2. * atan2(sqrt( pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2) ), dq.w()); +- double theta_deg = theta_rad * (180.0/M_PI); ++ double theta_rad = ++ 2. * atan2(sqrt(pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2)), dq.w()); ++ double theta_deg = theta_rad * (180.0 / M_PI); + + // update keyframes + bool newKeyframe = false; + +- if (abs(dd) > this->keyframe_thresh_dist_ || abs(theta_deg) > this->keyframe_thresh_rot_) { ++ if (abs(dd) > this->keyframe_thresh_dist_ ++ || abs(theta_deg) > this->keyframe_thresh_rot_) { + newKeyframe = true; + } + +@@ -1605,54 +1517,74 @@ void dlio::OdomNode::updateKeyframes() { + newKeyframe = false; + } + +- if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { ++ if (abs(dd) <= this->keyframe_thresh_dist_ ++ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { + newKeyframe = true; + } + + if (newKeyframe) { +- + // update keyframe vector +- std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); +- this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); ++ std::unique_lockkeyframes_mutex)> lock( ++ this->keyframes_mutex ++ ); ++ this->keyframes.push_back( ++ std::make_pair( ++ std::make_pair(this->lidarPose.p, this->lidarPose.q), ++ this->current_scan ++ ) ++ ); + this->keyframe_timestamps.push_back(this->scan_header_stamp); + this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); + this->keyframe_transformations.push_back(this->T_corr); + lock.unlock(); +- + } +- + } + + void dlio::OdomNode::setAdaptiveParams() { +- + // Spaciousness + float sp = this->metrics.spaciousness.back(); + +- if (sp < 0.5) { sp = 0.5; } +- if (sp > 5.0) { sp = 5.0; } ++ if (sp < 0.5) { ++ sp = 0.5; ++ } ++ if (sp > 5.0) { ++ sp = 5.0; ++ } + + this->keyframe_thresh_dist_ = sp; + + // Density + float den = this->metrics.density.back(); + +- if (den < 0.5*this->gicp_max_corr_dist_) { den = 0.5*this->gicp_max_corr_dist_; } +- if (den > 2.0*this->gicp_max_corr_dist_) { den = 2.0*this->gicp_max_corr_dist_; } ++ if (den < 0.5 * this->gicp_max_corr_dist_) { ++ den = 0.5 * this->gicp_max_corr_dist_; ++ } ++ if (den > 2.0 * this->gicp_max_corr_dist_) { ++ den = 2.0 * this->gicp_max_corr_dist_; ++ } + +- if (sp < 5.0) { den = 0.5*this->gicp_max_corr_dist_; }; +- if (sp > 5.0) { den = 2.0*this->gicp_max_corr_dist_; }; ++ if (sp < 5.0) { ++ den = 0.5 * this->gicp_max_corr_dist_; ++ }; ++ if (sp > 5.0) { ++ den = 2.0 * this->gicp_max_corr_dist_; ++ }; + + this->gicp.setMaxCorrespondenceDistance(den); + + // Concave hull alpha + this->concave_hull.setAlpha(this->keyframe_thresh_dist_); +- + } + +-void dlio::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vector frames) { +- ++void dlio::OdomNode::pushSubmapIndices( ++ std::vector dists, ++ int k, ++ std::vector frames ++) { + // make sure dists is not empty +- if (!dists.size()) { return; } ++ if (!dists.size()) { ++ return; ++ } + + // maintain max heap of at most k elements + std::priority_queue pq; +@@ -1671,14 +1603,13 @@ void dlio::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vec + + // get all elements smaller or equal to the kth smallest element + for (int i = 0; i < dists.size(); ++i) { +- if (dists[i] <= kth_element) ++ if (dists[i] <= kth_element) { + this->submap_kf_idx_curr.push_back(frames[i]); ++ } + } +- + } + + void dlio::OdomNode::buildSubmap(State vehicle_state) { +- + // clear vector of keyframe indices to use for submap + this->submap_kf_idx_curr.clear(); + +@@ -1687,9 +1618,11 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { + std::vector ds; + std::vector keyframe_nn; + for (int i = 0; i < this->num_processed_keyframes; i++) { +- float d = sqrt( pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) + +- pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) + +- pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) ); ++ float d = sqrt( ++ pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) ++ + pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) ++ + pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) ++ ); + ds.push_back(d); + keyframe_nn.push_back(i); + } +@@ -1720,38 +1653,47 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { + } + + // get indices for top kNN for concave hull +- this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); ++ this ++ ->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); + + // sort current and previous submap kf list of indices + std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); + std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end()); +- ++ + // remove duplicate indices +- auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); ++ auto last = std::unique( ++ this->submap_kf_idx_curr.begin(), ++ this->submap_kf_idx_curr.end() ++ ); + this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end()); + + // check if submap has changed from previous iteration +- if (this->submap_kf_idx_curr != this->submap_kf_idx_prev){ +- ++ if (this->submap_kf_idx_curr != this->submap_kf_idx_prev) { + this->submap_hasChanged = true; + + // Pause to prevent stealing resources from the main loop if it is running. this->pauseSubmapBuildIfNeeded(); // reinitialize submap cloud and normals - pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr submap_cloud_ (std::make_shared>()); - std::shared_ptr submap_normals_ (std::make_shared()); +- std::shared_ptr submap_normals_ (std::make_shared()); ++ pcl::PointCloud::Ptr submap_cloud_( ++ std::make_shared>() ++ ); ++ std::shared_ptr submap_normals_( ++ std::make_shared() ++ ); for (auto k : this->submap_kf_idx_curr) { -@@ -1780,7 +1519,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { +- + // create current submap cloud + lock.lock(); + *submap_cloud_ += *this->keyframes[k].second; + lock.unlock(); + + // grab corresponding submap cloud's normals +- submap_normals_->insert( std::end(*submap_normals_), +- std::begin(*(this->keyframe_normals[k])), std::end(*(this->keyframe_normals[k])) ); ++ submap_normals_->insert( ++ std::end(*submap_normals_), ++ std::begin(*(this->keyframe_normals[k])), ++ std::end(*(this->keyframe_normals[k])) ++ ); + } + + this->submap_cloud = submap_cloud_; +@@ -1768,33 +1710,39 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { + } + + void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { +- + // transform the new keyframe(s) and associated covariance list(s) +- std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); ++ std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + + for (int i = this->num_processed_keyframes; i < this->keyframes.size(); i++) { +- pcl::PointCloud::ConstPtr raw_keyframe = this->keyframes[i].second; +- std::shared_ptr raw_covariances = this->keyframe_normals[i]; ++ pcl::PointCloud::ConstPtr raw_keyframe = ++ this->keyframes[i].second; ++ std::shared_ptr raw_covariances = ++ this->keyframe_normals[i]; + Eigen::Matrix4f T = this->keyframe_transformations[i]; + lock.unlock(); Eigen::Matrix4d Td = T.cast(); - pcl::PointCloud::Ptr transformed_keyframe (boost::make_shared>()); -+ pcl::PointCloud::Ptr transformed_keyframe (std::make_shared>()); - pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); +- pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); ++ pcl::PointCloud::Ptr transformed_keyframe( ++ std::make_shared>() ++ ); ++ pcl::transformPointCloud(*raw_keyframe, *transformed_keyframe, T); + +- std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); +- std::transform(raw_covariances->begin(), raw_covariances->end(), transformed_covariances->begin(), +- [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); }); ++ std::shared_ptr transformed_covariances( ++ std::make_shared(raw_covariances->size()) ++ ); ++ std::transform( ++ raw_covariances->begin(), ++ raw_covariances->end(), ++ transformed_covariances->begin(), ++ [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); } ++ ); + + ++this->num_processed_keyframes; - std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); -@@ -1792,9 +1531,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { lock.lock(); this->keyframes[i].second = transformed_keyframe; this->keyframe_normals[i] = transformed_covariances; @@ -1423,27 +2699,398 @@ index bceae37..c0c625f 100644 } lock.unlock(); -@@ -2018,3 +1754,19 @@ void dlio::OdomNode::debug() { - std::cout << "+-------------------------------------------------------------------+" << std::endl; +@@ -1806,12 +1754,13 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { + } + + void dlio::OdomNode::pauseSubmapBuildIfNeeded() { +- std::unique_lockmain_loop_running_mutex)> lock(this->main_loop_running_mutex); +- this->submap_build_cv.wait(lock, [this]{ return !this->main_loop_running; }); ++ std::unique_lockmain_loop_running_mutex)> lock( ++ this->main_loop_running_mutex ++ ); ++ this->submap_build_cv.wait(lock, [this] { return !this->main_loop_running; }); + } + + void dlio::OdomNode::debug() { +- + // Total length traversed + double length_traversed = 0.; + Eigen::Vector3f p_curr = Eigen::Vector3f(0., 0., 0.); +@@ -1822,7 +1771,10 @@ void dlio::OdomNode::debug() { + continue; + } + p_curr = t.first; +- double l = sqrt(pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) + pow(p_curr[2] - p_prev[2], 2)); ++ double l = sqrt( ++ pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) ++ + pow(p_curr[2] - p_prev[2], 2) ++ ); + + if (l >= 0.1) { + length_traversed += l; +@@ -1833,7 +1785,8 @@ void dlio::OdomNode::debug() { + + // Average computation time + double avg_comp_time = +- std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) / this->comp_times.size(); ++ std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) ++ / this->comp_times.size(); + + // Average sensor rates + int win_size = 100; +@@ -1841,23 +1794,36 @@ void dlio::OdomNode::debug() { + double avg_lidar_rate; + if (this->imu_rates.size() < win_size) { + avg_imu_rate = +- std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) / this->imu_rates.size(); ++ std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) ++ / this->imu_rates.size(); + } else { +- avg_imu_rate = +- std::accumulate(this->imu_rates.end()-win_size, this->imu_rates.end(), 0.0) / win_size; ++ avg_imu_rate = std::accumulate( ++ this->imu_rates.end() - win_size, ++ this->imu_rates.end(), ++ 0.0 ++ ) ++ / win_size; + } + if (this->lidar_rates.size() < win_size) { + avg_lidar_rate = +- std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) / this->lidar_rates.size(); ++ std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) ++ / this->lidar_rates.size(); + } else { +- avg_lidar_rate = +- std::accumulate(this->lidar_rates.end()-win_size, this->lidar_rates.end(), 0.0) / win_size; ++ avg_lidar_rate = std::accumulate( ++ this->lidar_rates.end() - win_size, ++ this->lidar_rates.end(), ++ 0.0 ++ ) ++ / win_size; + } + + // RAM Usage + double vm_usage = 0.0; + double resident_set = 0.0; +- std::ifstream stat_stream("/proc/self/stat", std::ios_base::in); //get info from proc directory ++ std::ifstream stat_stream( ++ "/proc/self/stat", ++ std::ios_base::in ++ ); //get info from proc directory + std::string pid, comm, state, ppid, pgrp, session, tty_nr; + std::string tpgid, flags, minflt, cminflt, majflt, cmajflt; + std::string utime, stime, cutime, cstime, priority, nice; +@@ -1865,11 +1831,12 @@ void dlio::OdomNode::debug() { + unsigned long vsize; + long rss; + stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr +- >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt +- >> utime >> stime >> cutime >> cstime >> priority >> nice +- >> num_threads >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest ++ >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt >> utime ++ >> stime >> cutime >> cstime >> priority >> nice >> num_threads ++ >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest + stat_stream.close(); +- long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages ++ long page_size_kb = ++ sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages + vm_usage = vsize / 1024.0; + resident_set = rss * page_size_kb; + +@@ -1878,143 +1845,202 @@ void dlio::OdomNode::debug() { + clock_t now; + double cpu_percent; + now = times(&timeSample); +- if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU || +- timeSample.tms_utime < this->lastUserCPU) { +- cpu_percent = -1.0; ++ if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU ++ || timeSample.tms_utime < this->lastUserCPU) { ++ cpu_percent = -1.0; + } else { +- cpu_percent = (timeSample.tms_stime - this->lastSysCPU) + (timeSample.tms_utime - this->lastUserCPU); +- cpu_percent /= (now - this->lastCPU); +- cpu_percent /= this->numProcessors; +- cpu_percent *= 100.; ++ cpu_percent = (timeSample.tms_stime - this->lastSysCPU) ++ + (timeSample.tms_utime - this->lastUserCPU); ++ cpu_percent /= (now - this->lastCPU); ++ cpu_percent /= this->numProcessors; ++ cpu_percent *= 100.; + } + this->lastCPU = now; + this->lastSysCPU = timeSample.tms_stime; + this->lastUserCPU = timeSample.tms_utime; + this->cpu_percents.push_back(cpu_percent); + double avg_cpu_usage = +- std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) / this->cpu_percents.size(); ++ std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) ++ / this->cpu_percents.size(); + + // Print to terminal + printf("\033[2J\033[1;1H"); + +- std::cout << std::endl +- << "+-------------------------------------------------------------------+" << std::endl; +- std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" +- << std::endl; +- std::cout << "+-------------------------------------------------------------------+" << std::endl; ++ std::cout ++ << std::endl ++ << "+-------------------------------------------------------------------+" ++ << std::endl; ++ std::cout << "| Direct LiDAR-Inertial Odometry v" ++ << this->version_ << " |" << std::endl; ++ std::cout ++ << "+-------------------------------------------------------------------+" ++ << std::endl; + + std::time_t curr_time = this->scan_stamp; +- std::string asc_time = std::asctime(std::localtime(&curr_time)); asc_time.pop_back(); ++ std::string asc_time = std::asctime(std::localtime(&curr_time)); ++ asc_time.pop_back(); + std::cout << "| " << std::left << asc_time; + std::cout << std::right << std::setfill(' ') << std::setw(42) +- << "Elapsed Time: " + to_string_with_precision(this->elapsed_time, 2) + " seconds " +- << "|" << std::endl; ++ << "Elapsed Time: " ++ + to_string_with_precision(this->elapsed_time, 2) + " seconds " ++ << "|" << std::endl; + +- if ( !this->cpu_type.empty() ) { ++ if (!this->cpu_type.empty()) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << this->cpu_type + " x " + std::to_string(this->numProcessors) +- << "|" << std::endl; ++ << this->cpu_type + " x " + std::to_string(this->numProcessors) ++ << "|" << std::endl; + } + + if (this->sensor == dlio::SensorType::OUSTER) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Sensor Rates: Ouster @ " + to_string_with_precision(avg_lidar_rate, 2) +- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" +- << "|" << std::endl; ++ << "Sensor Rates: Ouster @ " ++ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " ++ + to_string_with_precision(avg_imu_rate, 2) + " Hz" ++ << "|" << std::endl; + } else if (this->sensor == dlio::SensorType::VELODYNE) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Sensor Rates: Velodyne @ " + to_string_with_precision(avg_lidar_rate, 2) +- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" +- << "|" << std::endl; ++ << "Sensor Rates: Velodyne @ " ++ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " ++ + to_string_with_precision(avg_imu_rate, 2) + " Hz" ++ << "|" << std::endl; + } else if (this->sensor == dlio::SensorType::HESAI) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Sensor Rates: Hesai @ " + to_string_with_precision(avg_lidar_rate, 2) +- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" +- << "|" << std::endl; ++ << "Sensor Rates: Hesai @ " ++ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " ++ + to_string_with_precision(avg_imu_rate, 2) + " Hz" ++ << "|" << std::endl; + } else if (this->sensor == dlio::SensorType::LIVOX) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Sensor Rates: Livox @ " + to_string_with_precision(avg_lidar_rate, 2) +- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" +- << "|" << std::endl; ++ << "Sensor Rates: Livox @ " ++ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " ++ + to_string_with_precision(avg_imu_rate, 2) + " Hz" ++ << "|" << std::endl; + } else { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Sensor Rates: Unknown LiDAR @ " + to_string_with_precision(avg_lidar_rate, 2) +- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" +- << "|" << std::endl; ++ << "Sensor Rates: Unknown LiDAR @ " ++ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " ++ + to_string_with_precision(avg_imu_rate, 2) + " Hz" ++ << "|" << std::endl; + } +- std::cout << "|===================================================================|" << std::endl; ++ std::cout ++ << "|===================================================================|" ++ << std::endl; + + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Position {W} [xyz] :: " + to_string_with_precision(this->state.p[0], 4) + " " +- + to_string_with_precision(this->state.p[1], 4) + " " +- + to_string_with_precision(this->state.p[2], 4) +- << "|" << std::endl; ++ << "Position {W} [xyz] :: " ++ + to_string_with_precision(this->state.p[0], 4) + " " ++ + to_string_with_precision(this->state.p[1], 4) + " " ++ + to_string_with_precision(this->state.p[2], 4) ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Orientation {W} [wxyz] :: " + to_string_with_precision(this->state.q.w(), 4) + " " +- + to_string_with_precision(this->state.q.x(), 4) + " " +- + to_string_with_precision(this->state.q.y(), 4) + " " +- + to_string_with_precision(this->state.q.z(), 4) +- << "|" << std::endl; ++ << "Orientation {W} [wxyz] :: " ++ + to_string_with_precision(this->state.q.w(), 4) + " " ++ + to_string_with_precision(this->state.q.x(), 4) + " " ++ + to_string_with_precision(this->state.q.y(), 4) + " " ++ + to_string_with_precision(this->state.q.z(), 4) ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Lin Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.lin.b[0], 4) + " " +- + to_string_with_precision(this->state.v.lin.b[1], 4) + " " +- + to_string_with_precision(this->state.v.lin.b[2], 4) +- << "|" << std::endl; ++ << "Lin Velocity {B} [xyz] :: " ++ + to_string_with_precision(this->state.v.lin.b[0], 4) + " " ++ + to_string_with_precision(this->state.v.lin.b[1], 4) + " " ++ + to_string_with_precision(this->state.v.lin.b[2], 4) ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Ang Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.ang.b[0], 4) + " " +- + to_string_with_precision(this->state.v.ang.b[1], 4) + " " +- + to_string_with_precision(this->state.v.ang.b[2], 4) +- << "|" << std::endl; ++ << "Ang Velocity {B} [xyz] :: " ++ + to_string_with_precision(this->state.v.ang.b[0], 4) + " " ++ + to_string_with_precision(this->state.v.ang.b[1], 4) + " " ++ + to_string_with_precision(this->state.v.ang.b[2], 4) ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Accel Bias [xyz] :: " + to_string_with_precision(this->state.b.accel[0], 8) + " " +- + to_string_with_precision(this->state.b.accel[1], 8) + " " +- + to_string_with_precision(this->state.b.accel[2], 8) +- << "|" << std::endl; ++ << "Accel Bias [xyz] :: " ++ + to_string_with_precision(this->state.b.accel[0], 8) + " " ++ + to_string_with_precision(this->state.b.accel[1], 8) + " " ++ + to_string_with_precision(this->state.b.accel[2], 8) ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Gyro Bias [xyz] :: " + to_string_with_precision(this->state.b.gyro[0], 8) + " " +- + to_string_with_precision(this->state.b.gyro[1], 8) + " " +- + to_string_with_precision(this->state.b.gyro[2], 8) +- << "|" << std::endl; ++ << "Gyro Bias [xyz] :: " ++ + to_string_with_precision(this->state.b.gyro[0], 8) + " " ++ + to_string_with_precision(this->state.b.gyro[1], 8) + " " ++ + to_string_with_precision(this->state.b.gyro[2], 8) ++ << "|" << std::endl; + +- std::cout << "| |" << std::endl; ++ std::cout ++ << "| |" ++ << std::endl; + + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Distance Traveled :: " + to_string_with_precision(length_traversed, 4) + " meters" +- << "|" << std::endl; ++ << "Distance Traveled :: " ++ + to_string_with_precision(length_traversed, 4) + " meters" ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Distance to Origin :: " +- + to_string_with_precision( sqrt(pow(this->state.p[0]-this->origin[0],2) + +- pow(this->state.p[1]-this->origin[1],2) + +- pow(this->state.p[2]-this->origin[2],2)), 4) + " meters" +- << "|" << std::endl; ++ << "Distance to Origin :: " ++ + to_string_with_precision( ++ sqrt( ++ pow(this->state.p[0] - this->origin[0], 2) ++ + pow(this->state.p[1] - this->origin[1], 2) ++ + pow(this->state.p[2] - this->origin[2], 2) ++ ), ++ 4 ++ ) ++ + " meters" ++ << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "Registration :: keyframes: " + std::to_string(this->keyframes.size()) + ", " +- + "deskewed points: " + std::to_string(this->deskew_size) +- << "|" << std::endl; +- std::cout << "| |" << std::endl; ++ << "Registration :: keyframes: " ++ + std::to_string(this->keyframes.size()) + ", " ++ + "deskewed points: " + std::to_string(this->deskew_size) ++ << "|" << std::endl; ++ std::cout ++ << "| |" ++ << std::endl; + + std::cout << std::right << std::setprecision(2) << std::fixed; +- std::cout << "| Computation Time :: " +- << std::setfill(' ') << std::setw(6) << this->comp_times.back()*1000. << " ms // Avg: " +- << std::setw(6) << avg_comp_time*1000. << " / Max: " +- << std::setw(6) << *std::max_element(this->comp_times.begin(), this->comp_times.end())*1000. ++ std::cout ++ << "| Computation Time :: " << std::setfill(' ') << std::setw(6) ++ << this->comp_times.back() * 1000. << " ms // Avg: " << std::setw(6) ++ << avg_comp_time * 1000. << " / Max: " << std::setw(6) ++ << *std::max_element(this->comp_times.begin(), this->comp_times.end()) ++ * 1000. + << " |" << std::endl; +- std::cout << "| Cores Utilized :: " +- << std::setfill(' ') << std::setw(6) << (cpu_percent/100.) * this->numProcessors << " cores // Avg: " +- << std::setw(6) << (avg_cpu_usage/100.) * this->numProcessors << " / Max: " +- << std::setw(6) << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) / 100.) +- * this->numProcessors ++ std::cout ++ << "| Cores Utilized :: " << std::setfill(' ') << std::setw(6) ++ << (cpu_percent / 100.) * this->numProcessors ++ << " cores // Avg: " << std::setw(6) ++ << (avg_cpu_usage / 100.) * this->numProcessors ++ << " / Max: " << std::setw(6) ++ << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) ++ / 100.) ++ * this->numProcessors + << " |" << std::endl; +- std::cout << "| CPU Load :: " +- << std::setfill(' ') << std::setw(6) << cpu_percent << " % // Avg: " +- << std::setw(6) << avg_cpu_usage << " / Max: " +- << std::setw(6) << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) ++ std::cout ++ << "| CPU Load :: " << std::setfill(' ') << std::setw(6) ++ << cpu_percent << " % // Avg: " << std::setw(6) << avg_cpu_usage ++ << " / Max: " << std::setw(6) ++ << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) + << " |" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) +- << "RAM Allocation :: " + to_string_with_precision(resident_set/1000., 2) + " MB" +- << "|" << std::endl; +- +- std::cout << "+-------------------------------------------------------------------+" << std::endl; ++ << "RAM Allocation :: " ++ + to_string_with_precision(resident_set / 1000., 2) + " MB" ++ << "|" << std::endl; + ++ std::cout ++ << "+-------------------------------------------------------------------+" ++ << std::endl; } + +// Explicit instantiations of PCL templates for dlio::Point +// This is needed because PCL doesn't automatically instantiate templates for +// custom point types -+#include +#include +#include -+#include +#include +#include + ++#include ++#include ++ +template class pcl::PCLBase; +template class pcl::search::Search; +template class pcl::KdTreeFLANN>; -+template class pcl::search::KdTree< -+ dlio::Point, pcl::KdTreeFLANN>>; -\ No newline at end of file ++template class pcl::search:: ++ KdTree>>; diff --git a/src/nano_gicp/lsq_registration.cc b/src/nano_gicp/lsq_registration.cc index 271cb03..b6c8f07 100644 --- a/src/nano_gicp/lsq_registration.cc From 76623c997fb1b1b18ce411549f05eab3c4bfb544 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 19:18:47 -0400 Subject: [PATCH 09/11] Revert "fix: regenerate dlio patch with macOS iterator compatibility" This reverts commit 7e4387fad4ebbea7cde49e67d3d23e24c0e2a0b2. --- cpp/bindings/pipelines/dlio.patch | 2021 +++-------------------------- 1 file changed, 187 insertions(+), 1834 deletions(-) diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch index dd347a95..9c53a9e6 100644 --- a/cpp/bindings/pipelines/dlio.patch +++ b/cpp/bindings/pipelines/dlio.patch @@ -109,10 +109,10 @@ index 91e47a0..e3e2ba0 100644 +# target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) +# target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h -index 47386ff..d04c526 100644 +index 47386ff..0416918 100644 --- a/include/dlio/dlio.h +++ b/include/dlio/dlio.h -@@ -31,9 +31,12 @@ +@@ -31,7 +31,10 @@ #include #include #include @@ -124,9 +124,7 @@ index 47386ff..d04c526 100644 #include +#endif - template - std::string to_string_with_precision(const T a_value, const int n = 6) -@@ -44,16 +47,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -44,16 +43,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) return out.str(); } @@ -143,7 +141,7 @@ index 47386ff..d04c526 100644 // BOOST #include #include -@@ -69,14 +62,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) +@@ -69,14 +58,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) #include #include #include @@ -158,7 +156,7 @@ index 47386ff..d04c526 100644 namespace dlio { enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN }; -@@ -84,7 +72,7 @@ namespace dlio { +@@ -84,7 +68,7 @@ namespace dlio { class OdomNode; class MapNode; @@ -167,7 +165,7 @@ index 47386ff..d04c526 100644 Point(): data{0.f, 0.f, 0.f, 1.f} {} PCL_ADD_POINT4D; -@@ -96,7 +84,7 @@ namespace dlio { +@@ -96,7 +80,7 @@ namespace dlio { // (Livox) absolute timestamp in (seconds * 10e9) }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -176,6 +174,32 @@ index 47386ff..d04c526 100644 } POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, +diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h +index 2c6732b..d88de52 100644 +--- a/include/nano_gicp/nanoflann_adaptor.h ++++ b/include/nano_gicp/nanoflann_adaptor.h +@@ -49,7 +49,7 @@ + #include + #include + +-#include "nano_gicp/nanoflann.h" ++#include + + namespace nanoflann + { +@@ -86,5 +86,5 @@ protected: + +- nanoflann::SearchParams _params; ++ nanoflann::SearchParameters _params; + + struct PointCloud_Adaptor + { +@@ -159,4 +159,4 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, +- static std::vector > indices_dist; ++ static std::vector> indices_dist; + indices_dist.reserve( 128 ); + + RadiusResultSet resultSet(radius, indices_dist); diff --git a/include/dlio/odom.h b/include/dlio/odom.h index c79ebd7..2c91f67 100644 --- a/include/dlio/odom.h @@ -446,63 +470,23 @@ index c79ebd7..2c91f67 100644 int num_threads_; bool verbose; -diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h -index 0e2c2d7..eddd512 100644 ---- a/include/nano_gicp/nanoflann_adaptor.h -+++ b/include/nano_gicp/nanoflann_adaptor.h -@@ -49,7 +49,7 @@ - #include - #include - --#include "nano_gicp/nanoflann.h" -+#include - - namespace nanoflann - { -@@ -86,7 +86,7 @@ public: - - protected: - -- nanoflann::SearchParams _params; -+ nanoflann::SearchParameters _params; - - struct PointCloud_Adaptor - { -@@ -156,7 +156,7 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, - std::vector &k_indices, - std::vector &k_sqr_distances) const - { -- static std::vector > indices_dist; -+ static std::vector> indices_dist; - indices_dist.reserve( 128 ); - - RadiusResultSet resultSet(radius, indices_dist); diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc -index bceae37..307ad52 100644 +index bceae37..c0c625f 100644 --- a/src/dlio/odom.cc +++ b/src/dlio/odom.cc -@@ -12,34 +12,22 @@ +@@ -12,9 +12,9 @@ #include "dlio/odom.h" -dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { -- ++dlio::OdomNode::OdomNode(const Params ¶ms) { + - this->getParams(); -+dlio::OdomNode::OdomNode(const Params& params) { + this->getParams(params); this->num_threads_ = omp_get_max_threads(); - this->dlio_initialized = false; - this->first_valid_scan = false; - this->first_imu_received = false; -- if (this->imu_calibrate_) {this->imu_calibrated = false;} -- else {this->imu_calibrated = true;} -+ if (this->imu_calibrate_) { -+ this->imu_calibrated = false; -+ } else { -+ this->imu_calibrated = true; -+ } +@@ -26,20 +26,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->deskew_status = false; this->deskew_size = 0; @@ -523,7 +507,7 @@ index bceae37..307ad52 100644 this->T = Eigen::Matrix4f::Identity(); this->T_prior = Eigen::Matrix4f::Identity(); this->T_corr = Eigen::Matrix4f::Identity(); -@@ -67,10 +55,18 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { +@@ -67,10 +53,10 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->first_imu_stamp = 0.; this->prev_imu_stamp = 0.; @@ -531,22 +515,14 @@ index bceae37..307ad52 100644 - this->deskewed_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); - this->current_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); - this->submap_cloud = pcl::PointCloud::ConstPtr (boost::make_shared>()); -+ this->original_scan = pcl::PointCloud::ConstPtr( -+ std::make_shared>() -+ ); -+ this->deskewed_scan = pcl::PointCloud::ConstPtr( -+ std::make_shared>() -+ ); -+ this->current_scan = pcl::PointCloud::ConstPtr( -+ std::make_shared>() -+ ); -+ this->submap_cloud = pcl::PointCloud::ConstPtr( -+ std::make_shared>() -+ ); ++ this->original_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->deskewed_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->current_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); ++ this->submap_cloud = pcl::PointCloud::ConstPtr (std::make_shared>()); this->num_processed_keyframes = 0; -@@ -79,7 +75,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { +@@ -79,7 +65,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->first_scan_stamp = 0.; this->elapsed_time = 0.; @@ -554,66 +530,8 @@ index bceae37..307ad52 100644 this->convex_hull.setDimension(3); this->concave_hull.setDimension(3); -@@ -112,8 +107,17 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - pcl::console::setVerbosityLevel(pcl::console::L_ERROR); - - this->crop.setNegative(true); -- this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_, -this->crop_size_, 1.0)); -- this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0)); -+ this->crop.setMin( -+ Eigen::Vector4f( -+ -this->crop_size_, -+ -this->crop_size_, -+ -this->crop_size_, -+ 1.0 -+ ) -+ ); -+ this->crop.setMax( -+ Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0) -+ ); - - this->voxel.setLeafSize(this->vf_res_, this->vf_res_, this->vf_res_); - -@@ -126,22 +130,23 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - - this->cpu_type = ""; - -- #ifdef HAS_CPUID -- unsigned int CPUInfo[4] = {0,0,0,0}; -+#ifdef HAS_CPUID -+ unsigned int CPUInfo[4] = {0, 0, 0, 0}; - __cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); - unsigned int nExIds = CPUInfo[0]; - for (unsigned int i = 0x80000000; i <= nExIds; ++i) { - __cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); -- if (i == 0x80000002) -+ if (i == 0x80000002) { - memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo)); -- else if (i == 0x80000003) -+ } else if (i == 0x80000003) { - memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo)); -- else if (i == 0x80000004) -+ } else if (i == 0x80000004) { - memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo)); -+ } - } - this->cpu_type = CPUBrandString; - boost::trim(this->cpu_type); -- #endif -+#endif - - FILE* file; - struct tms timeSample; -@@ -153,155 +158,87 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - - file = fopen("/proc/cpuinfo", "r"); - this->numProcessors = 0; -- while(fgets(line, 128, file) != nullptr) { -- if (strncmp(line, "processor", 9) == 0) this->numProcessors++; -+ while (fgets(line, 128, file) != nullptr) { -+ if (strncmp(line, "processor", 9) == 0) { -+ this->numProcessors++; -+ } +@@ -157,151 +142,81 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + if (strncmp(line, "processor", 9) == 0) this->numProcessors++; } fclose(file); - @@ -622,54 +540,51 @@ index bceae37..307ad52 100644 dlio::OdomNode::~OdomNode() {} -void dlio::OdomNode::getParams() { -+void dlio::OdomNode::getParams(const Params& params) { -+ this->verbose = params.verbose; - +- - // Version - ros::param::param("~dlio/version", this->version_, "0.0.0"); -+ this->deskew_ = params.deskew; -+ this->gravity_ = params.gravity; -+ this->time_offset_ = params.time_offset; - +- - // Frames - ros::param::param("~dlio/frames/odom", this->odom_frame, "odom"); - ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); - ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); - ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); -+ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; -+ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; ++void dlio::OdomNode::getParams(const Params ¶ms) { ++ this->verbose = params.verbose; - // Get Node NS and Remove Leading Character - std::string ns = ros::this_node::getNamespace(); - ns.erase(0,1); -+ this->submap_knn_ = params.submap_knn; -+ this->submap_kcv_ = params.submap_kcv; -+ this->submap_kcc_ = params.submap_kcc; ++ this->deskew_ = params.deskew; ++ this->gravity_ = params.gravity; ++ this->time_offset_ = params.time_offset; - // Concatenate Frame Name Strings - this->odom_frame = ns + "/" + this->odom_frame; - this->baselink_frame = ns + "/" + this->baselink_frame; - this->lidar_frame = ns + "/" + this->lidar_frame; - this->imu_frame = ns + "/" + this->imu_frame; -+ this->densemap_filtered_ = params.densemap_filtered; ++ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; ++ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - // Deskew FLag - ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); -+ this->wait_until_move_ = params.wait_until_move; ++ this->submap_knn_ = params.submap_knn; ++ this->submap_kcv_ = params.submap_kcv; ++ this->submap_kcc_ = params.submap_kcc; - // Gravity - ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); -+ this->crop_size_ = params.crop_size; ++ this->densemap_filtered_ = params.densemap_filtered; - // Compute time offset between lidar and imu - ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); -+ this->vf_use_ = params.vf_use; -+ this->vf_res_ = params.vf_res; ++ this->wait_until_move_ = params.wait_until_move; - // Keyframe Threshold - ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); - ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); -+ this->adaptive_params_ = params.adaptive_params; ++ this->crop_size_ = params.crop_size; - // Submap - ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); @@ -688,10 +603,13 @@ index bceae37..307ad52 100644 - // Voxel Grid Filter - ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); - ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); -- ++ this->vf_use_ = params.vf_use; ++ this->vf_res_ = params.vf_res; + - // Adaptive Parameters - ros::param::param("~dlio/adaptive", this->adaptive_params_, true); -- ++ this->adaptive_params_ = params.adaptive_params; + - // Extrinsics - std::vector t_default{0., 0., 0.}; - std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; @@ -724,9 +642,9 @@ index bceae37..307ad52 100644 - this->extrinsics.baselink2lidar.R = - Eigen::Map>(baselink2lidar_R.data(), 3, 3); + this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2imu.t; ++ this->extrinsics.baselink2imu.t; + this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2imu.R; ++ this->extrinsics.baselink2imu.R; this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity(); - this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t; @@ -787,9 +705,9 @@ index bceae37..307ad52 100644 - - ros::param::param("~dlio/verbose", this->verbose, true); + this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2lidar.t; ++ this->extrinsics.baselink2lidar.t; + this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2lidar.R; ++ this->extrinsics.baselink2lidar.R; + + this->calibrate_gyro_ = params.calibrate_gyro; + this->calibrate_accel_ = params.calibrate_accel; @@ -822,25 +740,8 @@ index bceae37..307ad52 100644 } void dlio::OdomNode::start() { -@@ -310,193 +247,25 @@ void dlio::OdomNode::start() { - } +@@ -318,185 +233,12 @@ void dlio::OdomNode::start() { - printf("\033[2J\033[1;1H"); -- std::cout << std::endl -- << "+-------------------------------------------------------------------+" << std::endl; -- std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" -- << std::endl; -- std::cout << "+-------------------------------------------------------------------+" << std::endl; -- -+ std::cout -+ << std::endl -+ << "+-------------------------------------------------------------------+" -+ << std::endl; -+ std::cout << "| Direct LiDAR-Inertial Odometry v" -+ << this->version_ << " |" << std::endl; -+ std::cout -+ << "+-------------------------------------------------------------------+" -+ << std::endl; } -void dlio::OdomNode::publishPose(const ros::TimerEvent& e) { @@ -951,7 +852,9 @@ index bceae37..307ad52 100644 - transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; - transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; - transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; -- ++void dlio::OdomNode::getScanFromROS( ++ const pcl::PointCloud::ConstPtr &pc, double stamp) { + - Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); - transformStamped.transform.rotation.w = qq.w(); - transformStamped.transform.rotation.x = qq.x(); @@ -1022,18 +925,13 @@ index bceae37..307ad52 100644 - - pcl::PointCloud::Ptr original_scan_ (boost::make_shared>()); - pcl::fromROSMsg(*pc, *original_scan_); -+void dlio::OdomNode::getScanFromROS( -+ const pcl::PointCloud::ConstPtr& pc, -+ double stamp -+) { + pcl::PointCloud::Ptr original_scan_( -+ std::make_shared>() -+ ); ++ std::make_shared>()); + *original_scan_ = *pc; // Remove NaNs std::vector idx; -@@ -508,37 +277,20 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) +@@ -508,28 +250,14 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) this->crop.filter(*original_scan_); // automatically detect sensor type @@ -1064,103 +962,41 @@ index bceae37..307ad52 100644 - this->scan_header_stamp = pc->header.stamp; + this->scan_header_stamp = stamp; this->original_scan = original_scan_; -- - } - - void dlio::OdomNode::preprocessPoints() { -- - // Deskew the original dlio-type scan - if (this->deskew_) { -- - this->deskewPointcloud(); - if (!this->first_valid_scan) { -@@ -546,13 +298,12 @@ void dlio::OdomNode::preprocessPoints() { - } + } +@@ -547,7 +275,7 @@ void dlio::OdomNode::preprocessPoints() { } else { -- + - this->scan_stamp = this->scan_header_stamp.toSec(); + this->scan_stamp = this->scan_header_stamp; // don't process scans until IMU data is present if (!this->first_valid_scan) { -- -- if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { -+ if (this->imu_buffer.empty() -+ || this->scan_stamp <= this->imu_buffer.back().stamp) { - return; - } - -@@ -560,102 +311,130 @@ void dlio::OdomNode::preprocessPoints() { - this->T_prior = this->T; // assume no motion for the first scan +@@ -574,7 +302,7 @@ void dlio::OdomNode::preprocessPoints() { - } else { -- - // IMU prior for second scan onwards -- std::vector> frames; -- frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, -- this->geo.prev_vel.cast(), {this->scan_stamp}); -- -- if (frames.size() > 0) { -- this->T_prior = frames.back(); -- } else { -- this->T_prior = this->T; -- } -+ std::vector> -+ frames; -+ frames = this->integrateImu( -+ this->prev_scan_stamp, -+ this->lidarPose.q, -+ this->lidarPose.p, -+ this->geo.prev_vel.cast(), -+ {this->scan_stamp} -+ ); - -+ if (frames.size() > 0) { -+ this->T_prior = frames.back(); -+ } else { -+ this->T_prior = this->T; -+ } } - pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -- pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, -- this->T_prior * this->extrinsics.baselink2lidar_T); -+ pcl::PointCloud::Ptr deskewed_scan_( -+ std::make_shared>() -+ ); -+ pcl::transformPointCloud( -+ *this->original_scan, -+ *deskewed_scan_, -+ this->T_prior * this->extrinsics.baselink2lidar_T -+ ); ++ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); + pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, + this->T_prior * this->extrinsics.baselink2lidar_T); this->deskewed_scan = deskewed_scan_; - this->deskew_status = false; - } - +@@ -584,7 +312,7 @@ void dlio::OdomNode::preprocessPoints() { // Voxel Grid Filter if (this->vf_use_) { -- pcl::PointCloud::Ptr current_scan_ + pcl::PointCloud::Ptr current_scan_ - (boost::make_shared>(*this->deskewed_scan)); -+ pcl::PointCloud::Ptr current_scan_( -+ std::make_shared>(*this->deskewed_scan) -+ ); ++ (std::make_shared>(*this->deskewed_scan)); this->voxel.setInputCloud(current_scan_); this->voxel.filter(*current_scan_); this->current_scan = current_scan_; - } else { - this->current_scan = this->deskewed_scan; - } -- - } +@@ -596,11 +324,11 @@ void dlio::OdomNode::preprocessPoints() { void dlio::OdomNode::deskewPointcloud() { -- + - pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr deskewed_scan_( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); deskewed_scan_->points.resize(this->original_scan->points.size()); // individual point timestamps should be relative to this time @@ -1169,276 +1005,44 @@ index bceae37..307ad52 100644 // sort points by timestamp and build list of timestamps std::function point_time_cmp; -- std::function, -- boost::range::index_value)> point_time_neq; -- std::function)> extract_point_time; -+ std::function, -+ boost::range::index_value -+ )> -+ point_time_neq; -+ std::function)> -+ extract_point_time; - - if (this->sensor == dlio::SensorType::OUSTER) { -- -- point_time_cmp = [](const PointType& p1, const PointType& p2) -- { return p1.t < p2.t; }; -- point_time_neq = [](boost::range::index_value p1, -- boost::range::index_value p2) -- { return p1.value().t != p2.value().t; }; -- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) -- { return sweep_ref_time + pt.value().t * 1e-9f; }; -+ point_time_cmp = [](const PointType& p1, const PointType& p2) { -+ return p1.t < p2.t; -+ }; -+ point_time_neq = []( -+ boost::range::index_value p1, -+ boost::range::index_value p2 -+ ) { return p1.value().t != p2.value().t; }; -+ extract_point_time = [&sweep_ref_time]( -+ boost::range::index_value pt -+ ) { return sweep_ref_time + pt.value().t * 1e-9f; }; - - } else if (this->sensor == dlio::SensorType::VELODYNE) { -- -- point_time_cmp = [](const PointType& p1, const PointType& p2) -- { return p1.time < p2.time; }; -- point_time_neq = [](boost::range::index_value p1, -- boost::range::index_value p2) -- { return p1.value().time != p2.value().time; }; -- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) -- { return sweep_ref_time + pt.value().time; }; -+ point_time_cmp = [](const PointType& p1, const PointType& p2) { -+ return p1.time < p2.time; -+ }; -+ point_time_neq = []( -+ boost::range::index_value p1, -+ boost::range::index_value p2 -+ ) { return p1.value().time != p2.value().time; }; -+ extract_point_time = [&sweep_ref_time]( -+ boost::range::index_value pt -+ ) { return sweep_ref_time + pt.value().time; }; - - } else if (this->sensor == dlio::SensorType::HESAI) { -- -- point_time_cmp = [](const PointType& p1, const PointType& p2) -- { return p1.timestamp < p2.timestamp; }; -- point_time_neq = [](boost::range::index_value p1, -- boost::range::index_value p2) -- { return p1.value().timestamp != p2.value().timestamp; }; -- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) -- { return pt.value().timestamp; }; -+ point_time_cmp = [](const PointType& p1, const PointType& p2) { -+ return p1.timestamp < p2.timestamp; -+ }; -+ point_time_neq = []( -+ boost::range::index_value p1, -+ boost::range::index_value p2 -+ ) { return p1.value().timestamp != p2.value().timestamp; }; -+ extract_point_time = [&sweep_ref_time]( -+ boost::range::index_value pt -+ ) { return pt.value().timestamp; }; - - } else if (this->sensor == dlio::SensorType::LIVOX) { -- point_time_cmp = [](const PointType& p1, const PointType& p2) -- { return p1.timestamp < p2.timestamp; }; -- point_time_neq = [](boost::range::index_value p1, -- boost::range::index_value p2) -- { return p1.value().timestamp != p2.value().timestamp; }; -- extract_point_time = [&sweep_ref_time](boost::range::index_value pt) -- { return pt.value().timestamp * 1e-9f; }; -+ point_time_cmp = [](const PointType& p1, const PointType& p2) { -+ return p1.timestamp < p2.timestamp; -+ }; -+ point_time_neq = []( -+ boost::range::index_value p1, -+ boost::range::index_value p2 -+ ) { return p1.value().timestamp != p2.value().timestamp; }; -+ extract_point_time = [&sweep_ref_time]( -+ boost::range::index_value pt -+ ) { return pt.value().timestamp * 1e-9f; }; - } - - // copy points into deskewed_scan_ in order of timestamp -- std::partial_sort_copy(this->original_scan->points.begin(), this->original_scan->points.end(), -- deskewed_scan_->points.begin(), deskewed_scan_->points.end(), point_time_cmp); -+ std::partial_sort_copy( -+ this->original_scan->points.begin(), -+ this->original_scan->points.end(), -+ deskewed_scan_->points.begin(), -+ deskewed_scan_->points.end(), -+ point_time_cmp -+ ); - - // filter unique timestamps - auto points_unique_timestamps = deskewed_scan_->points -- | boost::adaptors::indexed() -- | boost::adaptors::adjacent_filtered(point_time_neq); -+ | boost::adaptors::indexed() -+ | boost::adaptors::adjacent_filtered(point_time_neq); - - // extract timestamps from points and put them in their own list - std::vector timestamps; -@@ -664,46 +443,68 @@ void dlio::OdomNode::deskewPointcloud() { - // compute offset between sweep reference time and first point timestamp - double offset = 0.0; - if (this->time_offset_) { -- offset = sweep_ref_time - extract_point_time(*points_unique_timestamps.begin()); -+ offset = -+ sweep_ref_time - extract_point_time(*points_unique_timestamps.begin()); - } - - // build list of unique timestamps and indices of first point with each timestamp -- for (auto it = points_unique_timestamps.begin(); it != points_unique_timestamps.end(); it++) { -- timestamps.push_back(extract_point_time(*it) + offset); -- unique_time_indices.push_back(it->index()); -+ for (const auto& indexed_pt : points_unique_timestamps) { -+ timestamps.push_back(extract_point_time(indexed_pt) + offset); -+ unique_time_indices.push_back(indexed_pt.index()); - } - unique_time_indices.push_back(deskewed_scan_->points.size()); - - int median_pt_index = timestamps.size() / 2; -- this->scan_stamp = timestamps[median_pt_index]; // set this->scan_stamp to the timestamp of the median point -+ this->scan_stamp = timestamps -+ [median_pt_index]; // set this->scan_stamp to the timestamp of the median point - - // don't process scans until IMU data is present - if (!this->first_valid_scan) { -- if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { -+ if (this->imu_buffer.empty() -+ || this->scan_stamp <= this->imu_buffer.back().stamp) { - return; - } - - this->first_valid_scan = true; - this->T_prior = this->T; // assume no motion for the first scan -- pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); -+ pcl::transformPointCloud( -+ *deskewed_scan_, -+ *deskewed_scan_, -+ this->T_prior * this->extrinsics.baselink2lidar_T -+ ); - this->deskewed_scan = deskewed_scan_; - this->deskew_status = true; - return; - } - - // IMU prior & deskewing for second scan onwards -- std::vector> frames; -- frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, -- this->geo.prev_vel.cast(), timestamps); -- this->deskew_size = frames.size(); // if integration successful, equal to timestamps.size() -+ std::vector> -+ frames; -+ frames = this->integrateImu( -+ this->prev_scan_stamp, -+ this->lidarPose.q, -+ this->lidarPose.p, -+ this->geo.prev_vel.cast(), -+ timestamps -+ ); -+ this->deskew_size = -+ frames.size(); // if integration successful, equal to timestamps.size() - +@@ -700,8 +428,12 @@ void dlio::OdomNode::deskewPointcloud() { // if there are no frames between the start and end of the sweep // that probably means that there's a sync issue if (frames.size() != timestamps.size()) { - ROS_FATAL("Bad time sync between LiDAR and IMU!"); -+ if (verbose) { +- ++ if(verbose) { + printf("FATAL: "); + printf("Bad time sync between LiDAR and IMU!"); + printf("\n"); + } - ++ this->T_prior = this->T; -- pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); -+ pcl::transformPointCloud( -+ *deskewed_scan_, -+ *deskewed_scan_, -+ this->T_prior * this->extrinsics.baselink2lidar_T -+ ); + pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); this->deskewed_scan = deskewed_scan_; - this->deskew_status = false; - return; -@@ -714,12 +515,11 @@ void dlio::OdomNode::deskewPointcloud() { - - #pragma omp parallel for num_threads(this->num_threads_) - for (int i = 0; i < timestamps.size(); i++) { -- - Eigen::Matrix4f T = frames[i] * this->extrinsics.baselink2lidar_T; - - // transform point to world frame -- for (int k = unique_time_indices[i]; k < unique_time_indices[i+1]; k++) { -- auto &pt = deskewed_scan_->points[k]; -+ for (int k = unique_time_indices[i]; k < unique_time_indices[i + 1]; k++) { -+ auto& pt = deskewed_scan_->points[k]; - pt.getVector4fMap()[3] = 1.; - pt.getVector4fMap() = T * pt.getVector4fMap(); - } -@@ -727,19 +527,21 @@ void dlio::OdomNode::deskewPointcloud() { - - this->deskewed_scan = deskewed_scan_; - this->deskew_status = true; -- - } - - void dlio::OdomNode::initializeInputTarget() { -- - this->prev_scan_stamp = this->scan_stamp; - - // keep history of keyframes -- this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); -+ this->keyframes.push_back( -+ std::make_pair( -+ std::make_pair(this->lidarPose.p, this->lidarPose.q), -+ this->current_scan -+ ) -+ ); - this->keyframe_timestamps.push_back(this->scan_header_stamp); - this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); - this->keyframe_transformations.push_back(this->T_corr); -- - } - - void dlio::OdomNode::setInputSource() { -@@ -748,27 +550,34 @@ void dlio::OdomNode::setInputSource() { - } - - void dlio::OdomNode::initializeDLIO() { -- - // Wait for IMU - if (!this->first_imu_received || !this->imu_calibrated) { - return; +@@ -755,20 +487,25 @@ void dlio::OdomNode::initializeDLIO() { } this->dlio_initialized = true; - std::cout << std::endl << " DLIO initialized!" << std::endl; -- + if (verbose) { + std::cout << std::endl << " DLIO initialized!" << std::endl; + } + } -void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) { -- -- std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); +void dlio::OdomNode::callbackPointCloud( -+ const pcl::PointCloud::ConstPtr& pc, -+ double stamp -+) { -+ std::unique_lockmain_loop_running_mutex)> lock( -+ main_loop_running_mutex -+ ); ++ const pcl::PointCloud::ConstPtr &pc, double stamp) { + + std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); this->main_loop_running = true; lock.unlock(); - double then = ros::Time::now().toSec(); + double then = std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch() -+ ) -+ .count(); ++ std::chrono::system_clock::now().time_since_epoch()) ++ .count(); if (this->first_scan_stamp == 0.) { - this->first_scan_stamp = pc->header.stamp.toSec(); @@ -1446,7 +1050,7 @@ index bceae37..307ad52 100644 } // DLIO Initialization procedures (IMU calib, gravity align) -@@ -777,7 +586,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -777,7 +514,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } // Convert incoming scan into DLIO format @@ -1455,7 +1059,7 @@ index bceae37..307ad52 100644 // Preprocess points this->preprocessPoints(); -@@ -787,13 +596,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& +@@ -787,13 +524,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& } if (this->current_scan->points.size() <= this->gicp_min_num_points_) { @@ -1476,46 +1080,7 @@ index bceae37..307ad52 100644 // Set Adaptive Parameters if (this->adaptive_params_) { -@@ -807,8 +619,12 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - if (this->keyframes.size() == 0) { - this->initializeInputTarget(); - this->main_loop_running = false; -- this->submap_future = -- std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); -+ this->submap_future = std::async( -+ std::launch::async, -+ &dlio::OdomNode::buildKeyframesAndSubmap, -+ this, -+ this->state -+ ); - this->submap_future.wait(); // wait until completion - return; - } -@@ -822,8 +638,12 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - // Build keyframe normals and submap if needed (and if we're not already waiting) - if (this->new_submap_is_ready) { - this->main_loop_running = false; -- this->submap_future = -- std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); -+ this->submap_future = std::async( -+ std::launch::async, -+ &dlio::OdomNode::buildKeyframesAndSubmap, -+ this, -+ this->state -+ ); - } else { - lock.lock(); - this->main_loop_running = false; -@@ -832,69 +652,61 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - } - - // Update trajectory -- this->trajectory.push_back( std::make_pair(this->state.p, this->state.q) ); -+ this->trajectory.push_back(std::make_pair(this->state.p, this->state.q)); - - // Update time stamps -- this->lidar_rates.push_back( 1. / (this->scan_stamp - this->prev_scan_stamp) ); -+ this->lidar_rates.push_back(1. / (this->scan_stamp - this->prev_scan_stamp)); +@@ -839,50 +579,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& this->prev_scan_stamp = this->scan_stamp; this->elapsed_time = this->scan_stamp - this->first_scan_stamp; @@ -1532,12 +1097,10 @@ index bceae37..307ad52 100644 // Update some statistics - this->comp_times.push_back(ros::Time::now().toSec() - then); + this->comp_times.push_back( -+ std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch() -+ ) -+ .count() -+ - then -+ ); ++ std::chrono::duration_cast>( ++ std::chrono::system_clock::now().time_since_epoch()) ++ .count() - ++ then); this->gicp_hasConverged = this->gicp.hasConverged(); // Debug statements and publish custom DLIO message @@ -1546,14 +1109,13 @@ index bceae37..307ad52 100644 - this->debug_thread.detach(); + this->debug(); } -- -+ + this->geo.first_opt_done = true; } -void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -- -+void dlio::OdomNode::callbackImu(const ImuMeas& imu_raw) { ++void dlio::OdomNode::callbackImu(const ImuMeas &imu_raw) { + this->first_imu_received = true; - sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw ); @@ -1585,102 +1147,55 @@ index bceae37..307ad52 100644 } // IMU calibration procedure - do for three seconds - if (!this->imu_calibrated) { -- - static int num_samples = 0; -- static Eigen::Vector3f gyro_avg (0., 0., 0.); -- static Eigen::Vector3f accel_avg (0., 0., 0.); -+ static Eigen::Vector3f gyro_avg(0., 0., 0.); -+ static Eigen::Vector3f accel_avg(0., 0., 0.); +@@ -893,7 +626,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + static Eigen::Vector3f accel_avg (0., 0., 0.); static bool print = true; - if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) { -- + if ((imu.stamp - this->first_imu_stamp) < this->imu_calib_time_) { + num_samples++; - gyro_avg[0] += ang_vel[0]; -@@ -905,92 +717,113 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { +@@ -905,7 +638,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { accel_avg[1] += lin_accel[1]; accel_avg[2] += lin_accel[2]; - if(print) { -- std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; -+ if (verbose) { -+ std::cout << std::endl -+ << " Calibrating IMU for " << this->imu_calib_time_ -+ << " seconds... "; ++ if(verbose) { + std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; std::cout.flush(); print = false; - } +@@ -913,7 +646,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { } else { -- + - std::cout << "done" << std::endl << std::endl; -+ if (verbose) { ++ if(verbose) { + std::cout << "done" << std::endl << std::endl; + } gyro_avg /= num_samples; accel_avg /= num_samples; - -- Eigen::Vector3f grav_vec (0., 0., this->gravity_); -+ Eigen::Vector3f grav_vec(0., 0., this->gravity_); - - if (this->gravity_align_) { -- - // Estimate gravity vector - Only approximate if biases have not been pre-calibrated -- grav_vec = (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_); -- Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(grav_vec, Eigen::Vector3f(0., 0., this->gravity_)); -+ grav_vec = -+ (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_); -+ Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors( -+ grav_vec, -+ Eigen::Vector3f(0., 0., this->gravity_) -+ ); - - // set gravity aligned orientation - this->state.q = grav_q; -- this->T.block(0,0,3,3) = this->state.q.toRotationMatrix(); -+ this->T.block(0, 0, 3, 3) = this->state.q.toRotationMatrix(); - this->lidarPose.q = this->state.q; - - // rpy - auto euler = grav_q.toRotationMatrix().eulerAngles(2, 1, 0); -- double yaw = euler[0] * (180.0/M_PI); -- double pitch = euler[1] * (180.0/M_PI); -- double roll = euler[2] * (180.0/M_PI); -+ double yaw = euler[0] * (180.0 / M_PI); -+ double pitch = euler[1] * (180.0 / M_PI); -+ double roll = euler[2] * (180.0 / M_PI); - - // use alternate representation if the yaw is smaller - if (abs(remainder(yaw + 180.0, 360.0)) < abs(yaw)) { -- yaw = remainder(yaw + 180.0, 360.0); -+ yaw = remainder(yaw + 180.0, 360.0); +@@ -943,44 +678,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { pitch = remainder(180.0 - pitch, 360.0); -- roll = remainder(roll + 180.0, 360.0); -+ roll = remainder(roll + 180.0, 360.0); -+ } -+ if (verbose) { -+ std::cout << " Estimated initial attitude:" << std::endl; -+ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) -+ << std::endl; -+ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) -+ << std::endl; -+ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) -+ << std::endl; -+ std::cout << std::endl; + roll = remainder(roll + 180.0, 360.0); } - std::cout << " Estimated initial attitude:" << std::endl; - std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; - std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; - std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; - std::cout << std::endl; ++ if (verbose){ ++ std::cout << " Estimated initial attitude:" << std::endl; ++ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; ++ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; ++ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; ++ std::cout << std::endl; ++ } } if (this->calibrate_accel_) { -- + // subtract gravity from avg accel to get bias this->state.b.accel = accel_avg - grav_vec; - @@ -1688,31 +1203,23 @@ index bceae37..307ad52 100644 - << to_string_with_precision(this->state.b.accel[1], 8) << ", " - << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; + if (verbose) { -+ std::cout << " Accel biases [xyz]: " -+ << to_string_with_precision(this->state.b.accel[0], 8) -+ << ", " -+ << to_string_with_precision(this->state.b.accel[1], 8) -+ << ", " -+ << to_string_with_precision(this->state.b.accel[2], 8) -+ << std::endl; ++ std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " ++ << to_string_with_precision(this->state.b.accel[1], 8) << ", " ++ << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; + } } if (this->calibrate_gyro_) { -- + this->state.b.gyro = gyro_avg; - - std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " - << to_string_with_precision(this->state.b.gyro[1], 8) << ", " - << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; -+ if (verbose) { -+ std::cout << " Gyro biases [xyz]: " -+ << to_string_with_precision(this->state.b.gyro[0], 8) -+ << ", " -+ << to_string_with_precision(this->state.b.gyro[1], 8) -+ << ", " -+ << to_string_with_precision(this->state.b.gyro[2], 8) -+ << std::endl; ++ if(verbose) { ++ std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " ++ << to_string_with_precision(this->state.b.gyro[1], 8) << ", " ++ << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; + } } @@ -1721,15 +1228,11 @@ index bceae37..307ad52 100644 } } else { -- + - double dt = imu->header.stamp.toSec() - this->prev_imu_stamp; -- if (dt == 0) { dt = 1.0/200.0; } -- this->imu_rates.push_back( 1./dt ); + double dt = imu.stamp - this->prev_imu_stamp; -+ if (dt == 0) { -+ dt = 1.0 / 200.0; -+ } -+ this->imu_rates.push_back(1. / dt); + if (dt == 0) { dt = 1.0/200.0; } + this->imu_rates.push_back( 1./dt ); // Apply the calibrated bias to the new IMU measurements - this->imu_meas.stamp = imu->header.stamp.toSec(); @@ -1737,11 +1240,7 @@ index bceae37..307ad52 100644 this->imu_meas.dt = dt; this->prev_imu_stamp = this->imu_meas.stamp; -- Eigen::Vector3f lin_accel_corrected = (this->imu_accel_sm_ * lin_accel) - this->state.b.accel; -+ Eigen::Vector3f lin_accel_corrected = -+ (this->imu_accel_sm_ * lin_accel) - this->state.b.accel; - Eigen::Vector3f ang_vel_corrected = ang_vel - this->state.b.gyro; - +@@ -990,7 +728,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { this->imu_meas.lin_accel = lin_accel_corrected; this->imu_meas.ang_vel = ang_vel_corrected; @@ -1751,62 +1250,20 @@ index bceae37..307ad52 100644 this->mtx_imu.lock(); this->imu_buffer.push_front(this->imu_meas); this->mtx_imu.unlock(); -@@ -1002,18 +835,16 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - // Geometric Observer: Propagate State - this->propagateState(); - } -- - } -- - } - - void dlio::OdomNode::getNextPose() { -- - // Check if the new submap is ready to be used -- this->new_submap_is_ready = (this->submap_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready); -+ this->new_submap_is_ready = -+ (this->submap_future.wait_for(std::chrono::seconds(0)) -+ == std::future_status::ready); - - if (this->new_submap_is_ready && this->submap_hasChanged) { -- - // Set the current global submap as the target cloud - this->gicp.registerInputTarget(this->submap_cloud); - -@@ -1027,11 +858,14 @@ void dlio::OdomNode::getNextPose() { +@@ -1027,7 +766,7 @@ void dlio::OdomNode::getNextPose() { } // Align with current submap with global IMU transformation as initial guess - pcl::PointCloud::Ptr aligned (boost::make_shared>()); -+ pcl::PointCloud::Ptr aligned( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr aligned (std::make_shared>()); this->gicp.align(*aligned); // Get final transformation in global frame -- this->T_corr = this->gicp.getFinalTransformation(); // "correction" transformation -+ this->T_corr = -+ this->gicp.getFinalTransformation(); // "correction" transformation - this->T = this->T_corr * this->T_prior; - - // Update next global pose -@@ -1040,17 +874,20 @@ void dlio::OdomNode::getNextPose() { - - // Geometric observer update - this->updateState(); -- - } - --bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, -- boost::circular_buffer::reverse_iterator& begin_imu_it, -- boost::circular_buffer::reverse_iterator& end_imu_it) { +@@ -1046,11 +785,12 @@ void dlio::OdomNode::getNextPose() { + bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, + boost::circular_buffer::reverse_iterator& begin_imu_it, + boost::circular_buffer::reverse_iterator& end_imu_it) { - -+bool dlio::OdomNode::imuMeasFromTimeRange( -+ double start_time, -+ double end_time, -+ boost::circular_buffer::reverse_iterator& begin_imu_it, -+ boost::circular_buffer::reverse_iterator& end_imu_it -+) { + // Evalio run deterministically, nothing will ever appear here! if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { - // Wait for the latest IMU data @@ -1819,368 +1276,22 @@ index bceae37..307ad52 100644 } auto imu_it = this->imu_buffer.begin(); -@@ -1080,10 +917,15 @@ bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, - } - - std::vector> --dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen::Vector3f p_init, -- Eigen::Vector3f v_init, const std::vector& sorted_timestamps) { -- -- const std::vector> empty; -+dlio::OdomNode::integrateImu( -+ double start_time, -+ Eigen::Quaternionf q_init, -+ Eigen::Vector3f p_init, -+ Eigen::Vector3f v_init, -+ const std::vector& sorted_timestamps -+) { -+ const std::vector> -+ empty; - - if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) { - // invalid input, return empty vector -@@ -1092,14 +934,20 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen - - boost::circular_buffer::reverse_iterator begin_imu_it; - boost::circular_buffer::reverse_iterator end_imu_it; -- if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(), begin_imu_it, end_imu_it) == false) { -+ if (this->imuMeasFromTimeRange( -+ start_time, -+ sorted_timestamps.back(), -+ begin_imu_it, -+ end_imu_it -+ ) -+ == false) { - // not enough IMU measurements, return empty vector - return empty; - } - - // Backwards integration to find pose at first IMU sample - const ImuMeas& f1 = *begin_imu_it; -- const ImuMeas& f2 = *(begin_imu_it+1); -+ const ImuMeas& f2 = *(begin_imu_it + 1); - - // Time between first two IMU samples - double dt = f2.dt; -@@ -1112,26 +960,50 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen - Eigen::Vector3f alpha = alpha_dt / dt; - - // Average angular velocity (reversed) between first IMU sample and start_time -- Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5*alpha*idt); -+ Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5 * alpha * idt); - - // Set q_init to orientation at first IMU sample -- q_init = Eigen::Quaternionf ( -- q_init.w() - 0.5*( q_init.x()*omega_i[0] + q_init.y()*omega_i[1] + q_init.z()*omega_i[2] ) * idt, -- q_init.x() + 0.5*( q_init.w()*omega_i[0] - q_init.z()*omega_i[1] + q_init.y()*omega_i[2] ) * idt, -- q_init.y() + 0.5*( q_init.z()*omega_i[0] + q_init.w()*omega_i[1] - q_init.x()*omega_i[2] ) * idt, -- q_init.z() + 0.5*( q_init.x()*omega_i[1] - q_init.y()*omega_i[0] + q_init.w()*omega_i[2] ) * idt -+ q_init = Eigen::Quaternionf( -+ q_init.w() -+ - 0.5 -+ * (q_init.x() * omega_i[0] + q_init.y() * omega_i[1] + q_init.z() * omega_i[2]) -+ * idt, -+ q_init.x() -+ + 0.5 -+ * (q_init.w() * omega_i[0] - q_init.z() * omega_i[1] + q_init.y() * omega_i[2]) -+ * idt, -+ q_init.y() -+ + 0.5 -+ * (q_init.z() * omega_i[0] + q_init.w() * omega_i[1] - q_init.x() * omega_i[2]) -+ * idt, -+ q_init.z() -+ + 0.5 -+ * (q_init.x() * omega_i[1] - q_init.y() * omega_i[0] + q_init.w() * omega_i[2]) -+ * idt - ); - q_init.normalize(); - - // Average angular velocity between first two IMU samples -- Eigen::Vector3f omega = f1.ang_vel + 0.5*alpha_dt; -+ Eigen::Vector3f omega = f1.ang_vel + 0.5 * alpha_dt; - - // Orientation at second IMU sample -- Eigen::Quaternionf q2 ( -- q_init.w() - 0.5*( q_init.x()*omega[0] + q_init.y()*omega[1] + q_init.z()*omega[2] ) * dt, -- q_init.x() + 0.5*( q_init.w()*omega[0] - q_init.z()*omega[1] + q_init.y()*omega[2] ) * dt, -- q_init.y() + 0.5*( q_init.z()*omega[0] + q_init.w()*omega[1] - q_init.x()*omega[2] ) * dt, -- q_init.z() + 0.5*( q_init.x()*omega[1] - q_init.y()*omega[0] + q_init.w()*omega[2] ) * dt -+ Eigen::Quaternionf q2( -+ q_init.w() -+ - 0.5 -+ * (q_init.x() * omega[0] + q_init.y() * omega[1] + q_init.z() * omega[2]) -+ * dt, -+ q_init.x() -+ + 0.5 -+ * (q_init.w() * omega[0] - q_init.z() * omega[1] + q_init.y() * omega[2]) -+ * dt, -+ q_init.y() -+ + 0.5 -+ * (q_init.z() * omega[0] + q_init.w() * omega[1] - q_init.x() * omega[2]) -+ * dt, -+ q_init.z() -+ + 0.5 -+ * (q_init.x() * omega[1] - q_init.y() * omega[0] + q_init.w() * omega[2]) -+ * dt - ); - q2.normalize(); - -@@ -1147,21 +1019,33 @@ dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen - Eigen::Vector3f j = (a2 - a1) / dt; - - // Set v_init to velocity at first IMU sample (go backwards from start_time) -- v_init -= a1*idt + 0.5*j*idt*idt; -+ v_init -= a1 * idt + 0.5 * j * idt * idt; - - // Set p_init to position at first IMU sample (go backwards from start_time) -- p_init -= v_init*idt + 0.5*a1*idt*idt + (1/6.)*j*idt*idt*idt; -- -- return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps, begin_imu_it, end_imu_it); -+ p_init -= -+ v_init * idt + 0.5 * a1 * idt * idt + (1 / 6.) * j * idt * idt * idt; -+ -+ return this->integrateImuInternal( -+ q_init, -+ p_init, -+ v_init, -+ sorted_timestamps, -+ begin_imu_it, -+ end_imu_it -+ ); - } - - std::vector> --dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init, -- const std::vector& sorted_timestamps, -- boost::circular_buffer::reverse_iterator begin_imu_it, -- boost::circular_buffer::reverse_iterator end_imu_it) { -- -- std::vector> imu_se3; -+dlio::OdomNode::integrateImuInternal( -+ Eigen::Quaternionf q_init, -+ Eigen::Vector3f p_init, -+ Eigen::Vector3f v_init, -+ const std::vector& sorted_timestamps, -+ boost::circular_buffer::reverse_iterator begin_imu_it, -+ boost::circular_buffer::reverse_iterator end_imu_it -+) { -+ std::vector> -+ imu_se3; - - // Initialization - Eigen::Quaternionf q = q_init; -@@ -1177,7 +1061,6 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f - auto stamp_it = sorted_timestamps.begin(); - - for (; imu_it != end_imu_it; imu_it++) { -- - const ImuMeas& f0 = *prev_imu_it; - const ImuMeas& f = *imu_it; - -@@ -1189,14 +1072,18 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f - Eigen::Vector3f alpha = alpha_dt / dt; - - // Average angular velocity -- Eigen::Vector3f omega = f0.ang_vel + 0.5*alpha_dt; -+ Eigen::Vector3f omega = f0.ang_vel + 0.5 * alpha_dt; - - // Orientation -- q = Eigen::Quaternionf ( -- q.w() - 0.5*( q.x()*omega[0] + q.y()*omega[1] + q.z()*omega[2] ) * dt, -- q.x() + 0.5*( q.w()*omega[0] - q.z()*omega[1] + q.y()*omega[2] ) * dt, -- q.y() + 0.5*( q.z()*omega[0] + q.w()*omega[1] - q.x()*omega[2] ) * dt, -- q.z() + 0.5*( q.x()*omega[1] - q.y()*omega[0] + q.w()*omega[2] ) * dt -+ q = Eigen::Quaternionf( -+ q.w() -+ - 0.5 * (q.x() * omega[0] + q.y() * omega[1] + q.z() * omega[2]) * dt, -+ q.x() -+ + 0.5 * (q.w() * omega[0] - q.z() * omega[1] + q.y() * omega[2]) * dt, -+ q.y() -+ + 0.5 * (q.z() * omega[0] + q.w() * omega[1] - q.x() * omega[2]) * dt, -+ q.z() -+ + 0.5 * (q.x() * omega[1] - q.y() * omega[0] + q.w() * omega[2]) * dt - ); - q.normalize(); - -@@ -1215,19 +1102,28 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f - double idt = *stamp_it - f0.stamp; - - // Average angular velocity -- Eigen::Vector3f omega_i = f0.ang_vel + 0.5*alpha*idt; -+ Eigen::Vector3f omega_i = f0.ang_vel + 0.5 * alpha * idt; - - // Orientation -- Eigen::Quaternionf q_i ( -- q.w() - 0.5*( q.x()*omega_i[0] + q.y()*omega_i[1] + q.z()*omega_i[2] ) * idt, -- q.x() + 0.5*( q.w()*omega_i[0] - q.z()*omega_i[1] + q.y()*omega_i[2] ) * idt, -- q.y() + 0.5*( q.z()*omega_i[0] + q.w()*omega_i[1] - q.x()*omega_i[2] ) * idt, -- q.z() + 0.5*( q.x()*omega_i[1] - q.y()*omega_i[0] + q.w()*omega_i[2] ) * idt -+ Eigen::Quaternionf q_i( -+ q.w() -+ - 0.5 * (q.x() * omega_i[0] + q.y() * omega_i[1] + q.z() * omega_i[2]) -+ * idt, -+ q.x() -+ + 0.5 * (q.w() * omega_i[0] - q.z() * omega_i[1] + q.y() * omega_i[2]) -+ * idt, -+ q.y() -+ + 0.5 * (q.z() * omega_i[0] + q.w() * omega_i[1] - q.x() * omega_i[2]) -+ * idt, -+ q.z() -+ + 0.5 * (q.x() * omega_i[1] - q.y() * omega_i[0] + q.w() * omega_i[2]) -+ * idt - ); - q_i.normalize(); - - // Position -- Eigen::Vector3f p_i = p + v*idt + 0.5*a0*idt*idt + (1/6.)*j*idt*idt*idt; -+ Eigen::Vector3f p_i = -+ p + v * idt + 0.5 * a0 * idt * idt + (1 / 6.) * j * idt * idt * idt; - - // Transformation - Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); -@@ -1240,41 +1136,39 @@ dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f - } - - // Position -- p += v*dt + 0.5*a0*dt*dt + (1/6.)*j_dt*dt*dt; -+ p += v * dt + 0.5 * a0 * dt * dt + (1 / 6.) * j_dt * dt * dt; - - // Velocity -- v += a0*dt + 0.5*j_dt*dt; -+ v += a0 * dt + 0.5 * j_dt * dt; - - prev_imu_it = imu_it; -- - } - - return imu_se3; -- - } - - void dlio::OdomNode::propagateGICP() { -- -- this->lidarPose.p << this->T(0,3), this->T(1,3), this->T(2,3); -+ this->lidarPose.p << this->T(0, 3), this->T(1, 3), this->T(2, 3); - - Eigen::Matrix3f rotSO3; -- rotSO3 << this->T(0,0), this->T(0,1), this->T(0,2), -- this->T(1,0), this->T(1,1), this->T(1,2), -- this->T(2,0), this->T(2,1), this->T(2,2); -+ rotSO3 << this->T(0, 0), this->T(0, 1), this->T(0, 2), this->T(1, 0), -+ this->T(1, 1), this->T(1, 2), this->T(2, 0), this->T(2, 1), this->T(2, 2); - - Eigen::Quaternionf q(rotSO3); - - // Normalize quaternion -- double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); -- q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; -+ double norm = -+ sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); -+ q.w() /= norm; -+ q.x() /= norm; -+ q.y() /= norm; -+ q.z() /= norm; +@@ -1268,7 +1008,6 @@ void dlio::OdomNode::propagateGICP() { + double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); + q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; this->lidarPose.q = q; - } void dlio::OdomNode::propagateState() { -- - // Lock thread to prevent state from being accessed by UpdateState -- std::lock_guard lock( this->geo.mtx ); -+ std::lock_guard lock(this->geo.mtx); - - double dt = this->imu_meas.dt; - -@@ -1285,14 +1179,18 @@ void dlio::OdomNode::propagateState() { - world_accel = qhat._transformVector(this->imu_meas.lin_accel); - - // Accel propogation -- this->state.p[0] += this->state.v.lin.w[0]*dt + 0.5*dt*dt*world_accel[0]; -- this->state.p[1] += this->state.v.lin.w[1]*dt + 0.5*dt*dt*world_accel[1]; -- this->state.p[2] += this->state.v.lin.w[2]*dt + 0.5*dt*dt*(world_accel[2] - this->gravity_); -- -- this->state.v.lin.w[0] += world_accel[0]*dt; -- this->state.v.lin.w[1] += world_accel[1]*dt; -- this->state.v.lin.w[2] += (world_accel[2] - this->gravity_)*dt; -- this->state.v.lin.b = this->state.q.toRotationMatrix().inverse() * this->state.v.lin.w; -+ this->state.p[0] += -+ this->state.v.lin.w[0] * dt + 0.5 * dt * dt * world_accel[0]; -+ this->state.p[1] += -+ this->state.v.lin.w[1] * dt + 0.5 * dt * dt * world_accel[1]; -+ this->state.p[2] += this->state.v.lin.w[2] * dt -+ + 0.5 * dt * dt * (world_accel[2] - this->gravity_); -+ -+ this->state.v.lin.w[0] += world_accel[0] * dt; -+ this->state.v.lin.w[1] += world_accel[1] * dt; -+ this->state.v.lin.w[2] += (world_accel[2] - this->gravity_) * dt; -+ this->state.v.lin.b = -+ this->state.q.toRotationMatrix().inverse() * this->state.v.lin.w; +@@ -1369,36 +1108,36 @@ void dlio::OdomNode::updateState() { - // Gyro propogation - omega.w() = 0; -@@ -1306,13 +1204,11 @@ void dlio::OdomNode::propagateState() { - - this->state.v.ang.b = this->imu_meas.ang_vel; - this->state.v.ang.w = this->state.q.toRotationMatrix() * this->state.v.ang.b; -- - } - - void dlio::OdomNode::updateState() { -- - // Lock thread to prevent state from being accessed by PropagateState -- std::lock_guard lock( this->geo.mtx ); -+ std::lock_guard lock(this->geo.mtx); - - Eigen::Vector3f pin = this->lidarPose.p; - Eigen::Quaternionf qin = this->lidarPose.q; -@@ -1322,7 +1218,7 @@ void dlio::OdomNode::updateState() { - qhat = this->state.q; - - // Constuct error quaternion -- qe = qhat.conjugate()*qin; -+ qe = qhat.conjugate() * qin; - - double sgn = 1.; - if (qe.w() < 0) { -@@ -1331,7 +1227,7 @@ void dlio::OdomNode::updateState() { - - // Construct quaternion correction - qcorr.w() = 1 - abs(qe.w()); -- qcorr.vec() = sgn*qe.vec(); -+ qcorr.vec() = sgn * qe.vec(); - qcorr = qhat * qcorr; - - Eigen::Vector3f err = pin - this->state.p; -@@ -1344,13 +1240,15 @@ void dlio::OdomNode::updateState() { - - // Update accel bias - this->state.b.accel -= dt * this->geo_Kab_ * err_body; -- this->state.b.accel = this->state.b.accel.array().min(abias_max).max(-abias_max); -+ this->state.b.accel = -+ this->state.b.accel.array().min(abias_max).max(-abias_max); - - // Update gyro bias - this->state.b.gyro[0] -= dt * this->geo_Kgb_ * qe.w() * qe.x(); - this->state.b.gyro[1] -= dt * this->geo_Kgb_ * qe.w() * qe.y(); - this->state.b.gyro[2] -= dt * this->geo_Kgb_ * qe.w() * qe.z(); -- this->state.b.gyro = this->state.b.gyro.array().min(gbias_max).max(-gbias_max); -+ this->state.b.gyro = -+ this->state.b.gyro.array().min(gbias_max).max(-gbias_max); - - // Update state - this->state.p += dt * this->geo_Kp_ * err; -@@ -1366,54 +1264,60 @@ void dlio::OdomNode::updateState() { - this->geo.prev_p = this->state.p; - this->geo.prev_q = this->state.q; - this->geo.prev_vel = this->state.v.lin.w; -- } -sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -- ++dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas &imu_raw) { + - sensor_msgs::Imu::Ptr imu (new sensor_msgs::Imu); -+dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas& imu_raw) { + ImuMeas imu; // Copy header @@ -2190,27 +1301,20 @@ index bceae37..307ad52 100644 - static double prev_stamp = imu->header.stamp.toSec(); - double dt = imu->header.stamp.toSec() - prev_stamp; - prev_stamp = imu->header.stamp.toSec(); -- -- if (dt == 0) { dt = 1.0/200.0; } + static double prev_stamp = imu.stamp; + double dt = imu.stamp - prev_stamp; + prev_stamp = imu.stamp; + + if (dt == 0) { dt = 1.0/200.0; } - // Transform angular velocity (will be the same on a rigid body, so just rotate to ROS convention) - Eigen::Vector3f ang_vel(imu_raw->angular_velocity.x, - imu_raw->angular_velocity.y, - imu_raw->angular_velocity.z); -+ if (dt == 0) { -+ dt = 1.0 / 200.0; -+ } -+ + // Transform angular velocity (will be the same on a rigid body, so just + // rotate to ROS convention) -+ Eigen::Vector3f ang_vel( -+ imu_raw.ang_vel[0], -+ imu_raw.ang_vel[1], -+ imu_raw.ang_vel[2] -+ ); ++ Eigen::Vector3f ang_vel(imu_raw.ang_vel[0], imu_raw.ang_vel[1], ++ imu_raw.ang_vel[2]); Eigen::Vector3f ang_vel_cg = this->extrinsics.baselink2imu.R * ang_vel; @@ -2229,20 +1333,12 @@ index bceae37..307ad52 100644 - imu_raw->linear_acceleration.z); + // Transform linear acceleration (need to account for component due to + // translational difference) -+ Eigen::Vector3f lin_accel( -+ imu_raw.lin_accel[0], -+ imu_raw.lin_accel[1], -+ imu_raw.lin_accel[2] -+ ); ++ Eigen::Vector3f lin_accel(imu_raw.lin_accel[0], imu_raw.lin_accel[1], ++ imu_raw.lin_accel[2]); Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; - lin_accel_cg = lin_accel_cg -- + ((ang_vel_cg - ang_vel_cg_prev) / dt).cross(-this->extrinsics.baselink2imu.t) -- + ang_vel_cg.cross(ang_vel_cg.cross(-this->extrinsics.baselink2imu.t)); -+ + ((ang_vel_cg - ang_vel_cg_prev) / dt) -+ .cross(-this->extrinsics.baselink2imu.t) -+ + ang_vel_cg.cross(ang_vel_cg.cross(-this->extrinsics.baselink2imu.t)); +@@ -1408,9 +1147,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const ang_vel_cg_prev = ang_vel_cg; @@ -2254,442 +1350,70 @@ index bceae37..307ad52 100644 + imu.lin_accel[2] = lin_accel_cg[2]; return imu; -- - } - - void dlio::OdomNode::computeMetrics() { -@@ -1422,30 +1326,29 @@ void dlio::OdomNode::computeMetrics() { - } - - void dlio::OdomNode::computeSpaciousness() { -- - // compute range of points - std::vector ds; - - for (int i = 0; i < this->original_scan->points.size(); i++) { -- float d = std::sqrt(pow(this->original_scan->points[i].x, 2) + -- pow(this->original_scan->points[i].y, 2)); -+ float d = std::sqrt( -+ pow(this->original_scan->points[i].x, 2) -+ + pow(this->original_scan->points[i].y, 2) -+ ); - ds.push_back(d); - } - - // median -- std::nth_element(ds.begin(), ds.begin() + ds.size()/2, ds.end()); -- float median_curr = ds[ds.size()/2]; -+ std::nth_element(ds.begin(), ds.begin() + ds.size() / 2, ds.end()); -+ float median_curr = ds[ds.size() / 2]; - static float median_prev = median_curr; -- float median_lpf = 0.95*median_prev + 0.05*median_curr; -+ float median_lpf = 0.95 * median_prev + 0.05 * median_curr; - median_prev = median_lpf; - - // push -- this->metrics.spaciousness.push_back( median_lpf ); -- -+ this->metrics.spaciousness.push_back(median_lpf); - } - - void dlio::OdomNode::computeDensity() { -- - float density; - - if (!this->geo.first_opt_done) { -@@ -1455,23 +1358,22 @@ void dlio::OdomNode::computeDensity() { - } - static float density_prev = density; -- float density_lpf = 0.95*density_prev + 0.05*density; -+ float density_lpf = 0.95 * density_prev + 0.05 * density; - density_prev = density_lpf; - -- this->metrics.density.push_back( density_lpf ); -- -+ this->metrics.density.push_back(density_lpf); - } - - void dlio::OdomNode::computeConvexHull() { -- - // at least 4 keyframes for convex hull - if (this->num_processed_keyframes < 4) { - return; - } +@@ -1471,7 +1210,7 @@ void dlio::OdomNode::computeConvexHull() { // create a pointcloud with points at keyframes -- pcl::PointCloud::Ptr cloud = + pcl::PointCloud::Ptr cloud = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr (std::make_shared>()); std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1488,29 +1390,31 @@ void dlio::OdomNode::computeConvexHull() { +@@ -1488,10 +1227,10 @@ void dlio::OdomNode::computeConvexHull() { // get the indices of the keyframes on the convex hull pcl::PointCloud::Ptr convex_points = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr (std::make_shared>()); this->convex_hull.reconstruct(*convex_points); - pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr convex_hull_point_idx = -+ pcl::PointIndices::Ptr(std::make_shared()); ++ pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); this->convex_hull.getHullPointIndices(*convex_hull_point_idx); this->keyframe_convex.clear(); -- for (int i=0; iindices.size(); ++i) { -+ for (int i = 0; i < convex_hull_point_idx->indices.size(); ++i) { - this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]); - } -- - } - - void dlio::OdomNode::computeConcaveHull() { -- - // at least 5 keyframes for concave hull - if (this->num_processed_keyframes < 5) { - return; - } +@@ -1510,7 +1249,7 @@ void dlio::OdomNode::computeConcaveHull() { // create a pointcloud with points at keyframes -- pcl::PointCloud::Ptr cloud = + pcl::PointCloud::Ptr cloud = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr cloud = pcl::PointCloud::Ptr( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr (std::make_shared>()); std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1527,21 +1431,22 @@ void dlio::OdomNode::computeConcaveHull() { +@@ -1527,10 +1266,10 @@ void dlio::OdomNode::computeConcaveHull() { // get the indices of the keyframes on the concave hull pcl::PointCloud::Ptr concave_points = - pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr( -+ std::make_shared>() -+ ); ++ pcl::PointCloud::Ptr (std::make_shared>()); this->concave_hull.reconstruct(*concave_points); - pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr concave_hull_point_idx = -+ pcl::PointIndices::Ptr(std::make_shared()); ++ pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); this->concave_hull.getHullPointIndices(*concave_hull_point_idx); this->keyframe_concave.clear(); -- for (int i=0; iindices.size(); ++i) { -+ for (int i = 0; i < concave_hull_point_idx->indices.size(); ++i) { - this->keyframe_concave.push_back(concave_hull_point_idx->indices[i]); - } -- - } - - void dlio::OdomNode::updateKeyframes() { -- - // calculate difference in pose and rotation to all poses in trajectory - float closest_d = std::numeric_limits::infinity(); - int closest_idx = 0; -@@ -1550,14 +1455,15 @@ void dlio::OdomNode::updateKeyframes() { - int num_nearby = 0; - - for (const auto& k : this->keyframes) { -- - // calculate distance between current pose and pose in keyframes -- float delta_d = sqrt( pow(this->state.p[0] - k.first.first[0], 2) + -- pow(this->state.p[1] - k.first.first[1], 2) + -- pow(this->state.p[2] - k.first.first[2], 2) ); -+ float delta_d = sqrt( -+ pow(this->state.p[0] - k.first.first[0], 2) -+ + pow(this->state.p[1] - k.first.first[1], 2) -+ + pow(this->state.p[2] - k.first.first[2], 2) -+ ); - - // count the number nearby current pose -- if (delta_d <= this->keyframe_thresh_dist_ * 1.5){ -+ if (delta_d <= this->keyframe_thresh_dist_ * 1.5) { - ++num_nearby; - } - -@@ -1568,7 +1474,6 @@ void dlio::OdomNode::updateKeyframes() { - } - - keyframes_idx++; -- - } - - // get closest pose and corresponding rotation -@@ -1576,28 +1481,35 @@ void dlio::OdomNode::updateKeyframes() { - Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second; - - // calculate distance between current pose and closest pose from above -- float dd = sqrt( pow(this->state.p[0] - closest_pose[0], 2) + -- pow(this->state.p[1] - closest_pose[1], 2) + -- pow(this->state.p[2] - closest_pose[2], 2) ); -+ float dd = sqrt( -+ pow(this->state.p[0] - closest_pose[0], 2) -+ + pow(this->state.p[1] - closest_pose[1], 2) -+ + pow(this->state.p[2] - closest_pose[2], 2) -+ ); - - // calculate difference in orientation using SLERP - Eigen::Quaternionf dq; - - if (this->state.q.dot(closest_pose_r) < 0.) { - Eigen::Quaternionf lq = closest_pose_r; -- lq.w() *= -1.; lq.x() *= -1.; lq.y() *= -1.; lq.z() *= -1.; -+ lq.w() *= -1.; -+ lq.x() *= -1.; -+ lq.y() *= -1.; -+ lq.z() *= -1.; - dq = this->state.q * lq.inverse(); - } else { - dq = this->state.q * closest_pose_r.inverse(); - } - -- double theta_rad = 2. * atan2(sqrt( pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2) ), dq.w()); -- double theta_deg = theta_rad * (180.0/M_PI); -+ double theta_rad = -+ 2. * atan2(sqrt(pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2)), dq.w()); -+ double theta_deg = theta_rad * (180.0 / M_PI); - - // update keyframes - bool newKeyframe = false; - -- if (abs(dd) > this->keyframe_thresh_dist_ || abs(theta_deg) > this->keyframe_thresh_rot_) { -+ if (abs(dd) > this->keyframe_thresh_dist_ -+ || abs(theta_deg) > this->keyframe_thresh_rot_) { - newKeyframe = true; - } - -@@ -1605,54 +1517,74 @@ void dlio::OdomNode::updateKeyframes() { - newKeyframe = false; - } - -- if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { -+ if (abs(dd) <= this->keyframe_thresh_dist_ -+ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { - newKeyframe = true; - } - - if (newKeyframe) { -- - // update keyframe vector -- std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); -- this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); -+ std::unique_lockkeyframes_mutex)> lock( -+ this->keyframes_mutex -+ ); -+ this->keyframes.push_back( -+ std::make_pair( -+ std::make_pair(this->lidarPose.p, this->lidarPose.q), -+ this->current_scan -+ ) -+ ); - this->keyframe_timestamps.push_back(this->scan_header_stamp); - this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); - this->keyframe_transformations.push_back(this->T_corr); - lock.unlock(); -- - } -- - } - - void dlio::OdomNode::setAdaptiveParams() { -- - // Spaciousness - float sp = this->metrics.spaciousness.back(); - -- if (sp < 0.5) { sp = 0.5; } -- if (sp > 5.0) { sp = 5.0; } -+ if (sp < 0.5) { -+ sp = 0.5; -+ } -+ if (sp > 5.0) { -+ sp = 5.0; -+ } - - this->keyframe_thresh_dist_ = sp; - - // Density - float den = this->metrics.density.back(); - -- if (den < 0.5*this->gicp_max_corr_dist_) { den = 0.5*this->gicp_max_corr_dist_; } -- if (den > 2.0*this->gicp_max_corr_dist_) { den = 2.0*this->gicp_max_corr_dist_; } -+ if (den < 0.5 * this->gicp_max_corr_dist_) { -+ den = 0.5 * this->gicp_max_corr_dist_; -+ } -+ if (den > 2.0 * this->gicp_max_corr_dist_) { -+ den = 2.0 * this->gicp_max_corr_dist_; -+ } - -- if (sp < 5.0) { den = 0.5*this->gicp_max_corr_dist_; }; -- if (sp > 5.0) { den = 2.0*this->gicp_max_corr_dist_; }; -+ if (sp < 5.0) { -+ den = 0.5 * this->gicp_max_corr_dist_; -+ }; -+ if (sp > 5.0) { -+ den = 2.0 * this->gicp_max_corr_dist_; -+ }; - - this->gicp.setMaxCorrespondenceDistance(den); - - // Concave hull alpha - this->concave_hull.setAlpha(this->keyframe_thresh_dist_); -- - } - --void dlio::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vector frames) { -- -+void dlio::OdomNode::pushSubmapIndices( -+ std::vector dists, -+ int k, -+ std::vector frames -+) { - // make sure dists is not empty -- if (!dists.size()) { return; } -+ if (!dists.size()) { -+ return; -+ } - - // maintain max heap of at most k elements - std::priority_queue pq; -@@ -1671,14 +1603,13 @@ void dlio::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vec - - // get all elements smaller or equal to the kth smallest element - for (int i = 0; i < dists.size(); ++i) { -- if (dists[i] <= kth_element) -+ if (dists[i] <= kth_element) { - this->submap_kf_idx_curr.push_back(frames[i]); -+ } - } -- - } - - void dlio::OdomNode::buildSubmap(State vehicle_state) { -- - // clear vector of keyframe indices to use for submap - this->submap_kf_idx_curr.clear(); - -@@ -1687,9 +1618,11 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { - std::vector ds; - std::vector keyframe_nn; - for (int i = 0; i < this->num_processed_keyframes; i++) { -- float d = sqrt( pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) + -- pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) + -- pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) ); -+ float d = sqrt( -+ pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) -+ + pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) -+ + pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) -+ ); - ds.push_back(d); - keyframe_nn.push_back(i); - } -@@ -1720,38 +1653,47 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { - } - - // get indices for top kNN for concave hull -- this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); -+ this -+ ->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); - - // sort current and previous submap kf list of indices - std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); - std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end()); -- -+ - // remove duplicate indices -- auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); -+ auto last = std::unique( -+ this->submap_kf_idx_curr.begin(), -+ this->submap_kf_idx_curr.end() -+ ); - this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end()); - - // check if submap has changed from previous iteration -- if (this->submap_kf_idx_curr != this->submap_kf_idx_prev){ -- -+ if (this->submap_kf_idx_curr != this->submap_kf_idx_prev) { - this->submap_hasChanged = true; - - // Pause to prevent stealing resources from the main loop if it is running. +@@ -1739,7 +1478,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { this->pauseSubmapBuildIfNeeded(); // reinitialize submap cloud and normals - pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); -- std::shared_ptr submap_normals_ (std::make_shared()); -+ pcl::PointCloud::Ptr submap_cloud_( -+ std::make_shared>() -+ ); -+ std::shared_ptr submap_normals_( -+ std::make_shared() -+ ); ++ pcl::PointCloud::Ptr submap_cloud_ (std::make_shared>()); + std::shared_ptr submap_normals_ (std::make_shared()); for (auto k : this->submap_kf_idx_curr) { -- - // create current submap cloud - lock.lock(); - *submap_cloud_ += *this->keyframes[k].second; - lock.unlock(); - - // grab corresponding submap cloud's normals -- submap_normals_->insert( std::end(*submap_normals_), -- std::begin(*(this->keyframe_normals[k])), std::end(*(this->keyframe_normals[k])) ); -+ submap_normals_->insert( -+ std::end(*submap_normals_), -+ std::begin(*(this->keyframe_normals[k])), -+ std::end(*(this->keyframe_normals[k])) -+ ); - } - - this->submap_cloud = submap_cloud_; -@@ -1768,33 +1710,39 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { - } - - void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { -- - // transform the new keyframe(s) and associated covariance list(s) -- std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); -+ std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); - - for (int i = this->num_processed_keyframes; i < this->keyframes.size(); i++) { -- pcl::PointCloud::ConstPtr raw_keyframe = this->keyframes[i].second; -- std::shared_ptr raw_covariances = this->keyframe_normals[i]; -+ pcl::PointCloud::ConstPtr raw_keyframe = -+ this->keyframes[i].second; -+ std::shared_ptr raw_covariances = -+ this->keyframe_normals[i]; - Eigen::Matrix4f T = this->keyframe_transformations[i]; - lock.unlock(); +@@ -1780,7 +1519,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { Eigen::Matrix4d Td = T.cast(); - pcl::PointCloud::Ptr transformed_keyframe (boost::make_shared>()); -- pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); -+ pcl::PointCloud::Ptr transformed_keyframe( -+ std::make_shared>() -+ ); -+ pcl::transformPointCloud(*raw_keyframe, *transformed_keyframe, T); - -- std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); -- std::transform(raw_covariances->begin(), raw_covariances->end(), transformed_covariances->begin(), -- [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); }); -+ std::shared_ptr transformed_covariances( -+ std::make_shared(raw_covariances->size()) -+ ); -+ std::transform( -+ raw_covariances->begin(), -+ raw_covariances->end(), -+ transformed_covariances->begin(), -+ [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); } -+ ); - - ++this->num_processed_keyframes; ++ pcl::PointCloud::Ptr transformed_keyframe (std::make_shared>()); + pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); + std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); +@@ -1792,9 +1531,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { lock.lock(); this->keyframes[i].second = transformed_keyframe; this->keyframe_normals[i] = transformed_covariances; @@ -2699,398 +1423,27 @@ index bceae37..307ad52 100644 } lock.unlock(); -@@ -1806,12 +1754,13 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { - } - - void dlio::OdomNode::pauseSubmapBuildIfNeeded() { -- std::unique_lockmain_loop_running_mutex)> lock(this->main_loop_running_mutex); -- this->submap_build_cv.wait(lock, [this]{ return !this->main_loop_running; }); -+ std::unique_lockmain_loop_running_mutex)> lock( -+ this->main_loop_running_mutex -+ ); -+ this->submap_build_cv.wait(lock, [this] { return !this->main_loop_running; }); - } - - void dlio::OdomNode::debug() { -- - // Total length traversed - double length_traversed = 0.; - Eigen::Vector3f p_curr = Eigen::Vector3f(0., 0., 0.); -@@ -1822,7 +1771,10 @@ void dlio::OdomNode::debug() { - continue; - } - p_curr = t.first; -- double l = sqrt(pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) + pow(p_curr[2] - p_prev[2], 2)); -+ double l = sqrt( -+ pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) -+ + pow(p_curr[2] - p_prev[2], 2) -+ ); - - if (l >= 0.1) { - length_traversed += l; -@@ -1833,7 +1785,8 @@ void dlio::OdomNode::debug() { - - // Average computation time - double avg_comp_time = -- std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) / this->comp_times.size(); -+ std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) -+ / this->comp_times.size(); - - // Average sensor rates - int win_size = 100; -@@ -1841,23 +1794,36 @@ void dlio::OdomNode::debug() { - double avg_lidar_rate; - if (this->imu_rates.size() < win_size) { - avg_imu_rate = -- std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) / this->imu_rates.size(); -+ std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) -+ / this->imu_rates.size(); - } else { -- avg_imu_rate = -- std::accumulate(this->imu_rates.end()-win_size, this->imu_rates.end(), 0.0) / win_size; -+ avg_imu_rate = std::accumulate( -+ this->imu_rates.end() - win_size, -+ this->imu_rates.end(), -+ 0.0 -+ ) -+ / win_size; - } - if (this->lidar_rates.size() < win_size) { - avg_lidar_rate = -- std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) / this->lidar_rates.size(); -+ std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) -+ / this->lidar_rates.size(); - } else { -- avg_lidar_rate = -- std::accumulate(this->lidar_rates.end()-win_size, this->lidar_rates.end(), 0.0) / win_size; -+ avg_lidar_rate = std::accumulate( -+ this->lidar_rates.end() - win_size, -+ this->lidar_rates.end(), -+ 0.0 -+ ) -+ / win_size; - } - - // RAM Usage - double vm_usage = 0.0; - double resident_set = 0.0; -- std::ifstream stat_stream("/proc/self/stat", std::ios_base::in); //get info from proc directory -+ std::ifstream stat_stream( -+ "/proc/self/stat", -+ std::ios_base::in -+ ); //get info from proc directory - std::string pid, comm, state, ppid, pgrp, session, tty_nr; - std::string tpgid, flags, minflt, cminflt, majflt, cmajflt; - std::string utime, stime, cutime, cstime, priority, nice; -@@ -1865,11 +1831,12 @@ void dlio::OdomNode::debug() { - unsigned long vsize; - long rss; - stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr -- >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt -- >> utime >> stime >> cutime >> cstime >> priority >> nice -- >> num_threads >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest -+ >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt >> utime -+ >> stime >> cutime >> cstime >> priority >> nice >> num_threads -+ >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest - stat_stream.close(); -- long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages -+ long page_size_kb = -+ sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages - vm_usage = vsize / 1024.0; - resident_set = rss * page_size_kb; - -@@ -1878,143 +1845,202 @@ void dlio::OdomNode::debug() { - clock_t now; - double cpu_percent; - now = times(&timeSample); -- if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU || -- timeSample.tms_utime < this->lastUserCPU) { -- cpu_percent = -1.0; -+ if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU -+ || timeSample.tms_utime < this->lastUserCPU) { -+ cpu_percent = -1.0; - } else { -- cpu_percent = (timeSample.tms_stime - this->lastSysCPU) + (timeSample.tms_utime - this->lastUserCPU); -- cpu_percent /= (now - this->lastCPU); -- cpu_percent /= this->numProcessors; -- cpu_percent *= 100.; -+ cpu_percent = (timeSample.tms_stime - this->lastSysCPU) -+ + (timeSample.tms_utime - this->lastUserCPU); -+ cpu_percent /= (now - this->lastCPU); -+ cpu_percent /= this->numProcessors; -+ cpu_percent *= 100.; - } - this->lastCPU = now; - this->lastSysCPU = timeSample.tms_stime; - this->lastUserCPU = timeSample.tms_utime; - this->cpu_percents.push_back(cpu_percent); - double avg_cpu_usage = -- std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) / this->cpu_percents.size(); -+ std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) -+ / this->cpu_percents.size(); - - // Print to terminal - printf("\033[2J\033[1;1H"); - -- std::cout << std::endl -- << "+-------------------------------------------------------------------+" << std::endl; -- std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" -- << std::endl; -- std::cout << "+-------------------------------------------------------------------+" << std::endl; -+ std::cout -+ << std::endl -+ << "+-------------------------------------------------------------------+" -+ << std::endl; -+ std::cout << "| Direct LiDAR-Inertial Odometry v" -+ << this->version_ << " |" << std::endl; -+ std::cout -+ << "+-------------------------------------------------------------------+" -+ << std::endl; - - std::time_t curr_time = this->scan_stamp; -- std::string asc_time = std::asctime(std::localtime(&curr_time)); asc_time.pop_back(); -+ std::string asc_time = std::asctime(std::localtime(&curr_time)); -+ asc_time.pop_back(); - std::cout << "| " << std::left << asc_time; - std::cout << std::right << std::setfill(' ') << std::setw(42) -- << "Elapsed Time: " + to_string_with_precision(this->elapsed_time, 2) + " seconds " -- << "|" << std::endl; -+ << "Elapsed Time: " -+ + to_string_with_precision(this->elapsed_time, 2) + " seconds " -+ << "|" << std::endl; - -- if ( !this->cpu_type.empty() ) { -+ if (!this->cpu_type.empty()) { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << this->cpu_type + " x " + std::to_string(this->numProcessors) -- << "|" << std::endl; -+ << this->cpu_type + " x " + std::to_string(this->numProcessors) -+ << "|" << std::endl; - } - - if (this->sensor == dlio::SensorType::OUSTER) { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Sensor Rates: Ouster @ " + to_string_with_precision(avg_lidar_rate, 2) -- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" -- << "|" << std::endl; -+ << "Sensor Rates: Ouster @ " -+ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " -+ + to_string_with_precision(avg_imu_rate, 2) + " Hz" -+ << "|" << std::endl; - } else if (this->sensor == dlio::SensorType::VELODYNE) { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Sensor Rates: Velodyne @ " + to_string_with_precision(avg_lidar_rate, 2) -- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" -- << "|" << std::endl; -+ << "Sensor Rates: Velodyne @ " -+ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " -+ + to_string_with_precision(avg_imu_rate, 2) + " Hz" -+ << "|" << std::endl; - } else if (this->sensor == dlio::SensorType::HESAI) { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Sensor Rates: Hesai @ " + to_string_with_precision(avg_lidar_rate, 2) -- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" -- << "|" << std::endl; -+ << "Sensor Rates: Hesai @ " -+ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " -+ + to_string_with_precision(avg_imu_rate, 2) + " Hz" -+ << "|" << std::endl; - } else if (this->sensor == dlio::SensorType::LIVOX) { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Sensor Rates: Livox @ " + to_string_with_precision(avg_lidar_rate, 2) -- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" -- << "|" << std::endl; -+ << "Sensor Rates: Livox @ " -+ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " -+ + to_string_with_precision(avg_imu_rate, 2) + " Hz" -+ << "|" << std::endl; - } else { - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Sensor Rates: Unknown LiDAR @ " + to_string_with_precision(avg_lidar_rate, 2) -- + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" -- << "|" << std::endl; -+ << "Sensor Rates: Unknown LiDAR @ " -+ + to_string_with_precision(avg_lidar_rate, 2) + " Hz, IMU @ " -+ + to_string_with_precision(avg_imu_rate, 2) + " Hz" -+ << "|" << std::endl; - } +@@ -2018,3 +1754,19 @@ void dlio::OdomNode::debug() { + std::cout << "+-------------------------------------------------------------------+" << std::endl; -- std::cout << "|===================================================================|" << std::endl; -+ std::cout -+ << "|===================================================================|" -+ << std::endl; - - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Position {W} [xyz] :: " + to_string_with_precision(this->state.p[0], 4) + " " -- + to_string_with_precision(this->state.p[1], 4) + " " -- + to_string_with_precision(this->state.p[2], 4) -- << "|" << std::endl; -+ << "Position {W} [xyz] :: " -+ + to_string_with_precision(this->state.p[0], 4) + " " -+ + to_string_with_precision(this->state.p[1], 4) + " " -+ + to_string_with_precision(this->state.p[2], 4) -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Orientation {W} [wxyz] :: " + to_string_with_precision(this->state.q.w(), 4) + " " -- + to_string_with_precision(this->state.q.x(), 4) + " " -- + to_string_with_precision(this->state.q.y(), 4) + " " -- + to_string_with_precision(this->state.q.z(), 4) -- << "|" << std::endl; -+ << "Orientation {W} [wxyz] :: " -+ + to_string_with_precision(this->state.q.w(), 4) + " " -+ + to_string_with_precision(this->state.q.x(), 4) + " " -+ + to_string_with_precision(this->state.q.y(), 4) + " " -+ + to_string_with_precision(this->state.q.z(), 4) -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Lin Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.lin.b[0], 4) + " " -- + to_string_with_precision(this->state.v.lin.b[1], 4) + " " -- + to_string_with_precision(this->state.v.lin.b[2], 4) -- << "|" << std::endl; -+ << "Lin Velocity {B} [xyz] :: " -+ + to_string_with_precision(this->state.v.lin.b[0], 4) + " " -+ + to_string_with_precision(this->state.v.lin.b[1], 4) + " " -+ + to_string_with_precision(this->state.v.lin.b[2], 4) -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Ang Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.ang.b[0], 4) + " " -- + to_string_with_precision(this->state.v.ang.b[1], 4) + " " -- + to_string_with_precision(this->state.v.ang.b[2], 4) -- << "|" << std::endl; -+ << "Ang Velocity {B} [xyz] :: " -+ + to_string_with_precision(this->state.v.ang.b[0], 4) + " " -+ + to_string_with_precision(this->state.v.ang.b[1], 4) + " " -+ + to_string_with_precision(this->state.v.ang.b[2], 4) -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Accel Bias [xyz] :: " + to_string_with_precision(this->state.b.accel[0], 8) + " " -- + to_string_with_precision(this->state.b.accel[1], 8) + " " -- + to_string_with_precision(this->state.b.accel[2], 8) -- << "|" << std::endl; -+ << "Accel Bias [xyz] :: " -+ + to_string_with_precision(this->state.b.accel[0], 8) + " " -+ + to_string_with_precision(this->state.b.accel[1], 8) + " " -+ + to_string_with_precision(this->state.b.accel[2], 8) -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Gyro Bias [xyz] :: " + to_string_with_precision(this->state.b.gyro[0], 8) + " " -- + to_string_with_precision(this->state.b.gyro[1], 8) + " " -- + to_string_with_precision(this->state.b.gyro[2], 8) -- << "|" << std::endl; -+ << "Gyro Bias [xyz] :: " -+ + to_string_with_precision(this->state.b.gyro[0], 8) + " " -+ + to_string_with_precision(this->state.b.gyro[1], 8) + " " -+ + to_string_with_precision(this->state.b.gyro[2], 8) -+ << "|" << std::endl; - -- std::cout << "| |" << std::endl; -+ std::cout -+ << "| |" -+ << std::endl; - - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Distance Traveled :: " + to_string_with_precision(length_traversed, 4) + " meters" -- << "|" << std::endl; -+ << "Distance Traveled :: " -+ + to_string_with_precision(length_traversed, 4) + " meters" -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Distance to Origin :: " -- + to_string_with_precision( sqrt(pow(this->state.p[0]-this->origin[0],2) + -- pow(this->state.p[1]-this->origin[1],2) + -- pow(this->state.p[2]-this->origin[2],2)), 4) + " meters" -- << "|" << std::endl; -+ << "Distance to Origin :: " -+ + to_string_with_precision( -+ sqrt( -+ pow(this->state.p[0] - this->origin[0], 2) -+ + pow(this->state.p[1] - this->origin[1], 2) -+ + pow(this->state.p[2] - this->origin[2], 2) -+ ), -+ 4 -+ ) -+ + " meters" -+ << "|" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "Registration :: keyframes: " + std::to_string(this->keyframes.size()) + ", " -- + "deskewed points: " + std::to_string(this->deskew_size) -- << "|" << std::endl; -- std::cout << "| |" << std::endl; -+ << "Registration :: keyframes: " -+ + std::to_string(this->keyframes.size()) + ", " -+ + "deskewed points: " + std::to_string(this->deskew_size) -+ << "|" << std::endl; -+ std::cout -+ << "| |" -+ << std::endl; - - std::cout << std::right << std::setprecision(2) << std::fixed; -- std::cout << "| Computation Time :: " -- << std::setfill(' ') << std::setw(6) << this->comp_times.back()*1000. << " ms // Avg: " -- << std::setw(6) << avg_comp_time*1000. << " / Max: " -- << std::setw(6) << *std::max_element(this->comp_times.begin(), this->comp_times.end())*1000. -+ std::cout -+ << "| Computation Time :: " << std::setfill(' ') << std::setw(6) -+ << this->comp_times.back() * 1000. << " ms // Avg: " << std::setw(6) -+ << avg_comp_time * 1000. << " / Max: " << std::setw(6) -+ << *std::max_element(this->comp_times.begin(), this->comp_times.end()) -+ * 1000. - << " |" << std::endl; -- std::cout << "| Cores Utilized :: " -- << std::setfill(' ') << std::setw(6) << (cpu_percent/100.) * this->numProcessors << " cores // Avg: " -- << std::setw(6) << (avg_cpu_usage/100.) * this->numProcessors << " / Max: " -- << std::setw(6) << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) / 100.) -- * this->numProcessors -+ std::cout -+ << "| Cores Utilized :: " << std::setfill(' ') << std::setw(6) -+ << (cpu_percent / 100.) * this->numProcessors -+ << " cores // Avg: " << std::setw(6) -+ << (avg_cpu_usage / 100.) * this->numProcessors -+ << " / Max: " << std::setw(6) -+ << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) -+ / 100.) -+ * this->numProcessors - << " |" << std::endl; -- std::cout << "| CPU Load :: " -- << std::setfill(' ') << std::setw(6) << cpu_percent << " % // Avg: " -- << std::setw(6) << avg_cpu_usage << " / Max: " -- << std::setw(6) << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) -+ std::cout -+ << "| CPU Load :: " << std::setfill(' ') << std::setw(6) -+ << cpu_percent << " % // Avg: " << std::setw(6) << avg_cpu_usage -+ << " / Max: " << std::setw(6) -+ << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) - << " |" << std::endl; - std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) -- << "RAM Allocation :: " + to_string_with_precision(resident_set/1000., 2) + " MB" -- << "|" << std::endl; -- -- std::cout << "+-------------------------------------------------------------------+" << std::endl; -+ << "RAM Allocation :: " -+ + to_string_with_precision(resident_set / 1000., 2) + " MB" -+ << "|" << std::endl; - -+ std::cout -+ << "+-------------------------------------------------------------------+" -+ << std::endl; } + +// Explicit instantiations of PCL templates for dlio::Point +// This is needed because PCL doesn't automatically instantiate templates for +// custom point types ++#include +#include +#include ++#include +#include +#include + -+#include -+#include -+ +template class pcl::PCLBase; +template class pcl::search::Search; +template class pcl::KdTreeFLANN>; -+template class pcl::search:: -+ KdTree>>; ++template class pcl::search::KdTree< ++ dlio::Point, pcl::KdTreeFLANN>>; +\ No newline at end of file diff --git a/src/nano_gicp/lsq_registration.cc b/src/nano_gicp/lsq_registration.cc index 271cb03..b6c8f07 100644 --- a/src/nano_gicp/lsq_registration.cc From f8f84b3ffa4ed15566a67d1e1cb61edfd6339788 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Wed, 8 Apr 2026 20:42:04 -0400 Subject: [PATCH 10/11] Switch to fork of dlio --- cpp/bindings/pipelines/dlio.patch | 1496 ----------------------------- cpp/setup_pipelines.sh | 6 +- 2 files changed, 2 insertions(+), 1500 deletions(-) delete mode 100644 cpp/bindings/pipelines/dlio.patch diff --git a/cpp/bindings/pipelines/dlio.patch b/cpp/bindings/pipelines/dlio.patch deleted file mode 100644 index 9c53a9e6..00000000 --- a/cpp/bindings/pipelines/dlio.patch +++ /dev/null @@ -1,1496 +0,0 @@ -diff --git a/CMakeLists.txt b/CMakeLists.txt -index 91e47a0..e3e2ba0 100644 ---- a/CMakeLists.txt -+++ b/CMakeLists.txt -@@ -11,10 +11,11 @@ - ########################################################### - - cmake_minimum_required(VERSION 3.12.4) --project(direct_lidar_inertial_odometry) -+project(direct_lidar_inertial_odometry VERSION 1.1.1) - - add_compile_options(-std=c++17) - set(CMAKE_BUILD_TYPE "Release") -+set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - - find_package( PCL REQUIRED ) - include_directories(${PCL_INCLUDE_DIRS}) -@@ -33,50 +34,7 @@ else(OPENMP_FOUND) - message("ERROR: OpenMP could not be found.") - endif(OPENMP_FOUND) - --set(CMAKE_THREAD_PREFER_PTHREAD TRUE) --set(THREADS_PREFER_PTHREAD_FLAG TRUE) --find_package(Threads REQUIRED) -- --find_package(catkin REQUIRED COMPONENTS -- roscpp -- std_msgs -- sensor_msgs -- geometry_msgs -- nav_msgs -- pcl_ros -- message_generation --) -- --add_service_files( -- DIRECTORY srv -- FILES -- save_pcd.srv --) -- --generate_messages( -- DEPENDENCIES -- sensor_msgs -- std_msgs -- geometry_msgs --) -- --catkin_package( -- CATKIN_DEPENDS -- roscpp -- std_msgs -- sensor_msgs -- geometry_msgs -- pcl_ros -- INCLUDE_DIRS -- include -- LIBRARIES -- ${PROJECT_NAME} -- nano_gicp -- nanoflann --) -- - include_directories(include) --include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) - - # Not all machines have available - set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) -@@ -88,32 +46,23 @@ if(HAS_CPUID) - endif() - - # NanoFLANN --add_library(nanoflann STATIC -- src/nano_gicp/nanoflann.cc --) --target_link_libraries(nanoflann ${PCL_LIBRARIES}) -+find_package(nanoflann CONFIG REQUIRED) - - # NanoGICP - add_library(nano_gicp STATIC - src/nano_gicp/lsq_registration.cc - src/nano_gicp/nano_gicp.cc - ) --target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) -+target_link_libraries(nano_gicp PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann::nanoflann) -+target_include_directories(nano_gicp PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - - # Odometry Node --add_executable(dlio_odom_node src/dlio/odom_node.cc src/dlio/odom.cc) --add_dependencies(dlio_odom_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) -+add_library(dlio_odom_node src/dlio/odom.cc) - target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS}) --target_link_libraries(dlio_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp) -+target_link_libraries(dlio_odom_node PUBLIC ${PCL_LIBRARIES} ${OpenMP_LIBS} nano_gicp) -+target_include_directories(dlio_odom_node PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - - # Mapping Node --add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc) --add_dependencies(dlio_map_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) --target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) --target_link_libraries(dlio_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) -- --# Binaries --install( TARGETS dlio_odom_node dlio_map_node -- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) --install( DIRECTORY cfg launch -- DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -+# add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc) -+# target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) -+# target_link_libraries(dlio_map_node ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) -diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h -index 47386ff..0416918 100644 ---- a/include/dlio/dlio.h -+++ b/include/dlio/dlio.h -@@ -31,7 +31,10 @@ - #include - #include - #include --#include - #include -+#ifdef __APPLE__ -+#include -+#else - #include -+#endif - -@@ -44,16 +43,6 @@ std::string to_string_with_precision(const T a_value, const int n = 6) - return out.str(); - } - --// ROS --#include --#include --#include --#include --#include --#include --#include --#include -- - // BOOST - #include - #include -@@ -69,14 +58,9 @@ std::string to_string_with_precision(const T a_value, const int n = 6) - #include - #include - #include --#include --#include --#include --#include - - // DLIO - #include --#include - - namespace dlio { - enum class SensorType { OUSTER, VELODYNE, HESAI, LIVOX, UNKNOWN }; -@@ -84,7 +68,7 @@ namespace dlio { - class OdomNode; - class MapNode; - -- struct Point { -+ struct EIGEN_ALIGN16 Point { - Point(): data{0.f, 0.f, 0.f, 1.f} {} - - PCL_ADD_POINT4D; -@@ -96,7 +80,7 @@ namespace dlio { - // (Livox) absolute timestamp in (seconds * 10e9) - }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -- } EIGEN_ALIGN16; -+ }; - } - - POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, -diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h -index 2c6732b..d88de52 100644 ---- a/include/nano_gicp/nanoflann_adaptor.h -+++ b/include/nano_gicp/nanoflann_adaptor.h -@@ -49,7 +49,7 @@ - #include - #include - --#include "nano_gicp/nanoflann.h" -+#include - - namespace nanoflann - { -@@ -86,5 +86,5 @@ protected: - -- nanoflann::SearchParams _params; -+ nanoflann::SearchParameters _params; - - struct PointCloud_Adaptor - { -@@ -159,4 +159,4 @@ int KdTreeFLANN::radiusSearch(const PointT &point, double radius, -- static std::vector > indices_dist; -+ static std::vector> indices_dist; - indices_dist.reserve( 128 ); - - RadiusResultSet resultSet(radius, indices_dist); -diff --git a/include/dlio/odom.h b/include/dlio/odom.h -index c79ebd7..2c91f67 100644 ---- a/include/dlio/odom.h -+++ b/include/dlio/odom.h -@@ -10,35 +10,111 @@ - * * - ***********************************************************/ - -+#include -+ - #include "dlio/dlio.h" - - class dlio::OdomNode { - - public: -+ struct Params { -+ bool verbose = false; -+ -+ bool deskew = true; -+ double gravity = 9.80665; -+ bool time_offset = false; -+ -+ double keyframe_thresh_dist = 0.1; -+ double keyframe_thresh_rot = 1.0; -+ -+ int submap_knn = 10; -+ int submap_kcv = 10; -+ int submap_kcc = 10; -+ -+ bool densemap_filtered = true; -+ bool wait_until_move = false; -+ -+ double crop_size = 1.0; -+ -+ bool vf_use = true; -+ double vf_res = 0.05; -+ -+ bool adaptive_params = true; -+ -+ Eigen::Vector3f extrinsics_baselink2imu_t = Eigen::Vector3f::Zero(); -+ Eigen::Matrix3f extrinsics_baselink2imu_R = Eigen::Matrix3f::Identity(); -+ Eigen::Vector3f extrinsics_baselink2lidar_t = Eigen::Vector3f::Zero(); -+ Eigen::Matrix3f extrinsics_baselink2lidar_R = Eigen::Matrix3f::Identity(); -+ -+ bool calibrate_gyro = true; -+ bool calibrate_accel = true; -+ double imu_calib_time = 3.0; -+ int imu_buffer_size = 2000; -+ -+ bool gravity_align = true; -+ bool imu_calibrate = true; -+ -+ int gicp_min_num_points = 100; -+ int gicp_k_correspondences = 20; -+ double gicp_max_corr_dist = std::sqrt(std::numeric_limits::max()); -+ int gicp_max_iter = 64; -+ double gicp_transformation_ep = 0.0005; -+ double gicp_rotation_ep = 0.0005; -+ double gicp_init_lambda_factor = 1e-9; -+ -+ double geo_Kp = 1.0; -+ double geo_Kv = 1.0; -+ double geo_Kq = 1.0; -+ double geo_Kab = 1.0; -+ double geo_Kgb = 1.0; -+ double geo_abias_max = 1.0; -+ double geo_gbias_max = 1.0; -+ }; - -- OdomNode(ros::NodeHandle node_handle); -- ~OdomNode(); -+ struct ImuMeas { -+ double stamp; -+ double dt; // defined as the difference between the current and the previous -+ // measurement -+ Eigen::Vector3f ang_vel; -+ Eigen::Vector3f lin_accel; -+ }; - -- void start(); -+ struct ImuBias { -+ Eigen::Vector3f gyro; -+ Eigen::Vector3f accel; -+ }; - --private: -+ struct Frames { -+ Eigen::Vector3f b; -+ Eigen::Vector3f w; -+ }; - -- struct State; -- struct ImuMeas; -+ struct Velocity { -+ Frames lin; -+ Frames ang; -+ }; - -- void getParams(); -+ struct State { -+ Eigen::Vector3f p; // position in world frame -+ Eigen::Quaternionf q; // orientation in world frame -+ Velocity v; -+ ImuBias b; // imu biases in body frame -+ }; - -- void callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc); -- void callbackImu(const sensor_msgs::Imu::ConstPtr& imu); -+ OdomNode(const Params ¶ms); -+ ~OdomNode(); - -- void publishPose(const ros::TimerEvent& e); -+ void start(); -+ State getState() const { return state; }; -+ -+ void callbackPointCloud(const pcl::PointCloud::ConstPtr &pc, -+ double stamp); -+ void callbackImu(const ImuMeas &imu); - -- void publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); -- void publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); -- void publishKeyframe(std::pair, -- pcl::PointCloud::ConstPtr> kf, ros::Time timestamp); -+private: -+ void getParams(const Params ¶ms); - -- void getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc); -+ void getScanFromROS(const pcl::PointCloud::ConstPtr &pc, double stamp); - void preprocessPoints(); - void deskewPointcloud(); - void initializeInputTarget(); -@@ -70,7 +146,7 @@ private: - void computeSpaciousness(); - void computeDensity(); - -- sensor_msgs::Imu::Ptr transformImu(const sensor_msgs::Imu::ConstPtr& imu); -+ ImuMeas transformImu(const ImuMeas &imu); - - void updateKeyframes(); - void computeConvexHull(); -@@ -82,27 +158,6 @@ private: - - void debug(); - -- ros::NodeHandle nh; -- ros::Timer publish_timer; -- -- // Subscribers -- ros::Subscriber lidar_sub; -- ros::Subscriber imu_sub; -- -- // Publishers -- ros::Publisher odom_pub; -- ros::Publisher pose_pub; -- ros::Publisher path_pub; -- ros::Publisher kf_pose_pub; -- ros::Publisher kf_cloud_pub; -- ros::Publisher deskewed_pub; -- -- // ROS Msgs -- nav_msgs::Odometry odom_ros; -- geometry_msgs::PoseStamped pose_ros; -- nav_msgs::Path path_ros; -- geometry_msgs::PoseArray kf_pose_ros; -- - // Flags - std::atomic dlio_initialized; - std::atomic first_valid_scan; -@@ -113,12 +168,6 @@ private: - std::atomic deskew_status; - std::atomic deskew_size; - -- // Threads -- std::thread publish_thread; -- std::thread publish_keyframe_thread; -- std::thread metrics_thread; -- std::thread debug_thread; -- - // Trajectory - std::vector> trajectory; - double length_traversed; -@@ -126,7 +175,7 @@ private: - // Keyframes - std::vector, - pcl::PointCloud::ConstPtr>> keyframes; -- std::vector keyframe_timestamps; -+ std::vector keyframe_timestamps; - std::vector> keyframe_normals; - std::vector> keyframe_transformations; - std::mutex keyframes_mutex; -@@ -134,12 +183,6 @@ private: - // Sensor Type - dlio::SensorType sensor; - -- // Frames -- std::string odom_frame; -- std::string baselink_frame; -- std::string lidar_frame; -- std::string imu_frame; -- - // Preprocessing - pcl::CropBox crop; - pcl::VoxelGrid voxel; -@@ -173,7 +216,7 @@ private: - std::mutex main_loop_running_mutex; - - // Timestamps -- ros::Time scan_header_stamp; -+ double scan_header_stamp; - double scan_stamp; - double prev_scan_stamp; - double scan_dt; -@@ -206,17 +249,12 @@ private: - }; Extrinsics extrinsics; - - // IMU -- ros::Time imu_stamp; -+ double imu_stamp; - double first_imu_stamp; - double prev_imu_stamp; - double imu_dp, imu_dq_deg; - -- struct ImuMeas { -- double stamp; -- double dt; // defined as the difference between the current and the previous measurement -- Eigen::Vector3f ang_vel; -- Eigen::Vector3f lin_accel; -- }; ImuMeas imu_meas; -+ ImuMeas imu_meas; - - boost::circular_buffer imu_buffer; - std::mutex mtx_imu; -@@ -238,27 +276,7 @@ private: - }; Geo geo; - - // State Vector -- struct ImuBias { -- Eigen::Vector3f gyro; -- Eigen::Vector3f accel; -- }; -- -- struct Frames { -- Eigen::Vector3f b; -- Eigen::Vector3f w; -- }; -- -- struct Velocity { -- Frames lin; -- Frames ang; -- }; -- -- struct State { -- Eigen::Vector3f p; // position in world frame -- Eigen::Quaternionf q; // orientation in world frame -- Velocity v; -- ImuBias b; // imu biases in body frame -- }; State state; -+ State state; - - struct Pose { - Eigen::Vector3f p; // position in world frame -@@ -279,7 +297,7 @@ private: - int numProcessors; - - // Parameters -- std::string version_; -+ std::string version_ = "1.1.1"; - int num_threads_; - bool verbose; - -diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc -index bceae37..c0c625f 100644 ---- a/src/dlio/odom.cc -+++ b/src/dlio/odom.cc -@@ -12,9 +12,9 @@ - - #include "dlio/odom.h" - --dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { -+dlio::OdomNode::OdomNode(const Params ¶ms) { - -- this->getParams(); -+ this->getParams(params); - - this->num_threads_ = omp_get_max_threads(); - -@@ -26,20 +26,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - this->deskew_status = false; - this->deskew_size = 0; - -- this->lidar_sub = this->nh.subscribe("pointcloud", 1, -- &dlio::OdomNode::callbackPointCloud, this, ros::TransportHints().tcpNoDelay()); -- this->imu_sub = this->nh.subscribe("imu", 1000, -- &dlio::OdomNode::callbackImu, this, ros::TransportHints().tcpNoDelay()); -- -- this->odom_pub = this->nh.advertise("odom", 1, true); -- this->pose_pub = this->nh.advertise("pose", 1, true); -- this->path_pub = this->nh.advertise("path", 1, true); -- this->kf_pose_pub = this->nh.advertise("kf_pose", 1, true); -- this->kf_cloud_pub = this->nh.advertise("kf_cloud", 1, true); -- this->deskewed_pub = this->nh.advertise("deskewed", 1, true); -- -- this->publish_timer = this->nh.createTimer(ros::Duration(0.01), &dlio::OdomNode::publishPose, this); -- - this->T = Eigen::Matrix4f::Identity(); - this->T_prior = Eigen::Matrix4f::Identity(); - this->T_corr = Eigen::Matrix4f::Identity(); -@@ -67,10 +53,10 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - this->first_imu_stamp = 0.; - this->prev_imu_stamp = 0.; - -- this->original_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); -- this->deskewed_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); -- this->current_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); -- this->submap_cloud = pcl::PointCloud::ConstPtr (boost::make_shared>()); -+ this->original_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->deskewed_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->current_scan = pcl::PointCloud::ConstPtr (std::make_shared>()); -+ this->submap_cloud = pcl::PointCloud::ConstPtr (std::make_shared>()); - - this->num_processed_keyframes = 0; - -@@ -79,7 +65,6 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - - this->first_scan_stamp = 0.; - this->elapsed_time = 0.; -- this->length_traversed; - - this->convex_hull.setDimension(3); - this->concave_hull.setDimension(3); -@@ -157,151 +142,81 @@ dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { - if (strncmp(line, "processor", 9) == 0) this->numProcessors++; - } - fclose(file); -- - } - - dlio::OdomNode::~OdomNode() {} - --void dlio::OdomNode::getParams() { -- -- // Version -- ros::param::param("~dlio/version", this->version_, "0.0.0"); -- -- // Frames -- ros::param::param("~dlio/frames/odom", this->odom_frame, "odom"); -- ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); -- ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); -- ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); -+void dlio::OdomNode::getParams(const Params ¶ms) { -+ this->verbose = params.verbose; - -- // Get Node NS and Remove Leading Character -- std::string ns = ros::this_node::getNamespace(); -- ns.erase(0,1); -+ this->deskew_ = params.deskew; -+ this->gravity_ = params.gravity; -+ this->time_offset_ = params.time_offset; - -- // Concatenate Frame Name Strings -- this->odom_frame = ns + "/" + this->odom_frame; -- this->baselink_frame = ns + "/" + this->baselink_frame; -- this->lidar_frame = ns + "/" + this->lidar_frame; -- this->imu_frame = ns + "/" + this->imu_frame; -+ this->keyframe_thresh_dist_ = params.keyframe_thresh_dist; -+ this->keyframe_thresh_rot_ = params.keyframe_thresh_rot; - -- // Deskew FLag -- ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); -+ this->submap_knn_ = params.submap_knn; -+ this->submap_kcv_ = params.submap_kcv; -+ this->submap_kcc_ = params.submap_kcc; - -- // Gravity -- ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); -+ this->densemap_filtered_ = params.densemap_filtered; - -- // Compute time offset between lidar and imu -- ros::param::param("~dlio/odom/computeTimeOffset", this->time_offset_, false); -+ this->wait_until_move_ = params.wait_until_move; - -- // Keyframe Threshold -- ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); -- ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); -+ this->crop_size_ = params.crop_size; - -- // Submap -- ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); -- ros::param::param("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10); -- ros::param::param("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10); -- -- // Dense map resolution -- ros::param::param("~dlio/map/dense/filtered", this->densemap_filtered_, true); -- -- // Wait until movement to publish map -- ros::param::param("~dlio/map/waitUntilMove", this->wait_until_move_, false); -- -- // Crop Box Filter -- ros::param::param("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); -- -- // Voxel Grid Filter -- ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); -- ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); -+ this->vf_use_ = params.vf_use; -+ this->vf_res_ = params.vf_res; - -- // Adaptive Parameters -- ros::param::param("~dlio/adaptive", this->adaptive_params_, true); -+ this->adaptive_params_ = params.adaptive_params; - -- // Extrinsics -- std::vector t_default{0., 0., 0.}; -- std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; -- -- // center of gravity to imu -- std::vector baselink2imu_t, baselink2imu_R; -- ros::param::param>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default); -- ros::param::param>("~dlio/extrinsics/baselink2imu/R", baselink2imu_R, R_default); -- this->extrinsics.baselink2imu.t = -- Eigen::Vector3f(baselink2imu_t[0], baselink2imu_t[1], baselink2imu_t[2]); -- this->extrinsics.baselink2imu.R = -- Eigen::Map>(baselink2imu_R.data(), 3, 3); -+ this->extrinsics.baselink2imu.t = params.extrinsics_baselink2imu_t; -+ this->extrinsics.baselink2imu.R = params.extrinsics_baselink2imu_R; -+ this->extrinsics.baselink2lidar.t = params.extrinsics_baselink2lidar_t; -+ this->extrinsics.baselink2lidar.R = params.extrinsics_baselink2lidar_R; - -+ // Build 4x4 transformation matrices - this->extrinsics.baselink2imu_T = Eigen::Matrix4f::Identity(); -- this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = this->extrinsics.baselink2imu.t; -- this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = this->extrinsics.baselink2imu.R; -- -- // center of gravity to lidar -- std::vector baselink2lidar_t, baselink2lidar_R; -- ros::param::param>("~dlio/extrinsics/baselink2lidar/t", baselink2lidar_t, t_default); -- ros::param::param>("~dlio/extrinsics/baselink2lidar/R", baselink2lidar_R, R_default); -- -- this->extrinsics.baselink2lidar.t = -- Eigen::Vector3f(baselink2lidar_t[0], baselink2lidar_t[1], baselink2lidar_t[2]); -- this->extrinsics.baselink2lidar.R = -- Eigen::Map>(baselink2lidar_R.data(), 3, 3); -+ this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2imu.t; -+ this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2imu.R; - - this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity(); -- this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t; -- this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = this->extrinsics.baselink2lidar.R; -- -- // IMU -- ros::param::param("~dlio/odom/imu/calibration/accel", this->calibrate_accel_, true); -- ros::param::param("~dlio/odom/imu/calibration/gyro", this->calibrate_gyro_, true); -- ros::param::param("~dlio/odom/imu/calibration/time", this->imu_calib_time_, 3.0); -- ros::param::param("~dlio/odom/imu/bufferSize", this->imu_buffer_size_, 2000); -- -- std::vector accel_default{0., 0., 0.}; std::vector prior_accel_bias; -- std::vector gyro_default{0., 0., 0.}; std::vector prior_gyro_bias; -- -- ros::param::param("~dlio/odom/imu/approximateGravity", this->gravity_align_, true); -- ros::param::param("~dlio/imu/calibration", this->imu_calibrate_, true); -- ros::param::param>("~dlio/imu/intrinsics/accel/bias", prior_accel_bias, accel_default); -- ros::param::param>("~dlio/imu/intrinsics/gyro/bias", prior_gyro_bias, gyro_default); -- -- // scale-misalignment matrix -- std::vector imu_sm_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; -- std::vector imu_sm; -- -- ros::param::param>("~dlio/imu/intrinsics/accel/sm", imu_sm, imu_sm_default); -- -- if (!this->imu_calibrate_) { -- this->state.b.accel[0] = prior_accel_bias[0]; -- this->state.b.accel[1] = prior_accel_bias[1]; -- this->state.b.accel[2] = prior_accel_bias[2]; -- this->state.b.gyro[0] = prior_gyro_bias[0]; -- this->state.b.gyro[1] = prior_gyro_bias[1]; -- this->state.b.gyro[2] = prior_gyro_bias[2]; -- this->imu_accel_sm_ = Eigen::Map>(imu_sm.data(), 3, 3); -- } else { -- this->state.b.accel = Eigen::Vector3f(0., 0., 0.); -- this->state.b.gyro = Eigen::Vector3f(0., 0., 0.); -- this->imu_accel_sm_ = Eigen::Matrix3f::Identity(); -- } -- -- // GICP -- ros::param::param("~dlio/odom/gicp/minNumPoints", this->gicp_min_num_points_, 100); -- ros::param::param("~dlio/odom/gicp/kCorrespondences", this->gicp_k_correspondences_, 20); -- ros::param::param("~dlio/odom/gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_, -- std::sqrt(std::numeric_limits::max())); -- ros::param::param("~dlio/odom/gicp/maxIterations", this->gicp_max_iter_, 64); -- ros::param::param("~dlio/odom/gicp/transformationEpsilon", this->gicp_transformation_ep_, 0.0005); -- ros::param::param("~dlio/odom/gicp/rotationEpsilon", this->gicp_rotation_ep_, 0.0005); -- ros::param::param("~dlio/odom/gicp/initLambdaFactor", this->gicp_init_lambda_factor_, 1e-9); -- -- // Geometric Observer -- ros::param::param("~dlio/odom/geo/Kp", this->geo_Kp_, 1.0); -- ros::param::param("~dlio/odom/geo/Kv", this->geo_Kv_, 1.0); -- ros::param::param("~dlio/odom/geo/Kq", this->geo_Kq_, 1.0); -- ros::param::param("~dlio/odom/geo/Kab", this->geo_Kab_, 1.0); -- ros::param::param("~dlio/odom/geo/Kgb", this->geo_Kgb_, 1.0); -- ros::param::param("~dlio/odom/geo/abias_max", this->geo_abias_max_, 1.0); -- ros::param::param("~dlio/odom/geo/gbias_max", this->geo_gbias_max_, 1.0); -- -- ros::param::param("~dlio/verbose", this->verbose, true); -+ this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = -+ this->extrinsics.baselink2lidar.t; -+ this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = -+ this->extrinsics.baselink2lidar.R; -+ -+ this->calibrate_gyro_ = params.calibrate_gyro; -+ this->calibrate_accel_ = params.calibrate_accel; -+ this->imu_calib_time_ = params.imu_calib_time; -+ this->imu_buffer_size_ = params.imu_buffer_size; -+ -+ this->gravity_align_ = params.gravity_align; -+ this->imu_calibrate_ = params.imu_calibrate; -+ -+ // Initialize IMU accelerometer scale-misalignment matrix -+ this->state.b.accel = Eigen::Vector3f::Zero(); -+ this->state.b.gyro = Eigen::Vector3f::Zero(); -+ this->imu_accel_sm_ = Eigen::Matrix3f::Identity(); -+ -+ this->gicp_min_num_points_ = params.gicp_min_num_points; -+ this->gicp_k_correspondences_ = params.gicp_k_correspondences; -+ this->gicp_max_corr_dist_ = params.gicp_max_corr_dist; -+ this->gicp_max_iter_ = params.gicp_max_iter; -+ this->gicp_transformation_ep_ = params.gicp_transformation_ep; -+ this->gicp_rotation_ep_ = params.gicp_rotation_ep; -+ this->gicp_init_lambda_factor_ = params.gicp_init_lambda_factor; -+ -+ this->geo_Kp_ = params.geo_Kp; -+ this->geo_Kv_ = params.geo_Kv; -+ this->geo_Kq_ = params.geo_Kq; -+ this->geo_Kab_ = params.geo_Kab; -+ this->geo_Kgb_ = params.geo_Kgb; -+ this->geo_abias_max_ = params.geo_abias_max; -+ this->geo_gbias_max_ = params.geo_gbias_max; - } - - void dlio::OdomNode::start() { -@@ -318,185 +233,12 @@ void dlio::OdomNode::start() { - - } - --void dlio::OdomNode::publishPose(const ros::TimerEvent& e) { -- -- // nav_msgs::Odometry -- this->odom_ros.header.stamp = this->imu_stamp; -- this->odom_ros.header.frame_id = this->odom_frame; -- this->odom_ros.child_frame_id = this->baselink_frame; -- -- this->odom_ros.pose.pose.position.x = this->state.p[0]; -- this->odom_ros.pose.pose.position.y = this->state.p[1]; -- this->odom_ros.pose.pose.position.z = this->state.p[2]; -- -- this->odom_ros.pose.pose.orientation.w = this->state.q.w(); -- this->odom_ros.pose.pose.orientation.x = this->state.q.x(); -- this->odom_ros.pose.pose.orientation.y = this->state.q.y(); -- this->odom_ros.pose.pose.orientation.z = this->state.q.z(); -- -- this->odom_ros.twist.twist.linear.x = this->state.v.lin.w[0]; -- this->odom_ros.twist.twist.linear.y = this->state.v.lin.w[1]; -- this->odom_ros.twist.twist.linear.z = this->state.v.lin.w[2]; -- -- this->odom_ros.twist.twist.angular.x = this->state.v.ang.b[0]; -- this->odom_ros.twist.twist.angular.y = this->state.v.ang.b[1]; -- this->odom_ros.twist.twist.angular.z = this->state.v.ang.b[2]; -- -- this->odom_pub.publish(this->odom_ros); -- -- // geometry_msgs::PoseStamped -- this->pose_ros.header.stamp = this->imu_stamp; -- this->pose_ros.header.frame_id = this->odom_frame; -- -- this->pose_ros.pose.position.x = this->state.p[0]; -- this->pose_ros.pose.position.y = this->state.p[1]; -- this->pose_ros.pose.position.z = this->state.p[2]; -- -- this->pose_ros.pose.orientation.w = this->state.q.w(); -- this->pose_ros.pose.orientation.x = this->state.q.x(); -- this->pose_ros.pose.orientation.y = this->state.q.y(); -- this->pose_ros.pose.orientation.z = this->state.q.z(); -- -- this->pose_pub.publish(this->pose_ros); -- --} -- --void dlio::OdomNode::publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { -- this->publishCloud(published_cloud, T_cloud); -- -- // nav_msgs::Path -- this->path_ros.header.stamp = this->imu_stamp; -- this->path_ros.header.frame_id = this->odom_frame; -- -- geometry_msgs::PoseStamped p; -- p.header.stamp = this->imu_stamp; -- p.header.frame_id = this->odom_frame; -- p.pose.position.x = this->state.p[0]; -- p.pose.position.y = this->state.p[1]; -- p.pose.position.z = this->state.p[2]; -- p.pose.orientation.w = this->state.q.w(); -- p.pose.orientation.x = this->state.q.x(); -- p.pose.orientation.y = this->state.q.y(); -- p.pose.orientation.z = this->state.q.z(); -- -- this->path_ros.poses.push_back(p); -- this->path_pub.publish(this->path_ros); -- -- // transform: odom to baselink -- static tf2_ros::TransformBroadcaster br; -- geometry_msgs::TransformStamped transformStamped; -- -- transformStamped.header.stamp = this->imu_stamp; -- transformStamped.header.frame_id = this->odom_frame; -- transformStamped.child_frame_id = this->baselink_frame; -- -- transformStamped.transform.translation.x = this->state.p[0]; -- transformStamped.transform.translation.y = this->state.p[1]; -- transformStamped.transform.translation.z = this->state.p[2]; -- -- transformStamped.transform.rotation.w = this->state.q.w(); -- transformStamped.transform.rotation.x = this->state.q.x(); -- transformStamped.transform.rotation.y = this->state.q.y(); -- transformStamped.transform.rotation.z = this->state.q.z(); -- -- br.sendTransform(transformStamped); -- -- // transform: baselink to imu -- transformStamped.header.stamp = this->imu_stamp; -- transformStamped.header.frame_id = this->baselink_frame; -- transformStamped.child_frame_id = this->imu_frame; -- -- transformStamped.transform.translation.x = this->extrinsics.baselink2imu.t[0]; -- transformStamped.transform.translation.y = this->extrinsics.baselink2imu.t[1]; -- transformStamped.transform.translation.z = this->extrinsics.baselink2imu.t[2]; -- -- Eigen::Quaternionf q(this->extrinsics.baselink2imu.R); -- transformStamped.transform.rotation.w = q.w(); -- transformStamped.transform.rotation.x = q.x(); -- transformStamped.transform.rotation.y = q.y(); -- transformStamped.transform.rotation.z = q.z(); -- -- br.sendTransform(transformStamped); -- -- // transform: baselink to lidar -- transformStamped.header.stamp = this->imu_stamp; -- transformStamped.header.frame_id = this->baselink_frame; -- transformStamped.child_frame_id = this->lidar_frame; -- -- transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; -- transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; -- transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; -+void dlio::OdomNode::getScanFromROS( -+ const pcl::PointCloud::ConstPtr &pc, double stamp) { - -- Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); -- transformStamped.transform.rotation.w = qq.w(); -- transformStamped.transform.rotation.x = qq.x(); -- transformStamped.transform.rotation.y = qq.y(); -- transformStamped.transform.rotation.z = qq.z(); -- -- br.sendTransform(transformStamped); -- --} -- --void dlio::OdomNode::publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { -- -- if (this->wait_until_move_) { -- if (this->length_traversed < 0.1) { return; } -- } -- -- pcl::PointCloud::Ptr deskewed_scan_t_ (boost::make_shared>()); -- -- pcl::transformPointCloud (*published_cloud, *deskewed_scan_t_, T_cloud); -- -- // published deskewed cloud -- sensor_msgs::PointCloud2 deskewed_ros; -- pcl::toROSMsg(*deskewed_scan_t_, deskewed_ros); -- deskewed_ros.header.stamp = this->scan_header_stamp; -- deskewed_ros.header.frame_id = this->odom_frame; -- this->deskewed_pub.publish(deskewed_ros); -- --} -- --void dlio::OdomNode::publishKeyframe(std::pair, pcl::PointCloud::ConstPtr> kf, ros::Time timestamp) { -- -- // Push back -- geometry_msgs::Pose p; -- p.position.x = kf.first.first[0]; -- p.position.y = kf.first.first[1]; -- p.position.z = kf.first.first[2]; -- p.orientation.w = kf.first.second.w(); -- p.orientation.x = kf.first.second.x(); -- p.orientation.y = kf.first.second.y(); -- p.orientation.z = kf.first.second.z(); -- this->kf_pose_ros.poses.push_back(p); -- -- // Publish -- this->kf_pose_ros.header.stamp = timestamp; -- this->kf_pose_ros.header.frame_id = this->odom_frame; -- this->kf_pose_pub.publish(this->kf_pose_ros); -- -- // publish keyframe scan for map -- if (this->vf_use_) { -- if (kf.second->points.size() == kf.second->width * kf.second->height) { -- sensor_msgs::PointCloud2 keyframe_cloud_ros; -- pcl::toROSMsg(*kf.second, keyframe_cloud_ros); -- keyframe_cloud_ros.header.stamp = timestamp; -- keyframe_cloud_ros.header.frame_id = this->odom_frame; -- this->kf_cloud_pub.publish(keyframe_cloud_ros); -- } -- } else { -- sensor_msgs::PointCloud2 keyframe_cloud_ros; -- pcl::toROSMsg(*kf.second, keyframe_cloud_ros); -- keyframe_cloud_ros.header.stamp = timestamp; -- keyframe_cloud_ros.header.frame_id = this->odom_frame; -- this->kf_cloud_pub.publish(keyframe_cloud_ros); -- } -- --} -- --void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) { -- -- pcl::PointCloud::Ptr original_scan_ (boost::make_shared>()); -- pcl::fromROSMsg(*pc, *original_scan_); -+ pcl::PointCloud::Ptr original_scan_( -+ std::make_shared>()); -+ *original_scan_ = *pc; - - // Remove NaNs - std::vector idx; -@@ -508,28 +250,14 @@ void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) - this->crop.filter(*original_scan_); - - // automatically detect sensor type -- this->sensor = dlio::SensorType::UNKNOWN; -- for (auto &field : pc->fields) { -- if (field.name == "t") { -- this->sensor = dlio::SensorType::OUSTER; -- break; -- } else if (field.name == "time") { -- this->sensor = dlio::SensorType::VELODYNE; -- break; -- } else if (field.name == "timestamp" && original_scan_->points[0].timestamp < 1e14) { -- this->sensor = dlio::SensorType::HESAI; -- break; -- } else if (field.name == "timestamp" && original_scan_->points[0].timestamp > 1e14) { -- this->sensor = dlio::SensorType::LIVOX; -- break; -- } -- } -- -+ // NOTE: Ouster used here as it utilizes time since start of scan in -+ // nanoseconds like evalio -+ this->sensor = dlio::SensorType::OUSTER; - if (this->sensor == dlio::SensorType::UNKNOWN) { - this->deskew_ = false; - } - -- this->scan_header_stamp = pc->header.stamp; -+ this->scan_header_stamp = stamp; - this->original_scan = original_scan_; - - } -@@ -547,7 +275,7 @@ void dlio::OdomNode::preprocessPoints() { - - } else { - -- this->scan_stamp = this->scan_header_stamp.toSec(); -+ this->scan_stamp = this->scan_header_stamp; - - // don't process scans until IMU data is present - if (!this->first_valid_scan) { -@@ -574,7 +302,7 @@ void dlio::OdomNode::preprocessPoints() { - - } - -- pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); - pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, - this->T_prior * this->extrinsics.baselink2lidar_T); - this->deskewed_scan = deskewed_scan_; -@@ -584,7 +312,7 @@ void dlio::OdomNode::preprocessPoints() { - // Voxel Grid Filter - if (this->vf_use_) { - pcl::PointCloud::Ptr current_scan_ -- (boost::make_shared>(*this->deskewed_scan)); -+ (std::make_shared>(*this->deskewed_scan)); - this->voxel.setInputCloud(current_scan_); - this->voxel.filter(*current_scan_); - this->current_scan = current_scan_; -@@ -596,11 +324,11 @@ void dlio::OdomNode::preprocessPoints() { - - void dlio::OdomNode::deskewPointcloud() { - -- pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr deskewed_scan_ (std::make_shared>()); - deskewed_scan_->points.resize(this->original_scan->points.size()); - - // individual point timestamps should be relative to this time -- double sweep_ref_time = this->scan_header_stamp.toSec(); -+ double sweep_ref_time = this->scan_header_stamp; - - // sort points by timestamp and build list of timestamps - std::function point_time_cmp; -@@ -700,8 +428,12 @@ void dlio::OdomNode::deskewPointcloud() { - // if there are no frames between the start and end of the sweep - // that probably means that there's a sync issue - if (frames.size() != timestamps.size()) { -- ROS_FATAL("Bad time sync between LiDAR and IMU!"); -- -+ if(verbose) { -+ printf("FATAL: "); -+ printf("Bad time sync between LiDAR and IMU!"); -+ printf("\n"); -+ } -+ - this->T_prior = this->T; - pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); - this->deskewed_scan = deskewed_scan_; -@@ -755,20 +487,25 @@ void dlio::OdomNode::initializeDLIO() { - } - - this->dlio_initialized = true; -- std::cout << std::endl << " DLIO initialized!" << std::endl; -+ if (verbose) { -+ std::cout << std::endl << " DLIO initialized!" << std::endl; -+ } - - } - --void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) { -+void dlio::OdomNode::callbackPointCloud( -+ const pcl::PointCloud::ConstPtr &pc, double stamp) { - - std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); - this->main_loop_running = true; - lock.unlock(); - -- double then = ros::Time::now().toSec(); -+ double then = std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch()) -+ .count(); - - if (this->first_scan_stamp == 0.) { -- this->first_scan_stamp = pc->header.stamp.toSec(); -+ this->first_scan_stamp = stamp; - } - - // DLIO Initialization procedures (IMU calib, gravity align) -@@ -777,7 +514,7 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - } - - // Convert incoming scan into DLIO format -- this->getScanFromROS(pc); -+ this->getScanFromROS(pc, stamp); - - // Preprocess points - this->preprocessPoints(); -@@ -787,13 +524,16 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - } - - if (this->current_scan->points.size() <= this->gicp_min_num_points_) { -- ROS_FATAL("Low number of points in the cloud!"); -- return; -+ if (verbose) { -+ printf("FATAL: "); -+ printf("Low number of points in the cloud!"); -+ printf("\n"); -+ return; -+ } - } - - // Compute Metrics -- this->metrics_thread = std::thread( &dlio::OdomNode::computeMetrics, this ); -- this->metrics_thread.detach(); -+ this->computeMetrics(); - - // Set Adaptive Parameters - if (this->adaptive_params_) { -@@ -839,50 +579,43 @@ void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& - this->prev_scan_stamp = this->scan_stamp; - this->elapsed_time = this->scan_stamp - this->first_scan_stamp; - -- // Publish stuff to ROS -- pcl::PointCloud::ConstPtr published_cloud; -- if (this->densemap_filtered_) { -- published_cloud = this->current_scan; -- } else { -- published_cloud = this->deskewed_scan; -- } -- this->publish_thread = std::thread( &dlio::OdomNode::publishToROS, this, published_cloud, this->T_corr ); -- this->publish_thread.detach(); -- - // Update some statistics -- this->comp_times.push_back(ros::Time::now().toSec() - then); -+ this->comp_times.push_back( -+ std::chrono::duration_cast>( -+ std::chrono::system_clock::now().time_since_epoch()) -+ .count() - -+ then); - this->gicp_hasConverged = this->gicp.hasConverged(); - - // Debug statements and publish custom DLIO message - if (this->verbose) { -- this->debug_thread = std::thread( &dlio::OdomNode::debug, this ); -- this->debug_thread.detach(); -+ this->debug(); - } - - this->geo.first_opt_done = true; - } - --void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -+void dlio::OdomNode::callbackImu(const ImuMeas &imu_raw) { - - this->first_imu_received = true; - -- sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw ); -- this->imu_stamp = imu->header.stamp; -+ ImuMeas imu = this->transformImu(imu_raw); -+ this->imu_stamp = imu.stamp; - - Eigen::Vector3f lin_accel; - Eigen::Vector3f ang_vel; - - // Get IMU samples -- ang_vel[0] = imu->angular_velocity.x; -- ang_vel[1] = imu->angular_velocity.y; -- ang_vel[2] = imu->angular_velocity.z; -+ ang_vel[0] = imu.ang_vel[0]; -+ ang_vel[1] = imu.ang_vel[1]; -+ ang_vel[2] = imu.ang_vel[2]; - -- lin_accel[0] = imu->linear_acceleration.x; -- lin_accel[1] = imu->linear_acceleration.y; -- lin_accel[2] = imu->linear_acceleration.z; -+ lin_accel[0] = imu.lin_accel[0]; -+ lin_accel[1] = imu.lin_accel[1]; -+ lin_accel[2] = imu.lin_accel[2]; - - if (this->first_imu_stamp == 0.) { -- this->first_imu_stamp = imu->header.stamp.toSec(); -+ this->first_imu_stamp = imu.stamp; - } - - // IMU calibration procedure - do for three seconds -@@ -893,7 +626,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - static Eigen::Vector3f accel_avg (0., 0., 0.); - static bool print = true; - -- if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) { -+ if ((imu.stamp - this->first_imu_stamp) < this->imu_calib_time_) { - - num_samples++; - -@@ -905,7 +638,7 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - accel_avg[1] += lin_accel[1]; - accel_avg[2] += lin_accel[2]; - -- if(print) { -+ if(verbose) { - std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; - std::cout.flush(); - print = false; -@@ -913,7 +646,9 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - - } else { - -- std::cout << "done" << std::endl << std::endl; -+ if(verbose) { -+ std::cout << "done" << std::endl << std::endl; -+ } - - gyro_avg /= num_samples; - accel_avg /= num_samples; -@@ -943,44 +678,47 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - pitch = remainder(180.0 - pitch, 360.0); - roll = remainder(roll + 180.0, 360.0); - } -- std::cout << " Estimated initial attitude:" << std::endl; -- std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; -- std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; -- std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; -- std::cout << std::endl; -+ if (verbose){ -+ std::cout << " Estimated initial attitude:" << std::endl; -+ std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; -+ std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; -+ std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; -+ std::cout << std::endl; -+ } - } - - if (this->calibrate_accel_) { - - // subtract gravity from avg accel to get bias - this->state.b.accel = accel_avg - grav_vec; -- -- std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " -- << to_string_with_precision(this->state.b.accel[1], 8) << ", " -- << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; -+ if (verbose) { -+ std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " -+ << to_string_with_precision(this->state.b.accel[1], 8) << ", " -+ << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; -+ } - } - - if (this->calibrate_gyro_) { - - this->state.b.gyro = gyro_avg; -- -- std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " -- << to_string_with_precision(this->state.b.gyro[1], 8) << ", " -- << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; -+ if(verbose) { -+ std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " -+ << to_string_with_precision(this->state.b.gyro[1], 8) << ", " -+ << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; -+ } - } - - this->imu_calibrated = true; -- - } - - } else { - -- double dt = imu->header.stamp.toSec() - this->prev_imu_stamp; -+ double dt = imu.stamp - this->prev_imu_stamp; - if (dt == 0) { dt = 1.0/200.0; } - this->imu_rates.push_back( 1./dt ); - - // Apply the calibrated bias to the new IMU measurements -- this->imu_meas.stamp = imu->header.stamp.toSec(); -+ this->imu_meas.stamp = imu.stamp; - this->imu_meas.dt = dt; - this->prev_imu_stamp = this->imu_meas.stamp; - -@@ -990,7 +728,8 @@ void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { - this->imu_meas.lin_accel = lin_accel_corrected; - this->imu_meas.ang_vel = ang_vel_corrected; - -- // Store calibrated IMU measurements into imu buffer for manual integration later. -+ // Store calibrated IMU measurements into imu buffer for manual integration -+ // later. - this->mtx_imu.lock(); - this->imu_buffer.push_front(this->imu_meas); - this->mtx_imu.unlock(); -@@ -1027,7 +766,7 @@ void dlio::OdomNode::getNextPose() { - } - - // Align with current submap with global IMU transformation as initial guess -- pcl::PointCloud::Ptr aligned (boost::make_shared>()); -+ pcl::PointCloud::Ptr aligned (std::make_shared>()); - this->gicp.align(*aligned); - - // Get final transformation in global frame -@@ -1046,11 +785,12 @@ void dlio::OdomNode::getNextPose() { - bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, - boost::circular_buffer::reverse_iterator& begin_imu_it, - boost::circular_buffer::reverse_iterator& end_imu_it) { -- -+ // Evalio run deterministically, nothing will ever appear here! - if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { -- // Wait for the latest IMU data -- std::unique_lockmtx_imu)> lock(this->mtx_imu); -- this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); -+ // // Wait for the latest IMU data -+ // std::unique_lockmtx_imu)> lock(this->mtx_imu); -+ // this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); -+ return false; - } - - auto imu_it = this->imu_buffer.begin(); -@@ -1268,7 +1008,6 @@ void dlio::OdomNode::propagateGICP() { - double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); - q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; - this->lidarPose.q = q; -- - } - - void dlio::OdomNode::propagateState() { -@@ -1369,36 +1108,36 @@ void dlio::OdomNode::updateState() { - - } - --sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { -+dlio::OdomNode::ImuMeas dlio::OdomNode::transformImu(const ImuMeas &imu_raw) { - -- sensor_msgs::Imu::Ptr imu (new sensor_msgs::Imu); -+ ImuMeas imu; - - // Copy header -- imu->header = imu_raw->header; -+ imu.stamp = imu_raw.stamp; - -- static double prev_stamp = imu->header.stamp.toSec(); -- double dt = imu->header.stamp.toSec() - prev_stamp; -- prev_stamp = imu->header.stamp.toSec(); -+ static double prev_stamp = imu.stamp; -+ double dt = imu.stamp - prev_stamp; -+ prev_stamp = imu.stamp; - - if (dt == 0) { dt = 1.0/200.0; } - -- // Transform angular velocity (will be the same on a rigid body, so just rotate to ROS convention) -- Eigen::Vector3f ang_vel(imu_raw->angular_velocity.x, -- imu_raw->angular_velocity.y, -- imu_raw->angular_velocity.z); -+ // Transform angular velocity (will be the same on a rigid body, so just -+ // rotate to ROS convention) -+ Eigen::Vector3f ang_vel(imu_raw.ang_vel[0], imu_raw.ang_vel[1], -+ imu_raw.ang_vel[2]); - - Eigen::Vector3f ang_vel_cg = this->extrinsics.baselink2imu.R * ang_vel; - -- imu->angular_velocity.x = ang_vel_cg[0]; -- imu->angular_velocity.y = ang_vel_cg[1]; -- imu->angular_velocity.z = ang_vel_cg[2]; -+ imu.ang_vel[0] = ang_vel_cg[0]; -+ imu.ang_vel[1] = ang_vel_cg[1]; -+ imu.ang_vel[2] = ang_vel_cg[2]; - - static Eigen::Vector3f ang_vel_cg_prev = ang_vel_cg; - -- // Transform linear acceleration (need to account for component due to translational difference) -- Eigen::Vector3f lin_accel(imu_raw->linear_acceleration.x, -- imu_raw->linear_acceleration.y, -- imu_raw->linear_acceleration.z); -+ // Transform linear acceleration (need to account for component due to -+ // translational difference) -+ Eigen::Vector3f lin_accel(imu_raw.lin_accel[0], imu_raw.lin_accel[1], -+ imu_raw.lin_accel[2]); - - Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; - -@@ -1408,9 +1147,9 @@ sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::Const - - ang_vel_cg_prev = ang_vel_cg; - -- imu->linear_acceleration.x = lin_accel_cg[0]; -- imu->linear_acceleration.y = lin_accel_cg[1]; -- imu->linear_acceleration.z = lin_accel_cg[2]; -+ imu.lin_accel[0] = lin_accel_cg[0]; -+ imu.lin_accel[1] = lin_accel_cg[1]; -+ imu.lin_accel[2] = lin_accel_cg[2]; - - return imu; - -@@ -1471,7 +1210,7 @@ void dlio::OdomNode::computeConvexHull() { - - // create a pointcloud with points at keyframes - pcl::PointCloud::Ptr cloud = -- pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); - - std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); - for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1488,10 +1227,10 @@ void dlio::OdomNode::computeConvexHull() { - - // get the indices of the keyframes on the convex hull - pcl::PointCloud::Ptr convex_points = -- pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); - this->convex_hull.reconstruct(*convex_points); - -- pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); - this->convex_hull.getHullPointIndices(*convex_hull_point_idx); - - this->keyframe_convex.clear(); -@@ -1510,7 +1249,7 @@ void dlio::OdomNode::computeConcaveHull() { - - // create a pointcloud with points at keyframes - pcl::PointCloud::Ptr cloud = -- pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); - - std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); - for (int i = 0; i < this->num_processed_keyframes; i++) { -@@ -1527,10 +1266,10 @@ void dlio::OdomNode::computeConcaveHull() { - - // get the indices of the keyframes on the concave hull - pcl::PointCloud::Ptr concave_points = -- pcl::PointCloud::Ptr (boost::make_shared>()); -+ pcl::PointCloud::Ptr (std::make_shared>()); - this->concave_hull.reconstruct(*concave_points); - -- pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); -+ pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (std::make_shared()); - this->concave_hull.getHullPointIndices(*concave_hull_point_idx); - - this->keyframe_concave.clear(); -@@ -1739,7 +1478,7 @@ void dlio::OdomNode::buildSubmap(State vehicle_state) { - this->pauseSubmapBuildIfNeeded(); - - // reinitialize submap cloud and normals -- pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); -+ pcl::PointCloud::Ptr submap_cloud_ (std::make_shared>()); - std::shared_ptr submap_normals_ (std::make_shared()); - - for (auto k : this->submap_kf_idx_curr) { -@@ -1780,7 +1519,7 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { - - Eigen::Matrix4d Td = T.cast(); - -- pcl::PointCloud::Ptr transformed_keyframe (boost::make_shared>()); -+ pcl::PointCloud::Ptr transformed_keyframe (std::make_shared>()); - pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); - - std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); -@@ -1792,9 +1531,6 @@ void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { - lock.lock(); - this->keyframes[i].second = transformed_keyframe; - this->keyframe_normals[i] = transformed_covariances; -- -- this->publish_keyframe_thread = std::thread( &dlio::OdomNode::publishKeyframe, this, this->keyframes[i], this->keyframe_timestamps[i] ); -- this->publish_keyframe_thread.detach(); - } - - lock.unlock(); -@@ -2018,3 +1754,19 @@ void dlio::OdomNode::debug() { - std::cout << "+-------------------------------------------------------------------+" << std::endl; - - } -+ -+// Explicit instantiations of PCL templates for dlio::Point -+// This is needed because PCL doesn't automatically instantiate templates for -+// custom point types -+#include -+#include -+#include -+#include -+#include -+#include -+ -+template class pcl::PCLBase; -+template class pcl::search::Search; -+template class pcl::KdTreeFLANN>; -+template class pcl::search::KdTree< -+ dlio::Point, pcl::KdTreeFLANN>>; -\ No newline at end of file -diff --git a/src/nano_gicp/lsq_registration.cc b/src/nano_gicp/lsq_registration.cc -index 271cb03..b6c8f07 100644 ---- a/src/nano_gicp/lsq_registration.cc -+++ b/src/nano_gicp/lsq_registration.cc -@@ -42,10 +42,8 @@ - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *************************************************************************/ - --#include "dlio/dlio.h" - #include "nano_gicp/lsq_registration.h" -- --template class nano_gicp::LsqRegistration; -+#include "dlio/dlio.h" - - namespace nano_gicp { - -@@ -228,4 +226,7 @@ bool LsqRegistration::step_lm(Eigen::Isometry3d& x0, E - return false; - } - --} // namespace nano_gicp -+} // namespace nano_gicp -+ -+// Explicit template instantiation must come after all member definitions -+template class nano_gicp::LsqRegistration; -diff --git a/src/nano_gicp/nano_gicp.cc b/src/nano_gicp/nano_gicp.cc -index 906e3c3..b9c67a4 100644 ---- a/src/nano_gicp/nano_gicp.cc -+++ b/src/nano_gicp/nano_gicp.cc -@@ -42,10 +42,8 @@ - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *************************************************************************/ - --#include "dlio/dlio.h" - #include "nano_gicp/nano_gicp.h" -- --template class nano_gicp::NanoGICP; -+#include "dlio/dlio.h" - - namespace nano_gicp { - -@@ -391,4 +389,7 @@ bool NanoGICP::calculate_covariances( - return true; - } - --} // namespace nano_gicp -+} // namespace nano_gicp -+ -+// Explicit template instantiation must come after all member definitions -+template class nano_gicp::NanoGICP; diff --git a/cpp/setup_pipelines.sh b/cpp/setup_pipelines.sh index 21e24249..a56a97a3 100755 --- a/cpp/setup_pipelines.sh +++ b/cpp/setup_pipelines.sh @@ -65,13 +65,11 @@ git apply ../../pipelines/ct_icp.patch cd .. if [ ! -d "direct_lidar_inertial_odometry" ]; then - git clone https://github.com/vectr-ucla/direct_lidar_inertial_odometry.git + git clone https://github.com/contagon/direct_lidar_inertial_odometry.git fi cd direct_lidar_inertial_odometry git stash -# master branch as of writing -git checkout fc8d183f18cdcfb9bb4fc754c6d373cedc4cbd04 -git apply ../../pipelines/dlio.patch +git checkout master cd .. # ------------------------- Dependencies ------------------------- # From cace108688c4c590c7b45eff3aec3da95915a0f9 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Thu, 9 Apr 2026 14:00:39 -0400 Subject: [PATCH 11/11] Add in visualization to DLIO --- .gitignore | 5 ++++- cpp/bindings/pipelines/dlio.h | 6 ++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index 0369c469..fa89dd4c 100644 --- a/.gitignore +++ b/.gitignore @@ -29,4 +29,7 @@ data compile_commands.json # cloned pipelines -cpp/bindings/pipelines-src \ No newline at end of file +cpp/bindings/pipelines-src + +# beads +.beads/ diff --git a/cpp/bindings/pipelines/dlio.h b/cpp/bindings/pipelines/dlio.h index 9b1eb97a..b5f542f9 100644 --- a/cpp/bindings/pipelines/dlio.h +++ b/cpp/bindings/pipelines/dlio.h @@ -131,8 +131,7 @@ class DLIO: public ev::Pipeline { // Getters const std::map> map() override { - // DLIO doesn't expose map retrieval in the same way, return empty for now - return std::map>(); + return ev::make_map("point", *dlio_->getMap()); } // Setters @@ -176,6 +175,9 @@ class DLIO: public ev::Pipeline { 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: