cuvslam: IMU noise model from the imu_info stream

CMakeLists.txt

@@ -58,6 +58,8 @@ message(STATUS "cuVSLAM library: ${CUVSLAM_LIBRARY}")
 add_executable(cuvslam_odometry src/cuvslam_main.cpp)
 target_include_directories(cuvslam_odometry PRIVATE
   ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs
+  # Vendored lcm-gen output for types dimos-lcm does not ship yet (ImuInfo).
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/vendor
   ${LCM_INCLUDE_DIRS}
   ${CUVSLAM_INCLUDE_DIR}
 )

src/cuvslam_main.cpp

@@ -4,7 +4,7 @@
 // NVIDIA cuVSLAM visual odometry as a dimos native module.
 //
 // in:  image, camera_info (every camera on the one stream, told apart by frame_id),
-//      tf, depth_image, imu
+//      tf, depth_image, imu, imu_info (the IMU's noise model, published like camera_info)
 // out: odometry, corrected_odometry, tf
 //
 // Nothing is published while tracking is lost. cuVSLAM keeps one world frame for the
@@ -27,6 +27,7 @@
 #include "sensor_msgs/CameraInfo.hpp"
 #include "sensor_msgs/Image.hpp"
 #include "sensor_msgs/Imu.hpp"
+#include "sensor_msgs/ImuInfo.hpp"
 #include "tf2_msgs/TFMessage.hpp"
 #include "utils/depth_reproject.hpp"
 #include <Eigen/Geometry>
@@ -111,15 +112,9 @@ struct CuvslamConfig {
     /// 0 disables that limit.
     double speed_gate_max_linear;
     double speed_gate_max_angular;
-    /// cuVSLAM's Inertial mode is stereo plus one IMU.
+    /// cuVSLAM's Inertial mode is stereo plus one IMU. The noise model arrives on the
+    /// imu_info stream, the way camera intrinsics arrive on camera_info.
     bool enable_imu;
-    /// Flattened from the python ImuCalibration. All zero means enable_imu is off.
-    double imu_gyro_noise_density;
-    double imu_gyro_random_walk;
-    double imu_accel_noise_density;
-    double imu_accel_random_walk;
-    /// The rate actually fed. Declaring more than arrives never initialises alignment.
-    double imu_frequency;
     /// rgbd only: raw depth units per metre. 1000 for sixteen-bit millimetres.
     double depth_units_per_meter;
 };
@@ -145,6 +140,7 @@ class CuvslamOdometry : public Module {
         }
         if (cfg_.enable_imu) {
             builder.input<sensor_msgs::Imu>("imu", &CuvslamOdometry::on_imu, this);
+            builder.input<sensor_msgs::ImuInfo>("imu_info", &CuvslamOdometry::on_imu_info, this);
         }
 
         odometry_ = builder.output<nav_msgs::Odometry>("odometry");
@@ -298,11 +294,19 @@ class CuvslamOdometry : public Module {
         return -1;
     }
 
+    /// The IMU's noise model, published by the driver the way camera_info is.
+    void on_imu_info(const sensor_msgs::ImuInfo& info) {
+        if (tracker_) {
+            return;  // rig is fixed once the tracker exists
+        }
+        imu_info_ = info;
+        resolve_rig();
+    }
+
     /// Buffered. cuVSLAM requires Track() and RegisterImuMeasurement() in
     /// non-decreasing timestamp order; the round-robin dispatcher lets images
     /// overtake a 400 Hz IMU.
     void on_imu(const sensor_msgs::Imu& msg) {
-        imu_frame_ = msg.header.frame_id;
         if (!tracker_) {
             // No tracker yet; this is the window inertial init needs.
             ++imu_dropped_;
@@ -436,22 +440,27 @@ class CuvslamOdometry : public Module {
             rig.cameras.push_back(camera);
         }
         if (cfg_.enable_imu) {
+            if (!imu_info_) {
+                DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10),
+                                    "cuvslam: enable_imu is on but no imu_info has arrived");
+                return;
+            }
+            const std::string& imu_frame = imu_info_->header.frame_id;
             const std::optional<tf_client::Transform> rig_from_imu =
-                imu_frame_.empty() ? std::nullopt
-                                   : tf_client_.get_latest(rig_frame(), imu_frame_);
+                imu_frame.empty() ? std::nullopt : tf_client_.get_latest(rig_frame(), imu_frame);
             if (!rig_from_imu) {
                 DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10),
                                     "cuvslam: enable_imu is on but tf does not place the IMU",
-                                    logging::Field("imu_frame", imu_frame_));
+                                    logging::Field("imu_frame", imu_frame));
                 return;
             }
             cuvslam::ImuCalibration imu{};
             imu.rig_from_imu = to_pose(to_isometry(rig_from_imu->rigid));
