Add option 'use_imu_orientation' to disable using IMU orientation for initialization

Author: jlblancoc

include/mola_imu_preintegration/ImuInitialCalibrator.h

@@ -48,6 +48,11 @@ class ImuInitialCalibrator
         std::size_t required_samples = 50;  //!< Minimum required samples
         double      max_samples_age  = .75;  //!< Maximum samples age [seconds]
         double      gravity          = 9.81;  //<! Gravity magnitude [m/s²]
+
+        /** If provided by the IMU, prefer gravity-aligned orientation from the sensor instead of
+         * accelerometer data. Should be normally preferable, so default is true.
+         */
+        bool use_imu_orientation = true;
     };
 
     Parameters parameters;
@@ -88,4 +93,7 @@ class ImuInitialCalibrator
     std::map<double, const mrpt::obs::CObservationIMU> samples_;
 };
 
+// Remove when all distros have the version with "use_imu_orientation"
+#define MOLA_IMU_PREINT_HAS_USE_IMU_ORIENT_PARAM
+
 }  // namespace mola::imu

src/ImuInitialCalibrator.cpp

@@ -132,20 +132,23 @@ std::optional<ImuInitialCalibrator::Results> ImuInitialCalibrator::getCalibratio
     mrpt::math::TVector3D average_gyro(0, 0, 0);
     forEachGyro([&](const auto& omega) { average_gyro += omega; });
 
-    mrpt::poses::SO_average<3> so3_average;
-    forEachOrientation([&](const auto& pose) { so3_average.append(pose.getRotationMatrix()); });
     std::optional<mrpt::poses::CPose3D> avr_so3;
-    try
+    if (parameters.use_imu_orientation)
     {
-        auto rot = so3_average.get_average();
+        mrpt::poses::SO_average<3> so3_average;
+        forEachOrientation([&](const auto& pose) { so3_average.append(pose.getRotationMatrix()); });
+        try
+        {
+            auto rot = so3_average.get_average();
 
-        avr_so3.emplace();
-        avr_so3->setRotationMatrix(rot);
-    }
-    catch (const std::exception& e)
-    {
-        // Ignore, this means we had no data.
-        (void)e;
+            avr_so3.emplace();
+            avr_so3->setRotationMatrix(rot);
+        }
+        catch (const std::exception& e)
+        {
+            // Ignore, this means we had no data.
+            (void)e;
+        }
     }
 
     const std::size_t count = samples_.size();