Fix logic error in offline node (ros) and interval imu stats compute (core) (#29)

* cleaner error messages and log parsed imu and lidar frames for ros

* patch rviz config to change the global frame if a different odom frame is used

* revert offline node bag and buffer completion handling logic

* cannot compute accel mag variance if imu count is 1. modify the check preventing divide by 0 errors

* remove python/config/default.yaml since --dump_config does the same job

* bump version to v0.1.1 and update python readme slightly

Author: mehermvr

.github/ISSUE_TEMPLATE/bug_report.md

@@ -1,12 +1,18 @@
 ---
 name: Bug report
-about: Create a report in case rko lio is not working as expected
+about: Please use this if rko lio has an error, otherwise use the blank template
 title: "[BUG]"
 labels: bug
 assignees: ''
 
 ---
 
+<!--
+If you think your issue doesn't require all the following details, use the blank issue template instead.
+Please do note however, if you do fill in all the following details, you make life easier for me to debug the error.
+Otherwise, I might have to ask you some of these details again anyways.
+-->
+
 **Environment**
 - Which OS are you using?
 - What version of Python (eg., 3.10) or ROS (e.g., Jazzy) are you using?

cpp/rko_lio/core/lio.cpp

@@ -254,10 +254,8 @@ namespace rko_lio::core {
 // ==========================
 
 void LIO::initialize(const Secondsd lidar_time) {
-  std::cout << "Initializing LIO.\n";
-
   if (interval_stats.imu_count == 0) {
-    std::cerr << "WARNING: Cannot initialize. No imu measurements received.\n";
+    std::cerr << "[WARNING] Cannot initialize. No imu measurements received.\n";
     _initialized = true;
     return;
   }
@@ -283,14 +281,18 @@ void LIO::initialize(const Secondsd lidar_time) {
 
 // use the acceleration kalman filter to compute the two values we need for ori. reg.
 std::optional<AccelInfo> LIO::get_accel_info(const Sophus::SO3d& rotation_estimate, const Secondsd& time) {
-  if (interval_stats.imu_count == 0) {
-    // avoid nans, divide by 0
+  if (interval_stats.imu_count <= 1) {
+    std::cerr << "[WARNING] " << interval_stats.imu_count
+              << " IMU message(s) in interval between two lidar scans. Cannot compute "
+                 "acceleration statistics for orientation regularisation. Please check your data and its "
+                 "timestamping as likely there should not be so few IMU measurements between two LiDAR scans.\n";
     return std::nullopt;
   }
 
   const Eigen::Vector3d avg_imu_accel = interval_stats.imu_acceleration_sum / interval_stats.imu_count;
   const double accel_mag_variance = interval_stats.welford_sum_of_squares / (interval_stats.imu_count - 1);
   const double dt = (time - lidar_state.time).count();
+
   const Eigen::Vector3d& body_accel_measurement = avg_imu_accel + rotation_estimate.inverse() * gravity();
 
   const double max_acceleration_change = config.max_expected_jerk * dt;
@@ -320,7 +322,11 @@ std::optional<AccelInfo> LIO::get_accel_info(const Sophus::SO3d& rotation_estima
 
 void LIO::add_imu_measurement(const ImuControl& base_imu) {
   if (lidar_state.time < EPSILON_TIME) {
-    std::cout << "Skipping IMU, waiting for first LiDAR message.\n";
+    static bool warning_skip_till_first_lidar = false;
+    if (!warning_skip_till_first_lidar) {
+      std::cerr << "[WARNING - ONCE] Skipping IMU, waiting for first LiDAR message.\n";
+      warning_skip_till_first_lidar = true;
+    }
     _last_real_imu_time = base_imu.time;
     _last_real_base_imu_ang_vel = base_imu.angular_velocity;
     return;
@@ -334,7 +340,7 @@ void LIO::add_imu_measurement(const ImuControl& base_imu) {
 
   if (dt < 0.0) {
     // messages are out of sync. thats a problem, since we integrate gyro from last lidar time onwards
-    std::cerr << "WARNING: Received IMU message from the past. Can result in errors.\n";
+    std::cerr << "[WARNING] Received IMU message from the past. Can result in errors.\n";
     // skip this imu reading?
   }
 
@@ -379,7 +385,7 @@ void LIO::add_imu_measurement(const Sophus::SE3d& extrinsic_imu2base, const ImuC
       // causes numerical issues otherwise
       static bool warning_imu_too_close = false;
       if (!warning_imu_too_close) {
-        std::cerr << "WARNING - ONCE: Received IMU message with a very short delta to previous IMU message. Ignoring "
+        std::cerr << "[WARNING - ONCE] Received IMU message with a very short delta to previous IMU message. Ignoring "
                      "all such messages.\n";
         warning_imu_too_close = true;
       }
@@ -416,13 +422,13 @@ Vector3dVector LIO::register_scan(const Vector3dVector& scan, const TimestampVec
       return {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()};
     }
     if (interval_stats.imu_count == 0) {
-      std::cerr << "WARNING: No Imu measurements in interval to average. Assuming constant velocity motion.\n";
+      std::cerr << "[WARNING] No Imu measurements in interval to average. Assuming constant velocity motion.\n";
       return {Eigen::Vector3d::Zero(), lidar_state.angular_velocity};
     }
     const Eigen::Vector3d avg_body_accel = interval_stats.body_acceleration_sum / interval_stats.imu_count;
     const Eigen::Vector3d avg_ang_vel = interval_stats.angular_velocity_sum / interval_stats.imu_count;
     if (avg_body_accel.norm() > 50.0) {
-      std::cerr << "WARNING: Erratic body acceleration computed, norm > 50 m/s2. Either IMU data is corrupted, or you "
+      std::cerr << "[WARNING] Erratic body acceleration computed, norm > 50 m/s2. Either IMU data is corrupted, or you "
                    "should report an issue.";
     }
     return {avg_body_accel, avg_ang_vel};
@@ -538,7 +544,7 @@ void LIO::dump_results_to_disk(const std::filesystem::path& results_dir, const s
       std::cout << "Configuration written to " << config_file << "\n";
     }
   } catch (const std::filesystem::filesystem_error& ex) {
-    std::cerr << "WARNING: Cannot write files to disk, encountered filesystem error: " << ex.what() << "\n";
+    std::cerr << "[WARNING] Cannot write files to disk, encountered filesystem error: " << ex.what() << "\n";
   }
 }
 } // namespace rko_lio::core

python/README.md

@@ -158,6 +158,8 @@ This is deprecated and planned to be removed in a future release. I'm prioritisi
 
 ### Configuration
 
-All configurable parameters are defined in [`config/default.yaml`](config/default.yaml).
+You can dump the default set of parameters using `rko_lio --dump_config`.
 For descriptions of each parameter, see [config.md](../docs/config.md).
+Apart from these, you can specify `extrinsic_imu2base_quat_xyzw_xyz`, `extrinsic_lidar2base_quat_xyzw_xyz` in a config file (only) if you require.
+
 Some further requirements on the data are given in [data.md](../docs/data.md).

python/config/default.yaml

@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "rko_lio"
-version = "0.1.0"
+version = "0.1.1"
 description = "A Robust Approach for LiDAR-Inertial Odometry Without Sensor-Specific Modelling"
 authors = [{ name = "Meher Malladi", email = "rm.meher97@gmail.com" }]
 readme = "README.md"

python/pyproject.toml

@@ -309,7 +309,6 @@ def cli(
     if log_results and results_dir:
         results_dir.mkdir(parents=True, exist_ok=True)
         pipeline.dump_results_to_disk(results_dir, run_name)
-        print(f"Results dumped to: {results_dir} [prefix: {run_name}]")
 
 
 if __name__ == "__main__":

python/rko_lio/cli.py

@@ -16,15 +16,16 @@ Clone the repository into a colcon workspace's `src`. From the workspace folder,
 colcon build --packages-select rko_lio # --symlink-install --event-handlers console_direct+
 ```
 
-We provide a `colcon.pkg` [file](colcon.pkg) which defines default values for some CMake arguments. Details from [build.md](../docs/build.md) apply here, but most importantly we have `RKO_LIO_FETCH_CONTENT_DEPS=ON` by default. This option handles our optional dependencies automatically.
+We provide a `colcon.pkg` [file](colcon.pkg) which defines a default CMake configuration.
+Details from [build.md](../docs/build.md) apply here, but most importantly we have `CMAKE_BUILD_TYPE=Release` and `RKO_LIO_FETCH_CONTENT_DEPS=ON` by default.
+The last option handles our optional dependencies automatically.
 
-In case you'd like to provide our optional dependencies yourself, either modify `colcon.pkg` or override those CMake flags when invoking colcon, for example, by
+In case you'd like to change the build type or provide our optional dependencies yourself, please modify `colcon.pkg` manually. 
 
-```bash
-colcon build --packages-select rko_lio --cmake-args -DRKO_LIO_FETCH_CONTENT_DEPS=OFF # --event-handlers console_direct+
-```
+> I'd earlier assumed one could override colcon.pkg arguments via --cmake-args when invoking colcon, but this doesn't seem to be the case. If anyone knows a fix, please open an issue or PR!
 
-Also consider using `Ninja` as a generator to speed up builds instead of `Make`. If you do use Make, make sure (pun intended) to parallelize the build.
+Also consider using `Ninja` as a generator to speed up builds instead of `Make`.
+If you do use Make, make sure (pun intended) to parallelize the build.
 
 ## Usage
 

ros/README.md

@@ -381,20 +381,23 @@ def prepare_rviz_config(rviz_config_file: Path, parameters: dict) -> Path:
     rviz_config_file = Path(get_package_share_directory("rko_lio")) / rviz_config_file
 
     base_frame = parameters.get("base_frame", "")
-    if not base_frame:
+    odom_frame = parameters.get("odom_frame", "")
+    if not base_frame and not odom_frame:
         return rviz_config_file  # no override needed
 
     # Load default config
     with open(rviz_config_file, "r") as f:
         rviz_cfg = yaml.safe_load(f)
 
     try:
-        rviz_cfg["Visualization Manager"]["Views"]["Current"][
-            "Target Frame"
-        ] = base_frame
+        # Patch whichever frame is specified
+        if base_frame:
+            rviz_cfg["Visualization Manager"]["Views"]["Current"]["Target Frame"] = base_frame
+        if odom_frame:
+            rviz_cfg["Visualization Manager"]["Global Options"]["Fixed Frame"] = odom_frame
     except Exception as e:
         raise RuntimeError(
-            f"Could not patch RViz config with base_frame ({base_frame}): {e}"
+            f"Could not patch RViz config with frames (base_frame={base_frame}, odom_frame={odom_frame}): {e}"
         )
 
     # Write to a temp file

ros/launch/odometry.launch.py

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
   <name>rko_lio</name>
-  <version>0.1.0</version>
+  <version>0.1.1</version>
   <description>A Robust Approach for LiDAR-Inertial Odometry Without Sensor-Specific Modelling</description>
   <maintainer email="rm.meher97@gmail.com">Meher Malladi</maintainer>
   <license>MIT</license>

ros/package.xml

@@ -180,6 +180,7 @@ void Node::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr& imu_msg) {
                                "Either specify the frame id or the extrinsic manually.");
     }
     imu_frame = imu_msg->header.frame_id;
+    RCLCPP_INFO_STREAM(node->get_logger(), "Parsed the imu frame id as: " << imu_frame);
   }
   if (!check_and_set_extrinsics()) {
     // we assume that extrinsics are static. if they change, its better to query the tf directly in the registration
@@ -203,6 +204,7 @@ void Node::lidar_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& l
                                "Either specify the frame id or the extrinsic manually.");
     }
     lidar_frame = lidar_msg->header.frame_id;
+    RCLCPP_INFO_STREAM(node->get_logger(), "Parsed the lidar frame id as: " << lidar_frame);
   }
   if (!check_and_set_extrinsics()) {
     return;