From b6060ac06ca0d855bb951c3cb235f8823427c4d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 13:32:20 +0200 Subject: [PATCH 1/8] Add flyover playback to the trajectory viewer View > Flyover moves the camera along the LiDAR trajectory, with a bottom timeline to scrub, a real-time speed control, and an option to keep the selected camera image in sync with the playhead. Trajectory::nearest now returns an optional reference instead of a raw pointer. Co-Authored-By: Claude Opus 5.5 --- .../RosExport.cpp | 8 +- .../TrajectoryViewer.cpp | 235 ++++++++++++++++-- calib_core/include/CalibCore/Trajectory.h | 15 +- calib_core/src/Trajectory.cpp | 19 +- .../include/RaylibWidgets/OrbitCamera.h | 18 +- raylib_widgets/src/OrbitCamera.cpp | 11 + 6 files changed, 278 insertions(+), 28 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index b4421446..b533844f 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/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> 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()); diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 911594f3..07d6b86d 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -165,6 +165,95 @@ static float angularSpeedDegAt(const Trajectory& traj, const std::vector& return perPose[idx]; } +//! Time span of the trajectory in seconds; 0 with fewer than two poses. +static double trajectoryDurationSec(const Trajectory& traj) +{ + if (traj.poses.size() < 2) + return 0.0; + return double(traj.poses.back().ts_ns - traj.poses.front().ts_ns) * 1e-9; +} + +//! Smallest "round" tick step (s) that keeps ticks at least `minPx` apart. +static double timelineTickStep(double durationSec, float widthPx, float minPx) +{ + static const double kSteps[] = { 0.1, 0.2, 0.5, 1, 2, 5, 10, 15, 30, 60, 120, 300, 600, 900, 1800, 3600 }; + const double pxPerSec = widthPx / durationSec; + for (double step : kSteps) + if (step * pxPerSec >= minPx) + return step; + double step = kSteps[std::size(kSteps) - 1]; + while (step * pxPerSec < minPx) + step *= 2.0; + return step; +} + +//! Tick label: "m:ss" from a minute on, else seconds with a decimal only when +//! the tick step needs one. +static std::string formatTickLabel(double sec, double step) +{ + char buf[32]; + if (sec >= 60.0) + std::snprintf(buf, sizeof(buf), "%d:%02d", int(sec + 1e-6) / 60, int(sec + 1e-6) % 60); + else if (step < 1.0) + std::snprintf(buf, sizeof(buf), "%.1fs", sec); + else + std::snprintf(buf, sizeof(buf), "%.0fs", sec); + return buf; +} + +//! Timeline across the available width, with time ticks and a playhead. +//! Click or drag on it to scrub. +//! @param id ImGui id of the scrub area +//! @param progress 0-1 playhead position; written while the user drags +//! @param durationSec time the full width spans, for the tick labels +//! @param height total height, including the tick label row +static void flyoverTimeline(const char* id, float& progress, double durationSec, float height) +{ + ImDrawList* dl = ImGui::GetWindowDrawList(); + const ImVec2 p0 = ImGui::GetCursorScreenPos(); + const float w = std::max(1.f, ImGui::GetContentRegionAvail().x); + const float trackH = height - ImGui::GetTextLineHeight(); + const ImVec2 p1(p0.x + w, p0.y + trackH); + + ImGui::InvisibleButton(id, ImVec2(w, height)); + if (ImGui::IsItemActive()) + progress = std::clamp((ImGui::GetIO().MousePos.x - p0.x) / w, 0.f, 1.f); + if (ImGui::IsItemHovered() && durationSec > 0.0) + { + const float f = std::clamp((ImGui::GetIO().MousePos.x - p0.x) / w, 0.f, 1.f); + ImGui::SetTooltip("%.2f s", f * durationSec); + } + + dl->AddRectFilled(p0, p1, IM_COL32(40, 40, 40, 255), 3.f); + dl->AddRectFilled(p0, ImVec2(p0.x + w * progress, p1.y), IM_COL32(60, 110, 160, 255), 3.f, ImDrawFlags_RoundCornersLeft); + + if (durationSec > 0.0) + { + // Major ticks carry a label; four unlabelled minor ticks between them. + const double major = timelineTickStep(durationSec, w, 70.f); + const double minor = major / 5.0; + const double pxPerSec = w / durationSec; + for (int k = 0; k * minor <= durationSec + 1e-9; ++k) + { + const bool isMajor = k % 5 == 0; + const float x = p0.x + float(k * minor * pxPerSec); + dl->AddLine(ImVec2(x, p1.y - trackH * (isMajor ? 0.6f : 0.3f)), ImVec2(x, p1.y), IM_COL32(170, 170, 170, 255)); + if (isMajor) + { + const std::string label = formatTickLabel(k * minor, major); + const float lw = ImGui::CalcTextSize(label.c_str()).x; + const float lx = std::clamp(x - lw * 0.5f, p0.x, p0.x + w - lw); + dl->AddText(ImVec2(lx, p1.y), IM_COL32(200, 200, 200, 255), label.c_str()); + } + } + } + + const float px = p0.x + w * progress; + const ImU32 playheadCol = IM_COL32(255, 220, 60, 255); + dl->AddLine(ImVec2(px, p0.y), ImVec2(px, p1.y), playheadCol, 2.f); + dl->AddTriangleFilled(ImVec2(px - 5.f, p0.y), ImVec2(px + 5.f, p0.y), ImVec2(px, p0.y + 6.f), playheadCol); +} + using trajectory_viewer_shaders::kFS; using trajectory_viewer_shaders::kVS; @@ -277,6 +366,14 @@ struct AppState float maxTemporalDist = 0.5f; //!< s: skip images farther than this from the point (temporal) int maxWiggle = 1; //!< frames: search startIdx ± maxWiggle for a frustum hit (temporal) + // ── flyover ─────────────────────────────────────────────────────────────── + //! Flyover mode: the bottom timeline bar is shown and the camera sits on the + //! trajectory at @ref flyoverProgress (mouse orbit/pan/zoom have no effect). + bool flyover = false; + bool flyoverPlaying = false; //!< advance flyoverProgress each frame + float flyoverProgress = 0.f; //!< 0-1 position along the trajectory's time span + float flyoverSpeed = 1.f; //!< playback rate, multiple of real time + bool flyoverUpdateSelectedCamera = true; //!< select the camera image nearest the playhead // ── fast-rotation image filter ───────────────────────────────────────────── //! Per-pose angular speed (deg/s), parallel to traj.poses — filled by //! loadSession(). Images captured while the rig turns faster than @@ -453,6 +550,23 @@ static int64_t imageTimeOffsetNs(const AppState& s) return (int64_t)std::llround(s.timeOffsetSec * 1e9); } +//! Selects the camera image nearest `imageTs` (image clock) for Image Preview and +//! the frustum highlight. No-op without images, or when it is already selected. +static void selectImageNearest(AppState& s, int64_t imageTs) +{ + if (s.imageTsNs.empty()) + return; + auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), imageTs); + if (it == s.imageTsNs.end() || (it != s.imageTsNs.begin() && imageTs - *std::prev(it) < *it - imageTs)) + --it; + const int idx = int(it - s.imageTsNs.begin()); + if (idx == s.imgViewIdx) + return; + s.imgViewIdx = idx; + s.imgViewRequest.store(idx); + s.intensityProjNeedsUpdate = true; +} + //! Index every camera frame in the camera directory by timestamp. Also picks up //! the image dimensions -- read by the ROI default, the frustums and COLMAP's //! cameras.txt. @@ -1855,11 +1969,11 @@ static void drawScene(AppState& s) for (int64_t ts : s.imageTsNs) { - const TrajPose* pose = s.traj.nearest(ts + imageTimeOffsetNs(s)); + auto pose = s.traj.nearest(ts + imageTimeOffsetNs(s)); if (!pose) continue; - Vector3 origin = toVec3(pose->T * C); + Vector3 origin = toVec3(pose->get().T * C); bool hl = (ts == hlTs); Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; @@ -1875,7 +1989,7 @@ static void drawScene(AppState& s) for (int k = 0; k < 3; k++) { Eigen::Vector3f tip = R_wc.col(k) * (sc * 0.5f) + C; - DrawLine3D(origin, toVec3(pose->T * tip), hl ? fc : axisColors[k]); + DrawLine3D(origin, toVec3(pose->get().T * tip), hl ? fc : axisColors[k]); } continue; } @@ -1884,7 +1998,7 @@ static void drawScene(AppState& s) for (int k = 0; k < 4; k++) { Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * fs, ncy[k] * fs, fs) + C; - w[k] = toVec3(pose->T * pl); + w[k] = toVec3(pose->get().T * pl); } if (hl) @@ -1894,7 +2008,7 @@ static void drawScene(AppState& s) for (int k = 0; k < 4; k++) { Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * sc, ncy[k] * sc, sc) + C; - w2[k] = toVec3(pose->T * pl); + w2[k] = toVec3(pose->get().T * pl); } DrawTriangle3D(w2[0], w2[1], w2[2], Color{ 255, 255, 50, 40 }); DrawTriangle3D(w2[2], w2[3], w2[0], Color{ 255, 255, 50, 40 }); @@ -2049,7 +2163,6 @@ int main(int argc, char* argv[]) { bool imguiWants = ImGui::GetIO().WantCaptureMouse; s.orbit.updateEulerTransition(GetFrameTime()); - // Drag & drop the LIO result directory (this app's session), a CAMERA_0 directory or a // calibration *.json onto the window to load it -- raylib's GLFW backend surfaces OS // drag & drop the same way on Windows, Linux and macOS, so no platform-specific code @@ -2204,19 +2317,54 @@ int main(int argc, char* argv[]) // directly instead of raylib's BeginMode3D/EndMode3D. s.viewLocal = Eigen::Affine3f::Identity(); + // Flyover: while its bar is open the camera sits on the trajectory at + // the playhead (orbit.viewPose) instead of following the euler fields + // below -- also when paused, so scrubbing the timeline previews it. + if (s.flyover && s.flyoverPlaying) + { + const double durationSec = trajectoryDurationSec(s.traj); + if (durationSec > 0.0) + s.flyoverProgress += float(GetFrameTime() * s.flyoverSpeed / durationSec); + if (durationSec <= 0.0 || s.flyoverProgress >= 1.f) + { + s.flyoverProgress = std::min(s.flyoverProgress, 1.f); + s.flyoverPlaying = false; + } + } + s.orbit.viewPose.reset(); + if (s.flyover) + { + if (const auto pose = s.traj.nearest(s.flyoverProgress)) + { + s.orbit.setViewPose(pose->get().T); + if (s.flyoverUpdateSelectedCamera) + selectImageNearest(s, pose->get().ts_ns - imageTimeOffsetNs(s)); + } + } + if (!s.orbit.isOrtho) { s.orbit.applyPerspectiveProjection((int)ImGui::GetIO().DisplaySize.x, (int)ImGui::GetIO().DisplaySize.y); - Eigen::Vector3f rotationCenter(s.orbit.euler.rotationCenter.x, s.orbit.euler.rotationCenter.y, s.orbit.euler.rotationCenter.z); - s.viewLocal.translate(rotationCenter); - s.viewLocal.translate(Eigen::Vector3f(s.orbit.euler.translate.x, s.orbit.euler.translate.y, s.orbit.euler.translate.z)); - if (!s.orbit.lockZ) - s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateX * DEG2RAD, Eigen::Vector3f::UnitX())); + if (s.orbit.viewPose) + { + // viewPose is camera-to-world; the modelview matrix needs + // world-to-camera. + s.viewLocal = s.orbit.viewPose->inverse(); + } else - s.viewLocal.rotate(Eigen::AngleAxisf(-90.0f * DEG2RAD, Eigen::Vector3f::UnitX())); - s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateY * DEG2RAD, Eigen::Vector3f::UnitZ())); - s.viewLocal.translate(-rotationCenter); + { + Eigen::Vector3f rotationCenter( + s.orbit.euler.rotationCenter.x, s.orbit.euler.rotationCenter.y, s.orbit.euler.rotationCenter.z); + s.viewLocal.translate(rotationCenter); + s.viewLocal.translate(Eigen::Vector3f(s.orbit.euler.translate.x, s.orbit.euler.translate.y, s.orbit.euler.translate.z)); + if (!s.orbit.lockZ) + s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateX * DEG2RAD, Eigen::Vector3f::UnitX())); + else + s.viewLocal.rotate(Eigen::AngleAxisf(-90.0f * DEG2RAD, Eigen::Vector3f::UnitX())); + s.viewLocal.rotate(Eigen::AngleAxisf(s.orbit.euler.rotateY * DEG2RAD, Eigen::Vector3f::UnitZ())); + s.viewLocal.translate(-rotationCenter); + } rlMultMatrixf(s.viewLocal.matrix().data()); } @@ -2224,7 +2372,8 @@ int main(int argc, char* argv[]) { // Still updating viewLocal for the compass -- the rest of the // ortho projection + gizmo-view lookAt lives in - // OrbitCamera::updateOrtho(). + // OrbitCamera::updateOrtho(). viewPose is documented as + // perspective-only, so ortho mode ignores it same as before. s.viewLocal.rotate(Eigen::AngleAxisf((s.orbit.euler.rotateX + s.orbit.euler.rotateY) * DEG2RAD, Eigen::Vector3f::UnitZ())); s.orbit.updateOrtho(ratio); } @@ -2355,6 +2504,13 @@ int main(int argc, char* argv[]) if (ImGui::MenuItem("Center of rotation...", "Shift+R")) s.showCenterOfRotationWindow = true; ImGui::Separator(); + if (ImGui::MenuItem("Flyover", nullptr, &s.flyover, !s.traj.empty())) + { + s.flyoverProgress = 0.f; + s.flyoverPlaying = s.flyover; + } + ImGui::Separator(); + ImGui::SetNextItemWidth(140.f); ImGui::SliderFloat("Frustum scale", &s.frustumScale, 0.05f, 5.f, "%.2f"); ImGui::SetNextItemWidth(140.f); @@ -2797,6 +2953,55 @@ int main(int argc, char* argv[]) ImGui::End(); + // ── flyover bar, along the bottom of the 3D view ──────────────────────── + if (s.flyover) + { + const double durationSec = trajectoryDurationSec(s.traj); + const ImGuiStyle& style = ImGui::GetStyle(); + const float timelineH = 18.f + ImGui::GetTextLineHeight(); + const float barH = style.WindowPadding.y * 2.f + ImGui::GetFrameHeight() + style.ItemSpacing.y + timelineH; + ImGui::SetNextWindowPos(ImVec2(0.f, io.DisplaySize.y - barH), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(io.DisplaySize.x - panelW, barH), ImGuiCond_Always); + ImGui::Begin( + "##flyover", + nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoSavedSettings); + + if (ImGui::Button(s.flyoverPlaying ? "Pause" : "Play", ImVec2(60.f, 0.f))) + { + if (!s.flyoverPlaying && s.flyoverProgress >= 1.f) + s.flyoverProgress = 0.f; // replay from the start + s.flyoverPlaying = !s.flyoverPlaying; + } + ImGui::SameLine(); + ImGui::Text("%7.1f / %.1f s", s.flyoverProgress * durationSec, durationSec); + ImGui::SameLine(); + ImGui::SetNextItemWidth(160.f); + ImGui::SliderFloat("Speed", &s.flyoverSpeed, 0.1f, 50.f, "%.1fx", ImGuiSliderFlags_Logarithmic); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Playback rate, as a multiple of real time (Ctrl+click to type)"); + ImGui::SameLine(); + ImGui::BeginDisabled(s.imageTsNs.empty()); + ImGui::Checkbox("Update selected camera", &s.flyoverUpdateSelectedCamera); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip( + s.imageTsNs.empty() ? "No camera images loaded" + : "Select the camera image nearest the playhead (Image Preview, highlighted frustum)"); + ImGui::SameLine(); + const float closeW = ImGui::CalcTextSize("Close").x + style.FramePadding.x * 2.f; + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + std::max(0.f, ImGui::GetContentRegionAvail().x - closeW)); + if (ImGui::Button("Close")) + { + s.flyover = false; + s.flyoverPlaying = false; + } + + flyoverTimeline("##flyoverTimeline", s.flyoverProgress, durationSec, timelineH); + ImGui::End(); + } + raylib_widgets::showEulerCenterOfRotationWindow(s.showCenterOfRotationWindow, s.orbit); // ── shortcuts help window ─────────────────────────────────────────────── diff --git a/calib_core/include/CalibCore/Trajectory.h b/calib_core/include/CalibCore/Trajectory.h index 9688a194..5cc0994e 100644 --- a/calib_core/include/CalibCore/Trajectory.h +++ b/calib_core/include/CalibCore/Trajectory.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include namespace calib { @@ -33,10 +35,19 @@ 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> 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> nearest(float f) const; + //! True when no poses have been loaded. bool empty() const { return poses.empty(); } diff --git a/calib_core/src/Trajectory.cpp b/calib_core/src/Trajectory.cpp index 69466544..d2a6a4a9 100644 --- a/calib_core/src/Trajectory.cpp +++ b/calib_core/src/Trajectory.cpp @@ -44,14 +44,23 @@ 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> 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> Trajectory::nearest(float f) const +{ + if (poses.empty()) return std::nullopt; + const int64_t t0 = poses.front().ts_ns; + const int64_t t1 = poses.back().ts_ns; + const int64_t t = t0 + static_cast(static_cast(t1 - t0) * f); + return nearest(t); } } // namespace calib \ No newline at end of file diff --git a/raylib_widgets/include/RaylibWidgets/OrbitCamera.h b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h index a288ad9f..9b85f44b 100644 --- a/raylib_widgets/include/RaylibWidgets/OrbitCamera.h +++ b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h @@ -1,11 +1,14 @@ #pragma once +#include #include "raylib.h" +#include + // 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 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(), diff --git a/raylib_widgets/src/OrbitCamera.cpp b/raylib_widgets/src/OrbitCamera.cpp index 528a33d3..98876790 100644 --- a/raylib_widgets/src/OrbitCamera.cpp +++ b/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; From 279380716a482ef89dbcd7e13949309994f9eb8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 15:00:49 +0200 Subject: [PATCH 2/8] Draw trajectory viewer chunks locally with a LiDAR range filter Each LIO chunk is now its own GPU part in chunk-local coordinates, placed by its MRP pose at draw time. The vertex shrinks from 28 to 24 bytes: intensity as uint16, per-point LiDAR range as int16 cm, camera id as int32 (-2 marks points the ROI/mask rejected, replacing the separate in-ROI float). The range drives new Min/Max range filters and a Local depth color mode. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 161 ++++++++++++++---- .../TrajectoryViewerShaders.h | 39 ++++- 2 files changed, 158 insertions(+), 42 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 07d6b86d..fc6a995c 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -257,28 +257,92 @@ static void flyoverTimeline(const char* id, float& progress, double durationSec, using trajectory_viewer_shaders::kFS; using trajectory_viewer_shaders::kVS; +//! One point as kVS reads it. +struct GpuPoint +{ + float x, y, z; //!< chunk-local position (location 0) + float colorPacked; //!< float bits = 0x00RRGGBB (location 1) + uint16_t intensity; //!< 0-1 scaled to 0-65535 (location 2) + int16_t depthCm; //!< distance from the LiDAR when captured, cm; -1 = unknown (location 4) + int32_t cameraId; //!< image that colored it; -1 = projects into no image, -2 = rejected by ROI/mask (location 3) +}; +static_assert(sizeof(GpuPoint) == 24, "kVS expects a tightly packed 24-byte vertex"); + +//! The loaded cloud on the GPU: one part per LIO chunk, in chunk-local +//! coordinates, placed in the world by the chunk's pose at draw time. struct GpuCloud { - std::vector parts; - size_t count = 0; - float maxDist = 50.f; + struct Chunk + { + raylib_widgets::PointBufferPart part; + Matrix pose; //!< chunk-local -> world + }; + std::vector chunks; + size_t count = 0; //!< points over all chunks - void upload(const std::vector& data, float mx) + //! Uploads one chunk as a new part, after the ones already loaded. + //! @param pts the chunk's points; empty is a no-op + //! @param pose chunk-local -> world + //! @param name chunk name, for the warning + //! @note Warns on stderr above kMaxVerticesPerPart, which chunks aren't expected to reach. + void addChunk(const std::vector& pts, const Eigen::Affine3f& pose, const std::string& name) { - unload(); - if (data.empty()) + if (pts.empty()) return; - maxDist = mx; - count = data.size() / 7; - parts = raylib_widgets::uploadPointBufferParts(data.data(), count, { 3, 1, 1, 1, 1 }); - } - void draw() const - { - raylib_widgets::drawPointBufferParts(parts); + if (pts.size() > raylib_widgets::kMaxVerticesPerPart) + std::fprintf( + stderr, + "Warning: %s has %zu points, more than %zu per GPU buffer\n", + name.c_str(), + pts.size(), + raylib_widgets::kMaxVerticesPerPart); + + Chunk c; + c.part.count = (int)pts.size(); + c.part.vao = rlLoadVertexArray(); + rlEnableVertexArray(c.part.vao); + c.part.vbo = rlLoadVertexBuffer(pts.data(), c.part.count * (int)sizeof(GpuPoint), false); + constexpr int stride = sizeof(GpuPoint); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, stride, offsetof(GpuPoint, x)); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, stride, offsetof(GpuPoint, colorPacked)); + // rlSetVertexAttribute always converts to float; kVS reads these as uint/int. + glVertexAttribIPointer(2, 1, GL_UNSIGNED_SHORT, stride, (const void*)offsetof(GpuPoint, intensity)); + glVertexAttribIPointer(3, 1, GL_INT, stride, (const void*)offsetof(GpuPoint, cameraId)); + glVertexAttribIPointer(4, 1, GL_SHORT, stride, (const void*)offsetof(GpuPoint, depthCm)); + for (unsigned int loc = 0; loc <= 4; ++loc) + rlEnableVertexAttribute(loc); + rlDisableVertexArray(); + + // Matrix's fields are declared row by row, so this lists pose row by row. + const Eigen::Matrix4f& m = pose.matrix(); + c.pose = { m(0, 0), m(0, 1), m(0, 2), m(0, 3), m(1, 0), m(1, 1), m(1, 2), m(1, 3), + m(2, 0), m(2, 1), m(2, 2), m(2, 3), m(3, 0), m(3, 1), m(3, 2), m(3, 3) }; + chunks.push_back(c); + count += pts.size(); + } + //! Draws every chunk through its own pose; the caller binds the shader and sets + //! every uniform but the MVP. + //! @param view world -> camera (rlGetMatrixModelview()) + //! @param projection rlGetMatrixProjection() + //! @param locMVP location of the shader's mvp uniform + void draw(const Matrix& view, const Matrix& projection, int locMVP) const + { + for (const Chunk& c : chunks) + { + rlSetUniformMatrix(locMVP, MatrixMultiply(MatrixMultiply(c.pose, view), projection)); + rlEnableVertexArray(c.part.vao); + glDrawArrays(GL_POINTS, 0, c.part.count); + } + rlDisableVertexArray(); } void unload() { - raylib_widgets::unloadPointBufferParts(parts); + for (const Chunk& c : chunks) + { + rlUnloadVertexArray(c.part.vao); + rlUnloadVertexBuffer(c.part.vbo); + } + chunks.clear(); count = 0; } }; @@ -335,7 +399,7 @@ struct AppState GpuCloud cloud; Shader shader = {}; bool shaderOk = false; - int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; + int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1, locMinRange = -1, locMaxRange = -1, locDepthMax = -1; //! Driven in Euler mode (rotateX/rotateY/translate/rotationCenter/isOrtho), //! not azimuth/elevation/distance/target, through rlgl rather than raylib's @@ -357,6 +421,8 @@ struct AppState float pointSize = 1.f; int cloudDecim = 1; int drawDecim = 1; + float minRange = 0.f; //!< m: hide points the LiDAR saw closer than this + float maxRange = 0.f; //!< m: hide points the LiDAR saw farther than this; 0 = no limit bool multiImgColoring = true; //!< false = single image per chunk (midpoint) //! How each point is matched to a camera image: //! 0 = temporal — image nearest in time (± maxWiggle frames, within maxTemporalDist) @@ -385,7 +451,8 @@ struct AppState int angFilteredImgs = 0; //!< images skipped by the filter in the last colorize pass bool useImageColor = false; //!< true once a colorize pass produced RGB data - int colorMode = 0; //!< 0=intensity (jet), 1=RGB by image, 2=camera id + int colorMode = 0; //!< 0=intensity (jet), 1=RGB by image, 2=camera id, 3=in ROI/mask, 4=local depth (jet) + float depthColorMax = 50.f; //!< m: local depth mode maps 0..this onto the jet colormap int coloredPts = 0; //!< points that received RGB from an image int uncoloredPts = 0; //!< points left as intensity-gray (no image / out of frustum / outside ROI) @@ -776,7 +843,6 @@ static void loadCloud(AppState& s) // time(s) -> T_world_lidar, for interpolating the pose at each image time. std::map trajMap = buildTrajMap(s.traj); - std::vector gpuData; float mx = 0.f; float sumX = 0, sumY = 0, sumZ = 0; int cnt = 0; @@ -893,6 +959,8 @@ static void loadCloud(AppState& s) int nImgs = (int)chunkImgs.size(); const size_t segBegin = s.exportCloud.size(); + std::vector gpuPoints; // this chunk's points, uploaded as its own part below + gpuPoints.reserve((pc.points.size() + step - 1) / step); // ── step 4: colorize each point ───────────────────────────────────── // chunkImgs is sorted by ts (imageTsNs was sorted) @@ -905,10 +973,6 @@ static void loadCloud(AppState& s) if (M) pw = *M * pw; - gpuData.push_back(pw.x()); - gpuData.push_back(pw.y()); - gpuData.push_back(pw.z()); - const float rawIntensity = pt.intensity; float colorF = packGray(rawIntensity); float camIdF = -1.f; // which image colored this point (global index), -1 = none @@ -1062,10 +1126,19 @@ static void loadCloud(AppState& s) else ++uncoloredPts; - gpuData.push_back(colorF); - gpuData.push_back(rawIntensity); - gpuData.push_back(camIdF); - gpuData.push_back(inRoiF); + float rangeM = -1.f; // distance from the LiDAR when the point was captured; -1 = unknown + if (pt.ts_ns != 0) + { + if (const auto pose = s.traj.nearest(pt.ts_ns)) + rangeM = (pw - pose->get().T.translation()).norm(); + } + const uint16_t intensityU16 = (uint16_t)std::lround(std::clamp(rawIntensity, 0.f, 1.f) * 65535.f); + // int16 cm tops out at 327.67 m + const int16_t depthCm = rangeM < 0.f ? int16_t(-1) : (int16_t)std::min(32767L, std::lround(rangeM * 100.f)); + // -2 keeps kVS's "rejected by ROI/mask" state, which camIdF alone can't tell from "no image" + const int32_t cameraId = camIdF >= 0.f ? (int32_t)camIdF : (inRoiF == 0.f ? -2 : -1); + // chunk-local position: the GPU applies M per chunk (GpuCloud::draw) + gpuPoints.push_back({ pt.x, pt.y, pt.z, colorF, intensityU16, depthCm, cameraId }); uint32_t packed; std::memcpy(&packed, &colorF, 4); @@ -1093,6 +1166,7 @@ static void loadCloud(AppState& s) // exportCloud + its MRP correction pose). if (s.exportCloud.size() > segBegin) s.exportSegments.push_back({ key, M ? *M : Eigen::Affine3f::Identity(), segBegin, s.exportCloud.size() - segBegin }); + s.cloud.addChunk(gpuPoints, M ? *M : Eigen::Affine3f::Identity(), key); // chunkImgs and their cv::Mat memory are released here } @@ -1105,8 +1179,6 @@ static void loadCloud(AppState& s) if (cnt > 0) { - s.cloud.upload(gpuData, mx); - // Frame the loaded cloud, instant rather than eased -- this runs once on // load, with nothing to transition from. Set on both euler and eulerGoal // so no stale transition target survives from a previous session. @@ -2030,15 +2102,16 @@ static void drawScene(AppState& s) if (s.cloud.count > 0 && s.shaderOk) { rlDrawRenderBatchActive(); - Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); rlEnableShader(s.shader.id); - rlSetUniformMatrix(s.locMVP, mvp); rlSetUniform(s.locPS, &s.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(s.locCM, &s.colorMode, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(s.locDecim, &s.drawDecim, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(s.locMinRange, &s.minRange, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(s.locMaxRange, &s.maxRange, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(s.locDepthMax, &s.depthColorMax, RL_SHADER_UNIFORM_FLOAT, 1); int sel = (s.isolateCamera && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) ? s.imgViewIdx : -1; rlSetUniform(s.locSel, &sel, RL_SHADER_UNIFORM_INT, 1); - s.cloud.draw(); + s.cloud.draw(rlGetMatrixModelview(), rlGetMatrixProjection(), s.locMVP); rlDisableShader(); } } @@ -2111,6 +2184,9 @@ int main(int argc, char* argv[]) s.locCM = rlGetLocationUniform(s.shader.id, "colorMode"); s.locDecim = rlGetLocationUniform(s.shader.id, "drawDecim"); s.locSel = rlGetLocationUniform(s.shader.id, "selectedCamera"); + s.locMinRange = rlGetLocationUniform(s.shader.id, "minRange"); + s.locMaxRange = rlGetLocationUniform(s.shader.id, "maxRange"); + s.locDepthMax = rlGetLocationUniform(s.shader.id, "depthColorMax"); } glEnable(GL_PROGRAM_POINT_SIZE); @@ -2517,12 +2593,29 @@ int main(int argc, char* argv[]) ImGui::SliderFloat("Point size", &s.pointSize, 1.f, 20.f, "%.1f"); ImGui::SetNextItemWidth(140.f); ImGui::SliderInt("Draw decimation", &s.drawDecim, 1, 64); + ImGui::SetNextItemWidth(140.f); + ImGui::DragFloat("Min range", &s.minRange, 0.1f, 0.f, 327.f, "%.2f m"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Hide points closer than this to the LiDAR when they were captured"); + ImGui::SetNextItemWidth(140.f); + ImGui::DragFloat("Max range", &s.maxRange, 0.5f, 0.f, 327.f, "%.1f m"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Hide points farther than this from the LiDAR when they were captured; 0 = no limit"); + ImGui::Separator(); + ImGui::TextDisabled("Point color:"); + if (ImGui::MenuItem("Intensity", nullptr, s.colorMode == 0)) + s.colorMode = 0; + if (ImGui::MenuItem("Local depth", nullptr, s.colorMode == 4)) + s.colorMode = 4; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Distance from the LiDAR when the point was captured; gray = unknown"); + if (s.colorMode == 4) + { + ImGui::SetNextItemWidth(140.f); + ImGui::DragFloat("Depth color max", &s.depthColorMax, 0.5f, 1.f, 327.f, "%.1f m"); + } if (!s.imagesFilenamesInTime.empty()) { - ImGui::Separator(); - ImGui::TextDisabled("Point color:"); - if (ImGui::MenuItem("Intensity", nullptr, s.colorMode == 0)) - s.colorMode = 0; if (ImGui::MenuItem("RGB (image)", "Ctrl", s.colorMode == 1)) s.colorMode = 1; if (ImGui::MenuItem("Camera ID", nullptr, s.colorMode == 2)) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h index a8907f43..db76a66b 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h +++ b/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); } )"; From fb6b9817037726727d6f33031f45302583e28daf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 15:39:53 +0200 Subject: [PATCH 3/8] Smooth the trajectory viewer flyover camera The flyover camera now sits on a Hann-weighted average of the trajectory poses within +/- a window of the playhead (default 1 s), which removes the walking sway and hand shake without lag, and optionally drops roll to keep the horizon level. Both are controls in the flyover bar. The averaging (Trajectory::smoothedPose, Trajectory::timeAt) and calib::levelHorizon live in calib_core, with tests. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 22 +++++- calib_core/include/CalibCore/Trajectory.h | 21 ++++++ calib_core/src/Trajectory.cpp | 55 +++++++++++++- calib_core/tests/test_trajectory.cpp | 72 ++++++++++++++++++- 4 files changed, 164 insertions(+), 6 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index fc6a995c..8784a409 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -439,6 +439,8 @@ struct AppState bool flyoverPlaying = false; //!< advance flyoverProgress each frame float flyoverProgress = 0.f; //!< 0-1 position along the trajectory's time span float flyoverSpeed = 1.f; //!< playback rate, multiple of real time + float flyoverSmoothingSec = 1.f; //!< camera pose averaged over ± this much trajectory time; 0 = raw poses + bool flyoverLevelHorizon = true; //!< remove the camera's roll, keeping heading and pitch bool flyoverUpdateSelectedCamera = true; //!< select the camera image nearest the playhead // ── fast-rotation image filter ───────────────────────────────────────────── //! Per-pose angular speed (deg/s), parallel to traj.poses — filled by @@ -2410,11 +2412,16 @@ int main(int argc, char* argv[]) s.orbit.viewPose.reset(); if (s.flyover) { - if (const auto pose = s.traj.nearest(s.flyoverProgress)) + // The continuous playhead time, not the nearest pose's, so the + // smoothing window slides instead of stepping from pose to pose. + const int64_t ts = s.traj.timeAt(s.flyoverProgress); + if (auto pose = s.traj.smoothedPose(ts, s.flyoverSmoothingSec)) { - s.orbit.setViewPose(pose->get().T); + if (s.flyoverLevelHorizon) + pose->linear() = levelHorizon(pose->linear()); + s.orbit.setViewPose(*pose); if (s.flyoverUpdateSelectedCamera) - selectImageNearest(s, pose->get().ts_ns - imageTimeOffsetNs(s)); + selectImageNearest(s, ts - imageTimeOffsetNs(s)); } } @@ -3075,6 +3082,15 @@ int main(int argc, char* argv[]) if (ImGui::IsItemHovered()) ImGui::SetTooltip("Playback rate, as a multiple of real time (Ctrl+click to type)"); ImGui::SameLine(); + ImGui::SetNextItemWidth(120.f); + ImGui::SliderFloat("Smoothing", &s.flyoverSmoothingSec, 0.f, 5.f, "%.1f s"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Average the camera pose over +/- this much trajectory time; 0 = raw poses"); + ImGui::SameLine(); + ImGui::Checkbox("Level horizon", &s.flyoverLevelHorizon); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Remove camera roll; heading and pitch still follow the trajectory"); + ImGui::SameLine(); ImGui::BeginDisabled(s.imageTsNs.empty()); ImGui::Checkbox("Update selected camera", &s.flyoverUpdateSelectedCamera); ImGui::EndDisabled(); diff --git a/calib_core/include/CalibCore/Trajectory.h b/calib_core/include/CalibCore/Trajectory.h index 5cc0994e..06886d4a 100644 --- a/calib_core/include/CalibCore/Trajectory.h +++ b/calib_core/include/CalibCore/Trajectory.h @@ -48,9 +48,30 @@ struct Trajectory { //! @warning Same ordering requirement as @ref nearest(int64_t). [[nodiscard]] std::optional> 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 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 \ No newline at end of file diff --git a/calib_core/src/Trajectory.cpp b/calib_core/src/Trajectory.cpp index d2a6a4a9..c9d661fb 100644 --- a/calib_core/src/Trajectory.cpp +++ b/calib_core/src/Trajectory.cpp @@ -57,10 +57,61 @@ std::optional> Trajectory::nearest(int64_ std::optional> 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; - const int64_t t = t0 + static_cast(static_cast(t1 - t0) * f); - return nearest(t); + return t0 + static_cast(static_cast(t1 - t0) * f); +} + +std::optional 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(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(it->ts_ns - ts_ns) / static_cast(halfNs); // [-1, 1] + const float w = static_cast(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 \ No newline at end of file diff --git a/calib_core/tests/test_trajectory.cpp b/calib_core/tests/test_trajectory.cpp index 74d8755c..ff8876dc 100644 --- a/calib_core/tests/test_trajectory.cpp +++ b/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 #include +#include #include #include 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(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)); +} From 04440be1c9531ddea54b29eb4b057f5a4c7d9e68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 15:44:28 +0200 Subject: [PATCH 4/8] Draw a color scale for the trajectory viewer's Local depth mode A jet bar from 0 m to Depth color max, with five labels, in the 3D view's top-right corner while the Local depth color mode is active. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 66 +++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 8784a409..b54a530d 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -1216,6 +1216,69 @@ static cv::Vec3b jetColorBGR(float t) return cv::Vec3b((uchar)(b * 255.f), (uchar)(g * 255.f), (uchar)(r * 255.f)); } +//! Vertical color scale for the Local depth mode, in the 3D view's top-right +//! corner: 0 m at the bottom, `maxM` at the top, as kFS maps jet(depth / depthColorMax). +//! @param rightX right edge of the 3D view, screen px +//! @param topY top edge of the 3D view, screen px +//! @param maxM depth at the top of the scale; farther points are clamped to its color +static void drawDepthColorScale(float rightX, float topY, float maxM) +{ + constexpr float kMargin = 12.f, kPad = 6.f, kBarW = 14.f, kBarH = 200.f, kTickW = 4.f; + constexpr int kLabels = 5; // 0, 1/4, 1/2, 3/4, 1 of maxM + auto label = [maxM](int i) + { + char buf[16]; + std::snprintf(buf, sizeof(buf), maxM < 10.f ? "%.1f m" : "%.0f m", maxM * float(i) / float(kLabels - 1)); + return std::string(buf); + }; + float labelW = 0.f; + for (int i = 0; i < kLabels; ++i) + labelW = std::max(labelW, ImGui::CalcTextSize(label(i).c_str()).x); + const float textH = ImGui::GetTextLineHeight(); + const char* title = "Local depth"; + const float titleW = ImGui::CalcTextSize(title).x; + + const float boxW = std::max(titleW, labelW + kTickW + kPad + kBarW) + 2.f * kPad; + const float boxH = kPad + textH + kPad + kBarH + textH * 0.5f + kPad; + const ImVec2 box0(rightX - kMargin - boxW, topY + kMargin); + const ImVec2 box1(box0.x + boxW, box0.y + boxH); + const float barX0 = box1.x - kPad - kBarW; + const float barY0 = box0.y + kPad + textH + kPad; // top of the bar = maxM + const float barY1 = barY0 + kBarH; // bottom = 0 m + + // Background draw list: over the 3D scene, under menus and popups. + ImDrawList* dl = ImGui::GetBackgroundDrawList(); + dl->AddRectFilled(box0, box1, IM_COL32(20, 20, 20, 190), 4.f); + dl->AddText(ImVec2(box1.x - kPad - titleW, box0.y + kPad), IM_COL32(220, 220, 220, 255), title); + + // jet() is piecewise linear with knots at these t, so one vertical gradient + // per segment reproduces it exactly. + static const float kKnots[] = { 0.f, 0.125f, 0.375f, 0.625f, 0.875f, 1.f }; + auto jetU32 = [](float t) + { + const cv::Vec3b c = jetColorBGR(t); + return IM_COL32(c[2], c[1], c[0], 255); + }; + for (size_t k = 0; k + 1 < std::size(kKnots); ++k) + { + const float yTop = barY1 - kKnots[k + 1] * kBarH; + const float yBot = barY1 - kKnots[k] * kBarH; + const ImU32 cTop = jetU32(kKnots[k + 1]); + const ImU32 cBot = jetU32(kKnots[k]); + dl->AddRectFilledMultiColor(ImVec2(barX0, yTop), ImVec2(barX0 + kBarW, yBot), cTop, cTop, cBot, cBot); + } + dl->AddRect(ImVec2(barX0, barY0), ImVec2(barX0 + kBarW, barY1), IM_COL32(200, 200, 200, 255)); + + for (int i = 0; i < kLabels; ++i) + { + const float y = barY1 - kBarH * float(i) / float(kLabels - 1); + dl->AddLine(ImVec2(barX0 - kTickW, y), ImVec2(barX0, y), IM_COL32(200, 200, 200, 255)); + const std::string text = label(i); + const float w = ImGui::CalcTextSize(text.c_str()).x; + dl->AddText(ImVec2(barX0 - kTickW - 2.f - w, y - textH * 0.5f), IM_COL32(220, 220, 220, 255), text.c_str()); + } +} + //! Rasterizes a synthetic "intensity image" for the camera pose at imgTsAdj, //! reprojecting s.exportCloud through the same extrinsics and projectPoint() as //! the colorize pass, jet-colormapped over intensity with a per-pixel depth test @@ -3111,6 +3174,9 @@ int main(int argc, char* argv[]) ImGui::End(); } + if (s.colorMode == 4 && s.cloud.count > 0) + drawDepthColorScale(io.DisplaySize.x - panelW, menuBarH, s.depthColorMax); + raylib_widgets::showEulerCenterOfRotationWindow(s.showCenterOfRotationWindow, s.orbit); // ── shortcuts help window ─────────────────────────────────────────────── From 66f4637e608e78a0238f510e65e69ee622733093 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 15:50:08 +0200 Subject: [PATCH 5/8] Add a Shift+F shortcut for the trajectory viewer flyover The key and the View menu item share one toggle, and the shortcut is listed in the help table. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index b54a530d..a0a86e0b 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -66,6 +66,7 @@ static const std::vector appShortcuts = { { "", "I", "Isometric view" }, { "", "Z", "Reset camera" }, { "", "O", "Toggle orthographic/perspective" }, + { "", "Shift+F", "Toggle flyover" }, { "Special keys", "Left arrow", "Previous image (image preview)" }, { "", "Right arrow", "Next image (image preview)" }, { "Mouse related", "Left click + drag", "Orbit camera" }, @@ -1717,6 +1718,17 @@ static void exportE57Session(AppState& s) s.status = std::string("Export failed: ") + err; } +//! Opens the flyover bar and plays from the start, or closes it -- shared by the +//! View menu item and Shift+F. Opening is a no-op without a trajectory. +static void toggleFlyover(AppState& s) +{ + if (!s.flyover && s.traj.empty()) + return; + s.flyover = !s.flyover; + s.flyoverProgress = 0.f; + s.flyoverPlaying = s.flyover; +} + // ── File actions ───────────────────────────────────────────────────────────── //! Factored out so the File menu items and their keyboard shortcuts (in the //! main loop below) call the exact same code, matching the openSession()-style @@ -2405,6 +2417,8 @@ int main(int argc, char* argv[]) if (shiftDown && IsKeyPressed(KEY_R)) s.showCenterOfRotationWindow = true; + if (shiftDown && !ctrlDown && IsKeyPressed(KEY_F)) + toggleFlyover(s); // Ctrl+Right-click: ground-plane (Z=0) pick. if (!imguiWants && ctrlDown && IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) { @@ -2650,11 +2664,8 @@ int main(int argc, char* argv[]) if (ImGui::MenuItem("Center of rotation...", "Shift+R")) s.showCenterOfRotationWindow = true; ImGui::Separator(); - if (ImGui::MenuItem("Flyover", nullptr, &s.flyover, !s.traj.empty())) - { - s.flyoverProgress = 0.f; - s.flyoverPlaying = s.flyover; - } + if (ImGui::MenuItem("Flyover", "Shift+F", s.flyover, !s.traj.empty())) + toggleFlyover(s); ImGui::Separator(); ImGui::SetNextItemWidth(140.f); From 2b88ef53e0d65032ff93a66d16122b3531fe1b07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 16:36:55 +0200 Subject: [PATCH 6/8] Toggle trajectory viewer flyover playback with Space Space plays/pauses while the flyover bar is open, sharing one function with the bar's Play/Pause button, and is listed in the help table. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index a0a86e0b..b6de0460 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -69,6 +69,7 @@ static const std::vector appShortcuts = { { "", "Shift+F", "Toggle flyover" }, { "Special keys", "Left arrow", "Previous image (image preview)" }, { "", "Right arrow", "Next image (image preview)" }, + { "", "Space", "Play/pause flyover" }, { "Mouse related", "Left click + drag", "Orbit camera" }, { "", "Right click + drag", "Pan camera" }, { "", "Scroll", "Zoom camera" }, @@ -1729,6 +1730,15 @@ static void toggleFlyover(AppState& s) s.flyoverPlaying = s.flyover; } +//! Plays or pauses the flyover, replaying from the start once it has reached the +//! end -- shared by the bar's Play/Pause button and Space. +static void toggleFlyoverPlaying(AppState& s) +{ + if (!s.flyoverPlaying && s.flyoverProgress >= 1.f) + s.flyoverProgress = 0.f; + s.flyoverPlaying = !s.flyoverPlaying; +} + // ── File actions ───────────────────────────────────────────────────────────── //! Factored out so the File menu items and their keyboard shortcuts (in the //! main loop below) call the exact same code, matching the openSession()-style @@ -2419,6 +2429,8 @@ int main(int argc, char* argv[]) s.showCenterOfRotationWindow = true; if (shiftDown && !ctrlDown && IsKeyPressed(KEY_F)) toggleFlyover(s); + if (s.flyover && IsKeyPressed(KEY_SPACE)) + toggleFlyoverPlaying(s); // Ctrl+Right-click: ground-plane (Z=0) pick. if (!imguiWants && ctrlDown && IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) { @@ -3143,11 +3155,9 @@ int main(int argc, char* argv[]) ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoSavedSettings); if (ImGui::Button(s.flyoverPlaying ? "Pause" : "Play", ImVec2(60.f, 0.f))) - { - if (!s.flyoverPlaying && s.flyoverProgress >= 1.f) - s.flyoverProgress = 0.f; // replay from the start - s.flyoverPlaying = !s.flyoverPlaying; - } + toggleFlyoverPlaying(s); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Space"); ImGui::SameLine(); ImGui::Text("%7.1f / %.1f s", s.flyoverProgress * durationSec, durationSec); ImGui::SameLine(); From 606414a514863be66b4b8f8f182d86f6c96e5055 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 16:36:55 +0200 Subject: [PATCH 7/8] Add a --mask option to the trajectory viewer Loads the image mask at startup through the same path as File > Open Image Mask, before the session and calibration so their status line stays last. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index b6de0460..5204cd48 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -2208,7 +2208,8 @@ int main(int argc, char* argv[]) { CliArgs args = parseArgs(argc, argv); static const char* kDesc = "View LIO trajectory, colorize and export point clouds"; - const std::vector usage = { cliopt::MJS, cliopt::CAMERA_DIR, cliopt::CALIB }; + static const char* kMaskOpt = " --mask image mask for coloring: white keeps, black drops"; + const std::vector usage = { cliopt::MJS, cliopt::CAMERA_DIR, cliopt::CALIB, kMaskOpt }; if (args.help) { printUsage("TrajectoryViewer", kDesc, usage); @@ -2251,6 +2252,9 @@ int main(int argc, char* argv[]) if (!calib.empty()) strncpy(s.calibBuf, calib.c_str(), sizeof(s.calibBuf) - 1); + if (args.has("mask")) + strncpy(s.maskBuf, args.get("mask").c_str(), sizeof(s.maskBuf) - 1); + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); InitWindow(1400, 900, ("Trajectory Viewer " HDMAPPING_VERSION_STRING)); // raylib's default exit key (Esc) closes the window outright -- too easy @@ -2314,7 +2318,10 @@ int main(int argc, char* argv[]) } }); - // auto-load if args given + // auto-load if args given; the mask first, since it needs neither of the + // others and would otherwise overwrite their status line + if (s.maskBuf[0]) + loadMask(s); if (s.sessionBuf[0]) loadSession(s); if (s.calibBuf[0]) From b9c3136f79b6088efb75a50455088f79a07fd06d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 28 Sep 2026 17:44:43 +0200 Subject: [PATCH 8/8] Apply the view's Min/Max range to the trajectory viewer's LAZ export Export points carry the same int16 cm range the shader filters on, and one passesRangeFilter() mirrors kVS's cull, so the LAZ holds what is on screen. The status line reports how many points were filtered out. Co-Authored-By: Claude Opus 5.5 --- .../TrajectoryViewer.cpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 5204cd48..ca483d4f 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -356,8 +356,20 @@ struct ColorPt float intensity; int64_t ts_ns; bool validColor; //!< RGB sampled from an image; false = intensity-gray fallback + int16_t depthCm; //!< LiDAR range as GpuPoint::depthCm holds it; -1 = unknown }; +//! kVS's Min/Max range cull on the CPU, so exports drop the points the view hides. +//! @param depthCm GpuPoint/ColorPt depth; < 0 (unknown) always passes +//! @param maxRange <= 0 = no upper limit +static bool passesRangeFilter(int16_t depthCm, float minRange, float maxRange) +{ + if (depthCm < 0) + return true; + const float range = float(depthCm) * 0.01f; + return !(range < minRange || (maxRange > 0.f && range > maxRange)); +} + // ── Application state ───────────────────────────────────────────────────────── struct AppState { @@ -1155,7 +1167,8 @@ static void loadCloud(AppState& s) (uint8_t)(packed & 0xFF), rawIntensity, pt.ts_ns, - camIdF >= 0.f }); + camIdF >= 0.f, + depthCm }); float d2 = pw.squaredNorm(); if (d2 > mx * mx) @@ -1524,14 +1537,15 @@ static void clearMask(AppState& s) static void exportLAZ(AppState& s) { + // Same Min/Max range as the view, so the file holds what is on screen. auto keep = [&](const ColorPt& p) { - return !s.exportOnlyValidColor || p.validColor; + return (!s.exportOnlyValidColor || p.validColor) && passesRangeFilter(p.depthCm, s.minRange, s.maxRange); }; const size_t nOut = std::count_if(s.exportCloud.begin(), s.exportCloud.end(), keep); if (nOut == 0) { - s.status = s.exportCloud.empty() ? "No cloud to export" : "No points with valid color to export"; + s.status = s.exportCloud.empty() ? "No cloud to export" : "No points left after the color and range filters"; return; } @@ -1615,6 +1629,8 @@ static void exportLAZ(AppState& s) laszip_close_writer(writer); laszip_destroy(writer); s.status = "Exported " + std::to_string(nOut) + " pts → " + s.exportBuf; + if (nOut < s.exportCloud.size()) + s.status += " (" + std::to_string(s.exportCloud.size() - nOut) + " filtered out)"; } //! E57 counterpart of exportLAZ(): one Data3D block, points already in world @@ -3031,6 +3047,8 @@ int main(int argc, char* argv[]) ImGui::Checkbox("LAZ: only points with valid color", &s.exportOnlyValidColor); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Skip points that got no RGB from an image (no image / out of frustum / outside ROI or mask)"); + if (s.minRange > 0.f || s.maxRange > 0.f) + ImGui::TextDisabled("LAZ: Min/Max range from the View menu applies"); if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) actionExportColoredLAZ(s); if (ImGui::Button("Export colored E57", ImVec2(-1, 0)))