diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a19ba7f..0b7b89bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,7 @@ include(cmake/raylib.cmake) # Core Library # ============================================================================ add_subdirectory(core) +add_subdirectory(calib_core) set(CORE_LIBRARIES core) set(GUI_LIBRARIES imgui imguizmo implot) @@ -127,6 +128,9 @@ add_subdirectory(apps/mandeye_single_session_viewer) add_subdirectory(apps/livox_mid_360_intrinsic_calibration) add_subdirectory(apps/single_session_manual_coloring) add_subdirectory(apps/concatenate_multi_livox) +add_subdirectory(apps/camera_lidar_calibration) +add_subdirectory(apps/camera_lidar_trajectory_viewer) +add_subdirectory(apps/camera_lidar_intrinsics_calib) # NOTE(mwlasiuk) : disable warnings for third party libraries so they do not pollute build logs diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp new file mode 100644 index 00000000..ed12dc02 --- /dev/null +++ b/apps/camera_lidar_calibration/App.cpp @@ -0,0 +1,576 @@ +#include "App.h" +#include "imgui.h" +#include "rlImGui.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ── AppState::rebuildImageTexture ───────────────────────────────────────────── +void AppState::rebuildImageTexture() +{ + if (originalImage.empty()) + return; + + cv::Mat display = originalImage; + imageRectified = false; + + if (intrinsicsLoaded) + { + cv::Mat K = (cv::Mat_(3, 3) << intrinsics.fx, 0, intrinsics.cx, 0, intrinsics.fy, intrinsics.cy, 0, 0, 1); + // OpenCV distCoeffs order: k1 k2 p1 p2 k3 k4 k5 k6 (rational model) + cv::Mat D = + (cv::Mat_(1, 8) << intrinsics.k1, + intrinsics.k2, + intrinsics.p1, + intrinsics.p2, + intrinsics.k3, + intrinsics.k4, + intrinsics.k5, + intrinsics.k6); + + cv::Mat map1, map2; + cv::initUndistortRectifyMap(K, D, cv::Mat(), K, originalImage.size(), CV_16SC2, map1, map2); + cv::Mat rectified; + cv::remap(originalImage, rectified, map1, map2, cv::INTER_LINEAR); + display = rectified; + imageRectified = true; + } + + if (imageLoaded) + UnloadTexture(imageTexture); + Image rimg = {}; + rimg.data = display.data; + rimg.width = display.cols; + rimg.height = display.rows; + rimg.mipmaps = 1; + rimg.format = PIXELFORMAT_UNCOMPRESSED_R8G8B8; + imageTexture = LoadTextureFromImage(rimg); // copies pixels to GPU + imageLoaded = true; +} + +// ── AppState::loadImage ─────────────────────────────────────────────────────── +void AppState::loadImage(const char* path) +{ + cv::Mat bgr = cv::imread(path, cv::IMREAD_COLOR); + if (bgr.empty()) + { + statusMsg = std::string("Failed to load image: ") + path; + return; + } + cv::cvtColor(bgr, originalImage, cv::COLOR_BGR2RGB); + imageW = originalImage.cols; + imageH = originalImage.rows; + imagePath = path; + rebuildImageTexture(); + renderer.init(imageW, imageH); + statusMsg = imageRectified ? "Image loaded and rectified" : "Image loaded (raw)"; +} + +// ── AppState::loadCloud ─────────────────────────────────────────────────────── +static void centerOrbitOnCloud(AppState& s) +{ + s.orbit.target = { (s.cloud.minX + s.cloud.maxX) * 0.5f, (s.cloud.minZ + s.cloud.maxZ) * 0.5f, -(s.cloud.minY + s.cloud.maxY) * 0.5f }; + float span = std::max({ s.cloud.maxX - s.cloud.minX, s.cloud.maxY - s.cloud.minY, s.cloud.maxZ - s.cloud.minZ }); + s.orbit.distance = span * 0.8f; +} + +void AppState::loadCloud(const char* path) +{ + if (!cloud.load(path)) + { + statusMsg = std::string("Failed to load cloud: ") + path; + return; + } + cloudPaths = { path }; + renderer.uploadCloud(cloud); + centerOrbitOnCloud(*this); + statusMsg = ""; +} + +void AppState::addCloud(const char* path) +{ + PointCloud extra; + if (!extra.load(path)) + { + statusMsg = std::string("Failed to load: ") + path; + return; + } + // merge bounding box + if (cloud.empty()) + { + cloud = std::move(extra); + } + else + { + cloud.points.insert(cloud.points.end(), extra.points.begin(), extra.points.end()); + cloud.minX = std::min(cloud.minX, extra.minX); + cloud.maxX = std::max(cloud.maxX, extra.maxX); + cloud.minY = std::min(cloud.minY, extra.minY); + cloud.maxY = std::max(cloud.maxY, extra.maxY); + cloud.minZ = std::min(cloud.minZ, extra.minZ); + cloud.maxZ = std::max(cloud.maxZ, extra.maxZ); + } + cloudPaths.push_back(path); + renderer.uploadCloud(cloud); + centerOrbitOnCloud(*this); + statusMsg = ""; +} + +// ── OpenCV YAML intrinsics parser ───────────────────────────────────────────── +// Handles both styles produced by OpenCV/ROS calibration tools: +// data: [a, b, c] (flow, may span lines until ']') +// data: (block) +// - a +// - b +// Distortion order is OpenCV distCoeffs: k1 k2 p1 p2 k3 [k4 k5 k6 ...] +static void extractNumbers(const std::string& s, std::vector& out) +{ + const char* p = s.c_str(); + while (*p) + { + if ((*p >= '0' && *p <= '9') || *p == '-' || *p == '+' || *p == '.') + { + char* end = nullptr; + double v = std::strtod(p, &end); + if (end != p) + { + out.push_back(v); + p = end; + continue; + } + } + ++p; + } +} + +static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& imgH, std::string& err) +{ + std::ifstream f(path); + if (!f) + { + err = "cannot open file"; + return false; + } + + std::vector camMat, dist; + std::vector* active = nullptr; // section whose data we collect + std::vector* collecting = nullptr; + bool inFlow = false; + + std::string line; + while (std::getline(f, line)) + { + std::string trimmed = line; + trimmed.erase(0, trimmed.find_first_not_of(" \t")); + + if (inFlow) + { + extractNumbers(trimmed, *collecting); + if (trimmed.find(']') != std::string::npos) + { + inFlow = false; + collecting = nullptr; + } + continue; + } + + bool topLevel = !line.empty() && line[0] != ' ' && line[0] != '\t' && line[0] != '-'; + if (topLevel) + { + collecting = nullptr; + if (trimmed.rfind("camera_matrix:", 0) == 0) + active = &camMat; + else if (trimmed.rfind("distortion_coefficients:", 0) == 0) + active = &dist; + else + { + active = nullptr; + if (trimmed.rfind("image_width:", 0) == 0) + imgW = std::atoi(trimmed.c_str() + 12); + else if (trimmed.rfind("image_height:", 0) == 0) + imgH = std::atoi(trimmed.c_str() + 13); + } + continue; + } + + if (active && trimmed.rfind("data:", 0) == 0) + { + auto bracket = trimmed.find('['); + if (bracket != std::string::npos) + { + extractNumbers(trimmed.substr(bracket), *active); + if (trimmed.find(']') == std::string::npos) + { + collecting = active; + inFlow = true; + } + } + else + { + collecting = active; // block list follows + } + continue; + } + + if (collecting) + { + if (trimmed.rfind("- ", 0) == 0 || trimmed.rfind("-", 0) == 0) + extractNumbers(trimmed, *collecting); + else + collecting = nullptr; // rows:/cols: or another key ends the list + } + } + + if (camMat.size() < 9) + { + err = "camera_matrix needs 9 values"; + return false; + } + + // Row-major 3x3: [fx 0 cx; 0 fy cy; 0 0 1] + K.fx = static_cast(camMat[0]); + K.cx = static_cast(camMat[2]); + K.fy = static_cast(camMat[4]); + K.cy = static_cast(camMat[5]); + + auto d = [&](size_t i) + { + return i < dist.size() ? static_cast(dist[i]) : 0.f; + }; + K.k1 = d(0); + K.k2 = d(1); + K.p1 = d(2); + K.p2 = d(3); + K.k3 = d(4); + K.k4 = d(5); + K.k5 = d(6); + K.k6 = d(7); + return true; +} + +// ── AppState::loadIntrinsics ────────────────────────────────────────────────── +void AppState::loadIntrinsics(const char* path) +{ + std::string p = path; + auto dot = p.rfind('.'); + std::string ext = (dot != std::string::npos) ? p.substr(dot + 1) : ""; + for (auto& c : ext) + c = static_cast(tolower(c)); + + if (ext == "yml" || ext == "yaml") + { + int imgW = 0, imgH = 0; + std::string err; + if (!parseOpenCVYaml(path, intrinsics, imgW, imgH, err)) + { + statusMsg = std::string("YAML error: ") + err + " (" + path + ")"; + return; + } + intrinsicsLoaded = true; + rebuildImageTexture(); // re-rectify with the new coefficients + statusMsg = "Intrinsics loaded"; + if (imageRectified) + statusMsg += ", image rectified"; + if (imgW > 0) + { + statusMsg += " (camera " + std::to_string(imgW) + "x" + std::to_string(imgH) + ")"; + if (imageLoaded && (imgW != imageW || imgH != imageH)) + statusMsg += " WARNING: image is " + std::to_string(imageW) + "x" + std::to_string(imageH); + } + return; + } + + std::ifstream f(path); + if (!f) + { + statusMsg = std::string("Cannot open: ") + path; + return; + } + nlohmann::json j; + f >> j; + intrinsics.fx = j.value("fx", intrinsics.fx); + intrinsics.fy = j.value("fy", intrinsics.fy); + intrinsics.cx = j.value("cx", intrinsics.cx); + intrinsics.cy = j.value("cy", intrinsics.cy); + intrinsics.k1 = j.value("k1", 0.f); + intrinsics.k2 = j.value("k2", 0.f); + intrinsics.k3 = j.value("k3", 0.f); + intrinsics.k4 = j.value("k4", 0.f); + intrinsics.k5 = j.value("k5", 0.f); + intrinsics.k6 = j.value("k6", 0.f); + intrinsics.p1 = j.value("p1", 0.f); + intrinsics.p2 = j.value("p2", 0.f); + intrinsicsLoaded = true; + rebuildImageTexture(); + statusMsg = "Intrinsics loaded."; +} + +// ── AppState::loadCalibration ───────────────────────────────────────────────── +void AppState::loadCalibration(const char* path) +{ + std::ifstream f(path); + if (!f) + { + statusMsg = std::string("Cannot open: ") + path; + return; + } + nlohmann::json j; + try + { + f >> j; + } catch (...) + { + statusMsg = std::string("JSON parse error: ") + path; + return; + } + + bool gotIntrinsics = false, gotExtrinsics = false; + + if (j.contains("intrinsics")) + { + auto& ji = j["intrinsics"]; + intrinsics.fx = ji.value("fx", intrinsics.fx); + intrinsics.fy = ji.value("fy", intrinsics.fy); + intrinsics.cx = ji.value("cx", intrinsics.cx); + intrinsics.cy = ji.value("cy", intrinsics.cy); + intrinsics.k1 = ji.value("k1", 0.f); + intrinsics.k2 = ji.value("k2", 0.f); + intrinsics.k3 = ji.value("k3", 0.f); + intrinsics.k4 = ji.value("k4", 0.f); + intrinsics.k5 = ji.value("k5", 0.f); + intrinsics.k6 = ji.value("k6", 0.f); + intrinsics.p1 = ji.value("p1", 0.f); + intrinsics.p2 = ji.value("p2", 0.f); + intrinsicsLoaded = true; + gotIntrinsics = true; + } + + if (j.contains("extrinsics")) + { + auto& je = j["extrinsics"]; + // camera_position_in_world_xyz: [tx, ty, tz] + if (je.contains("camera_position_in_world_xyz") && je["camera_position_in_world_xyz"].size() >= 3) + { + auto& pos = je["camera_position_in_world_xyz"]; + extrinsics.tx = pos[0].get(); + 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) + { + 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(); + } + gotExtrinsics = true; + } + + if (!gotIntrinsics && !gotExtrinsics) + { + statusMsg = std::string("No intrinsics/extrinsics found in: ") + path; + return; + } + + if (gotIntrinsics) + rebuildImageTexture(); + + statusMsg = "Loaded"; + if (gotIntrinsics) + statusMsg += " intrinsics"; + if (gotIntrinsics && gotExtrinsics) + statusMsg += " +"; + if (gotExtrinsics) + statusMsg += " extrinsics"; + statusMsg += std::string(" from ") + path; +} + +// ── AppState::saveCalibration ───────────────────────────────────────────────── +void AppState::saveCalibration(const char* path) +{ + // World-frame convention: R = R_wc (camera orientation in world, ZYX Euler) + // 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::Vector3f C(extrinsics.tx, extrinsics.ty, extrinsics.tz); + Eigen::Vector3f ti = -(R.transpose() * C); // translation of T_lidar_to_camera + + nlohmann::json j; + 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_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) }, + { R(2, 0), R(2, 1), R(2, 2) } }; + j["extrinsics"]["T_lidar_to_camera_4x4"] = { + { R(0, 0), R(1, 0), R(2, 0), ti(0) }, { R(0, 1), R(1, 1), R(2, 1), ti(1) }, { R(0, 2), R(1, 2), R(2, 2), ti(2) }, { 0, 0, 0, 1 } + }; + + std::ofstream f(path); + if (!f) + { + statusMsg = std::string("Cannot write: ") + path; + return; + } + f << j.dump(4); + statusMsg = std::string("Saved to ") + path; + printf("Calibration saved to %s\n", path); +} + +// Shrinks/repositions the just-created window so it fits within the current +// monitor's usable area. Without this, a fixed 1400x900 window can be taller +// than the screen once the OS menu bar + title bar are accounted for (e.g. a +// 956pt-tall MacBook display leaves ~0 spare px at H=900), silently pushing +// the top of the window (and the first Files panel controls) off-screen +// behind the menu bar instead of erroring or scrolling. +static void fitWindowToScreen() +{ + int monitor = GetCurrentMonitor(); + // GetMonitorWidth/Height return the monitor's native PIXEL resolution + // (GLFW's glfwGetVideoMode), while GetScreenWidth/Height, SetWindowSize + // and SetWindowPosition all operate in logical points -- on a 2x Retina + // display that's a 2x unit mismatch. Divide by the DPI scale to bring + // the monitor size into the same points space everything else uses; + // without this, SetWindowPosition computes an X centered on a monitor + // twice too wide, pushing most of the window off the right edge of the + // actual (points-sized) screen. + Vector2 dpi = GetWindowScaleDPI(); + if (dpi.x <= 0.f) + dpi.x = 1.f; + if (dpi.y <= 0.f) + dpi.y = 1.f; + int monW = (int)(GetMonitorWidth(monitor) / dpi.x); + int monH = (int)(GetMonitorHeight(monitor) / dpi.y); + if (monW <= 0 || monH <= 0) + return; // monitor info unavailable, leave as-is + + const int marginW = 40; // side breathing room + const int marginH = 100; // OS menu bar + window title bar headroom + + int w = std::min(GetScreenWidth(), monW - marginW); + int h = std::min(GetScreenHeight(), monH - marginH); + if (w != GetScreenWidth() || h != GetScreenHeight()) + SetWindowSize(w, h); + + SetWindowPosition(std::max(0, (monW - w) / 2), 30); + + // SetWindowSize/SetWindowPosition only update GLFW's window state; raylib's + // cached mouse/window geometry (what rlImGui reads into io.MousePos every + // frame) isn't refreshed until the next PollInputEvents(), which otherwise + // wouldn't happen until the first EndDrawing() -- after rlImGuiSetup() has + // already run. Without this, every click lands offset from the cursor by + // however far this function just moved/resized the window. + PollInputEvents(); +} + +// ── App::run ────────────────────────────────────────────────────────────────── +void App::run() +{ + const int W = 1400, H = 900; + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + InitWindow(W, H, "LiDAR-Camera Calibration"); + fitWindowToScreen(); + // The 340px-wide side panel is fixed-width; below this the 3D/image + // views and the panel start overlapping instead of scrolling. + SetWindowMinSize(900, 500); + SetTargetFPS(60); + + rlImGuiSetup(true); // dark theme + + state.renderer.initPointShader(); + + // Load files passed as command-line arguments + if (!pendingImage.empty()) + state.loadImage(pendingImage.c_str()); + for (size_t i = 0; i < pendingClouds.size(); ++i) + (i == 0 ? state.loadCloud(pendingClouds[i].c_str()) : state.addCloud(pendingClouds[i].c_str())); + if (!pendingIntrinsics.empty()) + state.loadIntrinsics(pendingIntrinsics.c_str()); + if (!pendingCalibration.empty()) + state.loadCalibration(pendingCalibration.c_str()); + + while (!WindowShouldClose()) + { + update(); + draw(); + } + + if (state.imageLoaded) + UnloadTexture(state.imageTexture); + state.renderer.shutdown(); + rlImGuiShutdown(); + CloseWindow(); +} + +// ── App::update ─────────────────────────────────────────────────────────────── +void App::update() +{ + bool imguiWantMouse = ImGui::GetIO().WantCaptureMouse; + state.orbit.update(!imguiWantMouse); +} + +// ── App::draw ───────────────────────────────────────────────────────────────── +void App::draw() +{ + float panelW = 340.f; + float viewW = (float)GetScreenWidth() - panelW; + float viewH = (float)GetScreenHeight(); + float view3DY = viewH * 0.5f; // 3D starts at middle + + // ── Image + projection overlay (GPU, into render texture) + 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) + 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 - view3DY)); + + Camera3D cam3d = state.orbit.toRaylib(); + BeginMode3D(cam3d); + + state.renderer.draw3DCloud( + state.cloud, + state.vizParams, + state.intrinsics, + state.extrinsics, + state.imageTexture, + state.imageLoaded, + state.imageW, + state.imageH); + if (state.imageLoaded) + state.renderer.drawCameraFrustum(state.intrinsics, state.extrinsics, state.imageW, state.imageH); + state.renderer.drawAxes(2.f); + + // Grid on ground plane + DrawGrid(20, 1.f); + + EndMode3D(); + EndScissorMode(); + + // ── 3D label + DrawText("3D View [LMB: orbit | RMB: pan | Scroll: zoom]", 8, (int)view3DY + 4, 14, LIGHTGRAY); + + // ── Divider line + DrawLineEx(Vector2{ 0, view3DY }, Vector2{ viewW, view3DY }, 1.f, GRAY); + + // ── ImGui on top ───────────────────────────────────────────────────────── + rlImGuiBegin(); + state.ui.draw(state); + rlImGuiEnd(); + + EndDrawing(); +} diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h new file mode 100644 index 00000000..071d16e3 --- /dev/null +++ b/apps/camera_lidar_calibration/App.h @@ -0,0 +1,72 @@ +#pragma once +#include +#include +#include "Renderer.h" +#include "UI.h" +#include "raylib.h" +#include +#include +#include + +using namespace calib; + +struct AppState { + // ── loaded data ────────────────────────────────────────────────────────── + PointCloud cloud; + cv::Mat originalImage; // RGB, as loaded from disk + Texture2D imageTexture = {}; // displayed (rectified if possible) + bool imageLoaded = false; + bool imageRectified = false; + bool intrinsicsLoaded = false; // from file (defaults are guesses) + int imageW = 0, imageH = 0; + std::string imagePath; + std::vector cloudPaths; + + // ── calibration params ─────────────────────────────────────────────────── + Intrinsics intrinsics; + Extrinsics extrinsics; + + // ── visualization ───────────────────────────────────────────────────────── + VisualizationParams vizParams; + + // ── 3D camera ───────────────────────────────────────────────────────────── + OrbitCamera orbit; + + // ── sub-systems ─────────────────────────────────────────────────────────── + Renderer renderer; + UI ui; + + // ── misc ────────────────────────────────────────────────────────────────── + std::string statusMsg; + + // ── operations ──────────────────────────────────────────────────────────── + void loadImage(const char* path); + void loadCloud(const char* path); // clear + load + void addCloud(const char* path); // merge into existing cloud + void loadIntrinsics(const char* path); + void loadCalibration(const char* path); // full JSON (intrinsics + extrinsics) + void saveCalibration(const char* path); + // (Re)build the displayed texture: undistorts with current intrinsics + // when they were loaded from a file, otherwise shows the raw image. + void rebuildImageTexture(); +}; + +class App { +public: + // Call before run() to auto-load files after window init + void preloadImage(const char* path) { pendingImage = path; } + void preloadCloud(const char* path) { pendingClouds.push_back(path); } + void preloadIntrinsics(const char* path) { pendingIntrinsics = path; } + void preloadCalibration(const char* path){ pendingCalibration = path; } + + void run(); +private: + AppState state; + std::string pendingImage; + std::vector pendingClouds; + std::string pendingIntrinsics; + std::string pendingCalibration; + + void update(); + void draw(); +}; diff --git a/apps/camera_lidar_calibration/CMakeLists.txt b/apps/camera_lidar_calibration/CMakeLists.txt new file mode 100644 index 00000000..326244eb --- /dev/null +++ b/apps/camera_lidar_calibration/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(camera_lidar_calibration) + +# Interactive LiDAR-camera extrinsic calibration: aligns a point cloud (LAZ/LAS) +# to a camera image, with GPU-shader reprojection feedback. Ported from the +# sibling mandeye-colors project (see calib_core/CMakeLists.txt for the shared +# non-GUI logic). Its image-space projection/distortion overlay shader +# (Renderer.cpp) has no equivalent in core's ScanRenderer (core_raylib), so +# rendering stays bespoke here rather than reusing core_raylib -- this app +# links raylib/imgui_raylib/rlimgui directly instead, without pulling in +# core/core_math. +add_executable(camera_lidar_calibration + main.cpp + App.h App.cpp + UI.h UI.cpp + Renderer.h Renderer.cpp +) + +target_include_directories(camera_lidar_calibration PRIVATE + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include +) + +target_compile_definitions(camera_lidar_calibration PRIVATE WITH_GUI=1) + +target_link_libraries(camera_lidar_calibration PRIVATE + calib_core + raylib + imgui_raylib + rlimgui + ${OpenCV_LIBS} + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} +) + +if(MSVC) + target_compile_options(camera_lidar_calibration PRIVATE /W4) + target_compile_definitions(camera_lidar_calibration PRIVATE _USE_MATH_DEFINES LASZIP_API_VERSION) +else() + target_compile_options(camera_lidar_calibration PRIVATE -Wall -Wextra) + target_compile_definitions(camera_lidar_calibration PRIVATE LASZIP_API_VERSION) +endif() + +if(WIN32) + add_custom_command( + TARGET camera_lidar_calibration + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if(MSVC) + target_compile_options(camera_lidar_calibration PRIVATE /bigobj) +endif() + +install(TARGETS camera_lidar_calibration DESTINATION bin) diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp new file mode 100644 index 00000000..590a97c3 --- /dev/null +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -0,0 +1,468 @@ +#include "Renderer.h" +#include "rlgl.h" +#include "raymath.h" +// glad function pointers are compiled into raylib; the header only declares them +#include "external/glad.h" +#include +#include +#include + +// ── Jet colormap ───────────────────────────────────────────────────────────── +Color jetColor(float t) { + t = std::max(0.f, std::min(1.f, t)); + float r = std::max(0.f, std::min(1.f, 1.5f - std::abs(4.f*t - 3.f))); + float g = std::max(0.f, std::min(1.f, 1.5f - std::abs(4.f*t - 2.f))); + float b = std::max(0.f, std::min(1.f, 1.5f - std::abs(4.f*t - 1.f))); + return Color{ + static_cast(r * 255), + static_cast(g * 255), + static_cast(b * 255), + 255 + }; +} + +// ── OrbitCamera ─────────────────────────────────────────────────────────────── +Camera3D OrbitCamera::toRaylib() const { + float az = azimuth * (float)DEG2RAD; + float el = elevation * (float)DEG2RAD; + Vector3 pos = { + target.x + distance * std::cos(el) * std::sin(az), + target.y + distance * std::sin(el), + target.z + distance * std::cos(el) * std::cos(az) + }; + Camera3D cam; + cam.position = pos; + cam.target = target; + cam.up = {0.f, 1.f, 0.f}; + cam.fovy = 45.f; + cam.projection = CAMERA_PERSPECTIVE; + return cam; +} + +void OrbitCamera::update(bool active) { + if (!active) return; + + // Left-drag → orbit + if (IsMouseButtonDown(MOUSE_BUTTON_LEFT)) { + Vector2 d = GetMouseDelta(); + azimuth -= d.x * 0.4f; + elevation += d.y * 0.4f; + elevation = std::max(-89.f, std::min(89.f, elevation)); + } + // Right-drag → pan + if (IsMouseButtonDown(MOUSE_BUTTON_RIGHT)) { + Camera3D cam = toRaylib(); + Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); + Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); + Vector3 up = Vector3CrossProduct(right, fwd); + Vector2 d = GetMouseDelta(); + float speed = distance * 0.002f; + target = Vector3Add(target, Vector3Scale(right, -d.x * speed)); + target = Vector3Add(target, Vector3Scale(up, d.y * speed)); + } + // Scroll → zoom + float wheel = GetMouseWheelMove(); + if (wheel != 0.f) { + distance -= wheel * distance * 0.1f; + distance = std::max(0.5f, distance); + } +} + +// World-frame convention: E.rx/ry/rz = camera orientation in world (R_wc, ZYX Euler). +// 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::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). + return Matrix{ + R(0,0), R(1,0), R(2,0), ti(0), + R(0,1), R(1,1), R(2,1), ti(1), + R(0,2), R(1,2), R(2,2), ti(2), + 0.f, 0.f, 0.f, 1.f + }; +} + +// ── Renderer ────────────────────────────────────────────────────────────────── +void Renderer::init(int imgW, int imgH) { + if (imageTexValid) + UnloadRenderTexture(imageTex); + texW = imgW; + texH = imgH; + imageTex = LoadRenderTexture(imgW, imgH); + imageTexValid = true; +} + +void Renderer::shutdown() { + if (imageTexValid) { + UnloadRenderTexture(imageTex); + imageTexValid = false; + } + unloadCloudGPU(); + if (shaderValid) { + UnloadShader(pointShader); + shaderValid = false; + } +} + +// ── GPU point cloud shaders ────────────────────────────────────────────────── +// Explicit attribute locations so one VAO works with both programs: +// location 0 = position (raylib coords), location 1 = intensity. +static const char* kPointVS = R"( +#version 330 +layout(location = 0) in vec3 vertexPosition; +layout(location = 1) in float vertexIntensity; +uniform mat4 mvp; +uniform float pointSize; +uniform mat4 lidarToCam; // extrinsics (for RGB mode) +uniform vec4 K; // fx, fy, cx, cy +uniform vec2 imgSize; +out vec3 fragPos; +out float fragIntensity; +out vec2 fragUV; +out float fragCamDepth; +void main() { + fragPos = vertexPosition; + fragIntensity = vertexIntensity; + gl_Position = mvp * vec4(vertexPosition, 1.0); + gl_PointSize = pointSize; + + // Project into the camera image for RGB sampling (rectified → pinhole) + vec3 lidar = vec3(vertexPosition.x, -vertexPosition.z, vertexPosition.y); + vec3 pc = (lidarToCam * vec4(lidar, 1.0)).xyz; + fragCamDepth = pc.z; + vec2 uv = (K.xy * (pc.xy / max(pc.z, 1e-6)) + K.zw) / imgSize; + fragUV = uv; +} +)"; + +static const char* kPointFS = R"( +#version 330 +in vec3 fragPos; +in float fragIntensity; +in vec2 fragUV; +in float fragCamDepth; +uniform int colorMode; // 0 = distance, 1 = intensity, 2 = height, 3 = camera RGB +uniform vec2 heightRange; // min/max of raylib Y (lidar Z) +uniform float maxDist; +uniform float opacity; +uniform sampler2D imageTex; +out vec4 finalColor; + +vec3 jet(float t) { + t = clamp(t, 0.0, 1.0); + return clamp(vec3(1.5 - abs(4.0*t - 3.0), + 1.5 - abs(4.0*t - 2.0), + 1.5 - abs(4.0*t - 1.0)), 0.0, 1.0); +} + +void main() { + if (colorMode == 3) { + bool seen = fragCamDepth > 0.0 + && fragUV.x >= 0.0 && fragUV.x <= 1.0 + && fragUV.y >= 0.0 && fragUV.y <= 1.0; + // points the camera cannot see stay gray — shows the camera FOV + vec3 c = seen ? texture(imageTex, fragUV).rgb : vec3(0.25); + finalColor = vec4(c, opacity); + return; + } + float t; + if (colorMode == 1) + t = fragIntensity; + else if (colorMode == 2) + t = (fragPos.y - heightRange.x) / max(heightRange.y - heightRange.x, 1e-6); + else + t = length(fragPos) / max(maxDist, 1e-6); + finalColor = vec4(jet(t), opacity); +} +)"; + +// Projects lidar points directly onto the image plane. Position attribute is +// in raylib coords, converted back to lidar frame here. With w = z_cam the +// hardware clip rejects points behind the camera; optional rational+tangential +// distortion handles non-rectified images (pass zeros when rectified). +static const char* kProjVS = R"( +#version 330 +layout(location = 0) in vec3 vertexPosition; +layout(location = 1) in float vertexIntensity; +uniform mat4 lidarToCam; // extrinsics +uniform vec4 K; // fx, fy, cx, cy +uniform vec2 imgSize; +uniform vec3 kRad1; // k1 k2 k3 +uniform vec3 kRad2; // k4 k5 k6 +uniform vec2 pTan; // p1 p2 +uniform float pointSize; +out float fragDepth; +out float fragIntensity; +void main() { + // 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; + fragDepth = pc.z; + fragIntensity = vertexIntensity; + + vec2 n = pc.xy / max(pc.z, 1e-6); + float r2 = dot(n, n); + float radial = (1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2) + / (1.0 + kRad2.x*r2 + kRad2.y*r2*r2 + kRad2.z*r2*r2*r2); + vec2 d = n * radial + + vec2(2.0*pTan.x*n.x*n.y + pTan.y*(r2 + 2.0*n.x*n.x), + pTan.x*(r2 + 2.0*n.y*n.y) + 2.0*pTan.y*n.x*n.y); + vec2 uv = K.xy * d + K.zw; // pixel coords + + // pixel -> clip space (y down, like raylib's render-texture ortho) + gl_Position = vec4((2.0*uv.x/imgSize.x - 1.0) * pc.z, + -(2.0*uv.y/imgSize.y - 1.0) * pc.z, + 0.0, + pc.z); + gl_PointSize = pointSize; +} +)"; + +static const char* kProjFS = R"( +#version 330 +in float fragDepth; +in float fragIntensity; +uniform vec2 depthRange; +uniform float opacity; +uniform int colorMode; +out vec4 finalColor; + +vec3 jet(float t) { + t = clamp(t, 0.0, 1.0); + return clamp(vec3(1.5 - abs(4.0*t - 3.0), + 1.5 - abs(4.0*t - 2.0), + 1.5 - abs(4.0*t - 1.0)), 0.0, 1.0); +} + +void main() { + if (fragDepth < depthRange.x || fragDepth > depthRange.y) discard; + float t = (colorMode == 1) + ? fragIntensity + : (fragDepth - depthRange.x) / max(depthRange.y - depthRange.x, 1e-6); + finalColor = vec4(jet(t), opacity); +} +)"; + +void Renderer::initPointShader() { + pointShader = LoadShaderFromMemory(kPointVS, kPointFS); + shaderValid = pointShader.id > 0; + if (!shaderValid) { + TraceLog(LOG_ERROR, "Point cloud shader failed to compile"); + } else { + locMVP = rlGetLocationUniform(pointShader.id, "mvp"); + locPointSize = rlGetLocationUniform(pointShader.id, "pointSize"); + locColorMode = rlGetLocationUniform(pointShader.id, "colorMode"); + locHeightRange = rlGetLocationUniform(pointShader.id, "heightRange"); + locMaxDist = rlGetLocationUniform(pointShader.id, "maxDist"); + locOpacity = rlGetLocationUniform(pointShader.id, "opacity"); + locCamXform = rlGetLocationUniform(pointShader.id, "lidarToCam"); + locCamK = rlGetLocationUniform(pointShader.id, "K"); + locCamImgSize = rlGetLocationUniform(pointShader.id, "imgSize"); + locCamTex = rlGetLocationUniform(pointShader.id, "imageTex"); + } + + projShader = LoadShaderFromMemory(kProjVS, kProjFS); + projShaderValid = projShader.id > 0; + if (!projShaderValid) { + TraceLog(LOG_ERROR, "Projection shader failed to compile"); + } else { + locPrjXform = rlGetLocationUniform(projShader.id, "lidarToCam"); + locPrjK = rlGetLocationUniform(projShader.id, "K"); + locPrjImgSize = rlGetLocationUniform(projShader.id, "imgSize"); + locPrjRad1 = rlGetLocationUniform(projShader.id, "kRad1"); + locPrjRad2 = rlGetLocationUniform(projShader.id, "kRad2"); + locPrjTan = rlGetLocationUniform(projShader.id, "pTan"); + locPrjDepthRange = rlGetLocationUniform(projShader.id, "depthRange"); + locPrjOpacity = rlGetLocationUniform(projShader.id, "opacity"); + locPrjPointSize = rlGetLocationUniform(projShader.id, "pointSize"); + locPrjColorMode = rlGetLocationUniform(projShader.id, "colorMode"); + } + + // Allow gl_PointSize from the vertex shader (core profile requires this) + glEnable(GL_PROGRAM_POINT_SIZE); +} + +void Renderer::uploadCloud(const PointCloud& cloud) { + unloadCloudGPU(); + if (cloud.empty() || !shaderValid) return; + + // Interleaved: x, y, z (raylib coords), intensity + std::vector data; + data.reserve(cloud.points.size() * 4); + for (const auto& p : cloud.points) { + // LiDAR coords → raylib: X=x, Y=z (up), Z=-y + data.push_back(p.x); + data.push_back(p.z); + data.push_back(-p.y); + data.push_back(p.intensity); + } + + cloudVAO = rlLoadVertexArray(); + rlEnableVertexArray(cloudVAO); + cloudVBO = rlLoadVertexBuffer(data.data(), + static_cast(data.size() * sizeof(float)), + false); + const int stride = 4 * sizeof(float); + // locations fixed by layout() qualifiers in both shaders + rlSetVertexAttribute(0, 3, RL_FLOAT, false, stride, 0); + rlEnableVertexAttribute(0); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, stride, 3 * sizeof(float)); + rlEnableVertexAttribute(1); + rlDisableVertexArray(); + + cloudCount = static_cast(cloud.points.size()); +} + +void Renderer::unloadCloudGPU() { + if (cloudVAO) { rlUnloadVertexArray(cloudVAO); cloudVAO = 0; } + if (cloudVBO) { rlUnloadVertexBuffer(cloudVBO); cloudVBO = 0; } + cloudCount = 0; +} + +void Renderer::renderImageOverlay(const Texture2D& img, int imgW, int imgH, + const Intrinsics& K, const Extrinsics& E, + bool applyDistortion, + const VisualizationParams& vp) { + if (!imageTexValid) return; + + BeginTextureMode(imageTex); + ClearBackground(BLACK); + + 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) { + rlDrawRenderBatchActive(); // flush the image quad before raw GL draw + + Matrix xform = buildLidarToCamMatrix(E); + + float k[4] = {K.fx, K.fy, K.cx, K.cy}; + float imgSize[2] = {(float)texW, (float)texH}; + float rad1[3] = {0.f, 0.f, 0.f}; + float rad2[3] = {0.f, 0.f, 0.f}; + float tan2[2] = {0.f, 0.f}; + if (applyDistortion) { + rad1[0] = K.k1; rad1[1] = K.k2; rad1[2] = K.k3; + rad2[0] = K.k4; rad2[1] = K.k5; rad2[2] = K.k6; + tan2[0] = K.p1; tan2[1] = K.p2; + } + float depthRange[2] = {vp.depthMin, vp.depthMax}; + + rlEnableShader(projShader.id); + rlSetUniformMatrix(locPrjXform, xform); + rlSetUniform(locPrjK, k, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locPrjImgSize, imgSize, RL_SHADER_UNIFORM_VEC2, 1); + rlSetUniform(locPrjRad1, rad1, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locPrjRad2, rad2, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locPrjTan, tan2, RL_SHADER_UNIFORM_VEC2, 1); + rlSetUniform(locPrjDepthRange, depthRange, RL_SHADER_UNIFORM_VEC2, 1); + 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); + + rlEnableVertexArray(cloudVAO); + glDrawArrays(GL_POINTS, 0, cloudCount); + rlDisableVertexArray(); + rlDisableShader(); + } + + EndTextureMode(); +} + +void Renderer::draw3DCloud(const PointCloud& cloud, const VisualizationParams& vp, + const Intrinsics& K, const Extrinsics& E, + const Texture2D& image, bool hasImage, + int imgW, int imgH) { + if (cloudCount == 0 || !shaderValid) return; + + // Flush whatever raylib has batched so far (grid, lines) before raw GL draw + rlDrawRenderBatchActive(); + + Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + + // Furthest cloud corner from the LiDAR origin — normalizes distance coloring + float mx = std::max(std::fabs(cloud.minX), std::fabs(cloud.maxX)); + float my = std::max(std::fabs(cloud.minY), std::fabs(cloud.maxY)); + float mz = std::max(std::fabs(cloud.minZ), std::fabs(cloud.maxZ)); + float maxDist = std::sqrt(mx*mx + my*my + mz*mz); + + // heightRange is in raylib Y, which carries lidar Z + float heightRange[2] = {cloud.minZ, cloud.maxZ}; + + int colorMode = vp.colorMode; + if (colorMode == 3 && !hasImage) + colorMode = 0; // no image to sample — fall back to distance + + Matrix camXform = buildLidarToCamMatrix(E); + float k[4] = {K.fx, K.fy, K.cx, K.cy}; + float imgSize[2] = {(float)std::max(imgW, 1), (float)std::max(imgH, 1)}; + + rlEnableShader(pointShader.id); + rlSetUniformMatrix(locMVP, mvp); + rlSetUniform(locPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 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); + rlSetUniform(locOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniformMatrix(locCamXform, camXform); + rlSetUniform(locCamK, k, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locCamImgSize, imgSize, RL_SHADER_UNIFORM_VEC2, 1); + + if (colorMode == 3) { + rlActiveTextureSlot(0); + rlEnableTexture(image.id); + int slot = 0; + rlSetUniform(locCamTex, &slot, RL_SHADER_UNIFORM_INT, 1); + } + + rlEnableVertexArray(cloudVAO); + glDrawArrays(GL_POINTS, 0, cloudCount); + rlDisableVertexArray(); + rlDisableShader(); +} + +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); + + // Camera position in LiDAR frame is directly (E.tx, E.ty, E.tz) + Vector3 origin = {E.tx, E.tz, -E.ty}; // LiDAR→raylib + + // Four image corners in camera frame, at depth=scale + float corners[4][2] = { + {(0.f - K.cx) / K.fx, (0.f - K.cy) / K.fy}, + {(float(imgW) - K.cx) / K.fx, (0.f - K.cy) / K.fy}, + {(float(imgW) - K.cx) / K.fx, (float(imgH) - K.cy) / K.fy}, + {(0.f - K.cx) / K.fx, (float(imgH) - K.cy) / K.fy}, + }; + + // Transform corners: p_lidar = R_wc * pc_cam + C + Eigen::Vector3f C(E.tx, E.ty, E.tz); + auto toWorld = [&](float xn, float yn) -> Vector3 { + Eigen::Vector3f pl = R * Eigen::Vector3f(xn * scale, yn * scale, scale) + C; + return {pl.x(), pl.z(), -pl.y()}; + }; + Vector3 w[4]; + for (int i = 0; i < 4; i++) + w[i] = toWorld(corners[i][0], corners[i][1]); + + Color fc = YELLOW; + DrawLine3D(origin, w[0], fc); + DrawLine3D(origin, w[1], fc); + DrawLine3D(origin, w[2], fc); + DrawLine3D(origin, w[3], fc); + DrawLine3D(w[0], w[1], fc); + DrawLine3D(w[1], w[2], fc); + DrawLine3D(w[2], w[3], fc); + DrawLine3D(w[3], w[0], fc); +} + +void Renderer::drawAxes(float len) { + DrawLine3D({0,0,0}, {len, 0, 0}, RED); // X + DrawLine3D({0,0,0}, {0, len, 0}, GREEN); // Y (= LiDAR Z = up) + DrawLine3D({0,0,0}, {0, 0, -len}, BLUE); // Z (= LiDAR Y) +} diff --git a/apps/camera_lidar_calibration/Renderer.h b/apps/camera_lidar_calibration/Renderer.h new file mode 100644 index 00000000..e6b527dd --- /dev/null +++ b/apps/camera_lidar_calibration/Renderer.h @@ -0,0 +1,90 @@ +#pragma once +#include "raylib.h" +#include +#include +#include + +using namespace calib; + +struct OrbitCamera { + float azimuth = 30.f; // degrees + float elevation = 25.f; // degrees + float distance = 30.f; + Vector3 target = {0.f, 0.f, 0.f}; + + Camera3D toRaylib() const; + // Processes mouse input when active (mouse not over ImGui) + void update(bool active); +}; + +struct VisualizationParams { + float pointSize = 2.f; + float depthMin = 0.f; + float depthMax = 50.f; + float opacity = 1.f; + int colorMode = 0; // 0=depth(jet), 1=intensity, 2=height(z), 3=Camera RGB +}; + +Color jetColor(float t); // t in [0,1] + +class Renderer { +public: + RenderTexture2D imageTex = {}; // image + 2D projection overlay + bool imageTexValid = false; + + void init(int imgW, int imgH); + void shutdown(); + + // Compile the GPU point shader. Requires an active OpenGL context. + void initPointShader(); + + // Upload point cloud to a GPU vertex buffer (interleaved x,y,z,intensity, + // already in raylib coords). Replaces any previous buffer. + void uploadCloud(const PointCloud& cloud); + void unloadCloudGPU(); + + // Render image + GPU-projected point overlay into imageTex. + // If the displayed image is rectified, pass applyDistortion=false. + void renderImageOverlay(const Texture2D& img, int imgW, int imgH, + const Intrinsics& K, const Extrinsics& E, + bool applyDistortion, + const VisualizationParams& vp); + + // Draw 3D point cloud into current BeginMode3D context (GPU shader path). + // For colorMode 3 (camera RGB) pass the displayed image texture and the + // calibration; hasImage=false falls back to distance coloring. + void draw3DCloud(const PointCloud& cloud, + const VisualizationParams& vp, + const Intrinsics& K, const Extrinsics& E, + const Texture2D& image, bool hasImage, + int imgW, int imgH); + + // Draw camera frustum as lines in current BeginMode3D context + void drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, + int imgW, int imgH, float scale = 3.f); + + // Draw world axes at origin + void drawAxes(float len = 2.f); + +private: + int texW = 0, texH = 0; + + // GPU point cloud (VAO shared by both shaders via fixed attrib locations) + Shader pointShader = {}; + bool shaderValid = false; + unsigned int cloudVAO = 0; + unsigned int cloudVBO = 0; + int cloudCount = 0; + // 3D view shader uniforms + int locMVP = -1, locColorMode = -1, locHeightRange = -1; + int locMaxDist = -1, locOpacity = -1, locPointSize = -1; + int locCamXform = -1, locCamK = -1, locCamImgSize = -1, locCamTex = -1; + + // 2D image-projection shader + Shader projShader = {}; + bool projShaderValid = false; + int locPrjXform = -1, locPrjK = -1, locPrjImgSize = -1; + int locPrjRad1 = -1, locPrjRad2 = -1, locPrjTan = -1; + int locPrjDepthRange = -1, locPrjOpacity = -1; + int locPrjPointSize = -1, locPrjColorMode = -1; +}; diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp new file mode 100644 index 00000000..1b6a87e2 --- /dev/null +++ b/apps/camera_lidar_calibration/UI.cpp @@ -0,0 +1,338 @@ +#include "UI.h" +#include "App.h" +#include "imgui.h" +#include "rlImGui.h" +#include +#include +#include +#include +#include +#include + +// Copies `path` into `buf` (truncating to fit), for wiring a native-dialog +// result back into the same fixed-size char[] the text field edits. +static void setBuf(char* buf, size_t bufSize, const std::string& path) +{ + if (path.empty()) + return; + std::strncpy(buf, path.c_str(), bufSize - 1); + buf[bufSize - 1] = '\0'; +} + +// DragFloat with Shift=fine mode (10x smaller step) +static bool dragFloat(const char* label, float* v, float speed, float lo, float hi, const char* fmt = "%.3f") +{ + if (ImGui::GetIO().KeyShift) + speed *= 0.01f; + return ImGui::DragFloat(label, v, speed, lo, hi, fmt); +} + +static void helpMarker(const char* desc) +{ + ImGui::SameLine(); + ImGui::TextDisabled("(?)"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::TextUnformatted(desc); + ImGui::EndTooltip(); + } +} + +// ── Main draw ──────────────────────────────────────────────────────────────── +void UI::draw(AppState& state) +{ + ImGuiIO& io = ImGui::GetIO(); + float panelW = 340.f; + float panelH = (float)GetScreenHeight(); + + ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, 0), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(panelW, panelH), ImGuiCond_Always); + ImGui::Begin( + "Controls", + nullptr, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse); + + ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "LiDAR-Camera Calibration"); + ImGui::Separator(); + + // Alt = toggle Camera RGB ↔ Intensity (works anywhere in the window) + if (ImGui::IsKeyPressed(ImGuiKey_LeftAlt) || ImGui::IsKeyPressed(ImGuiKey_RightAlt)) + { + auto& cm = state.vizParams.colorMode; + if (cm == 3) + cm = 1; // RGB → Intensity + else + cm = 3; // anything → RGB + } + + panelStatus(state); + ImGui::Spacing(); + + if (ImGui::CollapsingHeader("Files", ImGuiTreeNodeFlags_DefaultOpen)) + panelFiles(state); + if (ImGui::CollapsingHeader("Intrinsics", ImGuiTreeNodeFlags_DefaultOpen)) + panelIntrinsics(state); + if (ImGui::CollapsingHeader("Extrinsics", ImGuiTreeNodeFlags_DefaultOpen)) + panelExtrinsics(state); + if (ImGui::CollapsingHeader("Visualization")) + panelVisualization(state); + + ImGui::End(); + + // ── Image view window (pan + zoom) ──────────────────────────────────── + if (state.renderer.imageTexValid) + { + float viewW = io.DisplaySize.x - panelW; + float viewH = io.DisplaySize.y * 0.5f; + ImGui::SetNextWindowPos(ImVec2(0, 0), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(viewW, viewH), ImGuiCond_Always); + ImGui::Begin( + "Image View", + nullptr, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoTitleBar); + 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. +void UI::drawImageView(AppState& state) +{ + const float imgW = (float)state.imageW; + const float imgH = (float)state.imageH; + if (imgW <= 0 || imgH <= 0) + return; + + // Reset view when a different image is loaded + if (state.imageW != viewImgW || state.imageH != viewImgH) + { + viewImgW = state.imageW; + viewImgH = state.imageH; + zoom2D = 1.f; + offX = offY = 0.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; + + ImVec2 imgScreenPos = ImVec2(origin.x + (avail.x - dispW) * 0.5f, origin.y + (avail.y - dispH) * 0.5f); + + 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); + + // ── input ────────────────────────────────────────────────────────────── + if (ImGui::IsWindowHovered()) + { + ImGuiIO& io = ImGui::GetIO(); + + if (io.MouseWheel != 0.f) + { + // 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; + } + + if (ImGui::IsMouseDragging(ImGuiMouseButton_Left)) + { + offX -= io.MouseDelta.x / scale; + offY -= io.MouseDelta.y / scale; + } + + if (ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Left)) + { + zoom2D = 1.f; + offX = offY = 0.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); +} + +// ── Files ──────────────────────────────────────────────────────────────────── +void UI::panelFiles(AppState& state) +{ + ImGui::PushItemWidth(-1); + + ImGui::Text("JPG image:"); + ImGui::InputText("##img", imagePathBuf, sizeof(imagePathBuf)); + if (ImGui::Button("Browse...##img", ImVec2(-1, 0))) + setBuf(imagePathBuf, sizeof(imagePathBuf), calib::fd::OpenFileDialogOneFile("Select camera image", calib::fd::ImageFilter)); + if (ImGui::Button("Load Image##btn", ImVec2(-1, 0))) + state.loadImage(imagePathBuf); + + ImGui::Spacing(); + ImGui::Text("LAZ/LAS point cloud:"); + ImGui::InputText("##laz", cloudPathBuf, sizeof(cloudPathBuf)); + if (ImGui::Button("Browse...##laz", ImVec2(-1, 0))) + setBuf(cloudPathBuf, sizeof(cloudPathBuf), calib::fd::OpenFileDialogOneFile("Select point cloud", calib::fd::LazFilter)); + { + float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; + if (ImGui::Button("Load##laz", ImVec2(hw, 0))) + state.loadCloud(cloudPathBuf); + ImGui::SameLine(); + if (ImGui::Button("Add##laz", ImVec2(hw, 0))) + state.addCloud(cloudPathBuf); + } + + ImGui::Spacing(); + ImGui::Text("Intrinsics JSON/YAML (optional):"); + ImGui::InputText("##intr", intrPathBuf, sizeof(intrPathBuf)); + if (ImGui::Button("Browse...##intr", ImVec2(-1, 0))) + setBuf(intrPathBuf, sizeof(intrPathBuf), calib::fd::OpenFileDialogOneFile("Select intrinsics file", calib::fd::IntrinsicsFilter)); + if (ImGui::Button("Load Intrinsics##btn", ImVec2(-1, 0))) + state.loadIntrinsics(intrPathBuf); + + ImGui::Separator(); + ImGui::Text("Calibration JSON:"); + ImGui::InputText("##save", savePath, sizeof(savePath)); + if (ImGui::Button("Browse...##calib", ImVec2(-1, 0))) + setBuf(savePath, sizeof(savePath), calib::fd::OpenFileDialogOneFile("Select calibration file", calib::fd::CalibJsonFilter)); + float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; + if (ImGui::Button("Load##calib", ImVec2(hw, 0))) + state.loadCalibration(savePath); + ImGui::SameLine(); + if (ImGui::Button("Save##calib", ImVec2(hw, 0))) + state.saveCalibration(savePath); + + ImGui::PopItemWidth(); +} + +// ── Intrinsics ──────────────────────────────────────────────────────────────── +void UI::panelIntrinsics(AppState& state) +{ + Intrinsics& K = state.intrinsics; + // Re-rectify only when an edit completes — remap on a full-res image + // is too slow to run on every drag tick. + bool edited = false; + auto drag = [&](const char* label, float* v, float speed, float lo, float hi, const char* fmt) + { + dragFloat(label, v, speed, lo, hi, fmt); + edited |= ImGui::IsItemDeactivatedAfterEdit(); + }; + + ImGui::PushItemWidth(-80.f); + drag("fx", &K.fx, 1.f, 1.f, 10000.f, "%.1f"); + drag("fy", &K.fy, 1.f, 1.f, 10000.f, "%.1f"); + drag("cx", &K.cx, 0.5f, 0.f, 10000.f, "%.1f"); + drag("cy", &K.cy, 0.5f, 0.f, 10000.f, "%.1f"); + ImGui::Separator(); + ImGui::Text("Radial (rational model):"); + drag("k1", &K.k1, 0.001f, -100.f, 100.f, "%.4f"); + drag("k2", &K.k2, 0.001f, -100.f, 100.f, "%.4f"); + drag("k3", &K.k3, 0.001f, -100.f, 100.f, "%.4f"); + drag("k4", &K.k4, 0.001f, -100.f, 100.f, "%.4f"); + drag("k5", &K.k5, 0.001f, -100.f, 100.f, "%.4f"); + drag("k6", &K.k6, 0.001f, -100.f, 100.f, "%.4f"); + ImGui::Text("Tangential:"); + drag("p1", &K.p1, 0.0001f, -1.f, 1.f, "%.5f"); + drag("p2", &K.p2, 0.0001f, -1.f, 1.f, "%.5f"); + helpMarker("Drag to adjust. Hold Ctrl+click to type a value."); + ImGui::PopItemWidth(); + + if (edited && state.intrinsicsLoaded) + state.rebuildImageTexture(); +} + +// ── Extrinsics ──────────────────────────────────────────────────────────────── +void UI::panelExtrinsics(AppState& state) +{ + Extrinsics& E = state.extrinsics; + + ImGui::PushItemWidth(-80.f); + + ImGui::Text("Camera position in world (m):"); + 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::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::Spacing(); + if (ImGui::Button("Reset Extrinsics", ImVec2(-1, 0))) + E = Extrinsics{}; + ImGui::PopItemWidth(); + + // Show current rotation matrix + if (ImGui::TreeNode("Rotation matrix")) + { + Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz); + for (int r = 0; r < 3; r++) + { + ImGui::Text("[ %6.3f %6.3f %6.3f ]", R(r, 0), R(r, 1), R(r, 2)); + } + ImGui::TreePop(); + } +} + +// ── Visualization ────────────────────────────────────────────────────────── +void UI::panelVisualization(AppState& state) +{ + VisualizationParams& vp = state.vizParams; + + // -140 (not -1): every widget here has a trailing label; -1 gives the + // slider/combo box the full row width and pushes the label off the + // right edge of the panel instead of leaving it room to draw. + ImGui::PushItemWidth(-140.f); + ImGui::SliderFloat("Point size", &vp.pointSize, 1.f, 20.f); + ImGui::SliderFloat("Depth min", &vp.depthMin, 0.f, vp.depthMax); + ImGui::SliderFloat("Depth max", &vp.depthMax, vp.depthMin + 0.1f, 200.f); + ImGui::SliderFloat("Opacity", &vp.opacity, 0.f, 1.f); + + const char* modes[] = { "Jet (depth)", "Jet (intensity)", "Jet (height)", "Camera RGB" }; + ImGui::Combo("Color mode", &vp.colorMode, modes, 4); + ImGui::PopItemWidth(); +} + +// ── Status bar ──────────────────────────────────────────────────────────────── +void UI::panelStatus(const AppState& state) +{ + if (!state.imagePath.empty()) + ImGui::TextColored(ImVec4(0, 1, 0, 1), "IMG: %s (%dx%d)", state.imagePath.c_str(), state.imageW, state.imageH); + else + ImGui::TextColored(ImVec4(1, 0.5f, 0, 1), "No image loaded"); + + if (!state.cloudPaths.empty()) + { + ImGui::TextColored(ImVec4(0, 1, 0, 1), "LAZ: %d file(s), %zu pts", (int)state.cloudPaths.size(), state.cloud.points.size()); + for (auto& p : state.cloudPaths) + ImGui::TextDisabled(" %s", p.c_str()); + } + else + { + ImGui::TextColored(ImVec4(1, 0.5f, 0, 1), "No point cloud loaded"); + } + + if (!state.statusMsg.empty()) + ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", state.statusMsg.c_str()); +} diff --git a/apps/camera_lidar_calibration/UI.h b/apps/camera_lidar_calibration/UI.h new file mode 100644 index 00000000..73bdf24e --- /dev/null +++ b/apps/camera_lidar_calibration/UI.h @@ -0,0 +1,32 @@ +#pragma once +#include +#include "Renderer.h" +#include +#include +#include + +struct AppState; + +class UI { +public: + // Called once per frame inside rlImGuiBegin()/rlImGuiEnd() + void draw(AppState& state); + +private: + char imagePathBuf[512] = {}; + char cloudPathBuf[512] = {}; + 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; + + void drawImageView(AppState& state); + void panelFiles(AppState& state); + void panelIntrinsics(AppState& state); + void panelExtrinsics(AppState& state); + void panelVisualization(AppState& state); + void panelStatus(const AppState& state); +}; diff --git a/apps/camera_lidar_calibration/main.cpp b/apps/camera_lidar_calibration/main.cpp new file mode 100644 index 00000000..0d4acd90 --- /dev/null +++ b/apps/camera_lidar_calibration/main.cpp @@ -0,0 +1,75 @@ +#include "App.h" +#include +#include +#include +#include +#include + +using namespace calib; +namespace fs = std::filesystem; + +static std::string ext(const std::string& path) { + auto pos = path.rfind('.'); + if (pos == std::string::npos) return ""; + std::string e = path.substr(pos + 1); + std::transform(e.begin(), e.end(), e.begin(), ::tolower); + return e; +} + +static bool isImage(const std::string& e) { + return e == "jpg" || e == "jpeg" || e == "png" || e == "bmp"; +} + +// Load each path by file type (clouds, images, intrinsics, calibration). +static void preloadByExt(App& app, const std::string& p) { + std::string e = ext(p); + if (isImage(e)) app.preloadImage(p.c_str()); + else if (e == "laz" || e == "las") app.preloadCloud(p.c_str()); + else if (e == "yml" || e == "yaml") app.preloadIntrinsics(p.c_str()); + else if (e == "json") app.preloadCalibration(p.c_str()); +} + +int main(int argc, char* argv[]) { + CliArgs args = parseArgs(argc, argv); + const std::vector usage = {cliopt::CAMERA_DIR, cliopt::LAZ, cliopt::CALIB}; + if (args.help) { + printUsage("CalibrationApp", "Camera/LiDAR calibration tool", usage); + return 0; + } + if (!args.valid) { + std::fprintf(stderr, "%s\n\n", args.error.c_str()); + printUsage("CalibrationApp", "Camera/LiDAR calibration tool", usage, /*toStderr=*/true); + return 1; + } + + App app; + + // --laz: one or more point clouds. + for (const auto& laz : args.getAll("laz")) + app.preloadCloud(laz.c_str()); + + // --calib: calibration json (intrinsic + extrinsic). + if (args.has("calib")) app.preloadCalibration(args.get("calib").c_str()); + + // --camera_dir: load the first image found in the directory. + if (args.has("camera_dir")) { + fs::path dir(args.get("camera_dir")); + if (fs::is_directory(dir)) { + std::vector imgs; + for (auto& e : fs::directory_iterator(dir)) + if (e.is_regular_file() && isImage(ext(e.path().filename().string()))) + imgs.push_back(e.path()); + std::sort(imgs.begin(), imgs.end()); + if (!imgs.empty()) app.preloadImage(imgs.front().string().c_str()); + } else if (fs::is_regular_file(dir)) { + app.preloadImage(dir.string().c_str()); + } + } + + // Positional files keep working by extension (drag-and-drop / shell glob). + for (const auto& p : args.positional) + preloadByExt(app, p); + + app.run(); + return 0; +} \ No newline at end of file diff --git a/apps/camera_lidar_intrinsics_calib/CMakeLists.txt b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt new file mode 100644 index 00000000..9f51bf54 --- /dev/null +++ b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(camera_lidar_intrinsics_calib) + +# Checkerboard-based camera intrinsic calibration (OpenCV rational distortion +# model). Ported from the sibling mandeye-colors project. Doesn't touch point +# clouds at all, so it only needs calib_core for CliArgs plus the same light +# raylib/imgui_raylib/rlimgui/OpenCV stack as camera_lidar_calibration -- no +# LASzip, no core_raylib. +add_executable(camera_lidar_intrinsics_calib + IntrinsicsCalib.cpp +) + +target_include_directories(camera_lidar_intrinsics_calib PRIVATE + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/json/include +) + +target_compile_definitions(camera_lidar_intrinsics_calib PRIVATE WITH_GUI=1) + +target_link_libraries(camera_lidar_intrinsics_calib PRIVATE + calib_core + raylib + imgui_raylib + rlimgui + ${OpenCV_LIBS} +) + +if(MSVC) + target_compile_options(camera_lidar_intrinsics_calib PRIVATE /W4) + target_compile_definitions(camera_lidar_intrinsics_calib PRIVATE _USE_MATH_DEFINES) +else() + target_compile_options(camera_lidar_intrinsics_calib PRIVATE -Wall -Wextra) +endif() + +if(WIN32) + add_custom_command( + TARGET camera_lidar_intrinsics_calib + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if(MSVC) + target_compile_options(camera_lidar_intrinsics_calib PRIVATE /bigobj) +endif() + +install(TARGETS camera_lidar_intrinsics_calib DESTINATION bin) diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp new file mode 100644 index 00000000..fd27dbaa --- /dev/null +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -0,0 +1,590 @@ +#include "imgui.h" +#include "raylib.h" +#include "rlImGui.h" +#include +#include +#include +#include +#include +#include +#include +#include +// findChessboardCorners/drawChessboardCorners and the CALIB_CB_* flags moved +// out of calib3d.hpp into objdetect.hpp in OpenCV 5. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace calib; +namespace fs = std::filesystem; + +// Copies `path` into `buf` (truncating to fit), for wiring a native-dialog +// result back into the same fixed-size char[] the matching text field edits. +static void setBuf(char* buf, size_t bufSize, const std::string& path) +{ + if (path.empty()) + return; + std::strncpy(buf, path.c_str(), bufSize - 1); + buf[bufSize - 1] = '\0'; +} + +// Shrinks/repositions the just-created window so it fits within the current +// monitor's usable area. Without this, a fixed-size window can be taller +// than the screen once the OS menu bar + title bar are accounted for, +// silently pushing the top of the window off-screen behind the menu bar +// instead of erroring or scrolling. +static void fitWindowToScreen() +{ + int monitor = GetCurrentMonitor(); + // GetMonitorWidth/Height return the monitor's native PIXEL resolution + // (GLFW's glfwGetVideoMode), while GetScreenWidth/Height, SetWindowSize + // and SetWindowPosition all operate in logical points -- on a 2x Retina + // display that's a 2x unit mismatch. Divide by the DPI scale to bring + // the monitor size into the same points space everything else uses; + // without this, SetWindowPosition computes an X centered on a monitor + // twice too wide, pushing most of the window off the right edge of the + // actual (points-sized) screen. + Vector2 dpi = GetWindowScaleDPI(); + if (dpi.x <= 0.f) + dpi.x = 1.f; + if (dpi.y <= 0.f) + dpi.y = 1.f; + int monW = (int)(GetMonitorWidth(monitor) / dpi.x); + int monH = (int)(GetMonitorHeight(monitor) / dpi.y); + if (monW <= 0 || monH <= 0) + return; // monitor info unavailable, leave as-is + + const int marginW = 40; // side breathing room + const int marginH = 100; // OS menu bar + window title bar headroom + + int w = std::min(GetScreenWidth(), monW - marginW); + int h = std::min(GetScreenHeight(), monH - marginH); + if (w != GetScreenWidth() || h != GetScreenHeight()) + SetWindowSize(w, h); + + SetWindowPosition(std::max(0, (monW - w) / 2), 30); + + // SetWindowSize/SetWindowPosition only update GLFW's window state; raylib's + // cached mouse/window geometry (what rlImGui reads into io.MousePos every + // frame) isn't refreshed until the next PollInputEvents(), which otherwise + // wouldn't happen until the first EndDrawing() -- after rlImGuiSetup() has + // already run. Without this, every click lands offset from the cursor by + // however far this function just moved/resized the window. + PollInputEvents(); +} + +// ── per-image state ─────────────────────────────────────────────────────────── +struct CalibImage +{ + std::string path; + cv::Mat rgb; + std::vector corners; + bool detected = false; + bool processed = false; +}; + +// ── application state ───────────────────────────────────────────────────────── +struct State +{ + // board parameters + int boardCols = 10; // inner corner count + int boardRows = 7; + float squareMm = 25.f; + + // loaded images + char dirBuf[512] = {}; + std::vector images; + int currentIdx = 0; + + // display texture (current image + drawn corners) + Texture2D tex = {}; + bool texOk = false; + int texIdx = -1; // which image is on GPU + + // calibration results + bool calibrated = false; + double rmsError = 0.0; + cv::Mat K, D; + cv::Size imageSize; + + // output + char outPath[512] = "intrinsics.json"; + std::string statusMsg; + + // background detection + std::thread detectThread; + std::atomic detectProgress{ -1 }; // -1=idle, [0,N)=index in progress, N=done + std::atomic detectStop{ false }; + int detectTotal = 0; + + bool isDetecting() const + { + int p = detectProgress.load(); + return p >= 0 && p < detectTotal; + } +}; + +// ── helpers ─────────────────────────────────────────────────────────────────── +static void loadDir(State& s) +{ + s.images.clear(); + s.calibrated = false; + s.currentIdx = 0; + s.texIdx = -1; + + fs::path dir(s.dirBuf); + if (!fs::is_directory(dir)) + { + s.statusMsg = "Not a directory: " + std::string(s.dirBuf); + return; + } + + const std::vector exts = { ".jpg", ".jpeg", ".png", ".bmp", ".tiff", ".tif" }; + std::vector paths; + for (auto& e : fs::directory_iterator(dir)) + { + if (!e.is_regular_file()) + continue; + std::string ext = e.path().extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + for (auto& x : exts) + if (ext == x) + { + paths.push_back(e.path().string()); + break; + } + } + std::sort(paths.begin(), paths.end()); + + for (auto& p : paths) + { + cv::Mat bgr = cv::imread(p, cv::IMREAD_COLOR); + if (bgr.empty()) + continue; + CalibImage ci; + ci.path = p; + cv::cvtColor(bgr, ci.rgb, cv::COLOR_BGR2RGB); + s.images.push_back(std::move(ci)); + } + s.statusMsg = "Loaded " + std::to_string(s.images.size()) + " images"; +} + +static void detectAll(State& s) +{ + if (s.isDetecting()) + return; // already running + if (s.images.empty()) + return; + + // reset processed flags so previous results are not stale + for (auto& ci : s.images) + ci.processed = false; + + s.detectTotal = (int)s.images.size(); + s.detectStop.store(false); + s.detectProgress.store(0); + s.statusMsg.clear(); + + // snapshot board params for the thread + int cols = s.boardCols, rows = s.boardRows; + + if (s.detectThread.joinable()) + s.detectThread.join(); + s.detectThread = std::thread( + [&s, cols, rows]() + { + cv::Size pat(cols, rows); + const int flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK; + // target width for detection — large enough to see corners, small enough to be fast + const float TARGET_W = 1500.f; + + for (int i = 0; i < (int)s.images.size(); i++) + { + if (s.detectStop.load()) + break; + s.detectProgress.store(i); + auto& ci = s.images[i]; + + cv::Mat gray; + cv::cvtColor(ci.rgb, gray, cv::COLOR_RGB2GRAY); + + // downsample for detection + float scale = (gray.cols > TARGET_W) ? TARGET_W / gray.cols : 1.f; + cv::Mat small; + if (scale < 1.f) + cv::resize(gray, small, cv::Size(), scale, scale, cv::INTER_AREA); + else + small = gray; + + bool found = cv::findChessboardCorners(small, pat, ci.corners, flags); + if (found) + { + // scale corners back to full resolution + if (scale < 1.f) + for (auto& pt : ci.corners) + pt *= (1.f / scale); + // subpix refinement on full-resolution image + // scale the search window proportionally to the image width + int win = std::max(11, (int)(11.f / scale) | 1); // keep odd + cv::cornerSubPix( + gray, + ci.corners, + cv::Size(win, win), + cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 50, 0.0001)); + } + ci.detected = found; + ci.processed = true; + } + s.detectProgress.store(s.detectTotal); + }); +} + +static void runCalibration(State& s) +{ + std::vector objPts; + objPts.reserve(s.boardCols * s.boardRows); + for (int r = 0; r < s.boardRows; r++) + for (int c = 0; c < s.boardCols; c++) + objPts.push_back(cv::Point3f(c * s.squareMm, r * s.squareMm, 0.f)); + + std::vector> allObj; + std::vector> allImg; + for (auto& ci : s.images) + { + if (!ci.detected) + continue; + allObj.push_back(objPts); + allImg.push_back(ci.corners); + s.imageSize = cv::Size(ci.rgb.cols, ci.rgb.rows); + } + + if ((int)allObj.size() < 4) + { + s.statusMsg = "Need at least 4 images with detected corners"; + return; + } + + s.K = cv::Mat::eye(3, 3, CV_64F); + s.D = cv::Mat::zeros(8, 1, CV_64F); + std::vector rvecs, tvecs; + + s.rmsError = cv::calibrateCamera(allObj, allImg, s.imageSize, s.K, s.D, rvecs, tvecs, cv::CALIB_RATIONAL_MODEL); + s.calibrated = true; + s.statusMsg = "RMS: " + std::to_string(s.rmsError).substr(0, 5) + " px (" + std::to_string(allObj.size()) + " images)"; +} + +static void saveJson(const State& s) +{ + if (!s.calibrated) + return; + double fx = s.K.at(0, 0); + double fy = s.K.at(1, 1); + double cx = s.K.at(0, 2); + double cy = s.K.at(1, 2); + // CALIB_RATIONAL_MODEL dist order: k1 k2 p1 p2 k3 k4 k5 k6 + auto d = [&](int i) + { + return i < s.D.rows ? s.D.at(i) : 0.0; + }; + + nlohmann::json j; + j["intrinsics"] = { { "fx", fx }, { "fy", fy }, { "cx", cx }, { "cy", cy }, { "k1", d(0) }, { "k2", d(1) }, + { "p1", d(2) }, { "p2", d(3) }, { "k3", d(4) }, { "k4", d(5) }, { "k5", d(6) }, { "k6", d(7) } }; + j["image_size"] = { s.imageSize.width, s.imageSize.height }; + j["rms_error"] = s.rmsError; + + std::ofstream f(s.outPath); + if (f) + f << j.dump(4); +} + +// Upload current image (with corners drawn) to a raylib texture. +static void refreshTex(State& s) +{ + if (s.images.empty()) + return; + s.currentIdx = std::max(0, std::min(s.currentIdx, (int)s.images.size() - 1)); + if (s.currentIdx == s.texIdx) + return; + + auto& ci = s.images[s.currentIdx]; + cv::Mat display = ci.rgb.clone(); + + if (ci.processed) + { + cv::Mat tmp; + cv::cvtColor(display, tmp, cv::COLOR_RGB2BGR); + cv::drawChessboardCorners(tmp, cv::Size(s.boardCols, s.boardRows), ci.corners, ci.detected); + cv::cvtColor(tmp, display, cv::COLOR_BGR2RGB); + } + + if (s.texOk) + UnloadTexture(s.tex); + Image img = {}; + img.data = display.data; + img.width = display.cols; + img.height = display.rows; + img.mipmaps = 1; + img.format = PIXELFORMAT_UNCOMPRESSED_R8G8B8; + s.tex = LoadTextureFromImage(img); + s.texOk = true; + s.texIdx = s.currentIdx; +} + +// ── entry point ─────────────────────────────────────────────────────────────── +int main(int argc, char* argv[]) +{ + CliArgs args = parseArgs(argc, argv); + if (args.help) + { + printUsage("IntrinsicsCalib", "Camera intrinsics calibration from a folder of images", { cliopt::CAMERA_DIR }); + return 0; + } + if (!args.valid) + { + std::fprintf(stderr, "%s\n\n", args.error.c_str()); + printUsage("IntrinsicsCalib", "Camera intrinsics calibration from a folder of images", { cliopt::CAMERA_DIR }, /*toStderr=*/true); + return 1; + } + + State state; + // --camera_dir, or the first positional, selects the image folder. + std::string dir = + args.has("camera_dir") ? args.get("camera_dir") : (!args.positional.empty() ? args.positional.front() : std::string{}); + if (!dir.empty()) + { + strncpy(state.dirBuf, dir.c_str(), sizeof(state.dirBuf) - 1); + loadDir(state); + } + + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + InitWindow(1280, 800, "Intrinsics Calibration"); + fitWindowToScreen(); + // PANEL_W below (330) is fixed; below this the image view and panel + // start overlapping instead of scrolling. + SetWindowMinSize(800, 500); + SetTargetFPS(60); + rlImGuiSetup(true); + + const float PANEL_W = 330.f; + + while (!WindowShouldClose()) + { + refreshTex(state); + + BeginDrawing(); + ClearBackground(Color{ 30, 30, 30, 255 }); + + // ── image view (left area) ──────────────────────────────────────────── + if (state.texOk) + { + float aw = GetScreenWidth() - PANEL_W; + float ah = GetScreenHeight(); + float sx = aw / state.tex.width; + float sy = ah / state.tex.height; + float sc = std::min(sx, sy); + float dw = state.tex.width * sc; + float dh = state.tex.height * sc; + DrawTexturePro( + state.tex, + { 0, 0, (float)state.tex.width, (float)state.tex.height }, + { (aw - dw) * 0.5f, (ah - dh) * 0.5f, dw, dh }, + { 0, 0 }, + 0.f, + WHITE); + } + + // ── ImGui panel (right) ─────────────────────────────────────────────── + rlImGuiBegin(); + ImGuiIO& io = ImGui::GetIO(); + ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - PANEL_W, 0), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(PANEL_W, io.DisplaySize.y), ImGuiCond_Always); + ImGui::Begin( + "Controls", + nullptr, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse); + + ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "Intrinsics Calibration"); + ImGui::Separator(); + + // ── board ───────────────────────────────────────────────────────────── + if (ImGui::CollapsingHeader("Checkerboard", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::PushItemWidth(-100.f); + ImGui::InputInt("Inner cols", &state.boardCols); + ImGui::InputInt("Inner rows", &state.boardRows); + ImGui::DragFloat("Square mm", &state.squareMm, 0.5f, 1.f, 500.f, "%.1f"); + state.boardCols = std::max(2, state.boardCols); + state.boardRows = std::max(2, state.boardRows); + ImGui::PopItemWidth(); + } + + // ── images ──────────────────────────────────────────────────────────── + if (ImGui::CollapsingHeader("Images", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::PushItemWidth(-1); + ImGui::Text("Image directory:"); + ImGui::InputText("##dir", state.dirBuf, sizeof(state.dirBuf)); + if (ImGui::Button("Browse...##dir", ImVec2(-1, 0))) + setBuf(state.dirBuf, sizeof(state.dirBuf), calib::fd::SelectFolder("Select checkerboard image directory")); + if (ImGui::Button("Load", ImVec2(-1, 0))) + loadDir(state); + ImGui::Text("%zu images", state.images.size()); + ImGui::PopItemWidth(); + + if (!state.images.empty()) + { + ImGui::Spacing(); + int n = (int)state.images.size(); + float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; + if (ImGui::Button("< Prev", ImVec2(hw, 0))) + { + state.currentIdx = (state.currentIdx - 1 + n) % n; + state.texIdx = -1; + } + ImGui::SameLine(); + if (ImGui::Button("Next >", ImVec2(hw, 0))) + { + state.currentIdx = (state.currentIdx + 1) % n; + state.texIdx = -1; + } + auto& ci = state.images[state.currentIdx]; + ImGui::Text("%d / %d %s", state.currentIdx + 1, n, fs::path(ci.path).filename().string().c_str()); + if (ci.processed) + { + if (ci.detected) + ImGui::TextColored(ImVec4(0, 1, 0, 1), "Corners found (%zu)", ci.corners.size()); + else + ImGui::TextColored(ImVec4(1, 0.3f, 0.3f, 1), "No corners detected"); + } + } + } + + // ── calibration ─────────────────────────────────────────────────────── + if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) + { + if (!state.images.empty()) + { + int prog = state.detectProgress.load(); + if (state.isDetecting()) + { + // show progress bar — button disabled + float frac = (float)prog / (float)state.detectTotal; + ImGui::ProgressBar(frac, ImVec2(-1, 0)); + ImGui::TextDisabled("Detecting %d / %d ...", prog, state.detectTotal); + state.texIdx = -1; // keep refreshing current image as it gets processed + } + else + { + // detection finished or not started — update status once + if (prog == state.detectTotal && state.detectTotal > 0) + { + int good2 = 0; + for (auto& ci : state.images) + if (ci.detected) + good2++; + state.statusMsg = "Detected: " + std::to_string(good2) + " / " + std::to_string(state.images.size()); + state.detectProgress.store(-1); // back to idle + if (state.detectThread.joinable()) + state.detectThread.join(); + } + if (ImGui::Button("Detect corners in all", ImVec2(-1, 0))) + detectAll(state); + } + } + + int good = 0; + for (auto& ci : state.images) + if (ci.detected) + good++; + if (!state.images.empty()) + ImGui::Text("Good images: %d / %zu", good, state.images.size()); + + if (good >= 4) + { + if (ImGui::Button("Run calibration", ImVec2(-1, 0))) + runCalibration(state); + } + } + + // ── results ─────────────────────────────────────────────────────────── + if (state.calibrated) + { + if (ImGui::CollapsingHeader("Results", ImGuiTreeNodeFlags_DefaultOpen)) + { + double fx = state.K.at(0, 0); + double fy = state.K.at(1, 1); + double cx = state.K.at(0, 2); + double cy = state.K.at(1, 2); + auto d = [&](int i) + { + return i < state.D.rows ? state.D.at(i) : 0.0; + }; + ImGui::Text("Image: %d x %d", state.imageSize.width, state.imageSize.height); + ImGui::Text("fx: %.2f", fx); + ImGui::Text("fy: %.2f", fy); + ImGui::Text("cx: %.2f", cx); + ImGui::Text("cy: %.2f", cy); + ImGui::Separator(); + ImGui::Text("k1: %.5f", d(0)); + ImGui::Text("k2: %.5f", d(1)); + ImGui::Text("p1: %.5f", d(2)); + ImGui::Text("p2: %.5f", d(3)); + ImGui::Text("k3: %.5f", d(4)); + ImGui::Text("k4: %.5f", d(5)); + ImGui::Text("k5: %.5f", d(6)); + ImGui::Text("k6: %.5f", d(7)); + ImGui::Separator(); + ImGui::TextColored( + state.rmsError < 1.0 ? ImVec4(0, 1, 0, 1) : ImVec4(1, 0.6f, 0, 1), "RMS reprojection: %.4f px", state.rmsError); + ImGui::Spacing(); + ImGui::PushItemWidth(-1); + ImGui::InputText("##out", state.outPath, sizeof(state.outPath)); + if (ImGui::Button("Browse...##out", ImVec2(-1, 0))) + { + std::string defaultName = fs::path(state.outPath).filename().string(); + setBuf( + state.outPath, + sizeof(state.outPath), + calib::fd::SaveFileDialog("Save intrinsics JSON", calib::fd::CalibJsonFilter, ".json", defaultName)); + } + if (ImGui::Button("Save JSON", ImVec2(-1, 0))) + { + saveJson(state); + state.statusMsg = std::string("Saved: ") + state.outPath; + } + ImGui::PopItemWidth(); + } + } + + // ── status ──────────────────────────────────────────────────────────── + if (!state.statusMsg.empty()) + { + ImGui::Separator(); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", state.statusMsg.c_str()); + } + + ImGui::End(); + rlImGuiEnd(); + EndDrawing(); + } + + // stop background detection if still running + state.detectStop.store(true); + if (state.detectThread.joinable()) + state.detectThread.join(); + + if (state.texOk) + UnloadTexture(state.tex); + rlImGuiShutdown(); + CloseWindow(); + return 0; +} \ No newline at end of file diff --git a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt new file mode 100644 index 00000000..5f8125ea --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(camera_lidar_trajectory_viewer) + +# Multi-camera trajectory/point-cloud viewer with LAZ export, plus optional +# COLMAP sparse-model and ROS 2 bag export. Ported from the sibling +# mandeye-colors project (see calib_core/CMakeLists.txt for the shared +# non-GUI logic). +# +# NOTE on rendering: this app's core feature is per-point "which camera +# colored this point" RGB assignment plus a per-camera isolation toggle +# (see the kFS fragment shader's colorCameraId/selectedCamera uniform and +# TrajectoryViewer.cpp's isolateCamera option) over a single merged point +# buffer built from all trajectory chunks. core's ScanRenderer +# (core/include/Core/raylib_render.hpp, used by core_raylib) is a per-scan +# renderer -- Flat/Intensity/Elevation/Distance color modes with one pose +# per scan -- with no equivalent for that per-point camera-source +# attribute, which is load-bearing here, not cosmetic. Re-fitting it would +# mean either dropping that feature or extending core's shared renderer +# contract (also used by multi_view_tls_registration) for a need specific +# to this app, so this app keeps its own GPU shader code (like +# camera_lidar_calibration's Renderer.cpp) and links raylib/imgui_raylib/ +# rlimgui directly rather than core_raylib. +add_executable(camera_lidar_trajectory_viewer + TrajectoryViewer.cpp + RosExport.h RosExport.cpp +) + +target_include_directories(camera_lidar_trajectory_viewer PRIVATE + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + # laszip_api_version.h is generated at configure time into LASzip's own + # binary dir (see calib_core/CMakeLists.txt's matching comment) -- this + # app includes directly, same as calib_core's + # PointCloud.cpp. + ${CMAKE_BINARY_DIR}/3rdparty/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include +) + +target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE WITH_GUI=1) + +target_link_libraries(camera_lidar_trajectory_viewer PRIVATE + calib_core + raylib + imgui_raylib + rlimgui + ${OpenCV_LIBS} + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} +) + +if(MSVC) + target_compile_options(camera_lidar_trajectory_viewer PRIVATE /W4) + target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE _USE_MATH_DEFINES LASZIP_API_VERSION) +else() + target_compile_options(camera_lidar_trajectory_viewer PRIVATE -Wall -Wextra) + target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE LASZIP_API_VERSION) +endif() + +# ── Optional ROS 2 bag export ───────────────────────────────────────────────── +# OFF by default so the project still builds on machines without ROS. Enable with +# cmake -DCALIB_ENABLE_ROS_EXPORT=ON (source a ROS 2 install first). +option(CALIB_ENABLE_ROS_EXPORT "Enable ROS 2 bag export in camera_lidar_trajectory_viewer (needs ROS 2)" OFF) +if(CALIB_ENABLE_ROS_EXPORT) + find_package(rclcpp REQUIRED) + find_package(rosbag2_cpp REQUIRED) + find_package(rosbag2_storage REQUIRED) + find_package(builtin_interfaces REQUIRED) + find_package(std_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(tf2_msgs REQUIRED) + # Link the exported targets with the keyword (PRIVATE) form so it matches the + # rest of this target's link calls (ament_target_dependencies uses the plain + # form, which CMake forbids mixing). + target_link_libraries(camera_lidar_trajectory_viewer PRIVATE + rclcpp::rclcpp + rosbag2_cpp::rosbag2_cpp + rosbag2_storage::rosbag2_storage + ${builtin_interfaces_TARGETS} + ${std_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${sensor_msgs_TARGETS} + ${tf2_msgs_TARGETS}) + target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE CALIB_ENABLE_ROS_EXPORT) + message(STATUS "camera_lidar_trajectory_viewer ROS 2 bag export: ENABLED") +else() + message(STATUS "camera_lidar_trajectory_viewer ROS 2 bag export: disabled (set -DCALIB_ENABLE_ROS_EXPORT=ON to enable)") +endif() + +if(WIN32) + add_custom_command( + TARGET camera_lidar_trajectory_viewer + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if(MSVC) + target_compile_options(camera_lidar_trajectory_viewer PRIVATE /bigobj) +endif() + +install(TARGETS camera_lidar_trajectory_viewer DESTINATION bin) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp new file mode 100644 index 00000000..9335fcd4 --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -0,0 +1,366 @@ +#include "RosExport.h" + +#ifndef CALIB_ENABLE_ROS_EXPORT +// ── Non-ROS build: provide a stub so the viewer always links. ───────────────── +bool exportRos2Bag(const RosExportInput&, const RosExportOptions&, std::string& status) { + status = "ROS export not available: built without CALIB_ENABLE_ROS_EXPORT"; + return false; +} +#else + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "PointCloud.h" + +#include +#include +#include +#include + +namespace { + +constexpr char kTopicTfStatic[] = "/tf_static"; +constexpr char kTopicTf[] = "/tf"; +constexpr char kTopicImgCompressed[]= "/camera/image_raw/compressed"; +constexpr char kTopicImgRaw[] = "/camera/image_raw"; +constexpr char kTopicCamInfo[] = "/camera/camera_info"; +constexpr char kTopicLidarUndist[] = "/lidar/points_undistorted"; +constexpr char kTopicLidarRaw[] = "/lidar/points_raw"; + +builtin_interfaces::msg::Time toRosTime(int64_t ns) { + builtin_interfaces::msg::Time t; + t.sec = static_cast(ns / 1000000000LL); + t.nanosec = static_cast(ns % 1000000000LL); + return t; +} + +geometry_msgs::msg::Transform toTransform(const Eigen::Affine3f& T) { + geometry_msgs::msg::Transform tf; + tf.translation.x = T.translation().x(); + tf.translation.y = T.translation().y(); + tf.translation.z = T.translation().z(); + Eigen::Quaternionf q(T.linear()); + q.normalize(); + tf.rotation.x = q.x(); + tf.rotation.y = q.y(); + tf.rotation.z = q.z(); + tf.rotation.w = q.w(); + return tf; +} + +// Build an xyz+intensity PointCloud2 over a slice of float quads [x,y,z,i]*n. +sensor_msgs::msg::PointCloud2 makeCloud(const std::string& frame, int64_t stampNs, + const std::vector& xyzi) { + using PF = sensor_msgs::msg::PointField; + sensor_msgs::msg::PointCloud2 pc; + pc.header.stamp = toRosTime(stampNs); + pc.header.frame_id = frame; + + const uint32_t n = static_cast(xyzi.size() / 4); + const char* names[4] = {"x", "y", "z", "intensity"}; + for (int k = 0; k < 4; ++k) { + PF f; + f.name = names[k]; + f.offset = static_cast(k * sizeof(float)); + f.datatype = PF::FLOAT32; + f.count = 1; + pc.fields.push_back(f); + } + pc.height = 1; + pc.width = n; + pc.is_bigendian = false; + pc.is_dense = true; + pc.point_step = 4 * sizeof(float); + pc.row_step = pc.point_step * n; + pc.data.resize(static_cast(pc.row_step)); + std::memcpy(pc.data.data(), xyzi.data(), pc.data.size()); + return pc; +} + +struct RawPt { int64_t ts; float x, y, z, intensity; }; + +} // namespace + +bool exportRos2Bag(const RosExportInput& in, + const RosExportOptions& opt, + std::string& status) { + const int step = std::max(1, opt.lidarDecim); + const bool haveTraj = !in.traj.poses.empty(); + + bool wantRaw = opt.exportLidarRaw; + if (wantRaw && !haveTraj) wantRaw = false; // need poses to undo motion + + rosbag2_storage::StorageOptions so; + so.uri = opt.outUri; + so.storage_id = opt.storageId; + rosbag2_cpp::ConverterOptions co; + co.input_serialization_format = "cdr"; + co.output_serialization_format = "cdr"; + + rosbag2_cpp::Writer writer; + try { + writer.open(so, co); + } catch (const std::exception& e) { + status = std::string("Failed to open bag '") + opt.outUri + "': " + e.what(); + return false; + } + + // Earliest timestamp in the dataset → stamp for the static transform. + int64_t startTs = 0; + if (haveTraj) startTs = in.traj.poses.front().ts_ns; + else if (!in.imageFiles.empty()) startTs = in.imageFiles.begin()->first; + + size_t nTf = 0, nImg = 0, nCloud = 0; + std::fprintf(stderr, "[RosExport] writing bag '%s' (%s)\n", + opt.outUri.c_str(), opt.storageId.c_str()); + + try { + // ── /tf_static : lidar -> camera (from extrinsics) ──────────────────── + if (opt.exportTf && in.calibLoaded) { + // Pre-create the topic with TRANSIENT_LOCAL durability (matching the + // standard static_transform_broadcaster) so tf listeners joining late + // still receive it; otherwise it is offered as VOLATILE and rejected. + rosbag2_storage::TopicMetadata tm; + tm.name = kTopicTfStatic; + tm.type = "tf2_msgs/msg/TFMessage"; + tm.serialization_format = "cdr"; + tm.offered_qos_profiles = { rclcpp::QoS(1).transient_local() }; + 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.translation() = Eigen::Vector3f(in.E.tx, in.E.ty, in.E.tz); + + geometry_msgs::msg::TransformStamped ts; + ts.header.stamp = toRosTime(startTs); + ts.header.frame_id = in.lidarFrame; + ts.child_frame_id = in.cameraFrame; + ts.transform = toTransform(T_lc); + + tf2_msgs::msg::TFMessage m; + m.transforms.push_back(ts); + writer.write(m, kTopicTfStatic, rclcpp::Time(startTs)); + } + + // ── /tf : map -> lidar, one message per trajectory pose ─────────────── + if (opt.exportTf && haveTraj) { + for (const auto& p : in.traj.poses) { + geometry_msgs::msg::TransformStamped ts; + ts.header.stamp = toRosTime(p.ts_ns); + ts.header.frame_id = in.mapFrame; + ts.child_frame_id = in.lidarFrame; + ts.transform = toTransform(p.T); + + tf2_msgs::msg::TFMessage m; + m.transforms.push_back(ts); + writer.write(m, kTopicTf, rclcpp::Time(p.ts_ns)); + ++nTf; + } + } + + std::fprintf(stderr, "[RosExport] tf: %zu transforms\n", nTf); + + // ── camera images (+ camera_info) ───────────────────────────────────── + if (opt.exportCamera && !in.imageFiles.empty()) { + // Rectification maps (built lazily once the image size is known). + // Mirrors App.cpp: undistort to the same K so that a pinhole + // projection — which is all RViz uses — lines up with the image. + const cv::Mat Km = (cv::Mat_(3, 3) << + in.K.fx, 0, in.K.cx, + 0, in.K.fy, in.K.cy, + 0, 0, 1); + const cv::Mat Dm = (cv::Mat_(1, 8) << + in.K.k1, in.K.k2, in.K.p1, in.K.p2, + in.K.k3, in.K.k4, in.K.k5, in.K.k6); + cv::Mat map1, map2; + bool mapsReady = false; + int camW = 0, camH = 0; + const bool rectify = opt.undistortCamera && in.calibLoaded; + // Original jpeg bytes can be copied verbatim only when we neither + // rectify nor need to re-encode (compressed + no undistort). + const bool copyJpegBytes = opt.compressCamera && !rectify; + + for (const auto& [ts, path] : in.imageFiles) { + std::vector outBytes; // jpeg, when compressed + cv::Mat outImg; // bgr8, when raw + + if (copyJpegBytes) { + std::ifstream f(path, std::ios::binary); + if (!f) continue; + outBytes.assign(std::istreambuf_iterator(f), + std::istreambuf_iterator()); + if (outBytes.empty()) continue; + } else { + cv::Mat bgr = cv::imread(path, cv::IMREAD_COLOR); + if (bgr.empty()) continue; + if (rectify) { + if (!mapsReady) { + cv::initUndistortRectifyMap(Km, Dm, cv::noArray(), Km, + bgr.size(), CV_16SC2, map1, map2); + mapsReady = true; + } + cv::Mat und; + cv::remap(bgr, und, map1, map2, cv::INTER_LINEAR); + bgr = und; + } + camW = bgr.cols; camH = bgr.rows; + if (opt.compressCamera) { + cv::imencode(".jpg", bgr, outBytes); + } else { + if (!bgr.isContinuous()) bgr = bgr.clone(); + outImg = bgr; + } + } + + if (opt.compressCamera) { + sensor_msgs::msg::CompressedImage img; + img.header.stamp = toRosTime(ts); + img.header.frame_id = in.cameraFrame; + img.format = "jpeg"; + img.data = std::move(outBytes); + writer.write(img, kTopicImgCompressed, rclcpp::Time(ts)); + } else { + sensor_msgs::msg::Image img; + img.header.stamp = toRosTime(ts); + img.header.frame_id = in.cameraFrame; + img.height = static_cast(outImg.rows); + img.width = static_cast(outImg.cols); + img.encoding = "bgr8"; + img.is_bigendian = 0; + img.step = static_cast(outImg.cols * 3); + img.data.assign(outImg.datastart, outImg.dataend); + writer.write(img, kTopicImgRaw, rclcpp::Time(ts)); + } + ++nImg; + + // CameraInfo alongside, once we know the resolution. + if (in.calibLoaded) { + if (camW == 0) { // copy-bytes path: peek dimensions once + cv::Mat probe = cv::imread(path, cv::IMREAD_COLOR); + if (!probe.empty()) { camW = probe.cols; camH = probe.rows; } + } + if (camW > 0) { + sensor_msgs::msg::CameraInfo ci; + ci.header.stamp = toRosTime(ts); + ci.header.frame_id = in.cameraFrame; + ci.height = static_cast(camH); + ci.width = static_cast(camW); + ci.distortion_model = "rational_polynomial"; + if (rectify) // image already rectified → no distortion + ci.d = {0, 0, 0, 0, 0, 0, 0, 0}; + else + ci.d = {in.K.k1, in.K.k2, in.K.p1, in.K.p2, + in.K.k3, in.K.k4, in.K.k5, in.K.k6}; + ci.k = {in.K.fx, 0.f, in.K.cx, + 0.f, in.K.fy, in.K.cy, + 0.f, 0.f, 1.f}; + ci.r = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + ci.p = {in.K.fx, 0.f, in.K.cx, 0.f, + 0.f, in.K.fy, in.K.cy, 0.f, + 0.f, 0.f, 1.f, 0.f}; + writer.write(ci, kTopicCamInfo, rclcpp::Time(ts)); + } + } + } + } + + std::fprintf(stderr, "[RosExport] camera: %zu images\n", nImg); + + // ── LiDAR : load chunks → map-frame points → time-windowed clouds ───── + if ((opt.exportLidarUndistorted || wantRaw) && !in.lidarChunks.empty()) { + std::vector pts; + for (const auto& ch : in.lidarChunks) { + PointCloud pc; + if (!pc.load(ch.lazPath)) continue; + for (size_t i = 0; i < pc.points.size(); i += step) { + const auto& p = pc.points[i]; + Eigen::Vector3f pw(p.x, p.y, p.z); + if (ch.hasM) pw = ch.M * pw; + pts.push_back({p.ts_ns, pw.x(), pw.y(), pw.z(), p.intensity}); + } + } + + if (!pts.empty()) { + std::sort(pts.begin(), pts.end(), + [](const RawPt& a, const RawPt& b) { return a.ts < b.ts; }); + const int64_t aggNs = std::max(1, (int64_t)(opt.aggregationSec * 1e9)); + const int64_t t0 = pts.front().ts; + std::fprintf(stderr, "[RosExport] lidar: %zu points, span %.2f s, window %.3f s\n", + pts.size(), (pts.back().ts - t0) / 1e9, opt.aggregationSec); + + // cache for the raw (sensor-frame) re-projection + const TrajPose* lastPose = nullptr; + Eigen::Affine3f lastInv = Eigen::Affine3f::Identity(); + + size_t i = 0; + while (i < pts.size()) { + const int64_t w = (pts[i].ts - t0) / aggNs; + const int64_t winStamp = t0 + w * aggNs; + size_t j = i; + while (j < pts.size() && (pts[j].ts - t0) / aggNs == w) ++j; + + if (opt.exportLidarUndistorted) { + std::vector buf; + buf.reserve((j - i) * 4); + for (size_t k = i; k < j; ++k) { + buf.push_back(pts[k].x); buf.push_back(pts[k].y); + buf.push_back(pts[k].z); buf.push_back(pts[k].intensity); + } + writer.write(makeCloud(in.mapFrame, winStamp, buf), + kTopicLidarUndist, rclcpp::Time(winStamp)); + ++nCloud; + } + if (wantRaw) { + std::vector buf; + buf.reserve((j - i) * 4); + for (size_t k = i; k < j; ++k) { + const TrajPose* p = in.traj.nearest(pts[k].ts); + if (p != lastPose) { lastPose = p; lastInv = p->T.inverse(); } + Eigen::Vector3f pl = lastInv * Eigen::Vector3f(pts[k].x, pts[k].y, pts[k].z); + buf.push_back(pl.x()); buf.push_back(pl.y()); + buf.push_back(pl.z()); buf.push_back(pts[k].intensity); + } + writer.write(makeCloud(in.lidarFrame, winStamp, buf), + kTopicLidarRaw, rclcpp::Time(winStamp)); + ++nCloud; + } + i = j; + } + } + } + } catch (const std::exception& e) { + status = std::string("Export failed while writing: ") + e.what(); + return false; + } + + writer.close(); + std::fprintf(stderr, "[RosExport] done: %zu tf, %zu img, %zu clouds\n", nTf, nImg, nCloud); + status = "Wrote bag '" + opt.outUri + "' (" + opt.storageId + "): " + + std::to_string(nTf) + " tf, " + + std::to_string(nImg) + " img, " + + std::to_string(nCloud) + " clouds" + + (opt.exportLidarRaw && !haveTraj ? " [raw skipped: no trajectory]" : ""); + return true; +} + +#endif // CALIB_ENABLE_ROS_EXPORT \ No newline at end of file diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.h b/apps/camera_lidar_trajectory_viewer/RosExport.h new file mode 100644 index 00000000..82518bb1 --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/RosExport.h @@ -0,0 +1,78 @@ +#pragma once +// +// Optional ROS 2 bag export for the Trajectory Viewer. +// +// This header is ROS-free on purpose: it only describes *what* to export so the +// viewer (and any other non-ROS translation unit) can include it unconditionally. +// The implementation in RosExport.cpp is the only place that pulls in rclcpp / +// rosbag2_cpp, and it is compiled only when CALIB_ENABLE_ROS_EXPORT is defined. +// +#include +#include +#include +#include +#include + +#include // Intrinsics, Extrinsics +#include // Trajectory, TrajPose + +using namespace calib; + +// Everything the exporter needs, gathered by the viewer. Plain data only. +struct RosExportInput { + // Frame names used in the bag. + std::string mapFrame = "map"; + std::string lidarFrame = "lidar"; + std::string cameraFrame = "camera"; + + // Trajectory of T_map_lidar poses (timestamps in nanoseconds, shared clock). + Trajectory traj; + + // Camera images, keyed by timestamp (ns) -> .jpg path. The map is inherently + // ordered by timestamp, so it doubles as the sorted list of image stamps. + std::map imageFiles; + bool calibLoaded = false; + Intrinsics K; + Extrinsics E; + + // 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. + struct Chunk { + std::string lazPath; + Eigen::Affine3f M = Eigen::Affine3f::Identity(); + bool hasM = false; + }; + std::vector lidarChunks; +}; + +struct RosExportOptions { + std::string outUri = "ros2_export"; // output bag directory (rosbag2 uri) + std::string storageId = "mcap"; // "mcap" or "sqlite3" + + bool exportTf = true; // /tf (dynamic) + /tf_static + bool exportCamera = true; // /camera/image_raw[/compressed] + /camera/camera_info + bool compressCamera = true; // true: CompressedImage (jpeg) ; false: raw Image (bgr8) + // Rectify (undistort) images to the pinhole model before writing. Needed for + // RViz-style overlays, which project with the pinhole P and ignore the + // distortion coefficients. When on, CameraInfo is published with zero D. + bool undistortCamera = true; + + // LiDAR can be exported in two flavours, independently: + // - undistorted: points as registered by LIO, in the map frame (already + // motion-compensated). Topic /lidar/points_undistorted, frame_id = map. + // - raw: points re-projected into the sensor frame at each point's stamp via + // the inverse trajectory pose (re-introduces scan motion). Topic + // /lidar/points_raw, frame_id = lidar, positioned live by /tf. + bool exportLidarUndistorted = true; + bool exportLidarRaw = false; + + double aggregationSec = 0.1; // LiDAR points grouped into windows of this length + int lidarDecim = 1; // keep every Nth point (>=1) +}; + +// Writes the bag. Returns true on success; `status` always gets a human-readable +// summary (or the error). Safe to call only when built with CALIB_ENABLE_ROS_EXPORT; +// otherwise a stub returns false explaining the build is non-ROS. +bool exportRos2Bag(const RosExportInput& in, + const RosExportOptions& opt, + std::string& status); \ No newline at end of file diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp new file mode 100644 index 00000000..72ccff29 --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -0,0 +1,1884 @@ +#include "RosExport.h" +#include "external/glad.h" +#include "imgui.h" +#include "raylib.h" +#include "raymath.h" +#include "rlImGui.h" +#include "rlgl.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace calib; +namespace fs = std::filesystem; + +// Copies `path` into `buf` (truncating to fit), for wiring a native-dialog +// result back into the same fixed-size char[] the matching text field edits. +static void setBuf(char* buf, size_t bufSize, const std::string& path) +{ + if (path.empty()) + return; + std::strncpy(buf, path.c_str(), bufSize - 1); + buf[bufSize - 1] = '\0'; +} + +// Shrinks/repositions the just-created window so it fits within the current +// monitor's usable area. Without this, a fixed 1400x900 window can be taller +// than the screen once the OS menu bar + title bar are accounted for (e.g. a +// 956pt-tall MacBook display leaves ~0 spare px at H=900), silently pushing +// the top of the window (and the first Session panel controls) off-screen +// behind the menu bar instead of erroring or scrolling. +static void fitWindowToScreen() +{ + int monitor = GetCurrentMonitor(); + // GetMonitorWidth/Height return the monitor's native PIXEL resolution + // (GLFW's glfwGetVideoMode), while GetScreenWidth/Height, SetWindowSize + // and SetWindowPosition all operate in logical points -- on a 2x Retina + // display that's a 2x unit mismatch. Divide by the DPI scale to bring + // the monitor size into the same points space everything else uses; + // without this, SetWindowPosition computes an X centered on a monitor + // twice too wide, pushing most of the window off the right edge of the + // actual (points-sized) screen. + Vector2 dpi = GetWindowScaleDPI(); + if (dpi.x <= 0.f) + dpi.x = 1.f; + if (dpi.y <= 0.f) + dpi.y = 1.f; + int monW = (int)(GetMonitorWidth(monitor) / dpi.x); + int monH = (int)(GetMonitorHeight(monitor) / dpi.y); + if (monW <= 0 || monH <= 0) + return; // monitor info unavailable, leave as-is + + const int marginW = 40; // side breathing room + const int marginH = 100; // OS menu bar + window title bar headroom + + int w = std::min(GetScreenWidth(), monW - marginW); + int h = std::min(GetScreenHeight(), monH - marginH); + if (w != GetScreenWidth() || h != GetScreenHeight()) + SetWindowSize(w, h); + + SetWindowPosition(std::max(0, (monW - w) / 2), 30); + + // SetWindowSize/SetWindowPosition only update GLFW's window state; raylib's + // cached mouse/window geometry (what rlImGui reads into io.MousePos every + // frame) isn't refreshed until the next PollInputEvents(), which otherwise + // wouldn't happen until the first EndDrawing() -- after rlImGuiSetup() has + // already run. Without this, every click lands offset from the cursor by + // however far this function just moved/resized the window. + PollInputEvents(); +} + +Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time) +{ + Eigen::Matrix4d ret(Eigen::Matrix4d::Zero()); + auto it_lower = trajectory.lower_bound(query_time); + auto it_next = it_lower; + + if (it_lower == trajectory.begin()) + { + return ret; + } + if (it_lower->first > query_time) + { + it_lower = std::prev(it_lower); + } + if (it_lower == trajectory.begin()) + { + return ret; + } + if (it_lower == trajectory.end()) + { + return ret; + } + + double t1 = it_lower->first; + double t2 = it_next->first; + double difft1 = t1 - query_time; + double difft2 = t2 - query_time; + if (t1 == t2 && std::fabs(difft1) < 0.1) + { + ret = Eigen::Matrix4d::Identity(); + ret.col(3).head<3>() = it_next->second.col(3).head<3>(); + ret.topLeftCorner(3, 3) = it_lower->second.topLeftCorner(3, 3); + return ret; + } + + // if (std::fabs(difft1) < 0.15 && std::fabs(difft2) < 0.15) + { + assert(t2 > t1); + assert(query_time > t1); + assert(query_time < t2); + ret = Eigen::Matrix4d::Identity(); + double res = (query_time - t1) / (t2 - t1); + Eigen::Vector3d diff = it_next->second.col(3).head<3>() - it_lower->second.col(3).head<3>(); + ret.col(3).head<3>() = it_next->second.col(3).head<3>() + diff * res; + Eigen::Matrix3d r1 = it_lower->second.topLeftCorner(3, 3).matrix(); + Eigen::Matrix3d r2 = it_next->second.topLeftCorner(3, 3).matrix(); + Eigen::Quaterniond q1(r1); + Eigen::Quaterniond q2(r2); + Eigen::Quaterniond qt = q1.slerp(res, q2); + ret.topLeftCorner(3, 3) = qt.toRotationMatrix(); + return ret; + } + + return ret; +} + +// Build a time(seconds) -> T_world_lidar map suitable for getInterpolatedPose(). +static std::map buildTrajMap(const Trajectory& traj) +{ + std::map m; + for (const auto& p : traj.poses) + m[p.ts_ns * 1e-9] = p.T.matrix().cast(); + return m; +} + +// Interpolated T_world_lidar at ts_ns. Returns false when ts_ns lies outside the +// trajectory range — getInterpolatedPose() signals that with a zero matrix. +static bool interpPose(const std::map& trajMap, int64_t ts_ns, Eigen::Affine3f& out) +{ + Eigen::Matrix4d T = getInterpolatedPose(trajMap, ts_ns * 1e-9); + if (T(3, 3) == 0.0) + return false; + out.matrix() = T.cast(); + return true; +} + +// ── GPU point cloud shader ──────────────────────────────────────────────────── +// colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI +static const char* kVS = R"( +#version 330 +layout(location = 0) in vec3 pos; +layout(location = 1) in float colorPacked; +layout(location = 2) in float lidarIntensity; +layout(location = 3) in float colorCameraId; // global image index that colored this point, or -1 +layout(location = 4) in float inRoi; // 1=inside ROI, 0=outside ROI, -1=projects into no image +uniform mat4 mvp; +uniform float pointSize; +uniform int drawDecim; +out float fragIntensity; +out vec4 vertColor; +flat out float fragColorCameraId; +flat out float fragInRoi; +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; + } + gl_Position = mvp * vec4(pos, 1.0); + gl_PointSize = pointSize; + uint p = floatBitsToUint(colorPacked); + float r = float((p >> 16) & 0xFFu) / 255.0; + float g = float((p >> 8) & 0xFFu) / 255.0; + float b = float( p & 0xFFu) / 255.0; + fragIntensity = lidarIntensity; + vertColor = vec4(r, g, b, 1.0); + fragColorCameraId = colorCameraId; + fragInRoi = inRoi; +} +)"; +static const char* kFS = R"( +#version 330 +in float fragIntensity; +in vec4 vertColor; +flat in float fragColorCameraId; +flat in float fragInRoi; +uniform int colorMode; +uniform int selectedCamera; // -1 = show all, else keep only points from this image +out vec4 finalColor; +vec3 jet(float t) { + t = clamp(t, 0.0, 1.0); + return clamp(vec3(1.5 - abs(4.0*t - 3.0), + 1.5 - abs(4.0*t - 2.0), + 1.5 - abs(4.0*t - 1.0)), 0.0, 1.0); +} +vec3 hsv2rgb(vec3 c) { + vec4 K = vec4(1.0, 2.0/3.0, 1.0/3.0, 3.0); + vec3 p = abs(fract(c.xxx + K.xyz) * 6.0 - K.www); + return c.z * mix(K.xxx, clamp(p - K.xxx, 0.0, 1.0), c.y); +} +// deterministic, well-spread color per integer camera id +vec3 idColor(float idf) { + float id = floor(idf + 0.5); + float hue = fract(id * 0.61803398875); // golden ratio + return hsv2rgb(vec3(hue, 0.85, 1.0)); +} +void main() { + if (selectedCamera >= 0) + { + if (selectedCamera != int(fragColorCameraId)) + discard; // do not draw + } + + if (colorMode == 1) + { + if (fragColorCameraId < 0.0) + discard; // not colored by any image — draw only colored points + finalColor = vertColor; + } + else if (colorMode == 2) + { + if (fragColorCameraId < 0.0) + discard; // not colored by any image — draw only colored points + finalColor = vec4(idColor(fragColorCameraId), 1.0); + } + else if (colorMode == 3) + { + // ROI membership: green = inside ROI, red = projects into an image but + // outside ROI, dim gray = projects into no image (spatial context). + if (fragInRoi < 0.0) + finalColor = vec4(0.28, 0.28, 0.28, 1.0); + else + finalColor = (fragInRoi > 0.5) ? vec4(0.15, 0.9, 0.2, 1.0) + : vec4(0.9, 0.15, 0.15, 1.0); + } + else finalColor = vec4(jet(fragIntensity), 1.0); +} +)"; + +struct GpuCloud +{ + unsigned int vao = 0, vbo = 0; + int count = 0; + float maxDist = 50.f; + + void upload(const std::vector& data, float mx) + { + unload(); + if (data.empty()) + return; + maxDist = mx; + vao = rlLoadVertexArray(); + rlEnableVertexArray(vao); + vbo = rlLoadVertexBuffer(data.data(), (int)(data.size() * sizeof(float)), false); + const int stride = 7 * sizeof(float); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, stride, 0); + rlEnableVertexAttribute(0); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, stride, 3 * sizeof(float)); + rlEnableVertexAttribute(1); + rlSetVertexAttribute(2, 1, RL_FLOAT, false, stride, 4 * sizeof(float)); + rlEnableVertexAttribute(2); + rlSetVertexAttribute(3, 1, RL_FLOAT, false, stride, 5 * sizeof(float)); + rlEnableVertexAttribute(3); + rlSetVertexAttribute(4, 1, RL_FLOAT, false, stride, 6 * sizeof(float)); + rlEnableVertexAttribute(4); + rlDisableVertexArray(); + count = (int)(data.size() / 7); + } + void unload() + { + if (vao) + { + rlUnloadVertexArray(vao); + vao = 0; + } + if (vbo) + { + rlUnloadVertexBuffer(vbo); + vbo = 0; + } + count = 0; + } +}; + +// ── Orbit camera (same as CalibrationApp) ───────────────────────────────────── +struct Orbit +{ + float az = 30.f, el = 25.f, dist = 30.f; + Vector3 target = {}; + Camera3D toRaylib() const + { + float a = az * (float)DEG2RAD, e = el * (float)DEG2RAD; + Camera3D c; + c.position = { target.x + dist * std::cos(e) * std::sin(a), + target.y + dist * std::sin(e), + target.z + dist * std::cos(e) * std::cos(a) }; + c.target = target; + c.up = { 0, 1, 0 }; + c.fovy = 45.f; + c.projection = CAMERA_PERSPECTIVE; + return c; + } + void update(bool active) + { + if (!active) + return; + if (IsMouseButtonDown(MOUSE_BUTTON_LEFT)) + { + Vector2 d = GetMouseDelta(); + az -= d.x * 0.4f; + el += d.y * 0.4f; + el = std::max(-89.f, std::min(89.f, el)); + } + if (IsMouseButtonDown(MOUSE_BUTTON_RIGHT)) + { + Camera3D cam = toRaylib(); + Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); + Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); + Vector3 up = Vector3CrossProduct(right, fwd); + Vector2 d = GetMouseDelta(); + float sp = dist * 0.002f; + target = Vector3Add(target, Vector3Scale(right, -d.x * sp)); + target = Vector3Add(target, Vector3Scale(up, d.y * sp)); + } + float w = GetMouseWheelMove(); + if (w != 0.f) + dist = std::max(0.5f, dist - w * dist * 0.1f); + } +}; + +struct ColorPt +{ + float x, y, z; + uint8_t r, g, b; + float intensity; + int64_t ts_ns; +}; + +// ── Application state ───────────────────────────────────────────────────────── +struct State +{ + Trajectory traj; + std::vector imageTsNs; + Intrinsics K; + Extrinsics E; + Roi roi; + bool calibLoaded = false; + int imgW = 4656, imgH = 3496; + + // loaded camera images: timestamp → resized BGR Mat + std::map imagesFilenamesInTime; + const float imgScale = 1.0f; + GpuCloud cloud; + Shader shader = {}; + bool shaderOk = false; + int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; + + Orbit orbit; + + // controls + bool showPath = true; + bool showFrustums = true; + bool isolateCamera = false; // render only points colored by the selected (preview) image + float frustumScale = 0.5f; + float pointSize = 2.f; + int cloudDecim = 5; + int drawDecim = 1; + bool multiImgColoring = true; // false = single image per chunk (midpoint) + // How each point is matched to a camera image: + // 0 = temporal — image nearest in time (± maxWiggle frames, within maxTemporalDist) + // 1 = geometry — among all chunk images the point projects into, the one + // with the smallest depth (closest camera) + int colorStrategy = 0; + float maxTemporalDist = 0.5f; // s: skip images farther than this from the point (temporal) + int maxWiggle = 1; // frames: search startIdx ± maxWiggle for a frustum hit (temporal) + bool useImageColor = false; // true once a colorize pass produced RGB data + int colorMode = 0; // 0=intensity (jet), 1=RGB by image, 2=camera id + int coloredPts = 0; // points that received RGB from an image + int uncoloredPts = 0; // points left as intensity-gray (no image / out of frustum / outside ROI) + + char sessionBuf[512] = {}; + char calibBuf[512] = {}; + char cameraBuf[512] = {}; + char exportBuf[512] = "colored.laz"; + std::vector exportCloud; + std::string status; + + // ── ROS 2 export ────────────────────────────────────────────────────────── + char rosOutBuf[512] = "ros2_export"; + int rosStorageIdx = 0; // 0 = mcap, 1 = sqlite3 + RosExportOptions ros; + std::thread rosThread; + std::atomic rosBusy{ false }; + std::mutex rosMtx; + std::string rosResult; + bool rosResultReady = false; + + // ── COLMAP export ───────────────────────────────────────────────────────── + char colmapBuf[512] = "colmap_out"; + bool colmapCopyImages = false; + int colmapPtDecim = 50; // splat-friendly default (~500k from a 25M cloud) + + // ── image viewer ──────────────────────────────────────────────────────── + int imgViewIdx = 0; + Texture2D imgViewTex = {}; + bool imgViewTexValid = false; + std::atomic imgViewRequest{ -1 }; + std::atomic imgViewStop{ false }; + std::atomic imgViewLoading{ false }; + std::mutex imgViewMtx; + cv::Mat imgViewPending; + bool imgViewHasNew = false; + std::thread imgViewThread; +}; + +// ── helpers ─────────────────────────────────────────────────────────────────── +static Vector3 toRL(float x, float y, float z) +{ + return { x, z, -y }; +} +static Vector3 toRL(const Eigen::Vector3f& v) +{ + return { v.x(), v.z(), -v.y() }; +} + +// Load all cam0_*.jpg from CAMERA_0 (sibling of session dir) into s.images, resized by s.imgScale. +static void loadImages(State& s) +{ + s.imagesFilenamesInTime.clear(); + fs::path camDir; + if (s.cameraBuf[0]) + { + camDir = fs::path(s.cameraBuf); + } + else + { + camDir = fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; + } + if (!fs::is_directory(camDir)) + { + s.status = "No CAMERA_0 dir found"; + return; + } + + int loaded = 0; + for (auto& e : fs::directory_iterator(camDir)) + { + std::string n = e.path().filename().string(); + if (n.rfind("cam0_", 0) != 0 || e.path().extension() != ".jpg") + continue; + try + { + // filename: cam0_.jpg → strip prefix (5) and ext (4) + int64_t ts = std::stoll(n.substr(5, n.size() - 9)); + s.imagesFilenamesInTime[ts] = e.path().string(); + ++loaded; + } catch (...) + { + } + } + s.status = "Images loaded: " + std::to_string(loaded) + " from " + camDir.string(); +} + +// Parse session_poses.mrp → map from chunk stem (e.g. "scan_lio_0") to Affine3f. +static std::map parseMRP(const fs::path& mrpPath) +{ + std::map result; + std::ifstream f(mrpPath); + if (!f) + return result; + int n; + f >> n; + for (int i = 0; i < n; i++) + { + std::string name; + f >> name; + auto dot = name.rfind('.'); + std::string key = (dot != std::string::npos) ? name.substr(0, dot) : name; + float raw[16]; + for (int r = 0; r < 16; r++) + f >> raw[r]; + if (!f) + continue; + Eigen::Matrix4f M4; + for (int r = 0; r < 4; r++) + for (int c = 0; c < 4; c++) + M4(r, c) = raw[r * 4 + c]; + result[key] = Eigen::Affine3f(M4); + } + return result; +} + +static void loadSession(State& s) +{ + s.traj.poses.clear(); + s.imageTsNs.clear(); + s.exportCloud.clear(); + s.cloud.unload(); + loadImages(s); + + fs::path d(s.sessionBuf); + if (!fs::is_directory(d)) + { + s.status = "Not a directory"; + return; + } + + // parse MRP if present + auto mrp = parseMRP(d / "session_poses.mrp"); + if (mrp.empty()) + mrp = parseMRP(d / "session_ini_poses.mri"); + + // trajectory CSVs — apply corresponding MRP transform per chunk + std::vector csvPaths; + for (auto& e : fs::directory_iterator(d)) + { + std::string n = e.path().filename().string(); + if (n.rfind("trajectory_lio_", 0) == 0 && e.path().extension() == ".csv") + csvPaths.push_back(e.path()); + } + std::sort(csvPaths.begin(), csvPaths.end()); + for (auto& cp : csvPaths) + { + std::string stem = cp.stem().string(); + std::string idx = stem.substr(stem.rfind('_') + 1); + std::string key = "scan_lio_" + idx; + const Eigen::Affine3f* M = mrp.count(key) ? &mrp.at(key) : nullptr; + s.traj.loadCSV(cp.string(), M); + } + s.traj.sort(); + + // camera image timestamps + fs::path camDir = s.cameraBuf[0] ? fs::path(s.cameraBuf) : d.parent_path() / "CAMERA_0"; + if (fs::is_directory(camDir)) + { + for (auto& e : fs::directory_iterator(camDir)) + { + std::string n = e.path().filename().string(); + if (n.rfind("cam0_", 0) == 0 && e.path().extension() == ".jpg") + { + try + { + int64_t ts = std::stoll(n.substr(5, n.size() - 9)); + s.imageTsNs.push_back(ts); + } catch (...) + { + } + } + } + std::sort(s.imageTsNs.begin(), s.imageTsNs.end()); + } + + s.status = "Poses: " + std::to_string(s.traj.poses.size()) + " Img: " + std::to_string(s.imageTsNs.size()) + + (mrp.empty() ? " (no MRP)" : " +MRP") + " — press Load cloud"; +} + +static void loadCloud(State& s) +{ + s.exportCloud.clear(); + s.cloud.unload(); + + fs::path d(s.sessionBuf); + if (!fs::is_directory(d)) + { + s.status = "No session loaded"; + return; + } + + auto mrp = parseMRP(d / "session_poses.mrp"); + if (mrp.empty()) + mrp = parseMRP(d / "session_ini_poses.mri"); + + std::vector lazPaths; + for (auto& e : fs::directory_iterator(d)) + { + std::string n = e.path().filename().string(); + if (n.rfind("scan_lio_", 0) == 0 && e.path().extension() == ".laz") + lazPaths.push_back(e.path()); + } + 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::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; + // OpenCV rational + tangential distortion applied to each projected point, so + // colours are sampled from the raw (distorted) images at the right pixel. + // With all-zero coefficients this reduces exactly to the pinhole model. + const float d_k1 = s.K.k1, d_k2 = s.K.k2, d_k3 = s.K.k3; + const float d_k4 = s.K.k4, d_k5 = s.K.k5, d_k6 = s.K.k6; + const float d_p1 = s.K.p1, d_p2 = s.K.p2; + // (x, y) = normalized camera coords (X/Z, Y/Z) → distorted normalized coords. + auto distort = [=](float x, float y, float& xd, float& yd) + { + float r2 = x * x + y * y; + float radial = (1.f + (d_k1 + (d_k2 + d_k3 * r2) * r2) * r2) / (1.f + (d_k4 + (d_k5 + d_k6 * r2) * r2) * r2); + xd = x * radial + 2.f * d_p1 * x * y + d_p2 * (r2 + 2.f * x * x); + yd = y * radial + d_p1 * (r2 + 2.f * y * y) + 2.f * d_p2 * x * y; + }; + + auto packGray = [](float intensity) -> float + { + uint8_t g = (uint8_t)(std::min(1.f, std::max(0.f, intensity)) * 255.f); + uint32_t p = (uint32_t(g) << 16) | (uint32_t(g) << 8) | uint32_t(g); + float f; + std::memcpy(&f, &p, 4); + return f; + }; + + struct ImgEntry + { + int64_t ts; + Eigen::Affine3f pose; // T_world_lidar at the image time (interpolated) + cv::Mat img; + int globalIdx; // index into s.imageTsNs (== imgViewIdx / selectedCamera) + }; + + // time(s) -> T_world_lidar, for interpolating the pose at each image time. + std::map trajMap = buildTrajMap(s.traj); + + std::vector gpuData; + float mx = 0.f; + float sumX = 0, sumY = 0, sumZ = 0; + int cnt = 0; + int step = std::max(1, s.cloudDecim); + int coloredChunks = 0; + int coloredPts = 0, uncoloredPts = 0; + + for (auto& lp : lazPaths) + { + std::string key = lp.stem().string(); // "scan_lio_N" + std::string idx = key.substr(key.rfind('_') + 1); + const Eigen::Affine3f* M = mrp.count(key) ? &mrp.at(key) : nullptr; + + // ── step 1: read chunk time range from the matching trajectory CSV ── + int64_t chunkFirst = 0, chunkLast = 0; + { + fs::path csvPath = d / ("trajectory_lio_" + idx + ".csv"); + std::ifstream cf(csvPath); + if (cf) + { + std::string line; + std::getline(cf, line); + while (std::getline(cf, line)) + { + if (line.empty()) + continue; + std::istringstream ss(line); + int64_t ts; + ss >> ts; + if (!ss) + continue; + if (!chunkFirst) + chunkFirst = ts; + chunkLast = ts; + } + } + } + + // ── step 2: collect images for this chunk ─────────────────────────── + std::vector chunkImgs; + if (canColor && chunkFirst && chunkLast) + { + if (s.multiImgColoring) + { + // new: every image whose timestamp falls inside the chunk range + auto it0 = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkFirst); + auto it1 = std::upper_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkLast); + for (auto it = it0; it != it1; ++it) + { + int64_t imgTs = *it; + auto fnIt = s.imagesFilenamesInTime.find(imgTs); + if (fnIt == s.imagesFilenamesInTime.end()) + continue; + Eigen::Affine3f pose; + if (!interpPose(trajMap, imgTs, pose)) + continue; + cv::Mat img = cv::imread(fnIt->second); + if (img.empty()) + continue; + int gidx = (int)(it - s.imageTsNs.begin()); + chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + } + } + else + { + // legacy: single image nearest to chunk midpoint + int64_t mid = chunkFirst; + auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), mid); + if (it == s.imageTsNs.end()) + --it; + else if (it != s.imageTsNs.begin()) + { + auto prev = std::prev(it); + if (std::abs(*prev - mid) < std::abs(*it - mid)) + it = prev; + } + int64_t imgTs = *it; + auto fnIt = s.imagesFilenamesInTime.find(imgTs); + Eigen::Affine3f pose; + if (fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) + { + cv::Mat img = cv::imread(fnIt->second); + int gidx = (int)(it - s.imageTsNs.begin()); + if (!img.empty()) + chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + } + } + } + if (!chunkImgs.empty()) + ++coloredChunks; + + // ── step 3: load point cloud ──────────────────────────────────────── + PointCloud pc; + if (!pc.load(lp.string())) + continue; + + int nImgs = (int)chunkImgs.size(); + + // ── step 4: colorize each point ───────────────────────────────────── + // chunkImgs is sorted by ts (imageTsNs was sorted) + // For each point: find nearest image by pt.ts_ns, expand outward until + // the point lands inside a frustum. + for (int i = 0; i < (int)pc.points.size(); i += step) + { + auto& pt = pc.points[i]; + Eigen::Vector3f pw(pt.x, pt.y, pt.z); + if (M) + pw = *M * pw; + + gpuData.push_back(pw.x()); + gpuData.push_back(pw.z()); + gpuData.push_back(-pw.y()); + + const float rawIntensity = pt.intensity; + float colorF = packGray(rawIntensity); + float camIdF = -1.f; // which image colored this point (global index), -1 = none + float inRoiF = -1.f; // 1=inside ROI, 0=outside ROI, -1=projects into no image + + if (nImgs > 0) + { + // nearest image by point timestamp + int startIdx = 0; + if (pt.ts_ns != 0) + { + auto it = std::lower_bound( + chunkImgs.begin(), + chunkImgs.end(), + pt.ts_ns, + [](const ImgEntry& e, int64_t t) + { + return e.ts < t; + }); + if (it == chunkImgs.end()) + --it; + else if (it != chunkImgs.begin()) + { + auto prev = std::prev(it); + if (std::abs(prev->ts - pt.ts_ns) < std::abs(it->ts - pt.ts_ns)) + it = prev; + } + startIdx = (int)(it - chunkImgs.begin()); + } + // Result of projecting the point into one image. + struct Hit + { + bool ok = false; // projects into frustum AND passes ROI filter + float depth = 0.f; // z in camera frame (only when ok) + float colorF = 0.f; // packed RGB (only when ok) + float inRoiF = -1.f; // 1 inside ROI, 0 outside, -1 not in frustum + int globalIdx = -1; + }; + auto probe = [&](int idx) -> Hit + { + Hit h; + if (idx < 0 || idx >= nImgs) + return h; + auto& e = chunkImgs[idx]; + Eigen::Vector3f pl = e.pose.inverse() * pw; + Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); + if (pc_.z() <= 0.05f) + return h; + float xd, yd; + distort(pc_.x() / pc_.z(), pc_.y() / pc_.z(), xd, yd); + int iu = (int)std::round(K_fx * xd + K_cx); + int iv = (int)std::round(K_fy * yd + K_cy); + if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) + return h; + // point projects into this image — record ROI membership so + // the "In ROI" render mode can show it, independent of whether + // the ROI filter is currently enabled. + bool haveRoi = s.roi.w > 0 && s.roi.h > 0; + bool insideRoi = !haveRoi || (iu >= s.roi.x && iu < s.roi.x + s.roi.w && iv >= s.roi.y && iv < s.roi.y + s.roi.h); + h.inRoiF = insideRoi ? 1.f : 0.f; + // outside the region of interest? leave the point uncolored + if (s.roi.enabled && !insideRoi) + return h; + cv::Vec3b bgr = e.img.at(iv, iu); + uint32_t p = (uint32_t(bgr[2]) << 16) | (uint32_t(bgr[1]) << 8) | uint32_t(bgr[0]); + std::memcpy(&h.colorF, &p, 4); + h.globalIdx = e.globalIdx; + h.depth = pc_.z(); + h.ok = true; + return h; + }; + // Record frustum/ROI membership even for images that don't win, so + // the "In ROI" render mode stays meaningful. Latest wins. + auto note = [&](const Hit& h) + { + if (h.inRoiF >= 0.f) + inRoiF = h.inRoiF; + }; + auto commit = [&](const Hit& h) + { + colorF = h.colorF; + camIdF = (float)h.globalIdx; + inRoiF = h.inRoiF; + }; + + if (s.colorStrategy == 1) + { + // Geometry: among every image the point projects into, keep the + // one with the smallest depth (closest camera → best resolution). + Hit best; + for (int idx = 0; idx < nImgs; ++idx) + { + Hit h = probe(idx); + note(h); + if (h.ok && (!best.ok || h.depth < best.depth)) + best = h; + } + if (best.ok) + commit(best); + } + else + { + // Temporal: search the temporally-nearest image, then its + // neighbours outward (±1, ±2, … ±maxWiggle), taking the first + // frustum hit. Only if the nearest image is within + // maxTemporalDist of the point. + const int64_t maxDtNs = (int64_t)(s.maxTemporalDist * 1e9); + if (std::abs(pt.ts_ns - chunkImgs[startIdx].ts) <= maxDtNs) + { + for (int w = 0; w <= s.maxWiggle && camIdF < 0.f; ++w) + { + Hit h = probe(startIdx - w); + note(h); + if (h.ok) + { + commit(h); + break; + } + if (w == 0) + continue; + h = probe(startIdx + w); + note(h); + if (h.ok) + { + commit(h); + break; + } + } + } + } + } + if (camIdF >= 0.f) + ++coloredPts; + else + ++uncoloredPts; + + gpuData.push_back(colorF); + gpuData.push_back(rawIntensity); + gpuData.push_back(camIdF); + gpuData.push_back(inRoiF); + + uint32_t packed; + std::memcpy(&packed, &colorF, 4); + s.exportCloud.push_back( + { pw.x(), + pw.y(), + pw.z(), + (uint8_t)((packed >> 16) & 0xFF), + (uint8_t)((packed >> 8) & 0xFF), + (uint8_t)(packed & 0xFF), + rawIntensity, + pt.ts_ns }); + + float d2 = pw.squaredNorm(); + if (d2 > mx * mx) + mx = std::sqrt(d2); + sumX += pw.x(); + sumY += pw.z(); + sumZ += -pw.y(); + cnt++; + } + // chunkImgs and their cv::Mat memory are released here + } + s.useImageColor = canColor && (coloredChunks > 0); + if (s.useImageColor) + s.colorMode = 1; // default to RGB display once RGB data is available + s.coloredPts = coloredPts; + s.uncoloredPts = uncoloredPts; + + if (cnt > 0) + { + s.cloud.upload(gpuData, mx); + s.orbit.target = { sumX / cnt, sumY / cnt, sumZ / cnt }; + s.orbit.dist = std::max(5.f, mx * 0.3f); + } + + s.status = "Pts: " + std::to_string(s.cloud.count) + " Poses: " + std::to_string(s.traj.poses.size()) + + " Imgs/chunk: " + std::to_string(coloredChunks > 0 ? coloredChunks : 0) + (s.useImageColor ? " +RGB" : ""); + if (s.useImageColor && cnt > 0) + { + double pct = 100.0 * coloredPts / cnt; + s.status += " | Colored: " + std::to_string(coloredPts) + " Uncolored: " + std::to_string(uncoloredPts) + " (" + + std::to_string((int)std::lround(pct)) + "%)"; + } +} + +static void loadCalib(State& s) +{ + std::ifstream f(s.calibBuf); + if (!f) + { + s.status = std::string("Cannot open: ") + s.calibBuf; + return; + } + nlohmann::json j; + f >> j; + if (j.contains("intrinsics")) + { + auto& ji = j["intrinsics"]; + s.K.fx = ji.value("fx", s.K.fx); + s.K.fy = ji.value("fy", s.K.fy); + s.K.cx = ji.value("cx", s.K.cx); + s.K.cy = ji.value("cy", s.K.cy); + // rational distortion model (used by ROS export to rectify images) + s.K.k1 = ji.value("k1", s.K.k1); + s.K.k2 = ji.value("k2", s.K.k2); + s.K.k3 = ji.value("k3", s.K.k3); + s.K.k4 = ji.value("k4", s.K.k4); + s.K.k5 = ji.value("k5", s.K.k5); + s.K.k6 = ji.value("k6", s.K.k6); + s.K.p1 = ji.value("p1", s.K.p1); + s.K.p2 = ji.value("p2", s.K.p2); + } + if (j.contains("extrinsics")) + { + auto& je = j["extrinsics"]; + if (je.contains("camera_position_in_world_xyz") && je["camera_position_in_world_xyz"].size() >= 3) + { + s.E.tx = je["camera_position_in_world_xyz"][0]; + 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) + { + 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]; + } + } + // Optional region of interest, in full-resolution image pixels: + // "roi": { "x": 0, "y": 0, "w": 4656, "h": 3496, "enabled": true } + // "enabled" defaults to true when the object is present; it only takes + // effect once w and h are positive. + if (j.contains("roi")) + { + auto& jr = j["roi"]; + s.roi.x = jr.value("x", 0); + s.roi.y = jr.value("y", 0); + s.roi.w = jr.value("w", 0); + s.roi.h = jr.value("h", 0); + s.roi.enabled = jr.value("enabled", true) && s.roi.w > 0 && s.roi.h > 0; + } + s.calibLoaded = true; + s.status = "Calibration loaded"; +} + +static void exportLAZ(State& s) +{ + if (s.exportCloud.empty()) + { + s.status = "No cloud to export"; + return; + } + + double xmin = s.exportCloud[0].x, xmax = xmin; + double ymin = s.exportCloud[0].y, ymax = ymin; + double zmin = s.exportCloud[0].z, zmax = zmin; + for (auto& p : s.exportCloud) + { + xmin = std::min(xmin, (double)p.x); + xmax = std::max(xmax, (double)p.x); + ymin = std::min(ymin, (double)p.y); + ymax = std::max(ymax, (double)p.y); + zmin = std::min(zmin, (double)p.z); + zmax = std::max(zmax, (double)p.z); + } + + laszip_POINTER writer = nullptr; + if (laszip_create(&writer)) + { + s.status = "laszip_create failed"; + return; + } + + laszip_header* header = nullptr; + laszip_get_header_pointer(writer, &header); + + header->version_major = 1; + header->version_minor = 2; + header->header_size = 227; + header->offset_to_point_data = 227; + header->point_data_format = 3; // XYZ + RGB + GPS time + header->point_data_record_length = 34; + header->number_of_point_records = (uint32_t)s.exportCloud.size(); + header->x_scale_factor = 0.001; + header->y_scale_factor = 0.001; + header->z_scale_factor = 0.001; + header->x_offset = xmin; + header->y_offset = ymin; + header->z_offset = zmin; + header->min_x = xmin; + header->max_x = xmax; + header->min_y = ymin; + header->max_y = ymax; + header->min_z = zmin; + header->max_z = zmax; + + laszip_BOOL compress = (std::strstr(s.exportBuf, ".laz") != nullptr) ? 1 : 0; + if (laszip_open_writer(writer, s.exportBuf, compress)) + { + laszip_CHAR* err = nullptr; + laszip_get_error(writer, &err); + s.status = std::string("Export failed: ") + (err ? err : "?"); + laszip_destroy(writer); + return; + } + + laszip_point* point = nullptr; + laszip_get_point_pointer(writer, &point); + + laszip_F64 coords[3]; + for (auto& p : s.exportCloud) + { + coords[0] = p.x; + coords[1] = p.y; + coords[2] = p.z; + laszip_set_coordinates(writer, coords); + point->rgb[0] = (laszip_U16)p.r << 8; + point->rgb[1] = (laszip_U16)p.g << 8; + point->rgb[2] = (laszip_U16)p.b << 8; + // intensity normalized [0,1] → LAS 16-bit field + point->intensity = (laszip_U16)(std::min(1.f, std::max(0.f, p.intensity)) * 65535.f); + // GPS time: ns since epoch → seconds (double) + point->gps_time = (laszip_F64)p.ts_ns * 1e-9; + laszip_write_point(writer); + } + + laszip_close_writer(writer); + laszip_destroy(writer); + s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; +} + +// Export a COLMAP sparse text model (cameras/images/points3D) from the current +// state. Poses are world->camera; the colored cloud becomes points3D. +static void exportColmap(State& s) +{ + if (!s.calibLoaded) + { + s.status = "COLMAP: load calibration first"; + return; + } + if (s.imagesFilenamesInTime.empty()) + { + s.status = "COLMAP: no images"; + return; + } + + fs::path out(s.colmapBuf); + fs::path sparse = out / "sparse"; + std::error_code ec; + fs::create_directories(sparse, ec); + if (ec) + { + s.status = "COLMAP: cannot create " + sparse.string(); + return; + } + + // 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.translation() = Eigen::Vector3f(s.E.tx, s.E.ty, s.E.tz); + + // cameras.txt — rational OpenCV model == COLMAP FULL_OPENCV (12 params) + { + std::ofstream f(sparse / "cameras.txt"); + f << std::setprecision(12); + f << "# Camera list with one line of data per camera:\n" + "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; + f << "1 FULL_OPENCV " << s.imgW << ' ' << s.imgH << ' ' << s.K.fx << ' ' << s.K.fy << ' ' << s.K.cx << ' ' << s.K.cy << ' ' + << s.K.k1 << ' ' << s.K.k2 << ' ' << s.K.p1 << ' ' << s.K.p2 << ' ' << s.K.k3 << ' ' << s.K.k4 << ' ' << s.K.k5 << ' ' << s.K.k6 + << '\n'; + } + + // images.txt — one image per camera frame, pose = world->camera + int nImg = 0; + { + std::ofstream f(sparse / "images.txt"); + f << std::setprecision(12); + f << "# Image list with two lines of data per image:\n" + "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; + auto trajMap = buildTrajMap(s.traj); + int id = 1; + for (auto& [ts, path] : s.imagesFilenamesInTime) + { + Eigen::Affine3f pose; + if (!interpPose(trajMap, ts, pose)) + continue; + Eigen::Affine3f T_wc = pose * T_lc; // camera in world + Eigen::Affine3f T_cw = T_wc.inverse(); // world -> camera + Eigen::Quaternionf q(T_cw.linear()); + q.normalize(); + Eigen::Vector3f t = T_cw.translation(); + std::string name = fs::path(path).filename().string(); + f << id << ' ' << q.w() << ' ' << q.x() << ' ' << q.y() << ' ' << q.z() << ' ' << t.x() << ' ' << t.y() << ' ' << t.z() << " 1 " + << name << '\n'; + f << '\n'; // empty POINTS2D line (no 2D-3D correspondences) + ++id; + ++nImg; + } + } + + // points3D.txt — the colored cloud (no tracks) + size_t nPts = 0; + { + std::ofstream f(sparse / "points3D.txt"); + f << "# 3D point list with one line of data per point:\n" + "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)\n"; + f << std::setprecision(9); + int step = std::max(1, s.colmapPtDecim); + size_t id = 1; + for (size_t i = 0; i < s.exportCloud.size(); i += step) + { + const auto& p = s.exportCloud[i]; + f << id << ' ' << p.x << ' ' << p.y << ' ' << p.z << ' ' << (int)p.r << ' ' << (int)p.g << ' ' << (int)p.b << " 0\n"; + ++id; + ++nPts; + } + } + + // points3D.ply — binary PLY (xyz + uchar rgb), same decimation. Convenient + // init cloud for 3DGS trainers and opens directly in CloudCompare. + { + int step = std::max(1, s.colmapPtDecim); + size_t n = (s.exportCloud.size() + step - 1) / step; + std::ofstream f(sparse / "points3D.ply", std::ios::binary); + f << "ply\nformat binary_little_endian 1.0\n" + << "element vertex " << n << "\n" + << "property float x\nproperty float y\nproperty float z\n" + << "property uchar red\nproperty uchar green\nproperty uchar blue\n" + << "end_header\n"; + for (size_t i = 0; i < s.exportCloud.size(); i += step) + { + const auto& p = s.exportCloud[i]; + f.write(reinterpret_cast(&p.x), sizeof(float) * 3); + f.write(reinterpret_cast(&p.r), 3); // r,g,b contiguous + } + } + + if (s.colmapCopyImages) + { + fs::path imgd = out / "images"; + fs::create_directories(imgd, ec); + for (auto& [ts, path] : s.imagesFilenamesInTime) + fs::copy_file(path, imgd / fs::path(path).filename(), fs::copy_options::overwrite_existing, ec); + } + + s.status = "COLMAP: " + std::to_string(nImg) + " images, " + std::to_string(nPts) + " points (+ply) -> " + sparse.string(); +} + +// Gather everything the ROS exporter needs from current viewer state. +static void buildRosInput(State& s, RosExportInput& in) +{ + in.traj = s.traj; + in.imageFiles = s.imagesFilenamesInTime; + in.calibLoaded = s.calibLoaded; + in.K = s.K; + in.E = s.E; + + fs::path d(s.sessionBuf); + if (!fs::is_directory(d)) + return; + + auto mrp = parseMRP(d / "session_poses.mrp"); + if (mrp.empty()) + mrp = parseMRP(d / "session_ini_poses.mri"); + + std::vector lazPaths; + for (auto& e : fs::directory_iterator(d)) + { + std::string n = e.path().filename().string(); + if (n.rfind("scan_lio_", 0) == 0 && e.path().extension() == ".laz") + lazPaths.push_back(e.path()); + } + std::sort(lazPaths.begin(), lazPaths.end()); + for (auto& lp : lazPaths) + { + RosExportInput::Chunk ch; + ch.lazPath = lp.string(); + std::string key = lp.stem().string(); // "scan_lio_N" + if (mrp.count(key)) + { + ch.M = mrp.at(key); + ch.hasM = true; + } + in.lidarChunks.push_back(std::move(ch)); + } +} + +static void exportRos(State& s) +{ + if (s.rosBusy.load()) + return; + + // Gather the (owning) input on the UI thread, then run the heavy export on a + // worker so the window keeps rendering. `in` and `opt` are owned by the thread. + RosExportInput in; + buildRosInput(s, in); + RosExportOptions opt = s.ros; + opt.outUri = s.rosOutBuf; + opt.storageId = (s.rosStorageIdx == 1) ? "sqlite3" : "mcap"; + + if (s.rosThread.joinable()) + s.rosThread.join(); + s.rosBusy = true; + s.status = "Exporting ROS 2 bag... (see console)"; + s.rosThread = std::thread( + [&s, in = std::move(in), opt]() mutable + { + std::string st; + exportRos2Bag(in, opt, st); + { + std::lock_guard lk(s.rosMtx); + s.rosResult = std::move(st); + s.rosResultReady = true; + } + s.rosBusy = false; + }); +} + +static void drawScene(State& s) +{ + // ── trajectory path ─────────────────────────────────────────────────────── + if (s.showPath) + { + for (size_t i = 1; i < s.traj.poses.size(); i++) + { + auto& a = s.traj.poses[i - 1]; + auto& b = s.traj.poses[i]; + DrawLine3D(toRL(a.T.translation()), toRL(b.T.translation()), Color{ 100, 200, 255, 220 }); + } + } + + // ── camera frustums ─────────────────────────────────────────────────────── + if (s.showFrustums && s.calibLoaded) + { + Eigen::Matrix3f R_wc = eulerZYXtoMat3(s.E.rx, s.E.ry, s.E.rz); + Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); + float fs = s.frustumScale; + float ncx[4] = { + (0.f - s.K.cx) / s.K.fx, (float(s.imgW) - s.K.cx) / s.K.fx, (float(s.imgW) - s.K.cx) / s.K.fx, (0.f - s.K.cx) / s.K.fx + }; + float ncy[4] = { + (0.f - s.K.cy) / s.K.fy, (0.f - s.K.cy) / s.K.fy, (float(s.imgH) - s.K.cy) / s.K.fy, (float(s.imgH) - s.K.cy) / s.K.fy + }; + + int64_t hlTs = + (!s.imageTsNs.empty() && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) ? s.imageTsNs[s.imgViewIdx] : -1; + + for (int64_t ts : s.imageTsNs) + { + const TrajPose* pose = s.traj.nearest(ts); + if (!pose) + continue; + + Vector3 origin = toRL(pose->T * C); + + Vector3 w[4]; + for (int k = 0; k < 4; k++) + { + Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * fs, ncy[k] * fs, fs) + C; + w[k] = toRL(pose->T * pl); + } + + bool hl = (ts == hlTs); + Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; + float sc = hl ? fs * 1.05f : fs; + + if (hl) + { + // filled quad highlight + Vector3 w2[4]; + for (int k = 0; k < 4; k++) + { + Eigen::Vector3f pl = R_wc * Eigen::Vector3f(ncx[k] * sc, ncy[k] * sc, sc) + C; + w2[k] = toRL(pose->T * pl); + } + DrawTriangle3D(w2[0], w2[1], w2[2], Color{ 255, 255, 50, 40 }); + DrawTriangle3D(w2[2], w2[3], w2[0], Color{ 255, 255, 50, 40 }); + DrawSphere(origin, fs * 0.04f, fc); + } + + DrawLine3D(origin, w[0], fc); + DrawLine3D(origin, w[1], fc); + DrawLine3D(origin, w[2], fc); + DrawLine3D(origin, w[3], fc); + DrawLine3D(w[0], w[1], fc); + DrawLine3D(w[1], w[2], fc); + DrawLine3D(w[2], w[3], fc); + DrawLine3D(w[3], w[0], fc); + } + } + + // ── GPU point cloud ─────────────────────────────────────────────────────── + if (s.cloud.count > 0 && s.shaderOk) + { + rlDrawRenderBatchActive(); + Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + rlEnableShader(s.shader.id); + rlSetUniformMatrix(s.locMVP, mvp); + rlSetUniform(s.locPS, &s.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(s.locCM, &s.colorMode, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(s.locDecim, &s.drawDecim, RL_SHADER_UNIFORM_INT, 1); + int sel = (s.isolateCamera && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) ? s.imgViewIdx : -1; + rlSetUniform(s.locSel, &sel, RL_SHADER_UNIFORM_INT, 1); + rlEnableVertexArray(s.cloud.vao); + glDrawArrays(GL_POINTS, 0, s.cloud.count); + rlDisableVertexArray(); + rlDisableShader(); + } +} + +// ── main ────────────────────────────────────────────────────────────────────── +int main(int argc, char* argv[]) +{ + CliArgs args = parseArgs(argc, argv); + static const char* kDesc = "View LIO trajectory, colorize and export point clouds"; + const std::vector usage = { cliopt::MJS, cliopt::CAMERA_DIR, cliopt::CALIB }; + if (args.help) + { + printUsage("TrajectoryViewer", kDesc, usage); + return 0; + } + if (!args.valid) + { + std::fprintf(stderr, "%s\n\n", args.error.c_str()); + printUsage("TrajectoryViewer", kDesc, usage, /*toStderr=*/true); + return 1; + } + + State s; + // --mjs gives the session manifest; the session directory is its parent. + std::string sessionDir; + if (args.has("mjs")) + sessionDir = fs::path(args.get("mjs")).parent_path().string(); + else if (!args.positional.empty()) + sessionDir = args.positional.front(); // back-compat + if (!sessionDir.empty()) + strncpy(s.sessionBuf, sessionDir.c_str(), sizeof(s.sessionBuf) - 1); + + if (args.has("camera_dir")) + strncpy(s.cameraBuf, args.get("camera_dir").c_str(), sizeof(s.cameraBuf) - 1); + + // --calib: calibration json (intrinsic + extrinsic). Fall back to any + // positional ending in .json for backward compatibility. + std::string calib = args.get("calib"); + if (calib.empty()) + for (const auto& p : args.positional) + if (p.size() > 5 && p.substr(p.size() - 5) == ".json") + { + calib = p; + break; + } + if (!calib.empty()) + strncpy(s.calibBuf, calib.c_str(), sizeof(s.calibBuf) - 1); + + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + InitWindow(1400, 900, "Trajectory Viewer"); + fitWindowToScreen(); + // panelW below is user-resizable but the 3D view still needs room. + SetWindowMinSize(900, 500); + SetTargetFPS(60); + rlImGuiSetup(true); + + s.shader = LoadShaderFromMemory(kVS, kFS); + s.shaderOk = s.shader.id > 0; + if (s.shaderOk) + { + s.locMVP = rlGetLocationUniform(s.shader.id, "mvp"); + s.locPS = rlGetLocationUniform(s.shader.id, "pointSize"); + s.locCM = rlGetLocationUniform(s.shader.id, "colorMode"); + s.locDecim = rlGetLocationUniform(s.shader.id, "drawDecim"); + s.locSel = rlGetLocationUniform(s.shader.id, "selectedCamera"); + } + glEnable(GL_PROGRAM_POINT_SIZE); + + // image viewer background loader thread + s.imgViewThread = std::thread( + [&s]() + { + int lastLoaded = -1; + while (!s.imgViewStop.load()) + { + int req = s.imgViewRequest.load(); + if (req != lastLoaded && req >= 0 && req < (int)s.imageTsNs.size()) + { + lastLoaded = req; + s.imgViewLoading = true; + int64_t ts = s.imageTsNs[req]; + auto it = s.imagesFilenamesInTime.find(ts); + if (it != s.imagesFilenamesInTime.end()) + { + cv::Mat img = cv::imread(it->second); + if (!img.empty()) + { + cv::cvtColor(img, img, cv::COLOR_BGR2RGB); + std::lock_guard lk(s.imgViewMtx); + s.imgViewPending = std::move(img); + s.imgViewHasNew = true; + } + } + s.imgViewLoading = false; + } + else + { + std::this_thread::sleep_for(std::chrono::milliseconds(8)); + } + } + }); + + // auto-load if args given + if (s.sessionBuf[0]) + loadSession(s); + if (s.calibBuf[0]) + loadCalib(s); + + float panelW = 420.f; + + while (!WindowShouldClose()) + { + bool imguiWants = ImGui::GetIO().WantCaptureMouse; + s.orbit.update(!imguiWants); + + // pick up the ROS export result from the worker thread (if any) + { + std::lock_guard lk(s.rosMtx); + if (s.rosResultReady) + { + s.status = s.rosResult; + s.rosResultReady = false; + } + } + + // Ctrl toggles point coloring: intensity (jet) <-> RGB + if (!ImGui::GetIO().WantCaptureKeyboard) + { + if (IsKeyPressed(KEY_LEFT_CONTROL) || IsKeyPressed(KEY_RIGHT_CONTROL)) + s.colorMode = (s.colorMode == 1) ? 0 : 1; + + if (IsKeyPressed(KEY_LEFT)) + { + s.imgViewIdx = std::max(s.imgViewIdx - 1, 0); + s.imgViewRequest.store(s.imgViewIdx); + } + if (IsKeyPressed(KEY_RIGHT)) + { + s.imgViewIdx = std::min(s.imgViewIdx + 1, (int)s.imageTsNs.size()); + s.imgViewRequest.store(s.imgViewIdx); + } + } + + BeginDrawing(); + ClearBackground(Color{ 25, 25, 25, 255 }); + + Camera3D cam = s.orbit.toRaylib(); + BeginMode3D(cam); + drawScene(s); + DrawGrid(20, 1.f); + // axes + DrawLine3D({ 0, 0, 0 }, { 2, 0, 0 }, RED); + DrawLine3D({ 0, 0, 0 }, { 0, 2, 0 }, GREEN); + DrawLine3D({ 0, 0, 0 }, { 0, 0, -2 }, BLUE); + EndMode3D(); + + // ── upload image viewer texture if worker produced one ──────────────── + { + cv::Mat toUpload; + { + std::lock_guard lk(s.imgViewMtx); + if (s.imgViewHasNew) + { + std::swap(toUpload, s.imgViewPending); + s.imgViewHasNew = false; + } + } + if (!toUpload.empty()) + { + if (s.imgViewTexValid) + UnloadTexture(s.imgViewTex); + Image ri = { toUpload.data, toUpload.cols, toUpload.rows, 1, PIXELFORMAT_UNCOMPRESSED_R8G8B8 }; + s.imgViewTex = LoadTextureFromImage(ri); + s.imgViewTexValid = s.imgViewTex.id > 0; + } + } + + // ── ImGui panel ─────────────────────────────────────────────────────── + rlImGuiBegin(); + ImGuiIO& io = ImGui::GetIO(); + ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, 0), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(panelW, io.DisplaySize.y), ImGuiCond_Always); + ImGui::Begin("##panel", nullptr, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse); + panelW = ImGui::GetWindowWidth(); + + ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "Trajectory Viewer"); + ImGui::Separator(); + + if (ImGui::CollapsingHeader("Session", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::PushItemWidth(-1); + ImGui::Text("LIO result directory:"); + ImGui::InputText("##sess", s.sessionBuf, sizeof(s.sessionBuf)); + if (ImGui::Button("Browse...##sess", ImVec2(-1, 0))) + setBuf(s.sessionBuf, sizeof(s.sessionBuf), calib::fd::SelectFolder("Select LIO result directory")); + ImGui::Text("CAMERA_0 directory (empty = auto):"); + ImGui::InputText("##cam", s.cameraBuf, sizeof(s.cameraBuf)); + if (ImGui::Button("Browse...##cam", ImVec2(-1, 0))) + setBuf(s.cameraBuf, sizeof(s.cameraBuf), calib::fd::SelectFolder("Select CAMERA_0 directory")); + if (ImGui::Button("Load session", ImVec2(-1, 0))) + loadSession(s); + if (!s.imagesFilenamesInTime.empty()) + ImGui::TextDisabled("%d images found", (int)s.imagesFilenamesInTime.size()); + ImGui::Separator(); + // Scoped narrower width: -1 (the block's default) gives this + // trailing-label widget the full row and clips its label off + // the right edge of the panel. + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputInt("Load decimation", &s.cloudDecim); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + s.cloudDecim = std::max(1, s.cloudDecim); + ImGui::Checkbox("Multi-image coloring", &s.multiImgColoring); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("ON: all images per chunk, per-point assignment\nOFF: single image per chunk (midpoint)"); + ImGui::Text("Coloring strategy:"); + ImGui::RadioButton("Temporal", &s.colorStrategy, 0); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Match each point to the image nearest in time\n(searched outward up to 'Wiggle' frames, within 'Max time')."); + ImGui::SameLine(); + ImGui::RadioButton("Geometry", &s.colorStrategy, 1); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Match each point to the chunk image it projects into\nwith the smallest depth (closest camera)."); + if (s.colorStrategy == 0) + { + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputInt("Wiggle (frames)", &s.maxWiggle); + s.maxWiggle = std::max(0, s.maxWiggle); + ImGui::InputFloat("Max time (s)", &s.maxTemporalDist, 0.05f, 0.5f, "%.2f"); + s.maxTemporalDist = std::max(0.f, s.maxTemporalDist); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + } + if (ImGui::Button("Load cloud", ImVec2(-1, 0))) + loadCloud(s); + ImGui::PopItemWidth(); + } + + if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::PushItemWidth(-1); + ImGui::Text("Calibration JSON:"); + ImGui::InputText("##cal", s.calibBuf, sizeof(s.calibBuf)); + if (ImGui::Button("Browse...##cal", ImVec2(-1, 0))) + setBuf( + s.calibBuf, + sizeof(s.calibBuf), + calib::fd::OpenFileDialogOneFile("Select calibration file", calib::fd::CalibJsonFilter)); + if (ImGui::Button("Load calibration", ImVec2(-1, 0))) + loadCalib(s); + if (s.calibLoaded) + { + ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); + ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + // Scoped narrower width -- see the "Load decimation" comment above. + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputInt("Image W", &s.imgW); + ImGui::InputInt("Image H", &s.imgH); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + ImGui::Separator(); + if (ImGui::Checkbox("Region of interest", &s.roi.enabled)) + { + // first enable with an empty ROI: default to the full image + if (s.roi.enabled && (s.roi.w <= 0 || s.roi.h <= 0)) + { + s.roi.x = 0; + s.roi.y = 0; + s.roi.w = s.imgW; + s.roi.h = s.imgH; + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Only points projecting inside the ROI get colored.\nDrawn on the image preview."); + if (s.roi.enabled) + { + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputInt("ROI x", &s.roi.x); + ImGui::InputInt("ROI y", &s.roi.y); + ImGui::InputInt("ROI w", &s.roi.w); + ImGui::InputInt("ROI h", &s.roi.h); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + } + } + ImGui::PopItemWidth(); + } + + if (ImGui::CollapsingHeader("Visualization", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::Checkbox("Show path", &s.showPath); + ImGui::Checkbox("Show frustums", &s.showFrustums); + ImGui::SliderFloat("Frustum scale", &s.frustumScale, 0.05f, 5.f, "%.2f"); + ImGui::SliderFloat("Point size", &s.pointSize, 1.f, 20.f, "%.1f"); + ImGui::SliderInt("Draw decimation", &s.drawDecim, 1, 64); + if (!s.imagesFilenamesInTime.empty()) + { + ImGui::Separator(); + ImGui::Text("Point color:"); + ImGui::RadioButton("Intensity", &s.colorMode, 0); + ImGui::SameLine(); + ImGui::RadioButton("RGB (image)", &s.colorMode, 1); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Ctrl toggles intensity (jet) <-> RGB"); + ImGui::SameLine(); + ImGui::RadioButton("Camera ID", &s.colorMode, 2); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Colors each point by the image that colored it"); + ImGui::SameLine(); + ImGui::RadioButton("In ROI", &s.colorMode, 3); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Green = projects inside the ROI, red = outside.\nPoints projecting into no image are hidden."); + } + } + + if (ImGui::CollapsingHeader("Image Preview", ImGuiTreeNodeFlags_DefaultOpen)) + { + if (s.imageTsNs.empty()) + { + ImGui::TextDisabled("Load session first"); + } + else + { + int nImgs = (int)s.imageTsNs.size(); + ImGui::PushItemWidth(-1); + bool moved = ImGui::SliderInt("##imgidx", &s.imgViewIdx, 0, nImgs - 1); + ImGui::PopItemWidth(); + ImGui::SameLine(0, 4); + ImGui::TextDisabled("%d/%d", s.imgViewIdx + 1, nImgs); + if (moved) + { + s.imgViewIdx = std::clamp(s.imgViewIdx, 0, nImgs - 1); + s.imgViewRequest.store(s.imgViewIdx); + } + ImGui::TextDisabled("ts: %lld", (long long)s.imageTsNs[s.imgViewIdx]); + ImGui::Checkbox("Only this camera's points", &s.isolateCamera); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Render only points colored by the selected image.\nNeeds 'Color by image (RGB)' enabled."); + if (s.imgViewLoading.load()) + ImGui::TextColored(ImVec4(1, 1, 0, 1), "Loading..."); + else if (s.imgViewTexValid) + ImGui::TextColored(ImVec4(0, 1, 0, 1), "%dx%d", s.imgViewTex.width, s.imgViewTex.height); + } + } + + if (ImGui::CollapsingHeader("Export", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::PushItemWidth(-1); + ImGui::Text("Output file (.laz / .las):"); + ImGui::InputText("##out", s.exportBuf, sizeof(s.exportBuf)); + if (ImGui::Button("Browse...##out", ImVec2(-1, 0))) + { + std::string defaultName = fs::path(s.exportBuf).filename().string(); + setBuf( + s.exportBuf, + sizeof(s.exportBuf), + calib::fd::SaveFileDialog("Export colored point cloud", calib::fd::LazFilter, ".laz", defaultName)); + } + if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) + exportLAZ(s); + if (!s.exportCloud.empty()) + ImGui::TextDisabled("%d pts ready to export", (int)s.exportCloud.size()); + ImGui::PopItemWidth(); + } + + if (ImGui::CollapsingHeader("ROS 2 Export")) + { +#ifdef CALIB_ENABLE_ROS_EXPORT + ImGui::PushItemWidth(-1); + ImGui::Text("Output bag directory:"); + ImGui::InputText("##rosout", s.rosOutBuf, sizeof(s.rosOutBuf)); + if (ImGui::Button("Browse...##rosout", ImVec2(-1, 0))) + setBuf(s.rosOutBuf, sizeof(s.rosOutBuf), calib::fd::SelectFolder("Select ROS 2 bag output directory")); + // Scoped narrower width -- see the "Load decimation" comment above. + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::Combo("Storage", &s.rosStorageIdx, "mcap\0sqlite3\0"); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + + ImGui::Separator(); + ImGui::Checkbox("TF + static TF", &s.ros.exportTf); + ImGui::Checkbox("Camera", &s.ros.exportCamera); + if (s.ros.exportCamera) + { + ImGui::Indent(); + ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("ON: CompressedImage (jpeg)\nOFF: raw Image bgr8"); + ImGui::Checkbox("Undistort (rectify)", &s.ros.undistortCamera); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)."); + ImGui::Unindent(); + } + ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); + ImGui::Checkbox("LiDAR raw (sensor frame)", &s.ros.exportLidarRaw); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Re-projects points into the lidar frame per-point\nusing the trajectory (needs poses loaded)."); + + ImGui::Separator(); + // Scoped narrower width -- see the "Load decimation" comment above. + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputDouble("Aggregation (s)", &s.ros.aggregationSec, 0.01, 0.1, "%.3f"); + s.ros.aggregationSec = std::max(0.001, s.ros.aggregationSec); + ImGui::InputInt("LiDAR decimation", &s.ros.lidarDecim); + s.ros.lidarDecim = std::max(1, s.ros.lidarDecim); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + + if (s.rosBusy.load()) + { + ImGui::BeginDisabled(); + ImGui::Button("Exporting...", ImVec2(-1, 0)); + ImGui::EndDisabled(); + } + else if (ImGui::Button("Export ROS 2 bag", ImVec2(-1, 0))) + { + exportRos(s); + } + ImGui::PopItemWidth(); +#else + ImGui::TextDisabled("Not available in this build"); + ImGui::TextDisabled("(rebuild with -DCALIB_ENABLE_ROS_EXPORT=ON)"); +#endif + } + + if (ImGui::CollapsingHeader("COLMAP Export")) + { + ImGui::PushItemWidth(-1); + ImGui::Text("Output project dir:"); + ImGui::InputText("##colmapout", s.colmapBuf, sizeof(s.colmapBuf)); + if (ImGui::Button("Browse...##colmapout", ImVec2(-1, 0))) + setBuf(s.colmapBuf, sizeof(s.colmapBuf), calib::fd::SelectFolder("Select COLMAP output directory")); + ImGui::Checkbox("Copy images into project", &s.colmapCopyImages); + // Scoped narrower width -- see the "Load decimation" comment above. + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + ImGui::InputInt("Point decimation", &s.colmapPtDecim); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); + s.colmapPtDecim = std::max(1, s.colmapPtDecim); + if (ImGui::Button("Export COLMAP model", ImVec2(-1, 0))) + exportColmap(s); + ImGui::TextDisabled("Writes sparse/{cameras,images,points3D}.txt"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Needs calibration + a loaded cloud (for points3D).\n" + "Point COLMAP image_path at the images dir."); + ImGui::PopItemWidth(); + } + + if (!s.status.empty()) + { + ImGui::Separator(); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", s.status.c_str()); + } + + ImGui::Separator(); + ImGui::TextDisabled("LMB: orbit RMB: pan Scroll: zoom"); + + ImGui::End(); + + // ── floating image viewer window ────────────────────────────────────── + if (s.imgViewTexValid) + { + ImGui::SetNextWindowPos(ImVec2(8, 8), ImGuiCond_Once); + ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_Once); + ImGui::Begin("Image##viewer", nullptr, ImGuiWindowFlags_NoScrollbar); + ImVec2 avail = ImGui::GetContentRegionAvail(); + float aspect = (float)s.imgViewTex.height / (float)s.imgViewTex.width; + int dispW = (int)avail.x; + int dispH = (int)(avail.x * aspect); + if (dispH > (int)avail.y) + { + dispH = (int)avail.y; + dispW = (int)(avail.y / aspect); + } + ImVec2 imgPos = ImGui::GetCursorScreenPos(); + rlImGuiImageSize(&s.imgViewTex, dispW, dispH); + // overlay the ROI, mapping full-res image pixels to the displayed rect + if (s.roi.enabled && s.imgViewTex.width > 0 && s.imgViewTex.height > 0) + { + float sx = (float)dispW / s.imgViewTex.width; + float sy = (float)dispH / s.imgViewTex.height; + ImVec2 a(imgPos.x + s.roi.x * sx, imgPos.y + s.roi.y * sy); + ImVec2 b(imgPos.x + (s.roi.x + s.roi.w) * sx, imgPos.y + (s.roi.y + s.roi.h) * sy); + ImGui::GetWindowDrawList()->AddRect( + a, + b, + IM_COL32(0, 255, 0, 255), + /*rounding=*/0.f, + /*thickness=*/2.f); + } + ImGui::End(); + } + + rlImGuiEnd(); + EndDrawing(); + } + + s.imgViewStop = true; + s.imgViewThread.join(); + if (s.rosThread.joinable()) + s.rosThread.join(); + if (s.imgViewTexValid) + UnloadTexture(s.imgViewTex); + + s.cloud.unload(); + if (s.shaderOk) + UnloadShader(s.shader); + rlImGuiShutdown(); + CloseWindow(); + return 0; +} diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt new file mode 100644 index 00000000..cb8a26e6 --- /dev/null +++ b/calib_core/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(calib_core) + +# Shared, GUI-independent logic for the camera_lidar_calibration app family +# (camera_lidar_calibration, camera_lidar_trajectory_viewer, +# camera_lidar_intrinsics_calib) -- LiDAR-camera projection math, LAS/LAZ +# point cloud loading, Mandeye trajectory CSV parsing, and CLI argument +# parsing. Deliberately depends on nothing but Eigen/LASzip/std -- no +# raylib/imgui/OpenCV here -- so it stays reusable and cheap to build for +# tools (like camera_lidar_intrinsics_calib) that don't need the others. +# +# All public types live in namespace calib (see include/CalibCore/*.h) to +# avoid colliding with core's own global (non-namespaced) PointCloud +# (core/include/Core/point_cloud.h), in case a future consumer links both +# calib_core and core/core_raylib in the same binary. +add_library(calib_core STATIC + src/Camera.cpp + src/PointCloud.cpp + src/Trajectory.cpp + src/CliArgs.cpp + src/FileDialog.cpp +) + +target_include_directories(calib_core PUBLIC + include +) + +target_include_directories(calib_core PRIVATE + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + # laszip_api_version.h is generated at configure time into LASzip's own + # binary dir (see 3rdparty/LASzip/CMakeLists.txt's configure_file), not + # exposed via any of its targets' usage requirements -- core's own LASzip + # usage (color_las_loader.cpp) goes through the C++ LASzip/LASreader + # classes instead and never needed this path, so nothing else in the repo + # wires it up. + ${CMAKE_BINARY_DIR}/3rdparty/LASzip/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master +) + +target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) + +if(MSVC) + target_compile_options(calib_core PRIVATE /W4) + target_compile_definitions(calib_core PRIVATE _USE_MATH_DEFINES LASZIP_API_VERSION) +else() + target_compile_options(calib_core PRIVATE -Wall -Wextra) + target_compile_definitions(calib_core PRIVATE LASZIP_API_VERSION) +endif() diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h new file mode 100644 index 00000000..ef315a12 --- /dev/null +++ b/calib_core/include/CalibCore/Camera.h @@ -0,0 +1,57 @@ +#pragma once +#include +#include +#include + +namespace calib +{ + + struct Intrinsics + { + float fx = 800.f, fy = 800.f; + float cx = 640.f, cy = 360.f; + // OpenCV rational distortion model: + // radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + float k1 = 0.f, k2 = 0.f, k3 = 0.f; + float k4 = 0.f, k5 = 0.f, k6 = 0.f; + // tangential + float p1 = 0.f, p2 = 0.f; + }; + + struct Extrinsics + { + // 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; + }; + + // Rectangular region of interest, in full-resolution image pixels. + // When enabled, only pixels inside [x, x+w) x [y, y+h) are considered valid + // (e.g. for coloring a point cloud); everything outside is ignored. + struct Roi + { + bool enabled = false; + 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); + + // 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). + // Returns false if depth <= 0 (behind camera). + bool projectPoint( + float px, + float py, + float pz, + const Intrinsics& K, + const Eigen::Matrix3f& R_wc, + const Eigen::Vector3f& t, + float& u, + float& v, + float& depth); + +} // namespace calib \ No newline at end of file diff --git a/calib_core/include/CalibCore/CliArgs.h b/calib_core/include/CalibCore/CliArgs.h new file mode 100644 index 00000000..6afeeb42 --- /dev/null +++ b/calib_core/include/CalibCore/CliArgs.h @@ -0,0 +1,78 @@ +#pragma once +#include +#include +#include + +namespace calib { + +// Shared command-line parsing for all CalibrationApp tools. +// +// Flags are stored generically in a multimap (key = flag name without the +// leading "--"), so the same parser serves every tool and new flags need no +// parser changes. Each tool just reads the keys it cares about and ignores the +// rest. Recognised conventions: +// +// --mjs session manifest file; the session directory is +// its parent folder (parent_path) +// --camera_dir directory of CAMERA_0 images +// --laz [b.laz ...] one or more point clouds (.laz / .las). May be +// repeated; consecutive non-flag tokens after a +// --laz are all taken as clouds. +// -h, --help print usage and exit +// +// A flag may take several values (each consecutive non-flag token becomes its +// own multimap entry) or none (stored once with an empty value). Tokens that +// don't follow a flag are collected into `positional`, preserving the old +// extension/drag-and-drop behaviour. +struct CliArgs { + std::multimap opts; // flag -> value(s) + std::vector positional; // non-flag arguments, in order + + bool help = false; // -h / --help was given + bool valid = true; // false on a malformed argument + std::string error; // message describing why valid == false + + // True if the flag was present at all (even with an empty value). + bool has(const std::string& key) const { return opts.find(key) != opts.end(); } + + // First value for `key`, or `def` if absent. + std::string get(const std::string& key, const std::string& def = {}) const { + auto it = opts.find(key); + return it == opts.end() ? def : it->second; + } + + // All values for `key`, in the order given on the command line. + std::vector getAll(const std::string& key) const { + std::vector v; + auto range = opts.equal_range(key); + for (auto it = range.first; it != range.second; ++it) v.push_back(it->second); + return v; + } +}; + +// Parse argv. Never terminates the process — the caller inspects `help` and +// `valid` and decides what to do. +CliArgs parseArgs(int argc, char* argv[]); + +// Pre-formatted help lines for the shared flags, so every tool describes the +// same flag the same way. An app passes the subset it actually honours to +// printUsage(); the -h/--help line is always added automatically. +namespace cliopt { +inline constexpr const char* MJS = + " --mjs session manifest file; the session\n" + " directory is its parent folder"; +inline constexpr const char* CAMERA_DIR = + " --camera_dir directory of CAMERA_0 images"; +inline constexpr const char* CALIB = + " --calib calibration file (intrinsic + extrinsic)"; +inline constexpr const char* LAZ = + " --laz [b.laz ...] one or more point clouds (.laz/.las); may repeat"; +} // namespace cliopt + +// Print usage for `appName` listing only `options` (e.g. {cliopt::MJS, ...}). +// `desc` is a one-line summary of the tool. Goes to stdout, or stderr when +// reporting an error (toStderr = true). +void printUsage(const char* appName, const char* desc, + const std::vector& options, bool toStderr = false); + +} // namespace calib \ No newline at end of file diff --git a/calib_core/include/CalibCore/FileDialog.h b/calib_core/include/CalibCore/FileDialog.h new file mode 100644 index 00000000..3c8479b9 --- /dev/null +++ b/calib_core/include/CalibCore/FileDialog.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include + +// Native file/folder picker dialogs (portable-file-dialogs), for the +// camera_lidar_calibration app family. A small, deliberately independent +// copy of core's Core/pfd_wrapper.hpp (namespace mandeye::fd) rather than a +// reuse of it: that wrapper is only built into the GUI-enabled `core` +// target, and linking `core` here would drag in core_math/session/SLAM code +// none of these apps otherwise need. portable-file-dialogs itself is a +// single vendored header (3rdparty/portable-file-dialogs-master) with no +// dependency on `core`, so wrapping it directly is cheap. +namespace calib::fd +{ + namespace internal + { + static std::string lastLocationHint = "."; + } + + const std::vector LazFilter = { "LAS/LAZ files (*.laz, *.las)", "*.laz *.las", "All files", "*" }; + const std::vector ImageFilter = { + "Image files (*.bmp, *.jpg, *.jpeg, *.png)", "*.bmp *.jpg *.jpeg *.png", "All files", "*" + }; + const std::vector CalibJsonFilter = { "Calibration JSON (*.json)", "*.json", "All files", "*" }; + const std::vector IntrinsicsFilter = { + "Camera intrinsics (*.json, *.yml, *.yaml)", "*.json *.yml *.yaml", "All files", "*" + }; + const std::vector SessionManifestFilter = { "Mandeye session manifest (*.mjs)", "*.mjs", "All files", "*" }; + + // Returns "" if the dialog was cancelled. + std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter); + + // Returns an empty vector if the dialog was cancelled. + std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect); + + // Returns "" if the dialog was cancelled. + std::string SaveFileDialog( + const std::string& title, + const std::vector& filter, + const std::string& defaultExtension = "", + const std::string& defaultFileName = ""); + + // Returns "" if the dialog was cancelled. + std::string SelectFolder(const std::string& title); +} // namespace calib::fd diff --git a/calib_core/include/CalibCore/PointCloud.h b/calib_core/include/CalibCore/PointCloud.h new file mode 100644 index 00000000..2e1a8e2c --- /dev/null +++ b/calib_core/include/CalibCore/PointCloud.h @@ -0,0 +1,26 @@ +#pragma once +#include +#include + +#include + +namespace calib { + +struct Point3D { + float x, y, z; + float intensity; // normalized to [0,1] + int64_t ts_ns = 0; // GPS time cast from gps_time field (0 if unavailable) +}; + +struct PointCloud { + std::vector points; + float minX = 0.f, maxX = 0.f; + float minY = 0.f, maxY = 0.f; + float minZ = 0.f, maxZ = 0.f; + + bool load(const std::string& path); + void clear(); + bool empty() const { return points.empty(); } +}; + +} // namespace calib diff --git a/calib_core/include/CalibCore/Trajectory.h b/calib_core/include/CalibCore/Trajectory.h new file mode 100644 index 00000000..eb880df7 --- /dev/null +++ b/calib_core/include/CalibCore/Trajectory.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include +#include +#include + +namespace calib { + +// One LiDAR pose from the trajectory CSV. +// T = T_world_lidar: p_world = T * p_lidar +struct TrajPose { + int64_t ts_ns = 0; + Eigen::Affine3f T = Eigen::Affine3f::Identity(); +}; + +struct Trajectory { + std::vector poses; + + // Load one trajectory_lio_N.csv. Appends to poses. + // If mrp != nullptr it is applied to every pose: T_corrected = *mrp * T_pose. + bool loadCSV(const std::string& path, const Eigen::Affine3f* mrp = nullptr); + + void sort(); + + const TrajPose* nearest(int64_t ts_ns) const; + + bool empty() const { return poses.empty(); } +}; + +} // namespace calib \ No newline at end of file diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp new file mode 100644 index 00000000..6e93d4ca --- /dev/null +++ b/calib_core/src/Camera.cpp @@ -0,0 +1,40 @@ +#include + +namespace calib { + +Eigen::Matrix3f eulerZYXtoMat3(float rx_deg, float ry_deg, float rz_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())) + .toRotationMatrix(); +} + +bool projectPoint(float px, float py, float pz, + const Intrinsics& K, + const Eigen::Matrix3f& R_wc, + const Eigen::Vector3f& t, + float& u, float& v, float& depth) { + // p_cam = R_wc^T * (p_lidar - C) + Eigen::Vector3f pc = R_wc.transpose() * (Eigen::Vector3f(px, py, pz) - t); + + depth = pc.z(); + if (depth <= 1e-4f) return false; + + float xn = pc.x() / depth; + float yn = pc.y() / depth; + + float r2 = xn*xn + yn*yn; + float r4 = r2 * r2; + float r6 = r4 * r2; + float radial = (1.f + K.k1*r2 + K.k2*r4 + K.k3*r6) + / (1.f + K.k4*r2 + K.k5*r4 + K.k6*r6); + float xd = xn*radial + 2.f*K.p1*xn*yn + K.p2*(r2 + 2.f*xn*xn); + float yd = yn*radial + K.p1*(r2 + 2.f*yn*yn) + 2.f*K.p2*xn*yn; + + u = K.fx * xd + K.cx; + v = K.fy * yd + K.cy; + return true; +} + +} // namespace calib \ No newline at end of file diff --git a/calib_core/src/CliArgs.cpp b/calib_core/src/CliArgs.cpp new file mode 100644 index 00000000..c0ea597a --- /dev/null +++ b/calib_core/src/CliArgs.cpp @@ -0,0 +1,57 @@ +#include +#include + +namespace calib { + +// A token is treated as a flag (and therefore stops value collection for the +// previous flag) when it starts with '-' and is more than a single '-'. +static bool isFlag(const char* tok) { + return tok[0] == '-' && tok[1] != '\0'; +} + +CliArgs parseArgs(int argc, char* argv[]) { + CliArgs a; + int i = 1; + while (i < argc) { + const char* tok = argv[i]; + + if (std::string(tok) == "-h" || std::string(tok) == "--help") { + a.help = true; + ++i; + continue; + } + + if (tok[0] == '-' && tok[1] == '-' && tok[2] != '\0') { + std::string key = tok + 2; // strip leading "--" + ++i; + // Collect every consecutive non-flag token as a value for this key. + bool any = false; + while (i < argc && !isFlag(argv[i])) { + a.opts.emplace(key, argv[i]); + ++i; + any = true; + } + if (!any) a.opts.emplace(key, std::string{}); // valueless flag + } else if (isFlag(tok)) { + a.valid = false; + a.error = std::string("unknown option: ") + tok; + ++i; + } else { + a.positional.emplace_back(tok); + ++i; + } + } + return a; +} + +void printUsage(const char* appName, const char* desc, + const std::vector& options, bool toStderr) { + std::FILE* f = toStderr ? stderr : stdout; + std::fprintf(f, "%s — %s\n\n", appName, desc); + std::fprintf(f, "Usage: %s [options] [files...]\n\nOptions:\n", appName); + for (const auto& o : options) + std::fprintf(f, "%s\n", o.c_str()); + std::fprintf(f, " -h, --help show this help and exit\n"); +} + +} // namespace calib \ No newline at end of file diff --git a/calib_core/src/FileDialog.cpp b/calib_core/src/FileDialog.cpp new file mode 100644 index 00000000..77dfed70 --- /dev/null +++ b/calib_core/src/FileDialog.cpp @@ -0,0 +1,65 @@ +#include + +#include + +#include + +namespace calib::fd +{ + std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter) + { + auto sel = OpenFileDialog(title, filter, false); + if (sel.empty()) + return ""; + + return std::filesystem::path(sel.back()).lexically_normal().string(); + } + + std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect) + { + std::vector files = pfd::open_file(title, internal::lastLocationHint, filter, multiselect).result(); + + for (auto& f : files) + f = std::filesystem::path(f).lexically_normal().string(); + + if (!files.empty()) + { + std::filesystem::path pfile(files.back()); + if (pfile.has_parent_path()) + internal::lastLocationHint = pfile.parent_path().string(); + } + return files; + } + + std::string SaveFileDialog( + const std::string& title, + const std::vector& filter, + const std::string& defaultExtension, + const std::string& defaultFileName) + { + std::string defaultPath = internal::lastLocationHint; + if (!defaultFileName.empty()) + defaultPath = (std::filesystem::path(internal::lastLocationHint) / defaultFileName).string(); + + std::string file = pfd::save_file(title, defaultPath, filter).result(); + if (file.empty()) + return file; + + std::filesystem::path pfile(file); + if (!pfile.has_extension()) + file += defaultExtension; + + if (pfile.has_parent_path()) + internal::lastLocationHint = pfile.parent_path().string(); + + return file; + } + + std::string SelectFolder(const std::string& title) + { + std::string folder = pfd::select_folder(title, internal::lastLocationHint).result(); + if (!folder.empty()) + internal::lastLocationHint = folder; + return folder; + } +} // namespace calib::fd diff --git a/calib_core/src/PointCloud.cpp b/calib_core/src/PointCloud.cpp new file mode 100644 index 00000000..df60d6e5 --- /dev/null +++ b/calib_core/src/PointCloud.cpp @@ -0,0 +1,90 @@ +#include +#include +#include +#include + +namespace calib { + +void PointCloud::clear() { + points.clear(); + minX = maxX = minY = maxY = minZ = maxZ = 0.f; +} + +bool PointCloud::load(const std::string& path) { + clear(); + + laszip_POINTER reader = nullptr; + if (laszip_create(&reader) != 0) { + fprintf(stderr, "laszip_create failed\n"); + return false; + } + + laszip_BOOL is_compressed = 0; + if (laszip_open_reader(reader, path.c_str(), &is_compressed) != 0) { + laszip_CHAR* err = nullptr; + laszip_get_error(reader, &err); + fprintf(stderr, "Cannot open %s: %s\n", path.c_str(), err ? err : "?"); + laszip_destroy(reader); + return false; + } + + laszip_header_struct* header = nullptr; + laszip_get_header_pointer(reader, &header); + + laszip_I64 npoints = (header->number_of_point_records > 0) + ? static_cast(header->number_of_point_records) + : static_cast(header->extended_number_of_point_records); + + laszip_point_struct* point = nullptr; + laszip_get_point_pointer(reader, &point); + + points.reserve(static_cast(npoints)); + + float minInt = std::numeric_limits::max(); + float maxInt = -std::numeric_limits::max(); + float xmin = 1e38f, xmax = -1e38f; + float ymin = 1e38f, ymax = -1e38f; + float zmin = 1e38f, zmax = -1e38f; + + for (laszip_I64 i = 0; i < npoints; ++i) { + if (laszip_read_point(reader) != 0) break; + + laszip_F64 coords[3]; + laszip_get_coordinates(reader, coords); + + Point3D p; + p.x = static_cast(coords[0]); + p.y = static_cast(coords[1]); + p.z = static_cast(coords[2]); + p.intensity = static_cast(point->intensity); + // gps_time is GPS time in seconds (LAS spec); ts_ns is nanoseconds + // everywhere else it's used (Trajectory, image matching), so convert here. + p.ts_ns = static_cast(point->gps_time * 1e9); + + if (p.x < xmin) xmin = p.x; if (p.x > xmax) xmax = p.x; + if (p.y < ymin) ymin = p.y; if (p.y > ymax) ymax = p.y; + if (p.z < zmin) zmin = p.z; if (p.z > zmax) zmax = p.z; + if (p.intensity < minInt) minInt = p.intensity; + if (p.intensity > maxInt) maxInt = p.intensity; + + points.push_back(p); + } + + // Normalize intensity to [0,1] + float intRange = (maxInt > minInt) ? (maxInt - minInt) : 1.f; + for (auto& p : points) + p.intensity = (p.intensity - minInt) / intRange; + + minX = xmin; maxX = xmax; + minY = ymin; maxY = ymax; + minZ = zmin; maxZ = zmax; + + laszip_close_reader(reader); + laszip_destroy(reader); + + printf("Loaded %zu points from %s\n", points.size(), path.c_str()); + return true; + +} + +} // namespace calib diff --git a/calib_core/src/Trajectory.cpp b/calib_core/src/Trajectory.cpp new file mode 100644 index 00000000..a5328521 --- /dev/null +++ b/calib_core/src/Trajectory.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include + +namespace calib { + +bool Trajectory::loadCSV(const std::string& path, const Eigen::Affine3f* mrp) { + std::ifstream f(path); + if (!f) return false; + + std::string line; + std::getline(f, line); // skip header + + while (std::getline(f, line)) { + if (line.empty()) continue; + std::istringstream ss(line); + TrajPose p; + float raw[12]; + ss >> p.ts_ns; + for (int i = 0; i < 12; i++) ss >> raw[i]; + if (!ss) continue; + + // row-major 3×4 → Affine3f + p.T.linear() << raw[0], raw[1], raw[2], + raw[4], raw[5], raw[6], + raw[8], raw[9], raw[10]; + p.T.translation() << raw[3], raw[7], raw[11]; + + if (mrp) p.T = *mrp * p.T; + poses.push_back(p); + } + return true; +} + +void Trajectory::sort() { + std::sort(poses.begin(), poses.end(), + [](const TrajPose& a, const TrajPose& b){ return a.ts_ns < b.ts_ns; }); +} + +const TrajPose* Trajectory::nearest(int64_t ts_ns) const { + if (poses.empty()) return nullptr; + auto it = std::lower_bound(poses.begin(), poses.end(), ts_ns, + [](const TrajPose& p, int64_t t){ return p.ts_ns < t; }); + if (it == poses.end()) return &poses.back(); + if (it == poses.begin()) return &poses.front(); + auto prev = std::prev(it); + return (std::abs(it->ts_ns - ts_ns) < std::abs(prev->ts_ns - ts_ns)) ? &*it : &*prev; +} + +} // namespace calib \ No newline at end of file diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a13e0ef9..bc4d409a 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -7,26 +7,50 @@ set(CORE_BASE_SOURCES src/control_points.cpp src/gnss.cpp src/ground_control_points.cpp + src/imu_preintegration.cpp + src/nmea.cpp + src/point_cloud.cpp + src/point_clouds.cpp + src/session.cpp + # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... +) + +# core_math holds the registration/optimization sources with auto-generated Jacobian headers (up to ~24k chars/line, expensive to compile); built once as a static lib shared by core and core_no_gui since none of it branches on WITH_GUI. +# hash_utils.cpp lives here (not CORE_BASE_SOURCES) because pair_wise_iterative_closest_point.cpp needs get_rgd_index_3d() from it -- keeping both in the same archive avoids a circular static-lib link dependency between core_math and core/core_no_gui. +set(CORE_MATH_SOURCES src/hash_utils.cpp src/icp.cpp - src/imu_preintegration.cpp src/ndt.cpp - src/nmea.cpp src/optimization_point_to_point_source_to_target.cpp src/optimize_distance_point_to_plane_source_to_target.cpp src/optimize_plane_to_plane_source_to_target.cpp src/optimize_point_to_plane_source_to_target.cpp src/optimize_point_to_projection_onto_plane_source_to_target.cpp src/pair_wise_iterative_closest_point.cpp - src/point_cloud.cpp - src/point_clouds.cpp src/pose_graph_loop_closure.cpp src/pose_graph_slam.cpp src/registration_plane_feature.cpp - src/session.cpp - # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) +add_library(core_math STATIC ${CORE_MATH_SOURCES}) +target_compile_definitions(core_math PRIVATE WITH_GUI=0) +target_link_libraries(core_math PRIVATE PROJ::proj spdlog::spdlog vqf Fusion wgs84_do_puwg92 plycpp WGS84toCartesian) +target_include_directories(core_math PRIVATE + include + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/include + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion +) +set_target_properties(core_math PROPERTIES POSITION_INDEPENDENT_CODE ON) +if(NOT MSVC) + # DWARF generation for these auto-generated Jacobian expression trees dominates -g compile time far more than -O2 codegen, so RelWithDebInfo builds this target without -g. + target_compile_options(core_math PRIVATE $<$:-g0>) +endif() + set(CORE_GUI_SOURCES src/manual_pose_graph_loop_closure.cpp src/observation_picking.cpp @@ -44,7 +68,7 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} PROJ::proj spdlog::spdlog vqf Fusion wgs84_do_puwg92 plycpp WGS84toCartesian) + target_link_libraries(${target_name} PRIVATE core_math ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} PROJ::proj spdlog::spdlog vqf Fusion wgs84_do_puwg92 plycpp WGS84toCartesian) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR}