Trajectory viewer: flyover playback, LiDAR range filter and local-depth coloring

apps/camera_lidar_trajectory_viewer/RosExport.cpp

@@ -363,7 +363,7 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s
                     opt.aggregationSec);
 
                 // cache for the raw (sensor-frame) re-projection
-                const TrajPose* lastPose = nullptr;
+                std::optional<std::reference_wrapper<const TrajPose>> lastPose;
                 Eigen::Affine3f lastInv = Eigen::Affine3f::Identity();
 
                 size_t i = 0;
@@ -395,11 +395,11 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s
                         buf.reserve((j - i) * 4);
                         for (size_t k = i; k < j; ++k)
                         {
-                            const TrajPose* p = in.traj.nearest(pts[k].ts);
-                            if (p != lastPose)
+                            auto p = in.traj.nearest(pts[k].ts);
+                            if (!lastPose || &lastPose->get() != &p->get())
                             {
                                 lastPose = p;
-                                lastInv = p->T.inverse();
+                                lastInv = p->get().T.inverse();
                             }
                             Eigen::Vector3f pl = lastInv * Eigen::Vector3f(pts[k].x, pts[k].y, pts[k].z);
                             buf.push_back(pl.x());

apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h

@@ -9,23 +9,34 @@
 
 namespace trajectory_viewer_shaders
 {
-    // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI/mask
+    // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet intensity, 1=RGB, 2=camera id, 3=in ROI/mask,
+    // 4=jet local depth (LiDAR range)
     inline constexpr const char* kVS = R"(
 #version 330
+// GLSL 330 has no 16-bit types: the integer attributes arrive widened to uint/int,
+// and must be bound with glVertexAttribIPointer (rlSetVertexAttribute always
+// converts to float).
 layout(location = 0) in vec3  pos;
 layout(location = 1) in float colorPacked;
-layout(location = 2) in float lidarIntensity;
-layout(location = 3) in float colorCameraId;   // global image index that colored this point, or -1
-layout(location = 4) in float inRoi;           // 1=kept by ROI+mask, 0=rejected by either, -1=projects into no image
+layout(location = 2) in uint  lidarIntensity;  // GL_UNSIGNED_SHORT: intensity scaled from 0 to 65535
+layout(location = 4) in int   lidarDepth;      // GL_SHORT: distance from the LiDAR's center in cm; < 0 = unknown
+layout(location = 3) in int   colorCameraId;   // GL_INT: global image index that colored this point;
+                                               // -1 = projects into no image, -2 = rejected by ROI/mask
+
 uniform mat4  mvp;
 uniform float pointSize;
 uniform int   drawDecim;
+uniform float minRange;  // m: hide points the LiDAR saw closer than this
+uniform float maxRange;  // m: hide points the LiDAR saw farther than this; <= 0 = no limit
 out float fragIntensity;
+out float fragDepth;   // m; < 0 = unknown
 out vec4 vertColor;
 flat out float fragColorCameraId;
 flat out float fragInRoi;
 void main() {
-    if (drawDecim > 1 && (gl_VertexID % drawDecim) != 0) {
+    float range = float(lidarDepth) * 0.01;
+    bool outOfRange = lidarDepth >= 0 && (range < minRange || (maxRange > 0.0 && range > maxRange));
+    if ((drawDecim > 1 && (gl_VertexID % drawDecim) != 0) || outOfRange) {
         gl_Position  = vec4(2.0, 2.0, 2.0, 1.0);
         gl_PointSize = 0.0;
         return;
@@ -36,21 +47,25 @@ void main() {
     float r = float((p >> 16) & 0xFFu) / 255.0;
     float g = float((p >>  8) & 0xFFu) / 255.0;
     float b = float( p        & 0xFFu) / 255.0;
-    fragIntensity = lidarIntensity;
+    fragIntensity = float(lidarIntensity) / 65535.0;
+    fragDepth = lidarDepth >= 0 ? range : -1.0;
     vertColor = vec4(r, g, b, 1.0);
-    fragColorCameraId = colorCameraId;
-    fragInRoi = inRoi;
+    fragColorCameraId = float(colorCameraId);
+    // kFS's in-ROI states: 1 = kept, 0 = rejected, -1 = projects into no image
+    fragInRoi = colorCameraId >= 0 ? 1.0 : (colorCameraId == -1 ? -1.0 : 0.0);
 }
 )";
 
     inline const std::string kFS = std::string(R"(
 #version 330
 in float fragIntensity;
+in float fragDepth;
 in vec4 vertColor;
 flat in float fragColorCameraId;
 flat in float fragInRoi;
 uniform int colorMode;
 uniform int selectedCamera;   // -1 = show all, else keep only points from this image
+uniform float depthColorMax;  // m: colorMode 4 maps 0..this onto the jet colormap
 out vec4 finalColor;
 )") + raylib_widgets::kJetColormapGLSL +
         R"(
@@ -95,6 +110,14 @@ void main() {
             finalColor = (fragInRoi > 0.5) ? vec4(0.15, 0.9, 0.2, 1.0)
                                            : vec4(0.9, 0.15, 0.15, 1.0);
     }
+    else if (colorMode == 4)
+    {
+        // local depth: distance from the LiDAR when captured; gray = no timestamp
+        if (fragDepth < 0.0)
+            finalColor = vec4(0.28, 0.28, 0.28, 1.0);
+        else
+            finalColor = vec4(jet(clamp(fragDepth / max(depthColorMax, 0.01), 0.0, 1.0)), 1.0);
+    }
     else finalColor = vec4(jet(fragIntensity), 1.0);
 }
 )";

calib_core/include/CalibCore/Trajectory.h

@@ -3,6 +3,8 @@
 #include <vector>
 #include <string>
 #include <cstdint>
+#include <optional>
+#include <functional>
 
 namespace calib {
 
@@ -33,13 +35,43 @@ struct Trajectory {
     //! Pose closest in time to `ts_ns`.
     //! @param ts_ns timestamp to look up, nanoseconds
     //! @return the nearest pose, clamped to the first or last one when `ts_ns`
-    //!         falls outside the trajectory, or nullptr when it is empty
+    //!         falls outside the trajectory, or nullopt when it is empty
     //! @warning Assumes @ref poses is sorted by timestamp -- it binary-searches.
     //!          Call @ref sort first, or the result is arbitrary.
-    const TrajPose* nearest(int64_t ts_ns) const;
+    [[nodiscard]] std::optional<std::reference_wrapper<const TrajPose>> nearest(int64_t ts_ns) const;
+
+    //! Pose at fractional position `f` along the trajectory's time span.
+    //! @param f 0 = @ref poses front (earliest), 1 = back (latest); not
+    //!        clamped -- values outside [0,1] extrapolate past the ends and
+    //!        get clamped by @ref nearest(int64_t) to the first/last pose.
+    //! @return nullopt when it is empty; see @ref nearest(int64_t) otherwise.
+    //! @warning Same ordering requirement as @ref nearest(int64_t).
+    [[nodiscard]] std::optional<std::reference_wrapper<const TrajPose>> nearest(float f) const;
+
+    //! Timestamp at fractional position `f` along the trajectory's time span.
+    //! @param f 0 = @ref poses front (earliest), 1 = back (latest); not clamped
+    //! @return 0 when it is empty
+    //! @warning Same ordering requirement as @ref nearest(int64_t).
+    [[nodiscard]] int64_t timeAt(float f) const;
+
+    //! Pose at `ts_ns`, averaged over ±`halfWindowSec` with Hann weights: a
+    //! zero-lag, stateless low-pass for playback, so the same time always gives
+    //! the same pose.
+    //! @param ts_ns window centre, nanoseconds
+    //! @param halfWindowSec window half-width, seconds; <= 0 gives the nearest pose
+    //! @return nullopt when it is empty; the nearest pose when no pose falls inside the window
+    //! @note Rotations are averaged as sign-aligned quaternions, accurate while the
+    //!       window spans no more than a few tens of degrees of turning.
+    //! @warning Same ordering requirement as @ref nearest(int64_t).
+    [[nodiscard]] std::optional<Eigen::Affine3f> smoothedPose(int64_t ts_ns, double halfWindowSec) const;
 
     //! True when no poses have been loaded.
     bool empty() const { return poses.empty(); }
 };
 
+//! Removes roll from a rotation in LiDAR axes (x forward, y left, z up): x keeps
+//! pointing where it did (heading and pitch), and y is turned level with world XY.
+//! @return R unchanged when x points (almost) straight up or down
+[[nodiscard]] Eigen::Matrix3f levelHorizon(const Eigen::Matrix3f& R);
+
 }  // namespace calib

calib_core/src/Trajectory.cpp

@@ -44,14 +44,74 @@ void Trajectory::sort() {
               [](const TrajPose& a, const TrajPose& b){ return a.ts_ns < b.ts_ns; });
 }
 
-const TrajPose* Trajectory::nearest(int64_t ts_ns) const {
-    if (poses.empty()) return nullptr;
+std::optional<std::reference_wrapper<const TrajPose>> Trajectory::nearest(int64_t ts_ns) const {
+    if (poses.empty()) return std::nullopt;
     auto it = std::lower_bound(poses.begin(), poses.end(), ts_ns,
         [](const TrajPose& p, int64_t t){ return p.ts_ns < t; });
-    if (it == poses.end())   return &poses.back();
-    if (it == poses.begin()) return &poses.front();
+    if (it == poses.end())   return std::cref(poses.back());
+    if (it == poses.begin()) return std::cref(poses.front());
     auto prev = std::prev(it);
-    return (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) ? &*it : &*prev;
+    return (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) ? std::cref(*it) : std::cref(*prev);
+}
+
+std::optional<std::reference_wrapper<const TrajPose>> Trajectory::nearest(float f) const
+{
+    if (poses.empty()) return std::nullopt;
+    return nearest(timeAt(f));
+}
+
+int64_t Trajectory::timeAt(float f) const
+{
+    if (poses.empty()) return 0;
+    const int64_t t0 = poses.front().ts_ns;
+    const int64_t t1 = poses.back().ts_ns;
+    return t0 + static_cast<int64_t>(static_cast<double>(t1 - t0) * f);
+}
+
+std::optional<Eigen::Affine3f> Trajectory::smoothedPose(int64_t ts_ns, double halfWindowSec) const
+{
+    const auto nearestPose = nearest(ts_ns);
+    if (!nearestPose) return std::nullopt;
+    const Eigen::Affine3f& nearestT = nearestPose->get().T;
+    if (halfWindowSec <= 0.0) return nearestT;
+
+    const int64_t halfNs = static_cast<int64_t>(halfWindowSec * 1e9);
+    const auto first = std::lower_bound(poses.begin(), poses.end(), ts_ns - halfNs,
+        [](const TrajPose& p, int64_t t){ return p.ts_ns < t; });
+    const auto last = std::upper_bound(first, poses.end(), ts_ns + halfNs,
+        [](int64_t t, const TrajPose& p){ return t < p.ts_ns; });
+
+    const Eigen::Quaternionf qRef(nearestT.linear());
+    Eigen::Vector3f posSum = Eigen::Vector3f::Zero();
+    Eigen::Vector4f quatSum = Eigen::Vector4f::Zero();
+    float weightSum = 0.f;
+    for (auto it = first; it != last; ++it) {
+        const double u = static_cast<double>(it->ts_ns - ts_ns) / static_cast<double>(halfNs); // [-1, 1]
+        const float w = static_cast<float>(0.5 * (1.0 + std::cos(M_PI * u)));
+        Eigen::Quaternionf q(it->T.linear());
+        if (q.dot(qRef) < 0.f) q.coeffs() = -q.coeffs(); // q and -q are the same rotation
+        posSum += w * it->T.translation();
+        quatSum += w * q.coeffs();
+        weightSum += w;
+    }
+    if (weightSum < 1e-6f) return nearestT; // poses too sparse to land inside the window
+
+    Eigen::Affine3f out = Eigen::Affine3f::Identity();
+    out.translation() = posSum / weightSum;
+    out.linear() = Eigen::Quaternionf(quatSum.normalized()).toRotationMatrix();
+    return out;
+}
+
+Eigen::Matrix3f levelHorizon(const Eigen::Matrix3f& R)
+{
+    const Eigen::Vector3f forward = R.col(0).normalized();
+    const Eigen::Vector3f left = Eigen::Vector3f::UnitZ().cross(forward);
+    if (left.norm() < 1e-3f) return R;
+    Eigen::Matrix3f out;
+    out.col(0) = forward;
+    out.col(1) = left.normalized();
+    out.col(2) = forward.cross(out.col(1));
+    return out;
 }
 
 }  // namespace calib

calib_core/tests/test_trajectory.cpp

@@ -1,13 +1,32 @@
-// Trajectory CSV loading tests; registers into test_camera.cpp's doctest main().
+// Trajectory loading and smoothing tests; registers into test_camera.cpp's doctest main().
 #include <doctest.h>
 
 #include <CalibCore/Trajectory.h>
 
+#include <cmath>
 #include <filesystem>
 #include <fstream>
 
 using namespace calib;
 
+//! 4 s at 10 Hz walking +x at 1 m/s, with a 2 Hz sideways sway (0.1 m) and roll
+//! (10 deg) that both peak at t = 2 s -- the kind of jitter smoothedPose() removes.
+static Trajectory swayingWalk()
+{
+    Trajectory t;
+    for (int k = 0; k <= 40; ++k)
+    {
+        const double sec = k * 0.1;
+        const double phase = std::cos(4.0 * M_PI * (sec - 2.0));
+        TrajPose p;
+        p.ts_ns = static_cast<int64_t>(k) * 100'000'000;
+        p.T.translation() = Eigen::Vector3f(float(sec), float(0.1 * phase), 0.f);
+        p.T.linear() = Eigen::AngleAxisf(float(10.0 * M_PI / 180.0 * phase), Eigen::Vector3f::UnitX()).toRotationMatrix();
+        t.poses.push_back(p);
+    }
+    return t;
+}
+
 TEST_CASE("Trajectory::loadCSV: fractional timestamp does not shift the pose columns")
 {
     // lidar_odometry_step_1 writes seconds * 1e9 as a double, so a row can
@@ -30,3 +49,54 @@ TEST_CASE("Trajectory::loadCSV: fractional timestamp does not shift the pose col
     CHECK(t.poses[1].T.linear().isIdentity(1e-6f));
     CHECK(t.poses[1].T.translation().isApprox(Eigen::Vector3f(4.f, 5.f, 6.f)));
 }
+
+TEST_CASE("Trajectory::smoothedPose: averages out sway and roll, keeps the path")
+{
+    const Trajectory t = swayingWalk();
+    const int64_t mid = t.timeAt(0.5f);
+    REQUIRE(mid == 2'000'000'000);
+
+    // The raw pose at the sway peak is 0.1 m off the path and rolled 10 deg.
+    const Eigen::Affine3f& raw = t.nearest(mid)->get().T;
+    CHECK(raw.translation().y() == doctest::Approx(0.1f));
+
+    const auto smooth = t.smoothedPose(mid, 1.0);
+    REQUIRE(smooth);
+    CHECK(smooth->translation().x() == doctest::Approx(2.0f).epsilon(1e-4));
+    CHECK(std::abs(smooth->translation().y()) < 1e-3f);
+    CHECK(Eigen::AngleAxisf(smooth->linear()).angle() < float(0.1 * M_PI / 180.0));
+}
+
+TEST_CASE("Trajectory::smoothedPose: nearest pose without a window, nullopt when empty")
+{
+    const Trajectory t = swayingWalk();
+    const int64_t mid = t.timeAt(0.5f);
+    const auto off = t.smoothedPose(mid, 0.0);
+    REQUIRE(off);
+    CHECK(off->isApprox(t.nearest(mid)->get().T));
+
+    // A window narrower than the 0.1 s pose spacing, between two poses, holds none.
+    const auto sparse = t.smoothedPose(mid + 30'000'000, 0.01);
+    REQUIRE(sparse);
+    CHECK(sparse->isApprox(t.nearest(mid + 30'000'000)->get().T));
+
+    CHECK_FALSE(Trajectory{}.smoothedPose(0, 1.0));
+}
+
+TEST_CASE("levelHorizon: removes roll, keeps heading and pitch")
+{
+    const Eigen::Matrix3f R = (Eigen::AngleAxisf(0.5f, Eigen::Vector3f::UnitZ()) *
+                               Eigen::AngleAxisf(-0.3f, Eigen::Vector3f::UnitY()) *
+                               Eigen::AngleAxisf(0.25f, Eigen::Vector3f::UnitX()))
+                                  .toRotationMatrix();
+    const Eigen::Matrix3f L = levelHorizon(R);
+    CHECK(L.col(0).isApprox(R.col(0)));
+    CHECK(std::abs(L.col(1).z()) < 1e-6f); // left axis is level
+    CHECK(L.col(2).z() > 0.f); // still upright
+    CHECK((L.transpose() * L).isIdentity(1e-5f));
+    CHECK(L.determinant() == doctest::Approx(1.0f));
+
+    // Looking straight up, there is no horizon to level.
+    const Eigen::Matrix3f up = Eigen::AngleAxisf(float(-M_PI / 2), Eigen::Vector3f::UnitY()).toRotationMatrix();
+    CHECK(levelHorizon(up).isApprox(up));
+}

raylib_widgets/include/RaylibWidgets/OrbitCamera.h

@@ -1,11 +1,14 @@
 #pragma once
+#include <Eigen/Geometry>
 #include "raylib.h"
 
+#include <optional>
+
 // Mouse-driven orbit/pan/zoom camera, shared between
 // apps/camera_lidar_calibration and apps/camera_lidar_trajectory_viewer (was
 // byte-for-byte duplicated as camera_lidar_calibration's OrbitCamera and
-// camera_lidar_trajectory_viewer's Orbit). Depends on nothing but raylib --
-// no Eigen, no core -- so it stays linkable from both the core_raylib side
+// camera_lidar_trajectory_viewer's Orbit). Depends on raylib and header-only
+// Eigen (setViewPose) -- no core -- so it stays linkable from both the core_raylib side
 // (which already pulls in core/core_math) and the calib_core side (which
 // deliberately doesn't) without adding coupling either way.
 namespace raylib_widgets {
@@ -151,6 +154,17 @@ struct OrbitCamera {
     // depends on nothing but raylib.
     void applyOrthoProjection(float aspect) const;
 
+    //! Camera-to-world pose in OpenGL axes (x right, y up, looking down -z), set by
+    //! setViewPose(). While set, the caller builds the view from it instead of from
+    //! `euler`, so mouse orbit/pan/zoom have no effect; reset() it to return to euler.
+    std::optional<Eigen::Affine3f> viewPose;
+
+    //! Places the camera at a pose, without animation. Used for flyovers.
+    //! @param view pose on the trajectory in LiDAR axes (x forward, y left, z up);
+    //!        stored in `viewPose` swapped to OpenGL axes
+    //! @note Only the perspective view uses it; ortho mode ignores it.
+    void setViewPose(Eigen::Affine3f view);
+
     // Builds this frame's ortho projection + folds an eye/center/up lookAt
     // (derived from euler.rotateX/rotateY and the ortho pan/height state)
     // into rlgl's current (projection) matrix -- was updateOrthoView(),

raylib_widgets/src/OrbitCamera.cpp

@@ -410,6 +410,17 @@ void OrbitCamera::moveEulerRotationCenterTo(Vector3 center) {
     startEulerTransition(euler.rotateX, euler.rotateY, Vector3{ -center.x, -center.y, euler.translate.z }, center);
 }
 
+void OrbitCamera::setViewPose(Eigen::Affine3f view) {
+    // Columns are the OpenGL camera axes in LiDAR coordinates: right = -y,
+    // up = +z, and +z (backward) = -x, so the camera looks along LiDAR +x.
+    Eigen::Matrix3f lidarFromGl;
+    lidarFromGl << 0.f, 0.f, -1.f,
+                  -1.f, 0.f,  0.f,
+                   0.f, 1.f,  0.f;
+    view.linear() = view.linear() * lidarFromGl;
+    viewPose = view;
+}
+
 void OrbitCamera::updateEulerTransition(float dt) {
     if (!eulerTransitionActive) return;