From 19b69f42ab910d44eb96b51e72717b27b1e18256 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 8 Sep 2026 03:27:01 +0200 Subject: [PATCH 1/2] Filter images by rotation speed Signed-off-by: Michal Pelka --- .../RosExport.cpp | 2 +- .../TrajectoryViewer.cpp | 115 +++++++++++++++++- 2 files changed, 113 insertions(+), 4 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 47efff9a..584be0e7 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -31,7 +31,7 @@ bool exportRos2Bag(const RosExportInput&, const RosExportOptions&, std::string& #include #include -#include "PointCloud.h" +#include #include #include diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 9b13ef02..4a432c03 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -103,6 +103,56 @@ static bool interpPose(const std::map& trajMap, int64_t return true; } +static constexpr double kRad2Deg = 57.295779513082320876; + +// Angular speed (deg/s) for every trajectory pose: the rotation change to the next +// pose divided by the time step. Result is parallel to traj.poses; the last entry +// repeats the previous one. Fewer than two poses -> all zeros. Non-increasing +// timestamps (chunk boundaries, duplicates) reuse the previous value. +static std::vector computePoseAngularSpeedDeg(const Trajectory& traj) +{ + const auto& poses = traj.poses; + std::vector speed(poses.size(), 0.f); + for (size_t i = 0; i + 1 < poses.size(); ++i) + { + const double dt = (poses[i + 1].ts_ns - poses[i].ts_ns) * 1e-9; + if (dt <= 1e-6) + { + speed[i] = (i > 0) ? speed[i - 1] : 0.f; + continue; + } + const Eigen::Matrix3f dR = poses[i].T.linear().transpose() * poses[i + 1].T.linear(); + const float ang = Eigen::AngleAxisf(dR).angle(); // [0, pi] rad + speed[i] = (float)(ang / dt * kRad2Deg); + } + if (poses.size() >= 2) + speed.back() = speed[poses.size() - 2]; + return speed; +} + +// Angular speed (deg/s) at the trajectory pose nearest ts_ns. 0 when there's no +// per-pose data (not loaded, or size mismatch with the trajectory). +static float angularSpeedDegAt(const Trajectory& traj, const std::vector& perPose, int64_t ts_ns) +{ + if (traj.poses.empty() || perPose.size() != traj.poses.size()) + return 0.f; + auto it = std::lower_bound(traj.poses.begin(), traj.poses.end(), ts_ns, + [](const TrajPose& p, int64_t t) { return p.ts_ns < t; }); + size_t idx; + if (it == traj.poses.end()) + idx = traj.poses.size() - 1; + else if (it == traj.poses.begin()) + idx = 0; + else + { + auto prev = std::prev(it); + idx = (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) + ? (size_t)(it - traj.poses.begin()) + : (size_t)(prev - traj.poses.begin()); + } + return perPose[idx]; +} + using trajectory_viewer_shaders::kFS; using trajectory_viewer_shaders::kVS; @@ -198,8 +248,8 @@ struct AppState bool showHelp = false; bool isolateCamera = false; // render only points colored by the selected (preview) image float frustumScale = 0.5f; - float pointSize = 2.f; - int cloudDecim = 5; + float pointSize = 1.f; + int cloudDecim = 1; int drawDecim = 1; bool multiImgColoring = true; // false = single image per chunk (midpoint) // How each point is matched to a camera image: @@ -209,6 +259,17 @@ struct AppState int colorStrategy = 0; 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) + + // ── 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 + // maxImageAngSpeedDeg are dropped from the colorize pass (motion-smeared). + std::vector poseAngSpeedDeg; + float poseAngSpeedMax = 0.f; // deg/s: peak over the whole session (display only) + bool filterFastImages = true; // drop motion-smeared frames from the colorize pass + float maxImageAngSpeedDeg = 60.f; // deg/s threshold + 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 coloredPts = 0; // points that received RGB from an image @@ -389,6 +450,9 @@ static void loadSession(AppState& s) s.imageTsNs.clear(); s.exportCloud.clear(); s.exportSegments.clear(); + s.poseAngSpeedDeg.clear(); + s.poseAngSpeedMax = 0.f; + s.angFilteredImgs = 0; s.cloud.unload(); loadImages(s); @@ -423,6 +487,12 @@ static void loadSession(AppState& s) } s.traj.sort(); + // angular speed per pose — feeds the "drop fast-rotating images" colorize filter + s.poseAngSpeedDeg = computePoseAngularSpeedDeg(s.traj); + s.poseAngSpeedMax = s.poseAngSpeedDeg.empty() + ? 0.f + : *std::max_element(s.poseAngSpeedDeg.begin(), s.poseAngSpeedDeg.end()); + // camera image timestamps fs::path camDir = s.cameraBuf[0] ? fs::path(s.cameraBuf) : d.parent_path() / "CAMERA_0"; if (fs::is_directory(camDir)) @@ -521,6 +591,8 @@ static void loadCloud(AppState& s) int step = std::max(1, s.cloudDecim); int coloredChunks = 0; int coloredPts = 0, uncoloredPts = 0; + int angFilteredImgs = 0; // camera frames dropped for excessive pose angular speed + const bool dropFastImgs = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f; for (auto& lp : lazPaths) { @@ -568,6 +640,12 @@ static void loadCloud(AppState& s) auto fnIt = s.imagesFilenamesInTime.find(imgTs); if (fnIt == s.imagesFilenamesInTime.end()) continue; + if (dropFastImgs && + angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg) + { + ++angFilteredImgs; + continue; + } Eigen::Affine3f pose; if (!interpPose(trajMap, imgTs, pose)) continue; @@ -594,7 +672,11 @@ static void loadCloud(AppState& s) int64_t imgTs = *it; auto fnIt = s.imagesFilenamesInTime.find(imgTs); Eigen::Affine3f pose; - if (fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) + const bool tooFast = dropFastImgs && + angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; + if (tooFast) + ++angFilteredImgs; + if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) { cv::Mat img = cv::imread(fnIt->second); int gidx = (int)(it - s.imageTsNs.begin()); @@ -804,6 +886,7 @@ static void loadCloud(AppState& s) s.colorMode = 1; // default to RGB display once RGB data is available s.coloredPts = coloredPts; s.uncoloredPts = uncoloredPts; + s.angFilteredImgs = angFilteredImgs; if (cnt > 0) { @@ -832,6 +915,8 @@ static void loadCloud(AppState& s) s.status += " | Colored: " + std::to_string(coloredPts) + " Uncolored: " + std::to_string(uncoloredPts) + " (" + std::to_string((int)std::lround(pct)) + "%)"; } + if (angFilteredImgs > 0) + s.status += " | Fast-img filtered: " + std::to_string(angFilteredImgs); } static void loadCalib(AppState& s) @@ -2000,6 +2085,24 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); } + ImGui::Checkbox("Drop fast-rotating images", &s.filterFastImages); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Exclude camera frames whose pose angular speed exceeds\n" + "the threshold — motion-smeared, and prone to smearing\n" + "colour onto the wrong points from the time offset."); + if (s.filterFastImages) + { + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputFloat("Max ang. speed (deg/s)", &s.maxImageAngSpeedDeg, 1.f, 10.f, "%.1f"); + s.maxImageAngSpeedDeg = std::max(0.f, s.maxImageAngSpeedDeg); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + if (s.poseAngSpeedMax > 0.f) + ImGui::TextDisabled("session peak: %.1f deg/s", s.poseAngSpeedMax); + if (s.angFilteredImgs > 0) + ImGui::TextDisabled("%d image(s) dropped last pass", s.angFilteredImgs); + } if (ImGui::Button("Load cloud", ImVec2(-1, 0))) loadCloud(s); ImGui::PopItemWidth(); @@ -2066,6 +2169,12 @@ int main(int argc, char* argv[]) s.imgViewRequest.store(s.imgViewIdx); } ImGui::TextDisabled("ts: %lld", (long long)s.imageTsNs[s.imgViewIdx]); + { + float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); + bool fast = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f && as > s.maxImageAngSpeedDeg; + ImGui::TextColored(fast ? ImVec4(1.f, 0.5f, 0.3f, 1.f) : ImGui::GetStyleColorVec4(ImGuiCol_TextDisabled), + "ang. speed: %.1f deg/s%s", as, fast ? " (dropped)" : ""); + } ImGui::Checkbox("Only this camera's points", &s.isolateCamera); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Render only points colored by the selected image.\nNeeds 'Color by image (RGB)' enabled."); From b5375eb1022ff8d15c21ce79556f41c0fe166bca Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Thu, 10 Sep 2026 11:01:32 +0200 Subject: [PATCH 2/2] make clang happy again Signed-off-by: Michal Pelka --- .../TrajectoryViewer.cpp | 39 +++++++++++-------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 4a432c03..441a33cc 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -136,8 +136,14 @@ static float angularSpeedDegAt(const Trajectory& traj, const std::vector& { if (traj.poses.empty() || perPose.size() != traj.poses.size()) return 0.f; - auto it = std::lower_bound(traj.poses.begin(), traj.poses.end(), ts_ns, - [](const TrajPose& p, int64_t t) { return p.ts_ns < t; }); + auto it = std::lower_bound( + traj.poses.begin(), + traj.poses.end(), + ts_ns, + [](const TrajPose& p, int64_t t) + { + return p.ts_ns < t; + }); size_t idx; if (it == traj.poses.end()) idx = traj.poses.size() - 1; @@ -146,9 +152,8 @@ static float angularSpeedDegAt(const Trajectory& traj, const std::vector& else { auto prev = std::prev(it); - idx = (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) - ? (size_t)(it - traj.poses.begin()) - : (size_t)(prev - traj.poses.begin()); + idx = (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) ? (size_t)(it - traj.poses.begin()) + : (size_t)(prev - traj.poses.begin()); } return perPose[idx]; } @@ -489,9 +494,7 @@ static void loadSession(AppState& s) // angular speed per pose — feeds the "drop fast-rotating images" colorize filter s.poseAngSpeedDeg = computePoseAngularSpeedDeg(s.traj); - s.poseAngSpeedMax = s.poseAngSpeedDeg.empty() - ? 0.f - : *std::max_element(s.poseAngSpeedDeg.begin(), s.poseAngSpeedDeg.end()); + s.poseAngSpeedMax = s.poseAngSpeedDeg.empty() ? 0.f : *std::max_element(s.poseAngSpeedDeg.begin(), s.poseAngSpeedDeg.end()); // camera image timestamps fs::path camDir = s.cameraBuf[0] ? fs::path(s.cameraBuf) : d.parent_path() / "CAMERA_0"; @@ -640,8 +643,7 @@ static void loadCloud(AppState& s) auto fnIt = s.imagesFilenamesInTime.find(imgTs); if (fnIt == s.imagesFilenamesInTime.end()) continue; - if (dropFastImgs && - angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg) + if (dropFastImgs && angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg) { ++angFilteredImgs; continue; @@ -672,8 +674,7 @@ static void loadCloud(AppState& s) int64_t imgTs = *it; auto fnIt = s.imagesFilenamesInTime.find(imgTs); Eigen::Affine3f pose; - const bool tooFast = dropFastImgs && - angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; + const bool tooFast = dropFastImgs && angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; if (tooFast) ++angFilteredImgs; if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) @@ -2087,9 +2088,10 @@ int main(int argc, char* argv[]) } ImGui::Checkbox("Drop fast-rotating images", &s.filterFastImages); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Exclude camera frames whose pose angular speed exceeds\n" - "the threshold — motion-smeared, and prone to smearing\n" - "colour onto the wrong points from the time offset."); + ImGui::SetTooltip( + "Exclude camera frames whose pose angular speed exceeds\n" + "the threshold — motion-smeared, and prone to smearing\n" + "colour onto the wrong points from the time offset."); if (s.filterFastImages) { ImGui::PopItemWidth(); @@ -2172,8 +2174,11 @@ int main(int argc, char* argv[]) { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); bool fast = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f && as > s.maxImageAngSpeedDeg; - ImGui::TextColored(fast ? ImVec4(1.f, 0.5f, 0.3f, 1.f) : ImGui::GetStyleColorVec4(ImGuiCol_TextDisabled), - "ang. speed: %.1f deg/s%s", as, fast ? " (dropped)" : ""); + ImGui::TextColored( + fast ? ImVec4(1.f, 0.5f, 0.3f, 1.f) : ImGui::GetStyleColorVec4(ImGuiCol_TextDisabled), + "ang. speed: %.1f deg/s%s", + as, + fast ? " (dropped)" : ""); } ImGui::Checkbox("Only this camera's points", &s.isolateCamera); if (ImGui::IsItemHovered())