Further fixes

Author: jlblancoc

scripts/clang-formatter.sh

@@ -1,3 +1,3 @@
 #!/bin/bash
 
-find kitti*/ mola*/ -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs -I FIL bash -c "echo FIL && clang-format-14 -i FIL"
+find include src -iname *.h -o -iname *.hpp -o -iname *.cpp -o -iname *.c | xargs -I FIL bash -c "echo FIL && clang-format-14 -i FIL"

src/LocalVelocityBuffer.cpp

@@ -179,16 +179,21 @@ std::string LocalVelocityBuffer::SamplesByTime::asString() const
     std::ostringstream oss;
     oss << "SamplesByTime:\n";
 
-    auto dumpMap = [&](const std::string& name, const auto& map)
+    auto dumpMap = [&](const std::string& name, const auto& map, bool isMatrix = false)
     {
         oss << "  " << name << ":\n";
         for (const auto& [t, val] : map)
         {
-            oss << "    [" << t << "] = " << val << "\n";
+            oss << "    [" << t << "] = ";
+            if (isMatrix)
+            {
+                oss << "\n";
+            }
+            oss << val << "\n";
         }
     };
 
-    dumpMap("q (orientation)", q);
+    dumpMap("q (orientation)", q, true);
     dumpMap("v_b (linear velocity)", v_b);
     dumpMap("a_b (linear acceleration)", a_b);
     dumpMap("w_b (angular velocity)", w_b);
@@ -203,7 +208,7 @@ std::string LocalVelocityBuffer::Sample::asString() const
 
     auto dumpOpt = [&](const std::string& name, const auto& opt)
     {
-        oss << "  " << name << ": ";
+        oss << "  " << name;
         if (opt.has_value())
         {
             oss << *opt << "\n";
@@ -214,10 +219,10 @@ std::string LocalVelocityBuffer::Sample::asString() const
         }
     };
 
-    dumpOpt("q (orientation)", q);
-    dumpOpt("v_b (linear velocity)", v_b);
-    dumpOpt("a_b (linear acceleration)", a_b);
-    dumpOpt("w_b (angular velocity)", w_b);
+    dumpOpt("q (orientation):\n", q);
+    dumpOpt("v_b (linear velocity): ", v_b);
+    dumpOpt("a_b (linear acceleration): ", a_b);
+    dumpOpt("w_b (angular velocity): ", w_b);
 
     return oss.str();
 }

src/trajectory_from_buffer.cpp

@@ -146,13 +146,21 @@ Trajectory mola::imu::trajectory_from_buffer(
     t[0].w_b = mrpt::containers::find_closest(samples.by_type.w_b, 0.0)->second - bias_gyro;
     t[0].a_b = mrpt::containers::find_closest(samples.by_type.a_b, 0.0)->second - bias_acc;
 
-    // 3) Copy the closest gravity-aligned hints on global orientations:
-    const auto closest_q = mrpt::containers::find_closest(samples.by_type.q, 0.0);
+    // 3) Copy the latest gravity-aligned hints on global orientations:
+    const double last_sample_rel_time = samples.by_time.rbegin()->first;
+    const auto closest_q = mrpt::containers::find_closest(samples.by_type.q, last_sample_rel_time);
     ASSERTMSG_(
         closest_q,
         "At least one entry with gravity-aligned orientation is needed for IMU integration");
     const double stamp_first_R_ga = closest_q->first;
-    t[stamp_first_R_ga].R_ga      = closest_q->second;
+    t[closest_q->first].R_ga      = closest_q->second;
+
+    // 3b) and copy the closest velocity from the given samples:
+    const auto closest_v_b =
+        mrpt::containers::find_closest(samples.by_type.v_b, last_sample_rel_time);
+    ASSERTMSG_(closest_v_b, "At least one entry with velocity is needed for IMU integration");
+    const double stamp_first_v_b = closest_v_b->first;
+    t[closest_v_b->first].v      = closest_v_b->second;
 
     // and assign the closest IMU reading to all frames:
     for (auto& [stamp, tp] : t)
@@ -251,12 +259,6 @@ Trajectory mola::imu::trajectory_from_buffer(
             });
     }
 
-    // 7) Copy the closest velocity from the given samples:
-    const auto closest_v_b = mrpt::containers::find_closest(samples.by_type.v_b, 0.0);
-    ASSERTMSG_(closest_v_b, "At least one entry with velocity is needed for IMU integration");
-    const double stamp_first_v_b = closest_v_b->first;
-    t[stamp_first_v_b].v         = closest_v_b->second;
-
     // 8) Integrate v:
     if (use_higher_order)
     {

tests/test-velocity-buffer.cpp

@@ -23,6 +23,7 @@
 #include <mrpt/containers/yaml.h>
 #include <mrpt/core/exceptions.h>
 #include <mrpt/core/round.h>
+#include <mrpt/poses/Lie/SO.h>
 #include <mrpt/system/os.h>
 
 #include <iostream>
@@ -118,18 +119,18 @@ void unit_test_yaml_roundtrip()
     buf.parameters.tolerance_search_stamp = 1e-2;
     buf.set_reference_zero_time(123.456);
 
-    LinearVelocity     v{1.0, 2.0, 3.0};
-    AngularVelocity    w{0.1, 0.2, 0.3};
-    LinearAcceleration a{9.8, 0.0, -9.8};
-    SO3                R = mrpt::math::CMatrixDouble33::Identity();
+    const LinearVelocity     v{1.0, 2.0, 3.0};
+    const AngularVelocity    w{0.1, 0.2, 0.3};
+    const LinearAcceleration a{9.8, 0.0, -9.8};
+    const SO3                R = mrpt::poses::CPose3D(0, 0, 0, 0.7, 0.0, 0.0).getRotationMatrix();
 
     buf.add_linear_velocity(123.400, v);
     buf.add_angular_velocity(123.410, w);
     buf.add_linear_acceleration(123.420, a);
     buf.add_orientation(123.430, R);
 
     // Serialize
-    auto yml = buf.toYAML();
+    const auto yml = buf.toYAML();
 
     yml.printAsYAML();
 
@@ -175,7 +176,12 @@ void unit_test_yaml_roundtrip()
     {
         const auto it = buf2.get_orientations().find(t);
         ASSERT_(it != buf2.get_orientations().end());
-        ASSERT_(R1 == it->second);
+        mrpt::math::CMatrixDouble33 error_R = R1.inverse() * it->second;
+        if (mrpt::poses::Lie::SO<3>::log(error_R).norm() > 1e-4)
+        {
+            std::cerr << "R1:\n" << R1 << "\nit->second:\n" << it->second << "\n";
+            THROW_EXCEPTION("Error in rotation matrix");
+        }
     }
 
     std::cout << "✅ LocalVelocityBuffer unit_test_yaml_roundtrip passed!" << std::endl;