diff --git a/CMakeLists.txt b/CMakeLists.txt index 154aac9..68bb066 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/src/cuvslam_main.cpp b/src/cuvslam_main.cpp index 2d04d44..377a130 100644 --- a/src/cuvslam_main.cpp +++ b/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 @@ -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("imu", &CuvslamOdometry::on_imu, this); + builder.input("imu_info", &CuvslamOdometry::on_imu_info, this); } odometry_ = builder.output("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 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(cfg_.imu_gyro_noise_density); - imu.gyroscope_random_walk = static_cast(cfg_.imu_gyro_random_walk); - imu.accelerometer_noise_density = static_cast(cfg_.imu_accel_noise_density); - imu.accelerometer_random_walk = static_cast(cfg_.imu_accel_random_walk); - imu.frequency = static_cast(cfg_.imu_frequency); + imu.gyroscope_noise_density = static_cast(imu_info_->gyro_noise_density); + imu.gyroscope_random_walk = static_cast(imu_info_->gyro_random_walk); + imu.accelerometer_noise_density = static_cast(imu_info_->accel_noise_density); + imu.accelerometer_random_walk = static_cast(imu_info_->accel_random_walk); + imu.frequency = static_cast(imu_info_->frequency); rig.imus = {imu}; } @@ -771,7 +780,7 @@ class CuvslamOdometry : public Module { std::vector cameras_; std::unordered_map camera_info_; std::uint64_t unplaced_images_{0}; - std::string imu_frame_; + std::optional imu_info_; /// child frame -> (its parent, parent_from_child). The bound is a cycle guard. Tf tf_client_; diff --git a/src/vendor/sensor_msgs/ImuInfo.hpp b/src/vendor/sensor_msgs/ImuInfo.hpp new file mode 100644 index 0000000..a55c6f7 --- /dev/null +++ b/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 + +#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(_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