-            imu.gyroscope_noise_density = static_cast<float>(cfg_.imu_gyro_noise_density);
-            imu.gyroscope_random_walk = static_cast<float>(cfg_.imu_gyro_random_walk);
-            imu.accelerometer_noise_density = static_cast<float>(cfg_.imu_accel_noise_density);
-            imu.accelerometer_random_walk = static_cast<float>(cfg_.imu_accel_random_walk);
-            imu.frequency = static_cast<float>(cfg_.imu_frequency);
+            imu.gyroscope_noise_density = static_cast<float>(imu_info_->gyro_noise_density);
+            imu.gyroscope_random_walk = static_cast<float>(imu_info_->gyro_random_walk);
+            imu.accelerometer_noise_density = static_cast<float>(imu_info_->accel_noise_density);
+            imu.accelerometer_random_walk = static_cast<float>(imu_info_->accel_random_walk);
+            imu.frequency = static_cast<float>(imu_info_->frequency);
             rig.imus = {imu};
         }
 
@@ -771,7 +780,7 @@ class CuvslamOdometry : public Module {
     std::vector<RigCamera> cameras_;
     std::unordered_map<std::string, sensor_msgs::CameraInfo> camera_info_;
     std::uint64_t unplaced_images_{0};
-    std::string imu_frame_;
+    std::optional<sensor_msgs::ImuInfo> imu_info_;
 
     /// child frame -> (its parent, parent_from_child). The bound is a cycle guard.
     Tf tf_client_;

src/vendor/sensor_msgs/ImuInfo.hpp

@@ -0,0 +1,202 @@
+/** THIS IS AN AUTOMATICALLY GENERATED FILE.  DO NOT MODIFY
+ * BY HAND!!
+ *
+ * Generated by lcm-gen 1.5.2
+ **/
+
+#ifndef __sensor_msgs_ImuInfo_hpp__
+#define __sensor_msgs_ImuInfo_hpp__
+
+#include <lcm/lcm_coretypes.h>
+
+#include "std_msgs/Header.hpp"
+
+namespace sensor_msgs
+{
+
+class ImuInfo
+{
+    public:
+        std_msgs::Header header;
+
+        double     gyro_noise_density;
+
+        double     gyro_random_walk;
+
+        double     accel_noise_density;
+
+        double     accel_random_walk;
+
+        double     frequency;
+
+    public:
+        /**
+         * Encode a message into binary form.
+         *
+         * @param buf The output buffer.
+         * @param offset Encoding starts at thie byte offset into @p buf.
+         * @param maxlen Maximum number of bytes to write.  This should generally be
+         *  equal to getEncodedSize().
+         * @return The number of bytes encoded, or <0 on error.
+         */
+        inline int encode(void *buf, int offset, int maxlen) const;
+
+        /**
+         * Check how many bytes are required to encode this message.
+         */
+        inline int getEncodedSize() const;
+
+        /**
+         * Decode a message from binary form into this instance.
+         *
+         * @param buf The buffer containing the encoded message.
+         * @param offset The byte offset into @p buf where the encoded message starts.
+         * @param maxlen The maximum number of bytes to read while decoding.
+         * @return The number of bytes decoded, or <0 if an error occured.
+         */
+        inline int decode(const void *buf, int offset, int maxlen);
+
+        /**
+         * Retrieve the 64-bit fingerprint identifying the structure of the message.
+         * Note that the fingerprint is the same for all instances of the same
+         * message type, and is a fingerprint on the message type definition, not on
+         * the message contents.
+         */
+        inline static int64_t getHash();
+
+        /**
+         * Returns "ImuInfo"
+         */
+        inline static const char* getTypeName();
+
+        // LCM support functions. Users should not call these
+        inline int _encodeNoHash(void *buf, int offset, int maxlen) const;
+        inline int _getEncodedSizeNoHash() const;
+        inline int _decodeNoHash(const void *buf, int offset, int maxlen);
+        inline static uint64_t _computeHash(const __lcm_hash_ptr *p);
+};
+
+int ImuInfo::encode(void *buf, int offset, int maxlen) const
+{
+    int pos = 0, tlen;
+    int64_t hash = getHash();
+
+    tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = this->_encodeNoHash(buf, offset + pos, maxlen - pos);
+    if (tlen < 0) return tlen; else pos += tlen;
+
+    return pos;
+}
+
+int ImuInfo::decode(const void *buf, int offset, int maxlen)
+{
+    int pos = 0, thislen;
+
+    int64_t msg_hash;
+    thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &msg_hash, 1);
+    if (thislen < 0) return thislen; else pos += thislen;
+    if (msg_hash != getHash()) return -1;
+
+    thislen = this->_decodeNoHash(buf, offset + pos, maxlen - pos);
+    if (thislen < 0) return thislen; else pos += thislen;
+
+    return pos;
+}
+
+int ImuInfo::getEncodedSize() const
+{
+    return 8 + _getEncodedSizeNoHash();
+}
+
+int64_t ImuInfo::getHash()
+{
+    static int64_t hash = static_cast<int64_t>(_computeHash(NULL));
+    return hash;
+}
+
+const char* ImuInfo::getTypeName()
+{
+    return "ImuInfo";
+}
+
+int ImuInfo::_encodeNoHash(void *buf, int offset, int maxlen) const
+{
+    int pos = 0, tlen;
+
+    tlen = this->header._encodeNoHash(buf, offset + pos, maxlen - pos);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->gyro_noise_density, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->gyro_random_walk, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->accel_noise_density, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->accel_random_walk, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->frequency, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    return pos;
+}
+
+int ImuInfo::_decodeNoHash(const void *buf, int offset, int maxlen)
+{
+    int pos = 0, tlen;
+
+    tlen = this->header._decodeNoHash(buf, offset + pos, maxlen - pos);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->gyro_noise_density, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->gyro_random_walk, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->accel_noise_density, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->accel_random_walk, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->frequency, 1);
+    if(tlen < 0) return tlen; else pos += tlen;
+
+    return pos;
+}
+
+int ImuInfo::_getEncodedSizeNoHash() const
+{
+    int enc_size = 0;
+    enc_size += this->header._getEncodedSizeNoHash();
+    enc_size += __double_encoded_array_size(NULL, 1);
+    enc_size += __double_encoded_array_size(NULL, 1);
+    enc_size += __double_encoded_array_size(NULL, 1);
+    enc_size += __double_encoded_array_size(NULL, 1);
+    enc_size += __double_encoded_array_size(NULL, 1);
+    return enc_size;
+}
+
+uint64_t ImuInfo::_computeHash(const __lcm_hash_ptr *p)
+{
+    const __lcm_hash_ptr *fp;
+    for(fp = p; fp != NULL; fp = fp->parent)
+        if(fp->v == ImuInfo::getHash)
+            return 0;
+    const __lcm_hash_ptr cp = { p, ImuInfo::getHash };
+
+    uint64_t hash = 0x437a196cbd6b4e49LL +
+         std_msgs::Header::_computeHash(&cp);
+
+    return (hash<<1) + ((hash>>63)&1);
+}
+
+}
+
+#endif