From 7dc978be714787ae04506440cb1d4817711079f7 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 15 Nov 2024 10:42:30 -0800 Subject: [PATCH 1/2] Optionally, generate estimates for imu orientation based on imu data --- src/imu_model.h | 58 +++++++++++++++++++++++++++++++++++++++ src/imu_packet_handler.h | 28 ++++++++++++++++--- src/os_cloud_nodelet.cpp | 4 ++- src/os_driver_nodelet.cpp | 4 ++- 4 files changed, 88 insertions(+), 6 deletions(-) create mode 100644 src/imu_model.h diff --git a/src/imu_model.h b/src/imu_model.h new file mode 100644 index 00000000..e544e743 --- /dev/null +++ b/src/imu_model.h @@ -0,0 +1,58 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + * + * @file imu-model.h + * @brief provide orientation esimtate based on imu data + */ + +#pragma once + +#include + +// A class that can estimates the current orientation based on imu telemetry +class ImuModel { + public: + /* + * TODO: Add description + * param: orientation represented as euler + * TODO: add initial angular velocity + */ + virtual void set_initial_state(const Eigen::Vector3d& orientation) = 0; + + /* + * TODO: Update + * params la: linear acceleration + * params av: angular velocity + */ + virtual Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) = 0; +}; + + +class SimpleImuModel : public ImuModel { + public: + void set_initial_state(const Eigen::Vector3d& orientation) override { + this->orientation = orientation; + } + + Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) override { + // TODO: compute delta t + double dt = 0.01; + + orientation.x() += av.x() * dt; + orientation.y() += av.y() * dt; + orientation.z() += av.z() * dt; + orientation.x() += atan2(la.y(), la.z()); + orientation.y() += atan2(-la.x(), sqrt(la.y() * la.y() + la.z() * la.z())); + + Eigen::Quaterniond q = + Eigen::AngleAxisd(orientation.x(), Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(orientation.y(), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(orientation.z(), Eigen::Vector3d::UnitZ()); + return q; + } + +private: + // using euler angles for now + Eigen::Vector3d orientation; +}; \ No newline at end of file diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h index 81b9ce4c..c2f286b3 100644 --- a/src/imu_packet_handler.h +++ b/src/imu_packet_handler.h @@ -14,6 +14,8 @@ #include "ouster_ros/os_ros.h" // clang-format on + +#include "imu_model.h" namespace ouster_ros { class ImuPacketHandler { @@ -25,7 +27,8 @@ class ImuPacketHandler { static HandlerType create_handler(const sensor::sensor_info& info, const std::string& frame, const std::string& timestamp_mode, - int64_t ptp_utc_tai_offset) { + int64_t ptp_utc_tai_offset, + bool estimate_orientation) { const auto& pf = sensor::get_format(info); using Timestamper = std::function; Timestamper timestamper; @@ -49,9 +52,26 @@ class ImuPacketHandler { }}; } - return [&pf, &frame, timestamper](const sensor::ImuPacket& imu_packet) { - return packet_to_imu_msg(pf, timestamper(imu_packet), frame, - imu_packet.buf.data()); + std::shared_ptr imu_model; + if (estimate_orientation) { + imu_model = std::make_shared(); + imu_model->set_initial_state(Eigen::Vector3d(0.0, 0.0, 0.0)); + } + + return [&pf, &frame, timestamper, estimate_orientation, imu_model](const sensor::ImuPacket& imu_packet) { + sensor_msgs::Imu msg = packet_to_imu_msg(pf, timestamper(imu_packet), + frame, imu_packet.buf.data()); + if (estimate_orientation) { + auto ts = pf.imu_gyro_ts(imu_packet.buf.data()); + Eigen::Vector3d la(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z); + Eigen::Vector3d av(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z); + Eigen::Quaternion q = imu_model->update(ts, la, av); + msg.orientation.x = q.x(); + msg.orientation.y = q.y(); + msg.orientation.z = q.z(); + msg.orientation.w = q.w(); + } + return msg; }; } }; diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index d3c81c09..c18d6bf3 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -154,11 +154,13 @@ class OusterCloud : public nodelet::Nodelet { auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); + auto estimate_imu_orientation = pnh.param("estimate_imu_orientation", false); if (impl::check_token(tokens, "IMU")) { imu_packet_handler = ImuPacketHandler::create_handler( info, tf_bcast.imu_frame_id(), timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); + static_cast(ptp_utc_tai_offset * 1e+9), + estimate_imu_orientation); } std::vector processors; diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 34984915..1d83fbf6 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -115,11 +115,13 @@ class OusterDriver : public OusterSensor { auto timestamp_mode = pnh.param("timestamp_mode", std::string{}); double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0); + auto estimate_imu_orientation = pnh.param("estimate_imu_orientation", false); if (impl::check_token(tokens, "IMU")) { imu_packet_handler = ImuPacketHandler::create_handler( info, tf_bcast.imu_frame_id(), timestamp_mode, - static_cast(ptp_utc_tai_offset * 1e+9)); + static_cast(ptp_utc_tai_offset * 1e+9), + estimate_imu_orientation); } std::vector processors; From a072dc89fe697def2078e7db4b371f0ab6d6aca9 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 15 Nov 2024 15:55:31 -0800 Subject: [PATCH 2/2] Use the initial orientation --- src/imu_model.h | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/imu_model.h b/src/imu_model.h index e544e743..4a3bbbef 100644 --- a/src/imu_model.h +++ b/src/imu_model.h @@ -18,7 +18,7 @@ class ImuModel { * param: orientation represented as euler * TODO: add initial angular velocity */ - virtual void set_initial_state(const Eigen::Vector3d& orientation) = 0; + virtual void set_initial_state(const Eigen::Vector3d& initial_orientation) = 0; /* * TODO: Update @@ -31,17 +31,15 @@ class ImuModel { class SimpleImuModel : public ImuModel { public: - void set_initial_state(const Eigen::Vector3d& orientation) override { - this->orientation = orientation; + void set_initial_state(const Eigen::Vector3d& initial_orientation) override { + this->initial_orientation = initial_orientation; } Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) override { - // TODO: compute delta t + // TODO: compute dt from consecutive ts double dt = 0.01; - orientation.x() += av.x() * dt; - orientation.y() += av.y() * dt; - orientation.z() += av.z() * dt; + Eigen::Vector3d orientation = initial_orientation + av * dt; orientation.x() += atan2(la.y(), la.z()); orientation.y() += atan2(-la.x(), sqrt(la.y() * la.y() + la.z() * la.z())); @@ -54,5 +52,5 @@ class SimpleImuModel : public ImuModel { private: // using euler angles for now - Eigen::Vector3d orientation; + Eigen::Vector3d initial_orientation; }; \ No newline at end of file