From 98a3c5484a42e3fe1fc9cefe515651bc53dac917 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 05:31:18 +0200 Subject: [PATCH 1/5] Solve for extrinsic in camera Signed-off-by: Michal Pelka --- apps/camera_lidar_calibration/App.cpp | 277 ++++++++++++++-- apps/camera_lidar_calibration/App.h | 41 +++ apps/camera_lidar_calibration/Renderer.cpp | 21 +- apps/camera_lidar_calibration/Renderer.h | 12 +- .../RendererShaders.h | 12 + apps/camera_lidar_calibration/UI.cpp | 295 ++++++++++++++---- apps/camera_lidar_calibration/UI.h | 10 +- .../RosExport.cpp | 2 +- .../TrajectoryViewer.cpp | 18 +- calib_core/CMakeLists.txt | 6 + calib_core/include/CalibCore/Camera.h | 27 +- .../CalibCore/CameraCalibrationSolver.h | 47 +++ calib_core/src/Camera.cpp | 8 +- core/CMakeLists.txt | 1 + raylib_widgets/CMakeLists.txt | 1 + .../include/RaylibWidgets/PointPicking.h | 38 +++ raylib_widgets/src/PointPicking.cpp | 44 +++ 17 files changed, 748 insertions(+), 112 deletions(-) create mode 100644 calib_core/include/CalibCore/CameraCalibrationSolver.h create mode 100644 raylib_widgets/include/RaylibWidgets/PointPicking.h create mode 100644 raylib_widgets/src/PointPicking.cpp diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index b79e0402..96809006 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -2,8 +2,10 @@ #include "imgui.h" #include "raymath.h" #include "rlImGui.h" +#include #include #include +#include #include #include #include @@ -59,6 +61,99 @@ void AppState::rebuildImageTexture() imageLoaded = true; } +// ── AppState correspondence picking ─────────────────────────────────────────── +void AppState::setPendingImagePoint(float u, float v) +{ + pendingPair.u = u; + pendingPair.v = v; + pendingHasImage = true; + + if (pendingHasCloud) + { + pairs.push_back(pendingPair); + pendingPair = CorrespondencePair{}; + pendingHasImage = pendingHasCloud = false; + statusMsg = "Pair " + std::to_string(pairs.size()) + " added"; + } + else + { + statusMsg = "Image point set -- Shift+click the matching point in the 3D View"; + } +} + +void AppState::setPendingCloudPoint(float px, float py, float pz) +{ + pendingPair.px = px; + pendingPair.py = py; + pendingPair.pz = pz; + pendingHasCloud = true; + + if (pendingHasImage) + { + pairs.push_back(pendingPair); + pendingPair = CorrespondencePair{}; + pendingHasImage = pendingHasCloud = false; + statusMsg = "Pair " + std::to_string(pairs.size()) + " added"; + } + else + { + statusMsg = "3D point set -- Shift+click the matching pixel in the Image View"; + } +} + +void AppState::clearPending() +{ + pendingPair = CorrespondencePair{}; + pendingHasImage = pendingHasCloud = false; + statusMsg = "Pending pick cleared"; +} + +void AppState::removePair(int index) +{ + if (index < 0 || index >= static_cast(pairs.size())) + return; + pairs.erase(pairs.begin() + index); + if (selectedPairIndex == index) + selectedPairIndex = -1; + else if (selectedPairIndex > index) + selectedPairIndex--; +} + +bool AppState::solvePairs() +{ + if (pairs.size() < 3) + { + statusMsg = "Need at least 3 pairs to solve"; + return false; + } + + std::vector corr; + corr.reserve(pairs.size()); + for (const auto& p : pairs) + { + calib::PointPixelCorrespondence c; + c.p = Eigen::Vector3d(p.px, p.py, p.pz); + c.u = p.u; + c.v = p.v; + corr.push_back(c); + } + + double rms = -1.0; + bool ok = calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); + if (!ok) + { + statusMsg = "Solve failed (degenerate correspondences)"; + return false; + } + + lastSolveRmsPixels = rms; + statusMsg = lockTranslation ? "Solved rotation (translation locked): RMS reprojection error " : "Solved extrinsics: RMS reprojection error "; + statusMsg += std::to_string(rms) + " px"; + if (!imageRectified && intrinsicsLoaded == false) + statusMsg += " (no intrinsics loaded -- distortion assumed zero)"; + return true; +} + // ── AppState::loadImage ─────────────────────────────────────────────────────── void AppState::loadImage(const char* path) { @@ -85,6 +180,17 @@ static void centerOrbitOnCloud(AppState& s) s.orbit.distance = span * 0.8f; } +// Keeps AppState::cloudPointsRaylib (used by 3D point picking) 1:1 in sync +// with AppState::cloud.points -- same LiDAR (x,y,z) -> raylib (x,z,-y) +// convention as Renderer::uploadCloud. +static void rebuildCloudPointsRaylib(AppState& s) +{ + s.cloudPointsRaylib.clear(); + s.cloudPointsRaylib.reserve(s.cloud.points.size()); + for (const auto& p : s.cloud.points) + s.cloudPointsRaylib.push_back(Vector3{ p.x, p.z, -p.y }); +} + void AppState::loadCloud(const char* path) { if (!cloud.load(path)) @@ -94,6 +200,7 @@ void AppState::loadCloud(const char* path) } cloudPaths = { path }; renderer.uploadCloud(cloud); + rebuildCloudPointsRaylib(*this); centerOrbitOnCloud(*this); statusMsg = ""; } @@ -123,6 +230,7 @@ void AppState::addCloud(const char* path) } cloudPaths.push_back(path); renderer.uploadCloud(cloud); + rebuildCloudPointsRaylib(*this); centerOrbitOnCloud(*this); statusMsg = ""; } @@ -367,13 +475,16 @@ void AppState::loadCalibration(const char* path) extrinsics.ty = pos[1].get(); extrinsics.tz = pos[2].get(); } - // camera_rotation_in_world_euler_zyx_deg: [rz, ry, rx] - if (je.contains("camera_rotation_in_world_euler_zyx_deg") && je["camera_rotation_in_world_euler_zyx_deg"].size() >= 3) + // camera_rotation_in_world_tait_bryan_omfika_deg: [om, fi, ka] + // -- R_wc = Rx(om)*Ry(fi)*Rz(ka), matching calib::Extrinsics' + // native rotation representation directly (no reordering/conversion). + if (je.contains("camera_rotation_in_world_tait_bryan_omfika_deg") && + je["camera_rotation_in_world_tait_bryan_omfika_deg"].size() >= 3) { - auto& rot = je["camera_rotation_in_world_euler_zyx_deg"]; - extrinsics.rz = rot[0].get(); - extrinsics.ry = rot[1].get(); - extrinsics.rx = rot[2].get(); + auto& rot = je["camera_rotation_in_world_tait_bryan_omfika_deg"]; + extrinsics.om = rot[0].get(); + extrinsics.fi = rot[1].get(); + extrinsics.ka = rot[2].get(); } gotExtrinsics = true; } @@ -400,9 +511,9 @@ void AppState::loadCalibration(const char* path) // ── AppState::saveCalibration ───────────────────────────────────────────────── void AppState::saveCalibration(const char* path) { - // World-frame convention: R = R_wc (camera orientation in world, ZYX Euler) + // World-frame convention: R = R_wc (camera orientation in world, om/fi/ka) // C = camera position in world. T_lidar_to_cam = [R_wc^T | -R_wc^T*C] - Eigen::Matrix3f R = eulerZYXtoMat3(extrinsics.rx, extrinsics.ry, extrinsics.rz); + Eigen::Matrix3f R = omFiKaToMat3(extrinsics.om, extrinsics.fi, extrinsics.ka); Eigen::Vector3f C(extrinsics.tx, extrinsics.ty, extrinsics.tz); Eigen::Vector3f ti = -(R.transpose() * C); // translation of T_lidar_to_camera @@ -410,7 +521,7 @@ void AppState::saveCalibration(const char* path) j["intrinsics"] = { { "fx", intrinsics.fx }, { "fy", intrinsics.fy }, { "cx", intrinsics.cx }, { "cy", intrinsics.cy }, { "k1", intrinsics.k1 }, { "k2", intrinsics.k2 }, { "k3", intrinsics.k3 }, { "k4", intrinsics.k4 }, { "k5", intrinsics.k5 }, { "k6", intrinsics.k6 }, { "p1", intrinsics.p1 }, { "p2", intrinsics.p2 } }; - j["extrinsics"]["camera_rotation_in_world_euler_zyx_deg"] = { extrinsics.rz, extrinsics.ry, extrinsics.rx }; + j["extrinsics"]["camera_rotation_in_world_tait_bryan_omfika_deg"] = { extrinsics.om, extrinsics.fi, extrinsics.ka }; j["extrinsics"]["camera_position_in_world_xyz"] = { C.x(), C.y(), C.z() }; j["extrinsics"]["camera_rotation_matrix_in_world"] = { { R(0, 0), R(0, 1), R(0, 2) }, { R(1, 0), R(1, 1), R(1, 2) }, @@ -434,7 +545,13 @@ void AppState::saveCalibration(const char* path) void App::run() { const int W = 1400, H = 900; - SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + // MSAA was costing real frame time once the 3D view grew to fill the + // whole window (was previously confined to half-height) -- point-cloud + // rendering is fill-rate-bound, and 4x MSAA multiplies per-pixel + // shading/blending cost across that now-doubled area. Point clouds are + // rendered as flat-shaded point sprites (no polygon edges to smooth), + // so the visual benefit was marginal anyway. + SetConfigFlags(FLAG_WINDOW_RESIZABLE); InitWindow(W, H, ("LiDAR-Camera Calibration " HDMAPPING_VERSION_STRING)); raylib_widgets::fitWindowToScreen(); // The 340px-wide side panel is fixed-width; below this the 3D/image @@ -469,34 +586,143 @@ void App::run() CloseWindow(); } +// Shift (either side) is the modifier that dedicates the mouse to point +// picking in both the Image View (UI::drawImageView) and the 3D View +// (updateAndDrawPicking3D below) -- mirrors the CTRL-to-pick convention +// core/src/control_points.cpp already uses elsewhere in this codebase. +static bool shiftHeld() +{ + return IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); +} + // ── App::update ─────────────────────────────────────────────────────────────── void App::update() { bool imguiWantMouse = ImGui::GetIO().WantCaptureMouse; - state.orbit.update(!imguiWantMouse); + // Freeze orbiting while Shift is held so LMB is dedicated to picking + // instead of being ambiguous between "start an orbit drag" and "click + // to pick". + bool allowOrbit = !imguiWantMouse && !shiftHeld(); + state.orbit.update(allowOrbit); +} + +// ── 3D point picking + correspondence markers ─────────────────────────────── +// Draws small markers for already-picked pairs and, while Shift is held over +// the 3D view, a live "nearest point under cursor" candidate -- the 3D-side +// counterpart to the image view's zoom loupe: a precision aid the user sees +// before committing a click. Must be called inside BeginMode3D/EndMode3D. +// +// Index labels for the markers are 2D text (DrawText), which must NOT be +// called while inside BeginMode3D/EndMode3D -- while a 3D projection matrix +// is active, DrawText's screen-pixel quads would get warped through it +// instead of rendering flat. So this only *collects* (screenPos, label) +// pairs here (GetWorldToScreen just needs the camera, not an active 3D +// pass); the caller draws them with plain DrawText() after EndMode3D(). +static void updateAndDrawPicking3D( + AppState& state, const Camera3D& cam3d, Rectangle viewport3D, std::vector>& outLabels) +{ + Vector3 camFwd = Vector3Normalize(Vector3Subtract(cam3d.target, cam3d.position)); + + for (size_t i = 0; i < state.pairs.size(); i++) + { + const auto& p = state.pairs[i]; + Vector3 wp = { p.px, p.pz, -p.py }; + DrawSphere(wp, 0.05f, YELLOW); + DrawLine3D(Vector3{ wp.x - 0.1f, wp.y, wp.z }, Vector3{ wp.x + 0.1f, wp.y, wp.z }, YELLOW); + DrawLine3D(Vector3{ wp.x, wp.y - 0.1f, wp.z }, Vector3{ wp.x, wp.y + 0.1f, wp.z }, YELLOW); + + if (Vector3DotProduct(Vector3Subtract(wp, cam3d.position), camFwd) > 0.f) + { + Vector2 s = GetWorldToScreen(wp, cam3d); + if (s.x >= viewport3D.x && s.x <= viewport3D.x + viewport3D.width && s.y >= viewport3D.y && + s.y <= viewport3D.y + viewport3D.height) + outLabels.emplace_back(s, (int)i); + } + } + + // Large 3-axis cross for the pair selected in the Correspondences + // panel, so it's easy to spot in the (usually much busier) 3D view. + if (state.selectedPairIndex >= 0 && state.selectedPairIndex < (int)state.pairs.size()) + { + const auto& p = state.pairs[state.selectedPairIndex]; + Vector3 wp = { p.px, p.pz, -p.py }; + const float armLen = 0.5f; + DrawLine3D(Vector3{ wp.x - armLen, wp.y, wp.z }, Vector3{ wp.x + armLen, wp.y, wp.z }, MAGENTA); + DrawLine3D(Vector3{ wp.x, wp.y - armLen, wp.z }, Vector3{ wp.x, wp.y + armLen, wp.z }, MAGENTA); + DrawLine3D(Vector3{ wp.x, wp.y, wp.z - armLen }, Vector3{ wp.x, wp.y, wp.z + armLen }, MAGENTA); + } + + if (state.pendingHasCloud) + { + Vector3 wp = { state.pendingPair.px, state.pendingPair.pz, -state.pendingPair.py }; + DrawSphere(wp, 0.07f, ORANGE); + } + + if (!shiftHeld() || ImGui::GetIO().WantCaptureMouse) + return; + + Vector2 mouse = GetMousePosition(); + if (mouse.x < viewport3D.x || mouse.x > viewport3D.x + viewport3D.width || mouse.y < viewport3D.y || + mouse.y > viewport3D.y + viewport3D.height) + return; + + // Plain (not viewport-scoped) ray: BeginMode3D derives its projection's + // aspect ratio from the *full* window framebuffer regardless of the + // BeginScissorMode() sub-region this 3D view is confined to (see + // PointPicking.h), so the ray must be built the same way -- matching + // OrbitCamera::pickGroundPlaneTarget's existing convention. + Ray ray = GetScreenToWorldRay(mouse, cam3d); + + size_t hitIndex = 0; + bool hit = raylib_widgets::pickNearestPoint( + state.cloudPointsRaylib.data(), state.cloudPointsRaylib.size(), ray, cam3d.fovy, (float)GetScreenHeight(), 12.f, hitIndex); + if (hit) + { + const auto& hp = state.cloud.points[hitIndex]; + Vector3 wp = state.cloudPointsRaylib[hitIndex]; + DrawSphere(wp, 0.08f, LIME); + + if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) + state.setPendingCloudPoint(hp.x, hp.y, hp.z); + } + else if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) + { + state.statusMsg = "No point found near cursor, try again"; + } } // ── App::draw ───────────────────────────────────────────────────────────────── +// The 3D view fills the whole background (no more fixed top/bottom split +// with the image view) -- the image is now a normal floating ImGui window +// (see UI::draw) that the user can move/resize/overlap on top of it, like +// any other panel. void App::draw() { float menuBarH = ImGui::GetFrameHeight(); float panelW = 340.f; float viewW = (float)GetScreenWidth() - panelW; float viewH = (float)GetScreenHeight() - menuBarH; - float view3DY = menuBarH + viewH * 0.5f; // 3D starts at middle, below the menu bar // ── Image + projection overlay (GPU, into render texture) + // Hide the projected-point overlay while Shift is held (picking mode) + // so it doesn't obscure the pixel the user is aiming for. if (state.imageLoaded) state.renderer.renderImageOverlay( - state.imageTexture, state.imageW, state.imageH, state.intrinsics, state.extrinsics, !state.imageRectified, state.vizParams); - - // ── 3D scene renders in the bottom-left area (as raylib background) + state.imageTexture, + state.imageW, + state.imageH, + state.intrinsics, + state.extrinsics, + !state.imageRectified, + state.vizParams, + !shiftHeld()); + + // ── 3D scene renders as the raylib background, filling the whole view BeginDrawing(); ClearBackground(Color{ 30, 30, 30, 255 }); - // Clipping for 3D region (bottom-left) // Note: raylib scissor is in screen coords (y-down) - BeginScissorMode(0, (int)view3DY, (int)viewW, (int)(viewH * 0.5f)); + BeginScissorMode(0, (int)menuBarH, (int)viewW, (int)viewH); Camera3D cam3d = state.orbit.toRaylib(); BeginMode3D(cam3d); @@ -517,9 +743,18 @@ void App::draw() // Grid on ground plane DrawGrid(20, 1.f); + Rectangle viewport3D = { 0.f, menuBarH, viewW, viewH }; + std::vector> pickLabels; + updateAndDrawPicking3D(state, cam3d, viewport3D, pickLabels); + EndMode3D(); EndScissorMode(); + // Index labels for the pair markers -- 2D text, drawn after EndMode3D + // (see updateAndDrawPicking3D's comment for why). + for (const auto& [screenPos, index] : pickLabels) + DrawText(std::to_string(index).c_str(), (int)screenPos.x + 8, (int)screenPos.y - 8, 14, YELLOW); + if (state.showCompassRuler) { Vector3 fwd = Vector3Normalize(Vector3Subtract(cam3d.target, cam3d.position)); @@ -529,10 +764,12 @@ void App::draw() } // ── 3D label - DrawText("3D View [LMB: orbit | RMB: pan | Scroll: zoom]", 8, (int)view3DY + 4, 14, LIGHTGRAY); + if (shiftHeld()) + DrawText("Shift+click to pick a 3D point (green = candidate)", 8, (int)menuBarH + 4, 14, YELLOW); + else + DrawText("3D View [LMB: orbit | RMB: pan | Scroll: zoom | Shift: pick]", 8, (int)menuBarH + 4, 14, LIGHTGRAY); - // ── Divider line - DrawLineEx(Vector2{ 0, view3DY }, Vector2{ viewW, view3DY }, 1.f, GRAY); + DrawFPS((int)viewW - 90, (int)menuBarH + 4); // ── ImGui on top ───────────────────────────────────────────────────────── rlImGuiBegin(); diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index 54573bc7..aa81695b 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -10,6 +10,15 @@ using namespace calib; +// One manually-picked correspondence between a LiDAR point and the image +// pixel it should project to. u/v are in the (undistorted) displayed-image +// pixel frame; px/py/pz are in the raw LiDAR frame. +struct CorrespondencePair +{ + float u = 0.f, v = 0.f; + float px = 0.f, py = 0.f, pz = 0.f; +}; + struct AppState { // ── loaded data ────────────────────────────────────────────────────────── @@ -22,6 +31,11 @@ struct AppState int imageW = 0, imageH = 0; std::string imagePath; std::vector cloudPaths; + // `cloud.points` converted to raylib world-space (x, z, -y), 1:1 index + // match with `cloud.points` -- kept in sync by loadCloud()/addCloud() so + // 3D point picking (raylib_widgets::pickNearestPoint) doesn't have to + // re-convert the whole cloud every frame. + std::vector cloudPointsRaylib; // ── calibration params ─────────────────────────────────────────────────── Intrinsics intrinsics; @@ -41,6 +55,33 @@ struct AppState // ── misc ────────────────────────────────────────────────────────────────── std::string statusMsg; + // ── LiDAR↔image correspondence picking ────────────────────────────────── + // Hold Shift and click in either the Image View or the 3D View to set + // the pending pick for that side (each click overwrites the previous + // pending pick for that side, in either order); once both sides are set + // the pair is completed automatically. + std::vector pairs; + CorrespondencePair pendingPair; + bool pendingHasImage = false; + bool pendingHasCloud = false; + double lastSolveRmsPixels = -1.0; // < 0 = no solve run yet + // Row selected in the Correspondences panel list (-1 = none); drawn as + // a large cross in both the 3D view and the image view. + int selectedPairIndex = -1; + // When set, "Solve Extrinsics from Pairs" refines orientation only and + // leaves tx/ty/tz exactly as they are -- for when the camera's position + // relative to the LiDAR is already known (e.g. measured by hand) and + // only orientation needs calibrating from the picked pairs. Also + // disables the tx/ty/tz drag sliders in the Extrinsics panel so they + // can't be nudged by accident while locked. + bool lockTranslation = false; + + void setPendingImagePoint(float u, float v); // completes the pair if a cloud point is already pending + void setPendingCloudPoint(float px, float py, float pz); // completes the pair if an image point is already pending + void clearPending(); // discards the in-progress pending pick, if any + void removePair(int index); + bool solvePairs(); // solves extrinsics from `pairs`, updates statusMsg + // ── operations ──────────────────────────────────────────────────────────── void loadImage(const char* path); void loadCloud(const char* path); // clear + load diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp index 57f9834c..1d16e3d3 100644 --- a/apps/camera_lidar_calibration/Renderer.cpp +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -24,11 +24,11 @@ Color jetColor(float t) // those definitions, which became a duplicate-symbol link error once // raylib_widgets/CMakeLists.txt started actually compiling that .cpp. -// World-frame convention: E.rx/ry/rz = camera orientation in world (R_wc, ZYX Euler). +// World-frame convention: E.om/fi/ka = camera orientation in world (R_wc = Rx*Ry*Rz). // E.tx/ty/tz = camera position in world. p_cam = R_wc^T * (p_lidar - C). static Matrix buildLidarToCamMatrix(const Extrinsics& E) { - Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz); + Eigen::Matrix3f R = omFiKaToMat3(E.om, E.fi, E.ka); Eigen::Vector3f ti = -(R.transpose() * Eigen::Vector3f(E.tx, E.ty, E.tz)); // Raylib Matrix struct fields: m0,m4,m8,m12 / m1,m5,m9,m13 / m2,m6,m10,m14 / m3,m7,m11,m15 // We store R^T with translation ti (lidar→cam transform). @@ -80,6 +80,7 @@ void Renderer::initPointShader() { locMVP = rlGetLocationUniform(pointShader.id, "mvp"); locPointSize = rlGetLocationUniform(pointShader.id, "pointSize"); + locDecim = rlGetLocationUniform(pointShader.id, "drawDecim"); locColorMode = rlGetLocationUniform(pointShader.id, "colorMode"); locHeightRange = rlGetLocationUniform(pointShader.id, "heightRange"); locMaxDist = rlGetLocationUniform(pointShader.id, "maxDist"); @@ -108,6 +109,7 @@ void Renderer::initPointShader() locPrjOpacity = rlGetLocationUniform(projShader.id, "opacity"); locPrjPointSize = rlGetLocationUniform(projShader.id, "pointSize"); locPrjColorMode = rlGetLocationUniform(projShader.id, "colorMode"); + locPrjDecim = rlGetLocationUniform(projShader.id, "drawDecim"); } // Allow gl_PointSize from the vertex shader (core profile requires this) @@ -162,7 +164,14 @@ void Renderer::unloadCloudGPU() } void Renderer::renderImageOverlay( - const Texture2D& img, int imgW, int imgH, const Intrinsics& K, const Extrinsics& E, bool applyDistortion, const VisualizationParams& vp) + const Texture2D& img, + int imgW, + int imgH, + const Intrinsics& K, + const Extrinsics& E, + bool applyDistortion, + const VisualizationParams& vp, + bool showOverlay) { if (!imageTexValid) return; @@ -173,7 +182,7 @@ void Renderer::renderImageOverlay( DrawTexturePro( img, Rectangle{ 0, 0, (float)imgW, (float)imgH }, Rectangle{ 0, 0, (float)texW, (float)texH }, Vector2{ 0, 0 }, 0.f, WHITE); - if (cloudCount > 0 && projShaderValid) + if (showOverlay && cloudCount > 0 && projShaderValid) { rlDrawRenderBatchActive(); // flush the image quad before raw GL draw @@ -208,6 +217,7 @@ void Renderer::renderImageOverlay( rlSetUniform(locPrjOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locPrjPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locPrjColorMode, &vp.colorMode, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locPrjDecim, &vp.drawDecim, RL_SHADER_UNIFORM_INT, 1); rlEnableVertexArray(cloudVAO); glDrawArrays(GL_POINTS, 0, cloudCount); @@ -256,6 +266,7 @@ void Renderer::draw3DCloud( rlEnableShader(pointShader.id); rlSetUniformMatrix(locMVP, mvp); rlSetUniform(locPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(locDecim, &vp.drawDecim, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(locColorMode, &colorMode, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(locHeightRange, heightRange, RL_SHADER_UNIFORM_VEC2, 1); rlSetUniform(locMaxDist, &maxDist, RL_SHADER_UNIFORM_FLOAT, 1); @@ -281,7 +292,7 @@ void Renderer::draw3DCloud( void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, int imgW, int imgH, float scale) { // World-frame convention: R_wc = camera orientation in world, C = camera position in world - Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz); + Eigen::Matrix3f R = omFiKaToMat3(E.om, E.fi, E.ka); // Camera position in LiDAR frame is directly (E.tx, E.ty, E.tz) Vector3 origin = { E.tx, E.tz, -E.ty }; // LiDAR→raylib diff --git a/apps/camera_lidar_calibration/Renderer.h b/apps/camera_lidar_calibration/Renderer.h index e48d0a54..36990ced 100644 --- a/apps/camera_lidar_calibration/Renderer.h +++ b/apps/camera_lidar_calibration/Renderer.h @@ -15,6 +15,9 @@ struct VisualizationParams float depthMax = 50.f; float opacity = 1.f; int colorMode = 0; // 0=depth(jet), 1=intensity, 2=height(z), 3=Camera RGB + // Draw only every Nth point (GPU-side, like camera_lidar_trajectory_viewer's + // "Draw decimation") -- 1 = draw all points. + int drawDecim = 1; }; Color jetColor(float t); // t in [0,1] @@ -38,6 +41,8 @@ class Renderer // Render image + GPU-projected point overlay into imageTex. // If the displayed image is rectified, pass applyDistortion=false. + // showOverlay=false draws just the plain image (e.g. while picking, so + // projected points don't obscure the pixel the user is aiming for). void renderImageOverlay( const Texture2D& img, int imgW, @@ -45,7 +50,8 @@ class Renderer const Intrinsics& K, const Extrinsics& E, bool applyDistortion, - const VisualizationParams& vp); + const VisualizationParams& vp, + bool showOverlay = true); // Draw 3D point cloud into current BeginMode3D context (GPU shader path). // For colorMode 3 (camera RGB) pass the displayed image texture and the @@ -77,7 +83,7 @@ class Renderer int cloudCount = 0; // 3D view shader uniforms int locMVP = -1, locColorMode = -1, locHeightRange = -1; - int locMaxDist = -1, locOpacity = -1, locPointSize = -1; + int locMaxDist = -1, locOpacity = -1, locPointSize = -1, locDecim = -1; int locCamXform = -1, locCamK = -1, locCamImgSize = -1, locCamTex = -1; // 2D image-projection shader @@ -86,5 +92,5 @@ class Renderer int locPrjXform = -1, locPrjK = -1, locPrjImgSize = -1; int locPrjRad1 = -1, locPrjRad2 = -1, locPrjTan = -1; int locPrjDepthRange = -1, locPrjOpacity = -1; - int locPrjPointSize = -1, locPrjColorMode = -1; + int locPrjPointSize = -1, locPrjColorMode = -1, locPrjDecim = -1; }; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index 41998bbf..3f803aea 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -18,6 +18,7 @@ layout(location = 0) in vec3 vertexPosition; layout(location = 1) in float vertexIntensity; uniform mat4 mvp; uniform float pointSize; +uniform int drawDecim; // draw only every Nth point; 1 = draw all uniform mat4 lidarToCam; // extrinsics (for RGB mode) uniform vec4 K; // fx, fy, cx, cy uniform vec2 imgSize; @@ -26,6 +27,11 @@ out float fragIntensity; out vec2 fragUV; out float fragCamDepth; void main() { + if (drawDecim > 1 && (gl_VertexID % drawDecim) != 0) { + gl_Position = vec4(2.0, 2.0, 2.0, 1.0); + gl_PointSize = 0.0; + return; + } fragPos = vertexPosition; fragIntensity = vertexIntensity; gl_Position = mvp * vec4(vertexPosition, 1.0); @@ -90,9 +96,15 @@ uniform vec3 kRad1; // k1 k2 k3 uniform vec3 kRad2; // k4 k5 k6 uniform vec2 pTan; // p1 p2 uniform float pointSize; +uniform int drawDecim; // draw only every Nth point; 1 = draw all out float fragDepth; out float fragIntensity; void main() { + if (drawDecim > 1 && (gl_VertexID % drawDecim) != 0) { + gl_Position = vec4(2.0, 2.0, 2.0, 1.0); + gl_PointSize = 0.0; + return; + } // raylib coords -> lidar: x = rx, y = -rz, z = ry vec3 lidar = vec3(vertexPosition.x, -vertexPosition.z, vertexPosition.y); vec3 pc = (lidarToCam * vec4(lidar, 1.0)).xyz; diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index ab4820f9..fbf57f02 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -84,29 +85,44 @@ void UI::draw(AppState& state) panelExtrinsics(state); if (ImGui::CollapsingHeader("Visualization")) panelVisualization(state); + if (ImGui::CollapsingHeader("Correspondences", ImGuiTreeNodeFlags_DefaultOpen)) + panelCorrespondences(state); ImGui::End(); // ── Image view window (pan + zoom) ──────────────────────────────────── + // A normal floating window (title bar, movable, resizable) that overlaps + // the 3D view, which now fills the whole background instead of being + // confined to half the screen -- simpler than the old fixed-region + // split-screen layout. ImGuiCond_FirstUseEver only sets this starting + // pos/size the very first time; the user's own placement afterward + // persists. if (state.renderer.imageTexValid) { float viewW = io.DisplaySize.x - panelW; - float viewH = (io.DisplaySize.y - menuBarH) * 0.5f; - ImGui::SetNextWindowPos(ImVec2(0, menuBarH), ImGuiCond_Always); - ImGui::SetNextWindowSize(ImVec2(viewW, viewH), ImGuiCond_Always); - ImGui::Begin( - "Image View", - nullptr, - ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoTitleBar); + float viewH = io.DisplaySize.y - menuBarH; + ImGui::SetNextWindowPos(ImVec2(0, menuBarH), ImGuiCond_FirstUseEver); + ImGui::SetNextWindowSize(ImVec2(viewW * 0.6f, viewH * 0.6f), ImGuiCond_FirstUseEver); + ImGui::Begin("Image View"); drawImageView(state); ImGui::End(); } } // ── Image view: pan + zoom ──────────────────────────────────────────────────── -// zoom = 1 means "fit to window". offX/offY = image coords of the top-left -// visible pixel. Wheel zooms anchored at the cursor, LMB-drag pans, -// double-click resets. +// Pan and zoom are both handled by letting ImGui do the work instead of a +// hand-rolled offX/offY/scale coordinate system: the image is displayed at +// (imgW*imgZoom, imgH*imgZoom) inside a plain scrollable child region, so +// dragging just moves the child's own scroll offset (SetScrollX/Y, which +// ImGui clamps into range for us every frame) and zoom is a bare size +// multiplier (matching apps/manual_color's PageUp/PageDown zoom -- not +// anchored to the cursor, kept simple on purpose). This removes an entire +// class of bugs the previous custom-coordinate version had (invalid +// offX/offY silently desyncing the sampled crop from where markers were +// drawn) by construction: there is no separate "crop rectangle" to keep in +// sync any more, ImGui::GetItemRectMin() after drawing the (full, +// un-cropped) image *is* the single source of truth every other coordinate +// in this function is built from. void UI::drawImageView(AppState& state) { const float imgW = (float)state.imageW; @@ -114,71 +130,119 @@ void UI::drawImageView(AppState& state) if (imgW <= 0 || imgH <= 0) return; - // Reset view when a different image is loaded - if (state.imageW != viewImgW || state.imageH != viewImgH) + // Reset zoom when a different image is loaded + if (state.imageW != lastImgW || state.imageH != lastImgH) { - viewImgW = state.imageW; - viewImgH = state.imageH; - zoom2D = 1.f; - offX = offY = 0.f; + lastImgW = state.imageW; + lastImgH = state.imageH; + imgZoom = 1.f; } - ImVec2 origin = ImGui::GetCursorScreenPos(); // content region top-left - ImVec2 avail = ImGui::GetContentRegionAvail(); - if (avail.x < 16 || avail.y < 16) - return; - - const float fitScale = std::min(avail.x / imgW, avail.y / imgH); - float scale = fitScale * zoom2D; - - // Displayed size and the visible sub-rect of the image - float dispW = std::min(avail.x, imgW * scale); - float dispH = std::min(avail.y, imgH * scale); - float srcW = dispW / scale; - float srcH = dispH / scale; + // Shift is the modifier that dedicates the mouse to picking, mirroring + // App.cpp's 3D-view picking and the CTRL-to-pick convention + // core/src/control_points.cpp already uses elsewhere in this codebase. + const bool picking = ImGui::GetIO().KeyShift; + ImGui::TextColored( + ImVec4(1, 1, 0, 0.8f), picking ? "Shift+click to pick a point" : "Scroll: zoom | Drag: pan | Dbl-click: reset zoom"); - ImVec2 imgScreenPos = ImVec2(origin.x + (avail.x - dispW) * 0.5f, origin.y + (avail.y - dispH) * 0.5f); + ImGui::BeginChild("image_scroll", ImVec2(0, 0), false, ImGuiWindowFlags_HorizontalScrollbar | ImGuiWindowFlags_NoScrollWithMouse); - ImGui::SetCursorScreenPos(imgScreenPos); - // Render textures are y-flipped: select the sub-rect with negative height - Rectangle src = { offX, imgH - offY, srcW, -srcH }; - rlImGuiImageRect(&state.renderer.imageTex.texture, (int)dispW, (int)dispH, src); + ImVec2 dispSize(imgW * imgZoom, imgH * imgZoom); + ImGui::Image((ImTextureID)state.renderer.imageTex.texture.id, dispSize, ImVec2(0, 1), ImVec2(1, 0)); + ImVec2 imgMin = ImGui::GetItemRectMin(); // screen pos of the image's top-left, already scroll-adjusted + bool hovered = ImGui::IsItemHovered(); + ImGuiIO& io = ImGui::GetIO(); - // ── input ────────────────────────────────────────────────────────────── - if (ImGui::IsWindowHovered()) + // Image pixel <-> screen coordinate conversion, shared by the click + // handling below, the magnifier loupe, and the correspondence-pair + // markers drawn further down. + auto screenToImg = [&](ImVec2 s) { - ImGuiIO& io = ImGui::GetIO(); + return ImVec2((s.x - imgMin.x) / imgZoom, (s.y - imgMin.y) / imgZoom); + }; + auto imgToScreen = [&](ImVec2 p) + { + return ImVec2(imgMin.x + p.x * imgZoom, imgMin.y + p.y * imgZoom); + }; + if (hovered) + { if (io.MouseWheel != 0.f) + imgZoom = std::clamp(imgZoom * std::exp(io.MouseWheel * 0.15f), 0.05f, 20.f); + + if (picking) { - // image point under the cursor stays put while zooming - // float mx = io.MousePos.x - imgScreenPos.x; - // float my = io.MousePos.y - imgScreenPos.y; - // float ix = offX + mx / scale; - // float iy = offY + my / scale; - - zoom2D = std::max(1.f, std::min(zoom2D * std::exp(io.MouseWheel * 0.15f), 100.f)); - scale = fitScale * zoom2D; - // offX = ix - mx / scale; - // offY = iy - my / scale; + // LMB is dedicated to picking here -- dragging must not pan. + if (ImGui::IsMouseClicked(ImGuiMouseButton_Left)) + { + ImVec2 p = screenToImg(io.MousePos); + state.setPendingImagePoint(std::clamp(p.x, 0.f, imgW), std::clamp(p.y, 0.f, imgH)); + } } - - if (ImGui::IsMouseDragging(ImGuiMouseButton_Left)) + else if (ImGui::IsMouseDragging(ImGuiMouseButton_Left)) { - offX -= io.MouseDelta.x / scale; - offY -= io.MouseDelta.y / scale; + ImGui::SetScrollX(ImGui::GetScrollX() - io.MouseDelta.x); + ImGui::SetScrollY(ImGui::GetScrollY() - io.MouseDelta.y); } if (ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Left)) + imgZoom = 1.f; + } + + // ── magnifier loupe (precision aid while picking an image pixel) ──────── + // Tooltip-based, mirroring apps/manual_color's draw_zoom_pick_point + // (auto-follows the cursor, crosshair at its center). + if (picking && hovered) + { + ImGui::BeginTooltip(); + + const float regionSz = 24.f; // crop size, in image pixels + const float loupeZoom = 6.f; + + ImVec2 p = screenToImg(io.MousePos); + float srcX = std::clamp(p.x - regionSz * 0.5f, 0.f, std::max(0.f, imgW - regionSz)); + float srcY = std::clamp(p.y - regionSz * 0.5f, 0.f, std::max(0.f, imgH - regionSz)); + ImVec2 uv0(srcX / imgW, 1.f - srcY / imgH); + ImVec2 uv1((srcX + regionSz) / imgW, 1.f - (srcY + regionSz) / imgH); + + ImGui::Image((ImTextureID)state.renderer.imageTex.texture.id, ImVec2(regionSz * loupeZoom, regionSz * loupeZoom), uv0, uv1); + + ImVec2 lMin = ImGui::GetItemRectMin(); + ImVec2 lMax = ImGui::GetItemRectMax(); + ImVec2 c((lMin.x + lMax.x) * 0.5f, (lMin.y + lMax.y) * 0.5f); + ImGui::GetForegroundDrawList()->AddLine(ImVec2(c.x - 10, c.y), ImVec2(c.x + 10, c.y), IM_COL32(0, 255, 0, 220), 1.5f); + ImGui::GetForegroundDrawList()->AddLine(ImVec2(c.x, c.y - 10), ImVec2(c.x, c.y + 10), IM_COL32(0, 255, 0, 220), 1.5f); + + ImGui::EndTooltip(); + } + + // ── correspondence-pair markers ───────────────────────────────────────── + // No manual "is this on screen" check needed -- the child region clips + // its own draw list, so markers scrolled out of view are simply cut off. + { + ImDrawList* dl = ImGui::GetWindowDrawList(); + for (size_t i = 0; i < state.pairs.size(); i++) + { + ImVec2 s = imgToScreen(ImVec2(state.pairs[i].u, state.pairs[i].v)); + dl->AddCircle(s, 6.f, IM_COL32(255, 255, 0, 255), 0, 2.f); + dl->AddText(ImVec2(s.x + 8, s.y - 8), IM_COL32(255, 255, 0, 255), std::to_string(i).c_str()); + } + if (state.pendingHasImage) { - zoom2D = 1.f; - offX = offY = 0.f; + ImVec2 s = imgToScreen(ImVec2(state.pendingPair.u, state.pendingPair.v)); + dl->AddCircle(s, 6.f, IM_COL32(255, 140, 0, 255), 0, 2.f); + } + // Large cross for the pair selected in the Correspondences panel -- + // matches the magenta cross drawn for it in the 3D view. + if (state.selectedPairIndex >= 0 && state.selectedPairIndex < (int)state.pairs.size()) + { + ImVec2 s = imgToScreen(ImVec2(state.pairs[state.selectedPairIndex].u, state.pairs[state.selectedPairIndex].v)); + dl->AddLine(ImVec2(s.x - 18, s.y), ImVec2(s.x + 18, s.y), IM_COL32(255, 0, 255, 255), 2.f); + dl->AddLine(ImVec2(s.x, s.y - 18), ImVec2(s.x, s.y + 18), IM_COL32(255, 0, 255, 255), 2.f); } } - // zoom indicator - ImGui::SetCursorScreenPos(ImVec2(origin.x + 6, origin.y + 4)); - ImGui::TextColored(ImVec4(1, 1, 0, 0.8f), "%.0f%% [wheel: zoom | drag: pan | dbl-click: reset]", zoom2D * fitScale * 100.f); + ImGui::EndChild(); } // ── Menu bar ───────────────────────────────────────────────────────────────── @@ -313,6 +377,18 @@ void UI::panelMenuBar(AppState& state) ImGui::EndMenu(); } + if (ImGui::BeginMenu("View")) + { + // GPU-side point subsampling (draws only every Nth point) for + // large clouds -- matches camera_lidar_trajectory_viewer's "Draw + // decimation" slider. + ImGui::SetNextItemWidth(140.f); + ImGui::SliderInt("Draw decimation", &state.vizParams.drawDecim, 1, 64); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Renders only every Nth point -- raise this if the 3D view is slow with a large cloud."); + ImGui::EndMenu(); + } + ImGui::EndMainMenuBar(); } @@ -400,27 +476,48 @@ void UI::panelExtrinsics(AppState& state) ImGui::PushItemWidth(-80.f); + ImGui::Checkbox("Lock translation", &state.lockTranslation); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Blocks tx/ty/tz from being edited here or changed by\n\"Solve Extrinsics from Pairs\" -- use when the camera\nposition is already known and only orientation needs solving."); + ImGui::Text("Camera position in world (m):"); + ImGui::BeginDisabled(state.lockTranslation); dragFloat("tx", &E.tx, 0.01f, -50.f, 50.f, "%.3f"); dragFloat("ty", &E.ty, 0.01f, -50.f, 50.f, "%.3f"); dragFloat("tz", &E.tz, 0.01f, -50.f, 50.f, "%.3f"); + ImGui::EndDisabled(); ImGui::Spacing(); - ImGui::Text("Camera orientation in world ZYX (deg):"); - dragFloat("rx", &E.rx, 0.1f, -180.f, 180.f, "%.2f"); - dragFloat("ry", &E.ry, 0.1f, -180.f, 180.f, "%.2f"); - dragFloat("rz", &E.rz, 0.1f, -180.f, 180.f, "%.2f"); - helpMarker("R_wc = Rz*Ry*Rx: camera orientation in LiDAR world.\nT_lidar2cam = R_wc^T * (p - C)."); + ImGui::Text("Camera orientation in world, om/fi/ka (deg):"); + dragFloat("om", &E.om, 0.1f, -180.f, 180.f, "%.2f"); + dragFloat("fi", &E.fi, 0.1f, -180.f, 180.f, "%.2f"); + dragFloat("ka", &E.ka, 0.1f, -180.f, 180.f, "%.2f"); + helpMarker( + "R_wc = Rx(om)*Ry(fi)*Rz(ka): camera orientation in LiDAR world.\nT_lidar2cam = R_wc^T * (p - C).\nAt fi=+/-90 deg (gimbal lock), om and ka are not individually\nunique -- only om+ka (or om-ka) is determined."); ImGui::Spacing(); if (ImGui::Button("Reset Extrinsics", ImVec2(-1, 0))) - E = Extrinsics{}; + { + // Respect the translation lock: only reset orientation while locked. + Extrinsics defaults; + if (state.lockTranslation) + { + E.om = defaults.om; + E.fi = defaults.fi; + E.ka = defaults.ka; + } + else + { + E = defaults; + } + } ImGui::PopItemWidth(); // Show current rotation matrix if (ImGui::TreeNode("Rotation matrix")) { - Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz); + Eigen::Matrix3f R = omFiKaToMat3(E.om, E.fi, E.ka); for (int r = 0; r < 3; r++) { ImGui::Text("[ %6.3f %6.3f %6.3f ]", R(r, 0), R(r, 1), R(r, 2)); @@ -450,6 +547,78 @@ void UI::panelVisualization(AppState& state) ImGui::Checkbox("Show compass/ruler (C)", &state.showCompassRuler); } +// ── Correspondences ────────────────────────────────────────────────────────── +// Pick image-pixel <-> LiDAR-point pairs, then solve extrinsics from them. +// Mirrors the list-with-remove-buttons + ">=3 needed" pattern used by +// ControlPoints::imgui (core/src/control_points.cpp). +void UI::panelCorrespondences(AppState& state) +{ + ImGui::TextWrapped("Hold Shift and click a point in the Image View or the 3D View. Picking both sides completes a pair."); + + ImGui::Text("Pending pair:"); + ImGui::SameLine(); + ImGui::TextColored(state.pendingHasImage ? ImVec4(0, 1, 0, 1) : ImVec4(0.6f, 0.6f, 0.6f, 1), "image"); + ImGui::SameLine(); + ImGui::TextColored(state.pendingHasCloud ? ImVec4(0, 1, 0, 1) : ImVec4(0.6f, 0.6f, 0.6f, 1), "3D point"); + if (state.pendingHasImage || state.pendingHasCloud) + { + ImGui::SameLine(); + if (ImGui::SmallButton("Clear")) + state.clearPending(); + } + + ImGui::Spacing(); + ImGui::Separator(); + + // R_wc/C for a live per-pair reprojection-error readout, using this + // app's own camera model (calib::projectPoint) -- so the numbers reflect + // whatever the current extrinsics are, whether from a solve or manual + // slider edits. + Eigen::Matrix3f R = omFiKaToMat3(state.extrinsics.om, state.extrinsics.fi, state.extrinsics.ka); + Eigen::Vector3f C(state.extrinsics.tx, state.extrinsics.ty, state.extrinsics.tz); + + int removeIndex = -1; + for (int i = 0; i < (int)state.pairs.size(); i++) + { + const auto& p = state.pairs[i]; + ImGui::PushID(i); + char label[64]; + std::snprintf(label, sizeof(label), "#%d px(%.0f,%.0f)", i, p.u, p.v); + // Click a row to highlight that pair as a large cross in the 3D + // and image views (click again to clear the selection). + if (ImGui::Selectable(label, state.selectedPairIndex == i, 0, ImVec2(140, 0))) + state.selectedPairIndex = (state.selectedPairIndex == i) ? -1 : i; + ImGui::SameLine(); + + float u, v, depth; + if (projectPoint(p.px, p.py, p.pz, state.intrinsics, R, C, u, v, depth)) + { + float err = std::sqrt((u - p.u) * (u - p.u) + (v - p.v) * (v - p.v)); + ImGui::TextColored(err < 5.f ? ImVec4(0, 1, 0, 1) : ImVec4(1, 0.4f, 0, 1), "err %.1fpx", err); + } + else + { + ImGui::TextColored(ImVec4(1, 0, 0, 1), "behind camera"); + } + ImGui::SameLine(); + if (ImGui::SmallButton("Remove")) + removeIndex = i; + ImGui::PopID(); + } + if (removeIndex >= 0) + state.removePair(removeIndex); + + ImGui::Spacing(); + ImGui::BeginDisabled(state.pairs.size() < 3); + if (ImGui::Button("Solve Extrinsics from Pairs", ImVec2(-1, 0))) + state.solvePairs(); + ImGui::EndDisabled(); + if (state.pairs.size() < 3) + ImGui::TextDisabled("At least 3 pairs needed"); + if (state.lastSolveRmsPixels >= 0.0) + ImGui::Text("Last solve RMS reprojection error: %.2f px", state.lastSolveRmsPixels); +} + // ── Status bar ──────────────────────────────────────────────────────────────── void UI::panelStatus(const AppState& state) { diff --git a/apps/camera_lidar_calibration/UI.h b/apps/camera_lidar_calibration/UI.h index deed3e8f..27c056f8 100644 --- a/apps/camera_lidar_calibration/UI.h +++ b/apps/camera_lidar_calibration/UI.h @@ -19,10 +19,11 @@ class UI char intrPathBuf[512] = {}; char savePath[512] = "calibration.json"; - // 2D image view pan/zoom state - float zoom2D = 1.f; // 1 = fit to window - float offX = 0.f, offY = 0.f; // image coords of top-left visible pixel - int viewImgW = 0, viewImgH = 0; + // 2D image view zoom -- pan is delegated entirely to ImGui's own child + // scroll offset (see drawImageView), so there is no offX/offY to track + // or clamp ourselves. + float imgZoom = 1.f; // 1 = actual size (1 image px = 1 screen px) + int lastImgW = 0, lastImgH = 0; // detects a newly-loaded image, to reset imgZoom void drawImageView(AppState& state); void panelMenuBar(AppState& state); @@ -30,6 +31,7 @@ class UI void panelIntrinsics(AppState& state); void panelExtrinsics(AppState& state); void panelVisualization(AppState& state); + void panelCorrespondences(AppState& state); void panelStatus(const AppState& state); // File actions -- shared by the File menu items and their keyboard diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 53669d08..e55b5a17 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -162,7 +162,7 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s writer.create_topic(tm); Eigen::Affine3f T_lc = Eigen::Affine3f::Identity(); - T_lc.linear() = eulerZYXtoMat3(in.E.rx, in.E.ry, in.E.rz); + T_lc.linear() = omFiKaToMat3(in.E.om, in.E.fi, in.E.ka); T_lc.translation() = Eigen::Vector3f(in.E.tx, in.E.ty, in.E.tz); geometry_msgs::msg::TransformStamped ts; diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 12069d5b..d5b17919 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -395,7 +395,7 @@ static void loadCloud(AppState& s) std::sort(lazPaths.begin(), lazPaths.end()); bool canColor = s.calibLoaded && !s.imagesFilenamesInTime.empty(); - Eigen::Matrix3f R_wc = canColor ? eulerZYXtoMat3(s.E.rx, s.E.ry, s.E.rz) : Eigen::Matrix3f::Identity(); + Eigen::Matrix3f R_wc = canColor ? omFiKaToMat3(s.E.om, s.E.fi, s.E.ka) : Eigen::Matrix3f::Identity(); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); float K_fx = s.K.fx * s.imgScale, K_fy = s.K.fy * s.imgScale; float K_cx = s.K.cx * s.imgScale, K_cy = s.K.cy * s.imgScale; @@ -770,11 +770,15 @@ static void loadCalib(AppState& s) s.E.ty = je["camera_position_in_world_xyz"][1]; s.E.tz = je["camera_position_in_world_xyz"][2]; } - if (je.contains("camera_rotation_in_world_euler_zyx_deg") && je["camera_rotation_in_world_euler_zyx_deg"].size() >= 3) + // camera_rotation_in_world_tait_bryan_omfika_deg: [om, fi, ka] -- + // R_wc = Rx(om)*Ry(fi)*Rz(ka), matching calib::Extrinsics' native + // rotation representation directly (no reordering/conversion). + if (je.contains("camera_rotation_in_world_tait_bryan_omfika_deg") && + je["camera_rotation_in_world_tait_bryan_omfika_deg"].size() >= 3) { - s.E.rz = je["camera_rotation_in_world_euler_zyx_deg"][0]; - s.E.ry = je["camera_rotation_in_world_euler_zyx_deg"][1]; - s.E.rx = je["camera_rotation_in_world_euler_zyx_deg"][2]; + s.E.om = je["camera_rotation_in_world_tait_bryan_omfika_deg"][0]; + s.E.fi = je["camera_rotation_in_world_tait_bryan_omfika_deg"][1]; + s.E.ka = je["camera_rotation_in_world_tait_bryan_omfika_deg"][2]; } } // Optional region of interest, in full-resolution image pixels: @@ -952,7 +956,7 @@ static void exportColmap(AppState& s) // T_lidar_camera (camera pose in the LiDAR frame, from the extrinsics) Eigen::Affine3f T_lc = Eigen::Affine3f::Identity(); - T_lc.linear() = eulerZYXtoMat3(s.E.rx, s.E.ry, s.E.rz); + T_lc.linear() = omFiKaToMat3(s.E.om, s.E.fi, s.E.ka); T_lc.translation() = Eigen::Vector3f(s.E.tx, s.E.ty, s.E.tz); // cameras.txt — rational OpenCV model == COLMAP FULL_OPENCV (12 params) @@ -1129,7 +1133,7 @@ static void drawScene(AppState& s) // ── camera frustums ─────────────────────────────────────────────────────── if (s.showFrustums && s.calibLoaded) { - Eigen::Matrix3f R_wc = eulerZYXtoMat3(s.E.rx, s.E.ry, s.E.rz); + Eigen::Matrix3f R_wc = omFiKaToMat3(s.E.om, s.E.fi, s.E.ka); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); float fs = s.frustumScale; float ncx[4] = { diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index cdc90526..6781fc83 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -21,6 +21,7 @@ add_library(calib_core STATIC src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp + src/CameraCalibrationSolver.cpp ) target_include_directories(calib_core PUBLIC @@ -37,6 +38,11 @@ target_include_directories(calib_core PRIVATE # classes instead and never needed this path, so nothing else in the repo # wires it up. ${CMAKE_BINARY_DIR}/3rdparty/LASzip/include + # Reuses the vendored analytic camera observation equations directly + # (CameraCalibrationSolver.cpp #includes the header from here) instead + # of copying the math -- same include-the-generated-header convention + # core/CMakeLists.txt already uses for its own observation equations. + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index ef315a12..02a623e5 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -22,9 +22,25 @@ namespace calib { // Camera position in LiDAR/world frame float tx = 0.f, ty = 0.f, tz = 0.f; - // Camera orientation in LiDAR/world frame — ZYX Euler, degrees. - // Default: standard camera (X=right, Y=down, Z=forward) aligned with LiDAR (X=forward). - float rx = -90.f, ry = 0.f, rz = -90.f; + // Camera orientation in LiDAR/world frame — Tait-Bryan om/fi/ka, + // degrees: R_wc = Rx(om) * Ry(fi) * Rz(ka). This is the SAME + // parameterization (and rotation order) as the vendored camera + // observation equations CameraCalibrationSolver reuses directly, so + // no conversion is needed between "what got picked/solved" and + // "what's stored here" -- no rx/ry/rz<->om/fi/ka round-trip. + // Default: standard camera (X=right, Y=down, Z=forward) aligned + // with LiDAR (X=forward) -- the same physical orientation the old + // ZYX-Euler default (rx=-90,ry=0,rz=-90) represented (within 0.1°), + // just written in this parameterization. fi is nudged to 89.9° (not + // exactly 90°) deliberately: at fi=90° exactly this parameterization + // hits gimbal lock -- om and ka become individually non-unique + // (only om+ka is determined) -- which made CameraCalibrationSolver's + // very first solve of a fresh session start right on top of a rank + // -deficient normal-equations block (confirmed in practice: the + // solver visibly struggled). 0.1° off is enough to make the (om,ka) + // block well-conditioned from the start while being visually + // identical to "aligned". + float om = -90.f, fi = 89.9f, ka = 0.f; }; // Rectangular region of interest, in full-resolution image pixels. @@ -36,8 +52,9 @@ namespace calib int x = 0, y = 0, w = 0, h = 0; }; - // R = Rz * Ry * Rx (ZYX Euler, degrees → rotation matrix) - Eigen::Matrix3f eulerZYXtoMat3(float rx_deg, float ry_deg, float rz_deg); + // R = Rx * Ry * Rz (Tait-Bryan om/fi/ka, degrees → rotation matrix). + // Matches Extrinsics' own om/fi/ka convention above. + Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg); // Project a point from LiDAR frame to image pixel (u, v). // R_wc = camera orientation in world, t = camera position in world. diff --git a/calib_core/include/CalibCore/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h new file mode 100644 index 00000000..d476dad7 --- /dev/null +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -0,0 +1,47 @@ +#pragma once +#include "Camera.h" +#include +#include + +namespace calib +{ + + // A single manually-picked correspondence: a 3D point in the LiDAR/world + // frame paired with the pixel it should project to in the camera image. + // Pixel coordinates are expected in the *undistorted* (ideal pinhole) + // frame -- i.e. picked from the rectified image display, see + // solveExtrinsicsFromCorrespondences() below. + struct PointPixelCorrespondence + { + Eigen::Vector3d p; + double u = 0.0, v = 0.0; + }; + + // Solves for the extrinsics (camera position + orientation) that best + // explain the given LiDAR-point <-> image-pixel correspondences via + // damped Gauss-Newton (Levenberg-Marquardt) on the reused observation + // equations. Intrinsics (fx, fy, cx, cy) are held fixed at their + // current values in K. extrinsicsInOut is used as the initial guess and + // is overwritten with the solved result. Pixel coordinates in + // `correspondences` must be in the undistorted/ideal-pinhole frame -- + // i.e. picked from the rectified image display (calib::Intrinsics's + // distortion terms are ignored here). + // + // fixTranslation=true blocks tx/ty/tz from being solved for -- they + // stay pinned at extrinsicsInOut's initial values and only orientation + // (3-DOF) is optimized. Useful when the camera position relative to the + // LiDAR is already known precisely (e.g. measured by hand) and only + // orientation needs refining from the picked pairs. + // + // Returns false (leaving extrinsicsInOut unchanged) if there are fewer + // than 3 correspondences, or fewer than the number of free parameters + // (3 with fixTranslation, else 6), or the normal-equations system is + // singular. + bool solveExtrinsicsFromCorrespondences( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + double* outRmsPixels = nullptr, + bool fixTranslation = false); + +} // namespace calib diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 6e93d4ca..e30b42ab 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -2,11 +2,11 @@ namespace calib { -Eigen::Matrix3f eulerZYXtoMat3(float rx_deg, float ry_deg, float rz_deg) { +Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { const float d2r = static_cast(M_PI) / 180.f; - return (Eigen::AngleAxisf(rz_deg * d2r, Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(ry_deg * d2r, Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(rx_deg * d2r, Eigen::Vector3f::UnitX())) + return (Eigen::AngleAxisf(om_deg * d2r, Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(fi_deg * d2r, Eigen::Vector3f::UnitY()) * + Eigen::AngleAxisf(ka_deg * d2r, Eigen::Vector3f::UnitZ())) .toRotationMatrix(); } diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 66d88641..a701749e 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -12,6 +12,7 @@ set(CORE_BASE_SOURCES src/point_cloud.cpp src/point_clouds.cpp src/session.cpp + src/tum.cpp # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) diff --git a/raylib_widgets/CMakeLists.txt b/raylib_widgets/CMakeLists.txt index abeb56b9..12ae74d9 100644 --- a/raylib_widgets/CMakeLists.txt +++ b/raylib_widgets/CMakeLists.txt @@ -17,6 +17,7 @@ add_library(raylib_widgets STATIC src/ShortcutsTable.cpp src/OrbitCamera.cpp src/CenterOfRotationWindow.cpp + src/PointPicking.cpp ) target_include_directories(raylib_widgets PUBLIC include) diff --git a/raylib_widgets/include/RaylibWidgets/PointPicking.h b/raylib_widgets/include/RaylibWidgets/PointPicking.h new file mode 100644 index 00000000..0cd24bc1 --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/PointPicking.h @@ -0,0 +1,38 @@ +#pragma once +#include "raylib.h" +#include + +// Mouse-driven picking of a single point out of a 3D point cloud. Depends on +// nothing but raylib -- no Eigen, no core, no calib_core -- see +// raylib_widgets/CMakeLists.txt. Callers own their own coordinate +// conventions (e.g. LiDAR-frame vs raylib-frame axes) and pass already +// raylib-world-space points in. +// +// Ray construction is deliberately NOT provided here: raylib's BeginMode3D +// derives its projection's aspect ratio from the *full* window framebuffer +// (CORE.Window.currentFbo.width/height in rcore.c), with no awareness of any +// BeginScissorMode() sub-region a caller might be confining the 3D view to +// -- so a "viewport-aware" ray helper scoped to that sub-region would +// actually be WRONG for apps (like this one) that use scissor-only +// sub-views without a matching custom rlViewport()/projection. Callers +// should build the ray with plain GetScreenToWorldRay(mouse, cam), matching +// what OrbitCamera::pickGroundPlaneTarget already does. +namespace raylib_widgets +{ + // Finds the point in `points` (raylib world-space coordinates, `count` + // entries) whose perpendicular distance to `ray` is smallest, rejecting + // it if that distance exceeds `pixelThreshold` screen pixels at the + // point's depth (converted via the camera's vertical FOV and the + // viewport's pixel height, so the tolerance stays roughly constant in + // screen space regardless of zoom). Returns true and sets outIndex on a + // hit; false (outIndex untouched) if `points` is empty or nothing is + // within tolerance. + bool pickNearestPoint( + const Vector3* points, + size_t count, + const Ray& ray, + float cameraFovYDeg, + float viewportHeightPx, + float pixelThreshold, + size_t& outIndex); +} // namespace raylib_widgets diff --git a/raylib_widgets/src/PointPicking.cpp b/raylib_widgets/src/PointPicking.cpp new file mode 100644 index 00000000..733573d8 --- /dev/null +++ b/raylib_widgets/src/PointPicking.cpp @@ -0,0 +1,44 @@ +#include "RaylibWidgets/PointPicking.h" +#include "raymath.h" +#include +#include + +namespace raylib_widgets +{ + bool pickNearestPoint( + const Vector3* points, size_t count, const Ray& ray, float cameraFovYDeg, float viewportHeightPx, float pixelThreshold, + size_t& outIndex) + { + if (points == nullptr || count == 0 || viewportHeightPx <= 0.f) + return false; + + const float halfFovyRad = cameraFovYDeg * DEG2RAD * 0.5f; + float bestPerpDist = std::numeric_limits::max(); + bool found = false; + + for (size_t i = 0; i < count; ++i) + { + Vector3 toP = Vector3Subtract(points[i], ray.position); + float depthAlong = Vector3DotProduct(toP, ray.direction); + if (depthAlong <= 0.f) + continue; // behind the camera + + Vector3 closest = Vector3Add(ray.position, Vector3Scale(ray.direction, depthAlong)); + float perpDist = Vector3Distance(points[i], closest); + + // World distance covered by one screen pixel at this depth, so + // the pixel tolerance stays constant in screen space. + float worldPerPixel = (2.f * depthAlong * std::tan(halfFovyRad)) / viewportHeightPx; + float maxWorldDist = pixelThreshold * worldPerPixel; + + if (perpDist <= maxWorldDist && perpDist < bestPerpDist) + { + bestPerpDist = perpDist; + outIndex = i; + found = true; + } + } + + return found; + } +} // namespace raylib_widgets From 3caab4a51b856814090d3ffdc6faa8c97378cf7a Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 06:37:18 +0200 Subject: [PATCH 2/5] Reparameterize camera extrinsics as a deviation from a fixed LiDAR-axis offset Extrinsics' om/fi/ka now represent a small rotation away from a constant kCameraLidarAxisOffset (the camera/LiDAR coordinate-convention alignment), instead of the full rotation directly. This makes the all-zero default already a physically sensible starting pose and keeps a real calibration's solved values far from the om/fi/ka gimbal-lock point (fi=+/-90 deg), which previously sat right on top of the default and made the very first solve of a session start on a rank-deficient normal-equations block. CameraCalibrationSolver threads the offset through the LM solve via a one-time point/translation rotation, so the vendored observation equations and the solve loop itself are untouched. Camera.h/.cpp now reuse core's own pose_tait_bryan_from_affine_matrix/affine_matrix_from_pose_tait_bryan for the om/fi/ka<->matrix conversion instead of duplicating that math. Saved calibration JSON now stores rotation as a matrix only (camera_rotation_matrix_in_world) -- convention-independent and portable -- instead of also carrying an om/fi/ka angle key; camera_lidar_calibration and camera_lidar_trajectory_viewer both decode it back into om/fi/ka on load. Co-Authored-By: Claude Sonnet 5 --- apps/camera_lidar_calibration/App.cpp | 25 +- apps/camera_lidar_calibration/UI.cpp | 1 + .../TrajectoryViewer.cpp | 18 +- calib_core/CMakeLists.txt | 7 + calib_core/include/CalibCore/Camera.h | 83 +++++-- calib_core/src/Camera.cpp | 34 ++- calib_core/src/CameraCalibrationSolver.cpp | 219 ++++++++++++++++++ 7 files changed, 341 insertions(+), 46 deletions(-) create mode 100644 calib_core/src/CameraCalibrationSolver.cpp diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 96809006..e289257c 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -475,16 +475,18 @@ void AppState::loadCalibration(const char* path) extrinsics.ty = pos[1].get(); extrinsics.tz = pos[2].get(); } - // camera_rotation_in_world_tait_bryan_omfika_deg: [om, fi, ka] - // -- R_wc = Rx(om)*Ry(fi)*Rz(ka), matching calib::Extrinsics' - // native rotation representation directly (no reordering/conversion). - if (je.contains("camera_rotation_in_world_tait_bryan_omfika_deg") && - je["camera_rotation_in_world_tait_bryan_omfika_deg"].size() >= 3) + // camera_rotation_matrix_in_world: 3x3 rows of R_wc. The file's only + // rotation representation (convention-independent, portable) -- + // decomposed into calib::Extrinsics' own om/fi/ka for internal + // use (UI sliders, solver initial guess). + if (je.contains("camera_rotation_matrix_in_world") && je["camera_rotation_matrix_in_world"].size() >= 3) { - auto& rot = je["camera_rotation_in_world_tait_bryan_omfika_deg"]; - extrinsics.om = rot[0].get(); - extrinsics.fi = rot[1].get(); - extrinsics.ka = rot[2].get(); + auto& m = je["camera_rotation_matrix_in_world"]; + Eigen::Matrix3f R; + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + R(r, c) = m[r][c].get(); + calib::omFiKaFromMat3(R, extrinsics.om, extrinsics.fi, extrinsics.ka); } gotExtrinsics = true; } @@ -521,7 +523,10 @@ void AppState::saveCalibration(const char* path) j["intrinsics"] = { { "fx", intrinsics.fx }, { "fy", intrinsics.fy }, { "cx", intrinsics.cx }, { "cy", intrinsics.cy }, { "k1", intrinsics.k1 }, { "k2", intrinsics.k2 }, { "k3", intrinsics.k3 }, { "k4", intrinsics.k4 }, { "k5", intrinsics.k5 }, { "k6", intrinsics.k6 }, { "p1", intrinsics.p1 }, { "p2", intrinsics.p2 } }; - j["extrinsics"]["camera_rotation_in_world_tait_bryan_omfika_deg"] = { extrinsics.om, extrinsics.fi, extrinsics.ka }; + // Rotation is stored as a matrix only -- convention-independent (no + // Euler/Tait-Bryan angle order or units to document/misread) and + // directly portable to any external tool. camera_rotation_matrix_in_world + // is the source of truth on load; see AppState::loadCalibration. j["extrinsics"]["camera_position_in_world_xyz"] = { C.x(), C.y(), C.z() }; j["extrinsics"]["camera_rotation_matrix_in_world"] = { { R(0, 0), R(0, 1), R(0, 2) }, { R(1, 0), R(1, 1), R(1, 2) }, diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index fbf57f02..0920db11 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -492,6 +492,7 @@ void UI::panelExtrinsics(AppState& state) ImGui::Text("Camera orientation in world, om/fi/ka (deg):"); dragFloat("om", &E.om, 0.1f, -180.f, 180.f, "%.2f"); dragFloat("fi", &E.fi, 0.1f, -180.f, 180.f, "%.2f"); + avoidGimbalLock(E.fi); dragFloat("ka", &E.ka, 0.1f, -180.f, 180.f, "%.2f"); helpMarker( "R_wc = Rx(om)*Ry(fi)*Rz(ka): camera orientation in LiDAR world.\nT_lidar2cam = R_wc^T * (p - C).\nAt fi=+/-90 deg (gimbal lock), om and ka are not individually\nunique -- only om+ka (or om-ka) is determined."); diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index d5b17919..b6ab49f9 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -770,15 +770,17 @@ static void loadCalib(AppState& s) s.E.ty = je["camera_position_in_world_xyz"][1]; s.E.tz = je["camera_position_in_world_xyz"][2]; } - // camera_rotation_in_world_tait_bryan_omfika_deg: [om, fi, ka] -- - // R_wc = Rx(om)*Ry(fi)*Rz(ka), matching calib::Extrinsics' native - // rotation representation directly (no reordering/conversion). - if (je.contains("camera_rotation_in_world_tait_bryan_omfika_deg") && - je["camera_rotation_in_world_tait_bryan_omfika_deg"].size() >= 3) + // camera_rotation_matrix_in_world: 3x3 rows of R_wc. The file's only + // rotation representation (convention-independent, portable) -- + // decomposed into calib::Extrinsics' own om/fi/ka for internal use. + if (je.contains("camera_rotation_matrix_in_world") && je["camera_rotation_matrix_in_world"].size() >= 3) { - s.E.om = je["camera_rotation_in_world_tait_bryan_omfika_deg"][0]; - s.E.fi = je["camera_rotation_in_world_tait_bryan_omfika_deg"][1]; - s.E.ka = je["camera_rotation_in_world_tait_bryan_omfika_deg"][2]; + auto& m = je["camera_rotation_matrix_in_world"]; + Eigen::Matrix3f R; + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + R(r, c) = m[r][c].get(); + omFiKaFromMat3(R, s.E.om, s.E.fi, s.E.ka); } } // Optional region of interest, in full-resolution image pixels: diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 6781fc83..99397ebc 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -43,6 +43,13 @@ target_include_directories(calib_core PRIVATE # of copying the math -- same include-the-generated-header convention # core/CMakeLists.txt already uses for its own observation equations. ${THIRDPARTY_DIRECTORY}/observation_equations/codes + # Camera.cpp reuses core's own pose_tait_bryan_from_affine_matrix / + # affine_matrix_from_pose_tait_bryan (core/include/Core/transformations.h) + # for the om/fi/ka<->matrix conversion instead of duplicating that math. + # Header-only and pulls in nothing but Eigen/std (see structures.h) -- + # doesn't violate calib_core's no-raylib/imgui/OpenCV rule above, and + # nothing here links the core/core_math library, just includes headers. + ${REPOSITORY_DIRECTORY}/core/include ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 02a623e5..65d3de88 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -18,29 +18,57 @@ namespace calib float p1 = 0.f, p2 = 0.f; }; + // Minimum distance (degrees) fi is kept away from the om/fi/ka + // parameterization's gimbal-lock points (fi = +/-90 deg), where om and + // ka become individually non-unique (only om+ka, or om-ka, is + // determined) and CameraCalibrationSolver's normal equations go + // rank-deficient in that 2x2 block. Used by UI code that edits fi + // interactively (see apps/camera_lidar_calibration/UI.cpp's + // avoidGimbalLock) so a manual drag can't land exactly on the + // singularity. Extrinsics' own default (below) no longer needs this -- + // see kCameraLidarAxisOffset -- but it's kept as a cheap safety net for + // whatever fi a user or a loaded file lands on. + constexpr float kGimbalLockEpsilonDeg = 0.1f; + + // Nudges fi_deg off the nearest gimbal-lock point (+/-90 deg) if it's + // within kGimbalLockEpsilonDeg of one, in place. A no-op otherwise. + // Safe to call unconditionally every frame after any edit to fi (manual + // slider drag, typed value, or loaded from a file) -- idempotent. + void avoidGimbalLock(float& fi_deg); + + // Fixed rotation baked into Extrinsics' om/fi/ka (see below): the + // "camera axes vs LiDAR axes" alignment -- camera X=right, Y=down, + // Z=forward matched to LiDAR X=forward, Y=left, Z=up. This is a + // constant coordinate-convention twist that has nothing to do with the + // actual calibration being solved for, so it's factored out as a fixed + // offset rather than folded into om/fi/ka: om=fi=ka=0 is then already + // the correct nominal alignment (Extrinsics' literal default), and + // om/fi/ka become exactly "how far off nominal the real mount is" -- + // normally a few degrees at most, so nowhere near the om/fi/ka + // parameterization's gimbal-lock points (fi=+/-90 deg) in practice, + // unlike the old scheme where fi had to carry this entire 90-degree + // twist directly and sat right on top of the singularity by default. + inline const Eigen::Matrix3f kCameraLidarAxisOffset = + (Eigen::Matrix3f() << 0.f, 0.f, 1.f, -1.f, 0.f, 0.f, 0.f, -1.f, 0.f).finished(); + struct Extrinsics { // Camera position in LiDAR/world frame float tx = 0.f, ty = 0.f, tz = 0.f; - // Camera orientation in LiDAR/world frame — Tait-Bryan om/fi/ka, - // degrees: R_wc = Rx(om) * Ry(fi) * Rz(ka). This is the SAME - // parameterization (and rotation order) as the vendored camera - // observation equations CameraCalibrationSolver reuses directly, so - // no conversion is needed between "what got picked/solved" and - // "what's stored here" -- no rx/ry/rz<->om/fi/ka round-trip. - // Default: standard camera (X=right, Y=down, Z=forward) aligned - // with LiDAR (X=forward) -- the same physical orientation the old - // ZYX-Euler default (rx=-90,ry=0,rz=-90) represented (within 0.1°), - // just written in this parameterization. fi is nudged to 89.9° (not - // exactly 90°) deliberately: at fi=90° exactly this parameterization - // hits gimbal lock -- om and ka become individually non-unique - // (only om+ka is determined) -- which made CameraCalibrationSolver's - // very first solve of a fresh session start right on top of a rank - // -deficient normal-equations block (confirmed in practice: the - // solver visibly struggled). 0.1° off is enough to make the (om,ka) - // block well-conditioned from the start while being visually - // identical to "aligned". - float om = -90.f, fi = 89.9f, ka = 0.f; + // Camera orientation in LiDAR/world frame, as a SMALL deviation from + // the fixed kCameraLidarAxisOffset alignment: R_wc = + // kCameraLidarAxisOffset * Rx(om) * Ry(fi) * Rz(ka). om/fi/ka are + // degrees, Tait-Bryan, matching CameraCalibrationSolver's own + // parameterization (om/fi/ka feed the vendored observation + // equations directly there too -- see CameraCalibrationSolver.cpp + // for how the offset is threaded through the solve without + // modifying those equations). + // Default: om=fi=ka=0, i.e. exactly the nominal alignment -- a + // real calibration only needs to move these by however far the + // actual camera mount deviates from nominal, typically a few + // degrees, so "0,0,0" is already a good initial guess, not just a + // mathematically convenient one. + float om = 0.f, fi = 0.f, ka = 0.f; }; // Rectangular region of interest, in full-resolution image pixels. @@ -52,10 +80,21 @@ namespace calib int x = 0, y = 0, w = 0, h = 0; }; - // R = Rx * Ry * Rz (Tait-Bryan om/fi/ka, degrees → rotation matrix). - // Matches Extrinsics' own om/fi/ka convention above. + // R = kCameraLidarAxisOffset * Rx * Ry * Rz (Tait-Bryan om/fi/ka, + // degrees → rotation matrix). Matches Extrinsics' own om/fi/ka + // convention above -- om=fi=ka=0 returns kCameraLidarAxisOffset exactly. Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg); + // Inverse of omFiKaToMat3: decomposes kCameraLidarAxisOffset^T * R + // assuming that equals Rx(om)*Ry(fi)*Rz(ka), for reading a rotation + // matrix (e.g. from a saved calibration file) back into Extrinsics' + // om/fi/ka fields. Calibration files store the rotation as a plain + // matrix (convention-independent, portable to any external tool, and + // knows nothing about kCameraLidarAxisOffset), while the app's own + // UI/solver work in om/fi/ka, so this conversion is needed at the file + // -I/O boundary either way. Result is passed through avoidGimbalLock. + void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg); + // Project a point from LiDAR frame to image pixel (u, v). // R_wc = camera orientation in world, t = camera position in world. // depth = z component in camera frame (positive = in front). @@ -71,4 +110,4 @@ namespace calib float& v, float& depth); -} // namespace calib \ No newline at end of file +} // namespace calib diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index e30b42ab..29cc82a9 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,13 +1,35 @@ #include +// Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- +// header-only, pulls in nothing but Eigen/std (see structures.h), so this +// doesn't violate calib_core's no-raylib/imgui/OpenCV design (see +// calib_core/CMakeLists.txt); nothing here links core/core_math. +#include + namespace calib { +void avoidGimbalLock(float& fi_deg) { + if (std::fabs(std::fabs(fi_deg) - 90.f) < kGimbalLockEpsilonDeg) + fi_deg = std::copysign(90.f - kGimbalLockEpsilonDeg, fi_deg); +} + Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { - const float d2r = static_cast(M_PI) / 180.f; - return (Eigen::AngleAxisf(om_deg * d2r, Eigen::Vector3f::UnitX()) * - Eigen::AngleAxisf(fi_deg * d2r, Eigen::Vector3f::UnitY()) * - Eigen::AngleAxisf(ka_deg * d2r, Eigen::Vector3f::UnitZ())) - .toRotationMatrix(); + TaitBryanPose pose; + pose.om = deg2rad(om_deg); + pose.fi = deg2rad(fi_deg); + pose.ka = deg2rad(ka_deg); + Eigen::Matrix3f Rdelta = affine_matrix_from_pose_tait_bryan(pose).linear().cast(); + return kCameraLidarAxisOffset * Rdelta; +} + +void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg) { + Eigen::Affine3d m = Eigen::Affine3d::Identity(); + m.linear() = (kCameraLidarAxisOffset.transpose() * R).cast(); + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m); + om_deg = static_cast(rad2deg(pose.om)); + fi_deg = static_cast(rad2deg(pose.fi)); + ka_deg = static_cast(rad2deg(pose.ka)); + avoidGimbalLock(fi_deg); } bool projectPoint(float px, float py, float pz, @@ -37,4 +59,4 @@ bool projectPoint(float px, float py, float pz, return true; } -} // namespace calib \ No newline at end of file +} // namespace calib diff --git a/calib_core/src/CameraCalibrationSolver.cpp b/calib_core/src/CameraCalibrationSolver.cpp new file mode 100644 index 00000000..98cc91fc --- /dev/null +++ b/calib_core/src/CameraCalibrationSolver.cpp @@ -0,0 +1,219 @@ +#include + +// Reuses (does not duplicate) the analytic camera observation equations +// vendored at 3rdparty/observation_equations -- a pure-pinhole (no lens +// distortion) perspective camera model parameterized as: +// t_wc = (tx,ty,tz) camera position in world +// R_wc = Rx(om)*Ry(fi)*Rz(ka) camera orientation in world (radians) +// This mirrors the #include-the-generated-header convention core/src/ +// control_points.cpp already uses for its own (point-to-point) vendored +// jacobian, rather than copying the math into this file. Provides, in the +// global namespace (the vendored header does not namespace itself): +// projection_perspective_camera_tait_bryan_wc(...) +// observation_equation_perspective_camera_tait_bryan_wc(...) -- delta = target - projected +// observation_equation_perspective_camera_tait_bryan_wc_jacobian(...) -- d(delta)/d[tx,ty,tz,om,fi,ka,px,py,pz], 2x9 +#include + +#include +#include + +namespace calib +{ + namespace + { + double sumSquaredResiduals( + const std::vector& correspondences, + double fx, + double fy, + double cx, + double cy, + double tx, + double ty, + double tz, + double om, + double fi, + double ka) + { + double sumSq = 0.0; + for (const auto& c : correspondences) + { + Eigen::Matrix delta; + observation_equation_perspective_camera_tait_bryan_wc( + delta, fx, fy, cx, cy, tx, ty, tz, om, fi, ka, c.p.x(), c.p.y(), c.p.z(), c.u, c.v); + sumSq += delta.squaredNorm(); + } + return sumSq; + } + } // namespace + + bool solveExtrinsicsFromCorrespondences( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + double* outRmsPixels, + bool fixTranslation) + { + // Free parameters are always a contiguous slice of the full + // [tx,ty,tz,om,fi,ka] (columns 0-5 of the 2x9 jacobian): all 6 of + // them normally, or just [om,fi,ka] (columns 3-5) when translation + // is pinned -- tx/ty/tz then never appear in the normal equations + // at all, so they cannot move. + const int nParams = fixTranslation ? 3 : 6; + const int colOffset = fixTranslation ? 3 : 0; + if (static_cast(correspondences.size()) < 3 || static_cast(correspondences.size()) * 2 < nParams) + return false; + + // calib::Extrinsics' om/fi/ka are a SMALL deviation from the fixed + // kCameraLidarAxisOffset alignment (R_wc = kCameraLidarAxisOffset * + // Rx(om)Ry(fi)Rz(ka) -- see Camera.h), but the vendored equations + // below assume om/fi/ka parameterize the FULL rotation on their + // own. Reconciling the two without touching the vendored equations + // or the LM loop at all: since + // p_cam = R_wc^T (p_world - C) + // = Rx(om)Ry(fi)Rz(ka)^T * (kCameraLidarAxisOffset^T*p_world - kCameraLidarAxisOffset^T*C) + // pre-rotating every point by kCameraLidarAxisOffset^T once up + // front, and solving for C' = kCameraLidarAxisOffset^T*C instead of + // the real camera position C, makes the vendored equations' own + // (px,py,pz) and (tx,ty,tz) exactly this p_world' and C' -- so + // om/fi/ka fed to/from them are directly Extrinsics' om/fi/ka, + // unchanged, and the rest of this function is untouched. + const Eigen::Matrix3d offsetT = kCameraLidarAxisOffset.transpose().cast(); + std::vector rotated; + rotated.reserve(correspondences.size()); + for (const auto& c : correspondences) + { + PointPixelCorrespondence rc; + rc.p = offsetT * c.p; + rc.u = c.u; + rc.v = c.v; + rotated.push_back(rc); + } + + const double d2r = M_PI / 180.0; + Eigen::Vector3d Cprime = offsetT * Eigen::Vector3d(extrinsicsInOut.tx, extrinsicsInOut.ty, extrinsicsInOut.tz); + double tx = Cprime.x(), ty = Cprime.y(), tz = Cprime.z(); + double om = extrinsicsInOut.om * d2r, fi = extrinsicsInOut.fi * d2r, ka = extrinsicsInOut.ka * d2r; + + const double fx = K.fx, fy = K.fy, cx = K.cx, cy = K.cy; + + double cost = sumSquaredResiduals(rotated, fx, fy, cx, cy, tx, ty, tz, om, fi, ka); + bool solvedOnce = false; + + // Levenberg-Marquardt: plain (undamped) Gauss-Newton can take huge, + // divergent steps when the initial guess is only moderately off + // (verified empirically -- a ~1.6m/several-degree initial error was + // enough to blow the plain-GN version up completely), because the + // normal-equations system is only well-conditioned near the true + // solution. Damping the diagonal and only accepting steps that + // actually reduce the residual keeps convergence robust without + // changing the underlying (reused, unmodified) observation + // equations at all. + double lambda = 1e-3; + constexpr int kMaxIterations = 50; + constexpr int kMaxLmTries = 16; + + for (int iter = 0; iter < kMaxIterations; ++iter) + { + Eigen::MatrixXd AtA = Eigen::MatrixXd::Zero(nParams, nParams); + Eigen::VectorXd AtB = Eigen::VectorXd::Zero(nParams); + + for (const auto& c : rotated) + { + Eigen::Matrix delta; + observation_equation_perspective_camera_tait_bryan_wc( + delta, fx, fy, cx, cy, tx, ty, tz, om, fi, ka, c.p.x(), c.p.y(), c.p.z(), c.u, c.v); + + Eigen::Matrix jFull; + observation_equation_perspective_camera_tait_bryan_wc_jacobian( + jFull, fx, fy, cx, cy, tx, ty, tz, om, fi, ka, c.p.x(), c.p.y(), c.p.z()); + + // Only the free-parameter columns -- the 3D points + // (columns 6-8) are fixed observations, not solved for, and + // translation (columns 0-2) is skipped entirely when + // fixTranslation is set. + Eigen::MatrixXd A = -jFull.block(0, colOffset, 2, nParams); + + AtA += A.transpose() * A; + AtB += A.transpose() * delta; + } + + bool improved = false; + for (int lmTry = 0; lmTry < kMaxLmTries; ++lmTry) + { + Eigen::MatrixXd damped = AtA; + for (int d = 0; d < nParams; ++d) + damped(d, d) += lambda * (AtA(d, d) > 0.0 ? AtA(d, d) : 1.0); + + Eigen::FullPivLU lu(damped); + if (!lu.isInvertible()) + { + lambda *= 10.0; + continue; + } + + Eigen::VectorXd x = lu.solve(AtB); + double tx2 = tx, ty2 = ty, tz2 = tz, om2 = om, fi2 = fi, ka2 = ka; + if (fixTranslation) + { + om2 += x(0); + fi2 += x(1); + ka2 += x(2); + } + else + { + tx2 += x(0); + ty2 += x(1); + tz2 += x(2); + om2 += x(3); + fi2 += x(4); + ka2 += x(5); + } + double newCost = sumSquaredResiduals(rotated, fx, fy, cx, cy, tx2, ty2, tz2, om2, fi2, ka2); + + if (newCost < cost) + { + tx = tx2; + ty = ty2; + tz = tz2; + om = om2; + fi = fi2; + ka = ka2; + cost = newCost; + lambda = std::max(lambda * 0.3, 1e-12); + solvedOnce = true; + improved = true; + + if (x.norm() < 1e-9) + iter = kMaxIterations; // converged -- stop the outer loop too + break; + } + + lambda *= 10.0; + } + + if (!improved) + break; // damping maxed out without an improving step -- stuck or converged + } + + if (!solvedOnce) + return false; + + // Un-rotate the solved C' back to the real camera position C = + // kCameraLidarAxisOffset * C' -- see the comment above the + // pre-rotation this reverses. om/fi/ka need no such conversion: + // they're Extrinsics' own deviation-from-offset angles already. + Eigen::Vector3d C = kCameraLidarAxisOffset.cast() * Eigen::Vector3d(tx, ty, tz); + extrinsicsInOut.tx = static_cast(C.x()); + extrinsicsInOut.ty = static_cast(C.y()); + extrinsicsInOut.tz = static_cast(C.z()); + extrinsicsInOut.om = static_cast(om / d2r); + extrinsicsInOut.fi = static_cast(fi / d2r); + extrinsicsInOut.ka = static_cast(ka / d2r); + + if (outRmsPixels) + *outRmsPixels = std::sqrt(cost / (2.0 * static_cast(correspondences.size()))); + + return true; + } + +} // namespace calib From a29da18eb3d7f625d83673ff2c44e547581c8276 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 14:16:19 +0200 Subject: [PATCH 3/5] Consume camera-LiDAR extrinsics as a matrix in the trajectory viewer TrajectoryViewer only ever reads a saved calibration; it had no reason to decode the file's rotation matrix into Extrinsics' om/fi/ka and reconvert back to a matrix at every use site (coloring, frustum draw, COLMAP export, ROS TF export). Store the loaded R_wc directly and use it as-is, dropping the omFiKaToMat3/omFiKaFromMat3 round trip. --- .gitignore | 2 ++ .../RosExport.cpp | 2 +- .../RosExport.h | 3 ++- .../TrajectoryViewer.cpp | 20 +++++++++---------- core/CMakeLists.txt | 1 - 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/.gitignore b/.gitignore index 95d2fa13..4b81de55 100644 --- a/.gitignore +++ b/.gitignore @@ -75,3 +75,5 @@ imgui.ini # deploy_mandeye.bat output /deploy + +.cache/ \ No newline at end of file diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index e55b5a17..47efff9a 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -162,7 +162,7 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s writer.create_topic(tm); Eigen::Affine3f T_lc = Eigen::Affine3f::Identity(); - T_lc.linear() = omFiKaToMat3(in.E.om, in.E.fi, in.E.ka); + T_lc.linear() = in.R_wc; T_lc.translation() = Eigen::Vector3f(in.E.tx, in.E.ty, in.E.tz); geometry_msgs::msg::TransformStamped ts; diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.h b/apps/camera_lidar_trajectory_viewer/RosExport.h index 04be7875..26df0753 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.h +++ b/apps/camera_lidar_trajectory_viewer/RosExport.h @@ -34,7 +34,8 @@ struct RosExportInput std::map imageFiles; bool calibLoaded = false; Intrinsics K; - Extrinsics E; + Extrinsics E; // tx/ty/tz (camera position); rotation is R_wc below, not E.om/fi/ka + Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame // LiDAR chunks: each .laz plus its optional MRP correction (T applied to the // points to bring them into the map frame). Points carry per-point ns stamps. diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index b6ab49f9..5b306b3d 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -154,7 +154,8 @@ struct AppState Trajectory traj; std::vector imageTsNs; Intrinsics K; - Extrinsics E; + Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka + Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame Roi roi; bool calibLoaded = false; int imgW = 4656, imgH = 3496; @@ -395,7 +396,7 @@ static void loadCloud(AppState& s) std::sort(lazPaths.begin(), lazPaths.end()); bool canColor = s.calibLoaded && !s.imagesFilenamesInTime.empty(); - Eigen::Matrix3f R_wc = canColor ? omFiKaToMat3(s.E.om, s.E.fi, s.E.ka) : Eigen::Matrix3f::Identity(); + Eigen::Matrix3f R_wc = canColor ? s.R_wc : Eigen::Matrix3f::Identity(); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); float K_fx = s.K.fx * s.imgScale, K_fy = s.K.fy * s.imgScale; float K_cx = s.K.cx * s.imgScale, K_cy = s.K.cy * s.imgScale; @@ -770,17 +771,15 @@ static void loadCalib(AppState& s) s.E.ty = je["camera_position_in_world_xyz"][1]; s.E.tz = je["camera_position_in_world_xyz"][2]; } - // camera_rotation_matrix_in_world: 3x3 rows of R_wc. The file's only - // rotation representation (convention-independent, portable) -- - // decomposed into calib::Extrinsics' own om/fi/ka for internal use. + // camera_rotation_matrix_in_world: 3x3 rows of R_wc, read straight into + // s.R_wc -- the viewer only ever consumes this calibration, so there's + // no need to round-trip it through Tait-Bryan angles. if (je.contains("camera_rotation_matrix_in_world") && je["camera_rotation_matrix_in_world"].size() >= 3) { auto& m = je["camera_rotation_matrix_in_world"]; - Eigen::Matrix3f R; for (int r = 0; r < 3; r++) for (int c = 0; c < 3; c++) - R(r, c) = m[r][c].get(); - omFiKaFromMat3(R, s.E.om, s.E.fi, s.E.ka); + s.R_wc(r, c) = m[r][c].get(); } } // Optional region of interest, in full-resolution image pixels: @@ -958,7 +957,7 @@ static void exportColmap(AppState& s) // T_lidar_camera (camera pose in the LiDAR frame, from the extrinsics) Eigen::Affine3f T_lc = Eigen::Affine3f::Identity(); - T_lc.linear() = omFiKaToMat3(s.E.om, s.E.fi, s.E.ka); + T_lc.linear() = s.R_wc; T_lc.translation() = Eigen::Vector3f(s.E.tx, s.E.ty, s.E.tz); // cameras.txt — rational OpenCV model == COLMAP FULL_OPENCV (12 params) @@ -1057,6 +1056,7 @@ static void buildRosInput(AppState& s, RosExportInput& in) in.calibLoaded = s.calibLoaded; in.K = s.K; in.E = s.E; + in.R_wc = s.R_wc; fs::path d(s.sessionBuf); if (!fs::is_directory(d)) @@ -1135,7 +1135,7 @@ static void drawScene(AppState& s) // ── camera frustums ─────────────────────────────────────────────────────── if (s.showFrustums && s.calibLoaded) { - Eigen::Matrix3f R_wc = omFiKaToMat3(s.E.om, s.E.fi, s.E.ka); + Eigen::Matrix3f R_wc = s.R_wc; Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); float fs = s.frustumScale; float ncx[4] = { diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a701749e..66d88641 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -12,7 +12,6 @@ set(CORE_BASE_SOURCES src/point_cloud.cpp src/point_clouds.cpp src/session.cpp - src/tum.cpp # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) From 7f5fbf4be297fa61df4ece994e3de45531f3ed8b Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 23:45:04 +0200 Subject: [PATCH 4/5] Fix SaveFileDialog not opening on macOS with a default filename AppleScript's choose-file-name only accepts a folder for default location; appending the filename broke it before any dialog showed. --- core/src/pfd_wrapper.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index bb832fd0..b861bf86 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -50,11 +50,22 @@ namespace mandeye::fd static std::shared_ptr save_file; // build default path (directory + suggested filename) + // + // portable-file-dialogs' macOS backend shells out to AppleScript's + // "choose file name", which only accepts an *existing folder* for + // "default location" (it has no separate "default name" parameter + // wired up). Appending defaultFileName turns this into a path to a + // file that doesn't exist yet, so the folder resolution throws + // before any dialog is shown. The zenity/kdialog/Windows backends + // are fine with a combined dir+filename path, so only combine them + // there. std::string defaultPath = internal::lastLocationHint; +#ifndef __APPLE__ if (!defaultFileName.empty()) { defaultPath = (std::filesystem::path(internal::lastLocationHint) / defaultFileName).string(); } +#endif file = pfd::save_file(title, defaultPath, filter).result(); From f94b53bf4544b821891293ba576eefe6bb18df7f Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 23:46:01 +0200 Subject: [PATCH 5/5] Clang format Signed-off-by: Michal Pelka --- apps/camera_lidar_calibration/App.cpp | 3 ++- apps/camera_lidar_calibration/UI.cpp | 13 +++++++------ calib_core/src/Camera.cpp | 5 ----- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index e289257c..ba4a1c40 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -147,7 +147,8 @@ bool AppState::solvePairs() } lastSolveRmsPixels = rms; - statusMsg = lockTranslation ? "Solved rotation (translation locked): RMS reprojection error " : "Solved extrinsics: RMS reprojection error "; + statusMsg = + lockTranslation ? "Solved rotation (translation locked): RMS reprojection error " : "Solved extrinsics: RMS reprojection error "; statusMsg += std::to_string(rms) + " px"; if (!imageRectified && intrinsicsLoaded == false) statusMsg += " (no intrinsics loaded -- distortion assumed zero)"; diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 0920db11..4517e7e2 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -143,7 +143,7 @@ void UI::drawImageView(AppState& state) // core/src/control_points.cpp already uses elsewhere in this codebase. const bool picking = ImGui::GetIO().KeyShift; ImGui::TextColored( - ImVec4(1, 1, 0, 0.8f), picking ? "Shift+click to pick a point" : "Scroll: zoom | Drag: pan | Dbl-click: reset zoom"); + ImVec4(1, 1, 0, 0.8f), picking ? "Shift+click to pick a point" : "Scroll: zoom | Drag: pan with middle | Dbl-click: reset zoom"); ImGui::BeginChild("image_scroll", ImVec2(0, 0), false, ImGuiWindowFlags_HorizontalScrollbar | ImGuiWindowFlags_NoScrollWithMouse); @@ -179,7 +179,7 @@ void UI::drawImageView(AppState& state) state.setPendingImagePoint(std::clamp(p.x, 0.f, imgW), std::clamp(p.y, 0.f, imgH)); } } - else if (ImGui::IsMouseDragging(ImGuiMouseButton_Left)) + else if (ImGui::IsMouseDragging(ImGuiMouseButton_Middle)) { ImGui::SetScrollX(ImGui::GetScrollX() - io.MouseDelta.x); ImGui::SetScrollY(ImGui::GetScrollY() - io.MouseDelta.y); @@ -306,7 +306,7 @@ void UI::actionOpenCalibration(AppState& state) void UI::actionSaveCalibration(AppState& state) { - std::string path = mandeye::fd::SaveFileDialog("Save calibration file", mandeye::fd::json_filter, ".json", "calibration.json"); + std::string path = mandeye::fd::SaveFileDialog("Save calibration file", mandeye::fd::json_filter); if (!path.empty()) { setBuf(savePath, sizeof(savePath), path); @@ -479,7 +479,8 @@ void UI::panelExtrinsics(AppState& state) ImGui::Checkbox("Lock translation", &state.lockTranslation); if (ImGui::IsItemHovered()) ImGui::SetTooltip( - "Blocks tx/ty/tz from being edited here or changed by\n\"Solve Extrinsics from Pairs\" -- use when the camera\nposition is already known and only orientation needs solving."); + "Blocks tx/ty/tz from being edited here or changed by\n\"Solve Extrinsics from Pairs\" -- use when the camera\nposition is " + "already known and only orientation needs solving."); ImGui::Text("Camera position in world (m):"); ImGui::BeginDisabled(state.lockTranslation); @@ -492,10 +493,10 @@ void UI::panelExtrinsics(AppState& state) ImGui::Text("Camera orientation in world, om/fi/ka (deg):"); dragFloat("om", &E.om, 0.1f, -180.f, 180.f, "%.2f"); dragFloat("fi", &E.fi, 0.1f, -180.f, 180.f, "%.2f"); - avoidGimbalLock(E.fi); dragFloat("ka", &E.ka, 0.1f, -180.f, 180.f, "%.2f"); helpMarker( - "R_wc = Rx(om)*Ry(fi)*Rz(ka): camera orientation in LiDAR world.\nT_lidar2cam = R_wc^T * (p - C).\nAt fi=+/-90 deg (gimbal lock), om and ka are not individually\nunique -- only om+ka (or om-ka) is determined."); + "R_wc = Rx(om)*Ry(fi)*Rz(ka): camera orientation in LiDAR world.\nT_lidar2cam = R_wc^T * (p - C).\nAt fi=+/-90 deg (gimbal lock), " + "om and ka are not individually\nunique -- only om+ka (or om-ka) is determined."); ImGui::Spacing(); if (ImGui::Button("Reset Extrinsics", ImVec2(-1, 0))) diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 29cc82a9..c2fbabd0 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -8,10 +8,6 @@ namespace calib { -void avoidGimbalLock(float& fi_deg) { - if (std::fabs(std::fabs(fi_deg) - 90.f) < kGimbalLockEpsilonDeg) - fi_deg = std::copysign(90.f - kGimbalLockEpsilonDeg, fi_deg); -} Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { TaitBryanPose pose; @@ -29,7 +25,6 @@ void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, floa om_deg = static_cast(rad2deg(pose.om)); fi_deg = static_cast(rad2deg(pose.fi)); ka_deg = static_cast(rad2deg(pose.ka)); - avoidGimbalLock(fi_deg); } bool projectPoint(float px, float py, float pz,