From 3a97ae4ca4b4ac21853fa9785cda46535f478c5b Mon Sep 17 00:00:00 2001 From: Michal Date: Thu, 30 Jul 2026 22:54:24 +0200 Subject: [PATCH 01/13] Compilation with ReleaseWithDebugInfo stucks on Xcode. We compile automatic jacoians (24 k chars headers) without debug symbols. --- core/CMakeLists.txt | 39 +++++++++++++++++++++++++++++++-------- 1 file changed, 31 insertions(+), 8 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a13e0ef9..9e1967f1 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -8,25 +8,48 @@ set(CORE_BASE_SOURCES src/gnss.cpp src/ground_control_points.cpp src/hash_utils.cpp - src/icp.cpp src/imu_preintegration.cpp - src/ndt.cpp src/nmea.cpp + src/pair_wise_iterative_closest_point.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. +set(CORE_MATH_SOURCES + src/icp.cpp + src/ndt.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 +67,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} From 0e0dbbdbf57bab9a21d95deeca91d2acc5c6b746 Mon Sep 17 00:00:00 2001 From: Michal Date: Sat, 1 Aug 2026 17:55:34 +0200 Subject: [PATCH 02/13] Fix circular core/core_math link dependency pair_wise_iterative_closest_point.cpp stayed in CORE_BASE_SOURCES while pose_graph_loop_closure.cpp (which calls PairWiseICP::compute) moved to CORE_MATH_SOURCES, splitting a symbol and its only definition across two static libs with a link-order dependency in the wrong direction. This broke linking for any executable that pulls in PoseGraphLoopClosure without referencing PairWiseICP directly, e.g. multi_view_tls_registration_step_2. pair_wise_iterative_closest_point.cpp also includes an auto-generated Jacobian header, so it belongs in core_math anyway. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- core/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 9e1967f1..dedd44cc 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -10,7 +10,6 @@ set(CORE_BASE_SOURCES src/hash_utils.cpp src/imu_preintegration.cpp src/nmea.cpp - src/pair_wise_iterative_closest_point.cpp src/point_cloud.cpp src/point_clouds.cpp src/session.cpp @@ -26,6 +25,7 @@ set(CORE_MATH_SOURCES 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/pose_graph_loop_closure.cpp src/pose_graph_slam.cpp src/registration_plane_feature.cpp From c8430cc126d89fd93e78287db2a5a6b53d4c8ca7 Mon Sep 17 00:00:00 2001 From: Michal Date: Sat, 1 Aug 2026 18:34:55 +0200 Subject: [PATCH 03/13] Move hash_utils.cpp into core_math too pair_wise_iterative_closest_point.cpp (moved into core_math in the previous commit) calls get_rgd_index_3d(), which lived in hash_utils.cpp under CORE_BASE_SOURCES -- reintroducing the same cross-archive circular dependency, just in the opposite direction (core_math needing a symbol from core/core_no_gui instead of the other way around). Nothing else in CORE_BASE_SOURCES/CORE_GUI_SOURCES calls into core_math, so moving hash_utils.cpp there too makes the dependency one-directional again (core/core_no_gui -> core_math, never the reverse). Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- core/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index dedd44cc..bc4d409a 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -7,7 +7,6 @@ set(CORE_BASE_SOURCES src/control_points.cpp src/gnss.cpp src/ground_control_points.cpp - src/hash_utils.cpp src/imu_preintegration.cpp src/nmea.cpp src/point_cloud.cpp @@ -17,7 +16,9 @@ set(CORE_BASE_SOURCES ) # 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/ndt.cpp src/optimization_point_to_point_source_to_target.cpp From 13f552825b5e33eaae18f98c23476165a84454b1 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Sun, 2 Aug 2026 00:04:28 +0200 Subject: [PATCH 04/13] Import calibration/trajectory-viewer/intrinsics tools from mandeye-colors Ports the three raylib/ImGui tools from the sibling mandeye-colors repo as new apps (camera_lidar_calibration, camera_lidar_trajectory_viewer, camera_lidar_intrinsics_calib), backed by a new calib_core static library for their shared non-GUI logic (camera projection math, LAS/LAZ loading, trajectory CSV parsing, CLI args). Reuses HDMapping's existing raylib/ imgui_raylib/rlimgui/Eigen/LASzip/OpenCV/json wiring instead of vendoring mandeye-colors' own duplicate copies of those dependencies. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- CMakeLists.txt | 4 + apps/camera_lidar_calibration/App.cpp | 448 ++++++ apps/camera_lidar_calibration/App.h | 72 + apps/camera_lidar_calibration/CMakeLists.txt | 61 + apps/camera_lidar_calibration/Renderer.cpp | 468 +++++++ apps/camera_lidar_calibration/Renderer.h | 90 ++ apps/camera_lidar_calibration/UI.cpp | 291 ++++ apps/camera_lidar_calibration/UI.h | 32 + apps/camera_lidar_calibration/main.cpp | 75 + .../CMakeLists.txt | 51 + .../IntrinsicsCalib.cpp | 456 +++++++ .../CMakeLists.txt | 106 ++ .../RosExport.cpp | 366 +++++ .../RosExport.h | 78 ++ .../TrajectoryViewer.cpp | 1203 +++++++++++++++++ calib_core/CMakeLists.txt | 48 + calib_core/include/CalibCore/Camera.h | 40 + calib_core/include/CalibCore/CliArgs.h | 78 ++ calib_core/include/CalibCore/PointCloud.h | 26 + calib_core/include/CalibCore/Trajectory.h | 30 + calib_core/src/Camera.cpp | 40 + calib_core/src/CliArgs.cpp | 57 + calib_core/src/PointCloud.cpp | 88 ++ calib_core/src/Trajectory.cpp | 51 + 24 files changed, 4259 insertions(+) create mode 100644 apps/camera_lidar_calibration/App.cpp create mode 100644 apps/camera_lidar_calibration/App.h create mode 100644 apps/camera_lidar_calibration/CMakeLists.txt create mode 100644 apps/camera_lidar_calibration/Renderer.cpp create mode 100644 apps/camera_lidar_calibration/Renderer.h create mode 100644 apps/camera_lidar_calibration/UI.cpp create mode 100644 apps/camera_lidar_calibration/UI.h create mode 100644 apps/camera_lidar_calibration/main.cpp create mode 100644 apps/camera_lidar_intrinsics_calib/CMakeLists.txt create mode 100644 apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp create mode 100644 apps/camera_lidar_trajectory_viewer/CMakeLists.txt create mode 100644 apps/camera_lidar_trajectory_viewer/RosExport.cpp create mode 100644 apps/camera_lidar_trajectory_viewer/RosExport.h create mode 100644 apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp create mode 100644 calib_core/CMakeLists.txt create mode 100644 calib_core/include/CalibCore/Camera.h create mode 100644 calib_core/include/CalibCore/CliArgs.h create mode 100644 calib_core/include/CalibCore/PointCloud.h create mode 100644 calib_core/include/CalibCore/Trajectory.h create mode 100644 calib_core/src/Camera.cpp create mode 100644 calib_core/src/CliArgs.cpp create mode 100644 calib_core/src/PointCloud.cpp create mode 100644 calib_core/src/Trajectory.cpp 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..34985927 --- /dev/null +++ b/apps/camera_lidar_calibration/App.cpp @@ -0,0 +1,448 @@ +#include "App.h" +#include "rlImGui.h" +#include "imgui.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); +} + +// ── 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"); + 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..0ce0d5b8 --- /dev/null +++ b/apps/camera_lidar_calibration/UI.cpp @@ -0,0 +1,291 @@ +#include "UI.h" +#include "App.h" +#include "imgui.h" +#include "rlImGui.h" +#include +#include +#include +#include +#include + +// 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("Load Image##btn", ImVec2(-1, 0))) + state.loadImage(imagePathBuf); + + ImGui::Spacing(); + ImGui::Text("LAZ/LAS point cloud:"); + ImGui::InputText("##laz", cloudPathBuf, sizeof(cloudPathBuf)); + { + 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("Load Intrinsics##btn", ImVec2(-1, 0))) + state.loadIntrinsics(intrPathBuf); + + ImGui::Separator(); + ImGui::Text("Calibration JSON:"); + ImGui::InputText("##save", savePath, sizeof(savePath)); + 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; + + ImGui::PushItemWidth(-1); + 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..84b57f14 --- /dev/null +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -0,0 +1,456 @@ +#include "raylib.h" +#include "rlImGui.h" +#include "imgui.h" +#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; + +// ── 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"); + 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("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("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..e219c4c6 --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -0,0 +1,1203 @@ +#include "raylib.h" +#include "rlgl.h" +#include "raymath.h" +#include "external/glad.h" +#include "rlImGui.h" +#include "imgui.h" +#include +#include +#include +#include "RosExport.h" +#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; + +// ── GPU point cloud shader ──────────────────────────────────────────────────── +// colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB +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 +uniform mat4 mvp; +uniform float pointSize; +uniform int drawDecim; +out float fragIntensity; +out vec4 vertColor; +flat out float fragColorCameraId; +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; +} +)"; +static const char* kFS = R"( +#version 330 +in float fragIntensity; +in vec4 vertColor; +flat in float fragColorCameraId; +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); +} +void main() { + if (colorMode == 1) + { + if (selectedCamera < 0) + { + finalColor = vertColor; + } + else + { + if (selectedCamera == int(fragColorCameraId)) + finalColor = vertColor; + else + discard; // render only points colored from the selected camera + } + } + 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 = 6 * 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); + rlDisableVertexArray(); + count = (int)(data.size() / 6); + } + 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; + 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 = 1; + int drawDecim = 1; + bool multiImgColoring = true; // false = single image per chunk (midpoint) + bool useImageColor = false; + + 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; + + 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; + const TrajPose* pose; + cv::Mat img; + int globalIdx; // index into s.imageTsNs (== imgViewIdx / selectedCamera) + }; + + 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; + + 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; + const TrajPose* pose = s.traj.nearest(imgTs); + if (!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 + chunkLast) / 2; + 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); + const TrajPose* pose = s.traj.nearest(imgTs); + if (fnIt != s.imagesFilenamesInTime.end() && 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 + + 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()); + } + + // try images expanding outward from startIdx; first frustum hit wins + auto tryImg = [&](int idx) -> bool { + if (idx < 0 || idx >= nImgs) return false; + auto& e = chunkImgs[idx]; + Eigen::Vector3f pl = e.pose->T.inverse() * pw; + Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); + if (pc_.z() <= 0.05f) return false; + int iu = (int)std::round(K_fx * pc_.x() / pc_.z() + K_cx); + int iv = (int)std::round(K_fy * pc_.y() / pc_.z() + K_cy); + if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) return false; + 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(&colorF, &p, 4); + camIdF = (float)e.globalIdx; + return true; + }; + + if (!tryImg(startIdx)) { + for (int delta = 1; delta < nImgs; ++delta) { + if (tryImg(startIdx + delta)) break; + if (tryImg(startIdx - delta)) break; + } + } + } + + gpuData.push_back(colorF); + gpuData.push_back(rawIntensity); + gpuData.push_back(camIdF); + + 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 (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" : ""); +} + +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]; + } + } + 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"; + int id = 1; + for (auto& [ts, path] : s.imagesFilenamesInTime) { + const TrajPose* pose = s.traj.nearest(ts); + if (!pose) continue; + Eigen::Affine3f T_wc = pose->T * 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); + int cm = s.useImageColor ? 1 : 0; + rlSetUniform(s.locCM, &cm, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(s.locDecim, &s.drawDecim, RL_SHADER_UNIFORM_INT, 1); + int sel = (s.isolateCamera && s.useImageColor && + 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"); + 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.useImageColor = !s.useImageColor; + + 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)); + ImGui::Text("CAMERA_0 directory (empty = auto):"); + ImGui::InputText("##cam", s.cameraBuf, sizeof(s.cameraBuf)); + 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(); + ImGui::InputInt("Load decimation", &s.cloudDecim); + 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)"); + 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("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); + ImGui::InputInt("Image W", &s.imgW); + ImGui::InputInt("Image H", &s.imgH); + } + 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::Checkbox("Color by image (RGB)", &s.useImageColor); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Ctrl toggles intensity (jet) <-> RGB"); + } + } + + 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("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)); + ImGui::Combo("Storage", &s.rosStorageIdx, "mcap\0sqlite3\0"); + + 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(); + 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); + + 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)); + ImGui::Checkbox("Copy images into project", &s.colmapCopyImages); + ImGui::InputInt("Point decimation", &s.colmapPtDecim); + 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); } + rlImGuiImageSize(&s.imgViewTex, dispW, dispH); + 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; +} \ No newline at end of file diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt new file mode 100644 index 00000000..7035ca45 --- /dev/null +++ b/calib_core/CMakeLists.txt @@ -0,0 +1,48 @@ +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 +) + +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 +) + +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..9cc11383 --- /dev/null +++ b/calib_core/include/CalibCore/Camera.h @@ -0,0 +1,40 @@ +#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; +}; + +// 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/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/PointCloud.cpp b/calib_core/src/PointCloud.cpp new file mode 100644 index 00000000..c44fc9a3 --- /dev/null +++ b/calib_core/src/PointCloud.cpp @@ -0,0 +1,88 @@ +#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); + p.ts_ns = static_cast(point->gps_time); + + 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 From 2eacb4406853dcce4cdafe77e948dc5ab45916a6 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Sun, 2 Aug 2026 00:14:33 +0200 Subject: [PATCH 05/13] Add native file/folder picker dialogs to the camera_lidar_* apps All three text fields that take a file or directory path (image, point cloud, intrinsics, calibration, session/CAMERA_0 dirs, export/ROS/COLMAP output paths) now have a "Browse..." button backed by portable-file-dialogs, matching the mandeye::fd wrapper core already uses elsewhere in HDMapping. Implemented as calib_core's own calib::fd (rather than reusing core/include/Core/pfd_wrapper.hpp directly) since that wrapper only builds into the GUI-enabled `core` target, and linking `core` here would pull in core_math/session/SLAM code these apps otherwise don't depend on -- portable-file-dialogs itself is a single vendored header with no dependency on `core`, so wrapping it directly in calib_core is cheap. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- apps/camera_lidar_calibration/UI.cpp | 186 +-- .../IntrinsicsCalib.cpp | 427 ++++--- .../TrajectoryViewer.cpp | 1136 +++++++++++------ calib_core/CMakeLists.txt | 2 + calib_core/include/CalibCore/FileDialog.h | 46 + calib_core/src/FileDialog.cpp | 65 + 6 files changed, 1202 insertions(+), 660 deletions(-) create mode 100644 calib_core/include/CalibCore/FileDialog.h create mode 100644 calib_core/src/FileDialog.cpp diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 0ce0d5b8..999535c8 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -2,23 +2,37 @@ #include "App.h" #include "imgui.h" #include "rlImGui.h" -#include -#include -#include +#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; +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) { +static void helpMarker(const char* desc) +{ ImGui::SameLine(); ImGui::TextDisabled("(?)"); - if (ImGui::IsItemHovered()) { + if (ImGui::IsItemHovered()) + { ImGui::BeginTooltip(); ImGui::TextUnformatted(desc); ImGui::EndTooltip(); @@ -26,25 +40,30 @@ static void helpMarker(const char* desc) { } // ── Main draw ──────────────────────────────────────────────────────────────── -void UI::draw(AppState& state) { +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::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::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)) { + 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 + if (cm == 3) + cm = 1; // RGB → Intensity + else + cm = 3; // anything → RGB } panelStatus(state); @@ -62,14 +81,16 @@ void UI::draw(AppState& 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; + 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); + ImGui::Begin( + "Image View", + nullptr, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoTitleBar); drawImageView(state); ImGui::End(); } @@ -79,20 +100,26 @@ void UI::draw(AppState& state) { // 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) { +void UI::drawImageView(AppState& state) +{ const float imgW = (float)state.imageW; const float imgH = (float)state.imageH; - if (imgW <= 0 || imgH <= 0) return; + 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; + 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; + 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; @@ -100,23 +127,23 @@ void UI::drawImageView(AppState& state) { // 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; + 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); + 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); + Rectangle src = { offX, imgH - offY, srcW, -srcH }; + rlImGuiImageRect(&state.renderer.imageTex.texture, (int)dispW, (int)dispH, src); // ── input ────────────────────────────────────────────────────────────── - if (ImGui::IsWindowHovered()) { + if (ImGui::IsWindowHovered()) + { ImGuiIO& io = ImGui::GetIO(); - if (io.MouseWheel != 0.f) { + 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; @@ -124,39 +151,46 @@ void UI::drawImageView(AppState& state) { // float iy = offY + my / scale; zoom2D = std::max(1.f, std::min(zoom2D * std::exp(io.MouseWheel * 0.15f), 100.f)); - scale = fitScale * zoom2D; + scale = fitScale * zoom2D; // offX = ix - mx / scale; // offY = iy - my / scale; } - if (ImGui::IsMouseDragging(ImGuiMouseButton_Left)) { + 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; + 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); + 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) { +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))) @@ -169,12 +203,16 @@ void UI::panelFiles(AppState& state) { 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); @@ -186,13 +224,14 @@ void UI::panelFiles(AppState& state) { } // ── Intrinsics ──────────────────────────────────────────────────────────────── -void UI::panelIntrinsics(AppState& state) { +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) { + 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(); }; @@ -221,7 +260,8 @@ void UI::panelIntrinsics(AppState& state) { } // ── Extrinsics ──────────────────────────────────────────────────────────────── -void UI::panelExtrinsics(AppState& state) { +void UI::panelExtrinsics(AppState& state) +{ Extrinsics& E = state.extrinsics; ImGui::PushItemWidth(-80.f); @@ -239,53 +279,57 @@ void UI::panelExtrinsics(AppState& state) { 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))) + if (ImGui::Button("Reset Extrinsics", ImVec2(-1, 0))) E = Extrinsics{}; ImGui::PopItemWidth(); // Show current rotation matrix - if (ImGui::TreeNode("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)); + 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) { +void UI::panelVisualization(AppState& state) +{ VisualizationParams& vp = state.vizParams; ImGui::PushItemWidth(-1); - 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); + 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"}; + 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) { +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); + 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"); + 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()); + 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"); + } + 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()); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", state.statusMsg.c_str()); } diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index 84b57f14..db18ef01 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -1,104 +1,128 @@ +#include "imgui.h" #include "raylib.h" #include "rlImGui.h" -#include "imgui.h" #include +#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 +#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'; +} + // ── per-image state ─────────────────────────────────────────────────────────── -struct CalibImage { +struct CalibImage +{ std::string path; - cv::Mat rgb; + cv::Mat rgb; std::vector corners; - bool detected = false; + bool detected = false; bool processed = false; }; // ── application state ───────────────────────────────────────────────────────── -struct State { +struct State +{ // board parameters - int boardCols = 10; // inner corner count - int boardRows = 7; - float squareMm = 25.f; + int boardCols = 10; // inner corner count + int boardRows = 7; + float squareMm = 25.f; // loaded images - char dirBuf[512] = {}; + char dirBuf[512] = {}; std::vector images; - int currentIdx = 0; + int currentIdx = 0; // display texture (current image + drawn corners) - Texture2D tex = {}; - bool texOk = false; - int texIdx = -1; // which image is on GPU + 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; + bool calibrated = false; + double rmsError = 0.0; + cv::Mat K, D; + cv::Size imageSize; // output - char outPath[512] = "intrinsics.json"; + 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; + 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 { + bool isDetecting() const + { int p = detectProgress.load(); return p >= 0 && p < detectTotal; } }; // ── helpers ─────────────────────────────────────────────────────────────────── -static void loadDir(State& s) { +static void loadDir(State& s) +{ s.images.clear(); s.calibrated = false; s.currentIdx = 0; - s.texIdx = -1; + s.texIdx = -1; fs::path dir(s.dirBuf); - if (!fs::is_directory(dir)) { + 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"}; + 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; + 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; } + if (ext == x) + { + paths.push_back(e.path().string()); + break; + } } std::sort(paths.begin(), paths.end()); - for (auto& p : paths) { + for (auto& p : paths) + { cv::Mat bgr = cv::imread(p, cv::IMREAD_COLOR); - if (bgr.empty()) continue; + if (bgr.empty()) + continue; CalibImage ci; ci.path = p; cv::cvtColor(bgr, ci.rgb, cv::COLOR_BGR2RGB); @@ -107,12 +131,16 @@ static void loadDir(State& s) { 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; +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; + for (auto& ci : s.images) + ci.processed = false; s.detectTotal = (int)s.images.size(); s.detectStop.store(false); @@ -122,50 +150,60 @@ static void detectAll(State& s) { // 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 (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) - 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)); + 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; } - ci.detected = found; - ci.processed = true; - } - s.detectProgress.store(s.detectTotal); - }); + s.detectProgress.store(s.detectTotal); + }); } -static void runCalibration(State& s) { +static void runCalibration(State& s) +{ std::vector objPts; objPts.reserve(s.boardCols * s.boardRows); for (int r = 0; r < s.boardRows; r++) @@ -174,14 +212,17 @@ static void runCalibration(State& s) { std::vector> allObj; std::vector> allImg; - for (auto& ci : s.images) { - if (!ci.detected) continue; + 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) { + if ((int)allObj.size() < 4) + { s.statusMsg = "Need at least 4 images with detected corners"; return; } @@ -190,85 +231,91 @@ static void runCalibration(State& s) { 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.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)"; + 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; +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; }; + 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; + 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); + 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; +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; + if (s.currentIdx == s.texIdx) + return; - auto& ci = s.images[s.currentIdx]; + auto& ci = s.images[s.currentIdx]; cv::Mat display = ci.rgb.clone(); - if (ci.processed) { + 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::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; + 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.tex = LoadTextureFromImage(img); s.texOk = true; s.texIdx = s.currentIdx; } // ── entry point ─────────────────────────────────────────────────────────────── -int main(int argc, char* argv[]) { +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}); + if (args.help) + { + printUsage("IntrinsicsCalib", "Camera intrinsics calibration from a folder of images", { cliopt::CAMERA_DIR }); return 0; } - if (!args.valid) { + 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); + 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()) { + 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); } @@ -280,25 +327,30 @@ int main(int argc, char* argv[]) { const float PANEL_W = 330.f; - while (!WindowShouldClose()) { + while (!WindowShouldClose()) + { refreshTex(state); BeginDrawing(); - ClearBackground(Color{30, 30, 30, 255}); + ClearBackground(Color{ 30, 30, 30, 255 }); // ── image view (left area) ──────────────────────────────────────────── - if (state.texOk) { + 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 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); + 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) ─────────────────────────────────────────────── @@ -306,55 +358,61 @@ int main(int argc, char* argv[]) { 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::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)) { + 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"); + 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)) { + 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()) { + 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))) { + 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))) { + 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) { + 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()); + 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"); } @@ -362,24 +420,32 @@ int main(int argc, char* argv[]) { } // ── calibration ─────────────────────────────────────────────────────── - if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) { - if (!state.images.empty()) { + if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) + { + if (!state.images.empty()) + { int prog = state.detectProgress.load(); - if (state.isDetecting()) { + 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 { + 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) { + 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(); + 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); @@ -387,24 +453,32 @@ int main(int argc, char* argv[]) { } int good = 0; - for (auto& ci : state.images) if (ci.detected) good++; + 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 (good >= 4) + { if (ImGui::Button("Run calibration", ImVec2(-1, 0))) runCalibration(state); } } // ── results ─────────────────────────────────────────────────────────── - if (state.calibrated) { - if (ImGui::CollapsingHeader("Results", ImGuiTreeNodeFlags_DefaultOpen)) { + 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; }; + 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); @@ -421,12 +495,20 @@ int main(int argc, char* argv[]) { 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); + 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("Save JSON", ImVec2(-1, 0))) { + 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; } @@ -435,7 +517,8 @@ int main(int argc, char* argv[]) { } // ── status ──────────────────────────────────────────────────────────── - if (!state.statusMsg.empty()) { + if (!state.statusMsg.empty()) + { ImGui::Separator(); ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", state.statusMsg.c_str()); } @@ -447,9 +530,11 @@ int main(int argc, char* argv[]) { // stop background detection if still running state.detectStop.store(true); - if (state.detectThread.joinable()) state.detectThread.join(); + if (state.detectThread.joinable()) + state.detectThread.join(); - if (state.texOk) UnloadTexture(state.tex); + if (state.texOk) + UnloadTexture(state.tex); rlImGuiShutdown(); CloseWindow(); return 0; diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index e219c4c6..ccfd6a2b 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -1,33 +1,34 @@ +#include "RosExport.h" +#include "external/glad.h" +#include "imgui.h" #include "raylib.h" -#include "rlgl.h" #include "raymath.h" -#include "external/glad.h" #include "rlImGui.h" -#include "imgui.h" -#include +#include "rlgl.h" #include -#include -#include "RosExport.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 -#include #include -#include -#include -#include -#include -#include +#include #include -#include -#include +#include using namespace calib; namespace fs = std::filesystem; @@ -96,184 +97,247 @@ void main() { } )"; -struct GpuCloud { +struct GpuCloud +{ unsigned int vao = 0, vbo = 0; int count = 0; float maxDist = 50.f; - void upload(const std::vector& data, float mx) { + void upload(const std::vector& data, float mx) + { unload(); - if (data.empty()) return; + if (data.empty()) + return; maxDist = mx; vao = rlLoadVertexArray(); rlEnableVertexArray(vao); - vbo = rlLoadVertexBuffer(data.data(), (int)(data.size()*sizeof(float)), false); + vbo = rlLoadVertexBuffer(data.data(), (int)(data.size() * sizeof(float)), false); const int stride = 6 * sizeof(float); rlSetVertexAttribute(0, 3, RL_FLOAT, false, stride, 0); rlEnableVertexAttribute(0); - rlSetVertexAttribute(1, 1, RL_FLOAT, false, stride, 3*sizeof(float)); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, stride, 3 * sizeof(float)); rlEnableVertexAttribute(1); - rlSetVertexAttribute(2, 1, RL_FLOAT, false, stride, 4*sizeof(float)); + rlSetVertexAttribute(2, 1, RL_FLOAT, false, stride, 4 * sizeof(float)); rlEnableVertexAttribute(2); - rlSetVertexAttribute(3, 1, RL_FLOAT, false, stride, 5*sizeof(float)); + rlSetVertexAttribute(3, 1, RL_FLOAT, false, stride, 5 * sizeof(float)); rlEnableVertexAttribute(3); rlDisableVertexArray(); count = (int)(data.size() / 6); } - void unload() { - if (vao) { rlUnloadVertexArray(vao); vao = 0; } - if (vbo) { rlUnloadVertexBuffer(vbo); vbo = 0; } + void unload() + { + if (vao) + { + rlUnloadVertexArray(vao); + vao = 0; + } + if (vbo) + { + rlUnloadVertexBuffer(vbo); + vbo = 0; + } count = 0; } }; // ── Orbit camera (same as CalibrationApp) ───────────────────────────────────── -struct Orbit { +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 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.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)) { + 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; + 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)) { + 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)); + 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); + 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; }; - +struct ColorPt +{ + float x, y, z; + uint8_t r, g, b; + float intensity; + int64_t ts_ns; +}; // ── Application state ───────────────────────────────────────────────────────── -struct State { - Trajectory traj; +struct State +{ + Trajectory traj; std::vector imageTsNs; - Intrinsics K; - Extrinsics E; - bool calibLoaded = false; - int imgW = 4656, imgH = 3496; + Intrinsics K; + Extrinsics E; + 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; + GpuCloud cloud; + Shader shader = {}; + bool shaderOk = false; + int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; - Orbit orbit; + 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 = 1; - int drawDecim = 1; - bool multiImgColoring = true; // false = single image per chunk (midpoint) - bool useImageColor = false; - - char sessionBuf[512] = {}; - char calibBuf[512] = {}; - char cameraBuf[512] = {}; - char exportBuf[512] = "colored.laz"; + 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 = 1; + int drawDecim = 1; + bool multiImgColoring = true; // false = single image per chunk (midpoint) + bool useImageColor = false; + + 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; + 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) + 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; + 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()}; } +// 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'; +} + +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) { +static void loadImages(State& s) +{ s.imagesFilenamesInTime.clear(); fs::path camDir; - if (s.cameraBuf[0]) { + if (s.cameraBuf[0]) + { camDir = fs::path(s.cameraBuf); - } else { + } + else + { camDir = fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; } - if (!fs::is_directory(camDir)) { s.status = "No CAMERA_0 dir found"; return; } + if (!fs::is_directory(camDir)) + { + s.status = "No CAMERA_0 dir found"; + return; + } int loaded = 0; - for (auto& e : fs::directory_iterator(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") continue; - try { + 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 (...) {} + } 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) { +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; + 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; + 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++) @@ -283,7 +347,8 @@ static std::map parseMRP(const fs::path& mrpPath) return result; } -static void loadSession(State& s) { +static void loadSession(State& s) +{ s.traj.poses.clear(); s.imageTsNs.clear(); s.exportCloud.clear(); @@ -291,63 +356,80 @@ static void loadSession(State& s) { loadImages(s); fs::path d(s.sessionBuf); - if (!fs::is_directory(d)) { s.status = "Not a directory"; return; } + 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"); + 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)) { + 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) { + for (auto& cp : csvPaths) + { std::string stem = cp.stem().string(); - std::string idx = stem.substr(stem.rfind('_') + 1); - std::string key = "scan_lio_" + idx; + 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)) { + 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 { + 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 (...) {} + } 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"; + 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) { +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; } + 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"); + if (mrp.empty()) + mrp = parseMRP(d / "session_ini_poses.mri"); std::vector lazPaths; - for (auto& e : fs::directory_iterator(d)) { + 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()); @@ -360,27 +442,33 @@ static void loadCloud(State& s) { 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; - auto packGray = [](float intensity) -> float { + 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; + float f; + std::memcpy(&f, &p, 4); + return f; }; - struct ImgEntry { - int64_t ts; + struct ImgEntry + { + int64_t ts; const TrajPose* pose; - cv::Mat img; - int globalIdx; // index into s.imageTsNs (== imgViewIdx / selectedCamera) + cv::Mat img; + int globalIdx; // index into s.imageTsNs (== imgViewIdx / selectedCamera) }; std::vector gpuData; float mx = 0.f; - float sumX = 0, sumY = 0, sumZ = 0; int cnt = 0; + float sumX = 0, sumY = 0, sumZ = 0; + int cnt = 0; int step = std::max(1, s.cloudDecim); int coloredChunks = 0; - for (auto& lp : lazPaths) { - std::string key = lp.stem().string(); // "scan_lio_N" + 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; @@ -389,13 +477,21 @@ static void loadCloud(State& s) { { 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; + 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; } } @@ -403,46 +499,61 @@ static void loadCloud(State& s) { // ── step 2: collect images for this chunk ─────────────────────────── std::vector chunkImgs; - if (canColor && chunkFirst && chunkLast) { - if (s.multiImgColoring) { + 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) { + for (auto it = it0; it != it1; ++it) + { int64_t imgTs = *it; auto fnIt = s.imagesFilenamesInTime.find(imgTs); - if (fnIt == s.imagesFilenamesInTime.end()) continue; + if (fnIt == s.imagesFilenamesInTime.end()) + continue; const TrajPose* pose = s.traj.nearest(imgTs); - if (!pose) continue; + if (!pose) + continue; cv::Mat img = cv::imread(fnIt->second); - if (img.empty()) continue; + if (img.empty()) + continue; int gidx = (int)(it - s.imageTsNs.begin()); - chunkImgs.push_back({imgTs, pose, std::move(img), gidx}); + chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); } - } else { + } + else + { // legacy: single image nearest to chunk midpoint int64_t mid = (chunkFirst + chunkLast) / 2; auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), mid); - if (it == s.imageTsNs.end()) --it; - else if (it != s.imageTsNs.begin()) { + 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; + if (std::abs(*prev - mid) < std::abs(*it - mid)) + it = prev; } int64_t imgTs = *it; auto fnIt = s.imagesFilenamesInTime.find(imgTs); const TrajPose* pose = s.traj.nearest(imgTs); - if (fnIt != s.imagesFilenamesInTime.end() && pose) { + if (fnIt != s.imagesFilenamesInTime.end() && 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 (!img.empty()) + chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); } } } - if (!chunkImgs.empty()) ++coloredChunks; + if (!chunkImgs.empty()) + ++coloredChunks; // ── step 3: load point cloud ──────────────────────────────────────── PointCloud pc; - if (!pc.load(lp.string())) continue; + if (!pc.load(lp.string())) + continue; int nImgs = (int)chunkImgs.size(); @@ -450,10 +561,12 @@ static void loadCloud(State& s) { // 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) { + 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; + if (M) + pw = *M * pw; gpuData.push_back(pw.x()); gpuData.push_back(pw.z()); @@ -461,16 +574,26 @@ static void loadCloud(State& s) { const float rawIntensity = pt.intensity; float colorF = packGray(rawIntensity); - float camIdF = -1.f; // which image colored this point (global index), -1 = none + float camIdF = -1.f; // which image colored this point (global index), -1 = none - if (nImgs > 0) { + 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()) { + 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; @@ -479,15 +602,19 @@ static void loadCloud(State& s) { } // try images expanding outward from startIdx; first frustum hit wins - auto tryImg = [&](int idx) -> bool { - if (idx < 0 || idx >= nImgs) return false; + auto tryImg = [&](int idx) -> bool + { + if (idx < 0 || idx >= nImgs) + return false; auto& e = chunkImgs[idx]; - Eigen::Vector3f pl = e.pose->T.inverse() * pw; + Eigen::Vector3f pl = e.pose->T.inverse() * pw; Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); - if (pc_.z() <= 0.05f) return false; + if (pc_.z() <= 0.05f) + return false; int iu = (int)std::round(K_fx * pc_.x() / pc_.z() + K_cx); int iv = (int)std::round(K_fy * pc_.y() / pc_.z() + K_cy); - if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) return false; + if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) + return false; 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(&colorF, &p, 4); @@ -495,10 +622,14 @@ static void loadCloud(State& s) { return true; }; - if (!tryImg(startIdx)) { - for (int delta = 1; delta < nImgs; ++delta) { - if (tryImg(startIdx + delta)) break; - if (tryImg(startIdx - delta)) break; + if (!tryImg(startIdx)) + { + for (int delta = 1; delta < nImgs; ++delta) + { + if (tryImg(startIdx + delta)) + break; + if (tryImg(startIdx - delta)) + break; } } } @@ -507,57 +638,79 @@ static void loadCloud(State& s) { gpuData.push_back(rawIntensity); gpuData.push_back(camIdF); - 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}); + 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++; + 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 (cnt > 0) { + 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.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" : ""); + 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" : ""); } -static void loadCalib(State& s) { +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")) { + 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); + 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); + 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")) { + if (j.contains("extrinsics")) + { auto& je = j["extrinsics"]; - if (je.contains("camera_position_in_world_xyz") && - je["camera_position_in_world_xyz"].size() >= 3) { + 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) { + 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]; @@ -567,50 +720,76 @@ static void loadCalib(State& s) { s.status = "Calibration loaded"; } -static void exportLAZ(State& s) { - if (s.exportCloud.empty()) { s.status = "No cloud to export"; return; } +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); + 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; } + 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->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; + 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); + 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_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; + 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; @@ -629,19 +808,32 @@ static void exportLAZ(State& s) { // 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; } +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; } + 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.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) @@ -650,10 +842,9 @@ static void exportColmap(State& s) { 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'; + 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 @@ -665,18 +856,22 @@ static void exportColmap(State& s) { "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; int id = 1; - for (auto& [ts, path] : s.imagesFilenamesInTime) { + for (auto& [ts, path] : s.imagesFilenamesInTime) + { const TrajPose* pose = s.traj.nearest(ts); - if (!pose) continue; - Eigen::Affine3f T_wc = pose->T * T_lc; // camera in world - Eigen::Affine3f T_cw = T_wc.inverse(); // world -> camera - Eigen::Quaternionf q(T_cw.linear()); q.normalize(); + if (!pose) + continue; + Eigen::Affine3f T_wc = pose->T * 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; + 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; } } @@ -689,11 +884,12 @@ static void exportColmap(State& s) { 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) { + 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; + f << id << ' ' << p.x << ' ' << p.y << ' ' << p.z << ' ' << (int)p.r << ' ' << (int)p.g << ' ' << (int)p.b << " 0\n"; + ++id; + ++nPts; } } @@ -708,154 +904,180 @@ static void exportColmap(State& s) { << "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) { + 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 + f.write(reinterpret_cast(&p.r), 3); // r,g,b contiguous } } - if (s.colmapCopyImages) { + 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); + 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(); + 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; +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; + in.K = s.K; + in.E = s.E; fs::path d(s.sessionBuf); - if (!fs::is_directory(d)) return; + if (!fs::is_directory(d)) + return; auto mrp = parseMRP(d / "session_poses.mrp"); - if (mrp.empty()) mrp = parseMRP(d / "session_ini_poses.mri"); + if (mrp.empty()) + mrp = parseMRP(d / "session_ini_poses.mri"); std::vector lazPaths; - for (auto& e : fs::directory_iterator(d)) { + 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) { + 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; } + 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; +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.outUri = s.rosOutBuf; opt.storageId = (s.rosStorageIdx == 1) ? "sqlite3" : "mcap"; - if (s.rosThread.joinable()) s.rosThread.join(); + 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); + s.status = "Exporting ROS 2 bag... (see console)"; + s.rosThread = std::thread( + [&s, in = std::move(in), opt]() mutable { - std::lock_guard lk(s.rosMtx); - s.rosResult = std::move(st); - s.rosResultReady = true; - } - s.rosBusy = false; - }); + 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) { +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}); + 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) { + 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) { + 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; + 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; + 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; + Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; float sc = hl ? fs * 1.05f : fs; - if (hl) { + 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; + 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}); + 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); + 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) { + 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); int cm = s.useImageColor ? 1 : 0; - rlSetUniform(s.locCM, &cm, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(s.locCM, &cm, RL_SHADER_UNIFORM_INT, 1); rlSetUniform(s.locDecim, &s.drawDecim, RL_SHADER_UNIFORM_INT, 1); - int sel = (s.isolateCamera && s.useImageColor && - s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) - ? s.imgViewIdx : -1; + int sel = (s.isolateCamera && s.useImageColor && 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); @@ -865,15 +1087,18 @@ static void drawScene(State& s) { } // ── main ────────────────────────────────────────────────────────────────────── -int main(int argc, char* argv[]) { +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) { + const std::vector usage = { cliopt::MJS, cliopt::CAMERA_DIR, cliopt::CALIB }; + if (args.help) + { printUsage("TrajectoryViewer", kDesc, usage); return 0; } - if (!args.valid) { + if (!args.valid) + { std::fprintf(stderr, "%s\n\n", args.error.c_str()); printUsage("TrajectoryViewer", kDesc, usage, /*toStderr=*/true); return 1; @@ -882,77 +1107,99 @@ int main(int argc, char* argv[]) { 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("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); + 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); + 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"); SetTargetFPS(60); rlImGuiSetup(true); - s.shader = LoadShaderFromMemory(kVS, kFS); + 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"); + 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"); + 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.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)); } - 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); + if (s.sessionBuf[0]) + loadSession(s); + if (s.calibBuf[0]) + loadCalib(s); float panelW = 420.f; - while (!WindowShouldClose()) { + 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; + if (s.rosResultReady) + { + s.status = s.rosResult; s.rosResultReady = false; } } @@ -976,16 +1223,16 @@ int main(int argc, char* argv[]) { } BeginDrawing(); - ClearBackground(Color{25, 25, 25, 255}); + 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); + 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 ──────────────── @@ -993,16 +1240,18 @@ int main(int argc, char* argv[]) { cv::Mat toUpload; { std::lock_guard lk(s.imgViewMtx); - if (s.imgViewHasNew) { + 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); + 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; } } @@ -1012,21 +1261,25 @@ int main(int argc, char* argv[]) { 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); + 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::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "Trajectory Viewer"); ImGui::Separator(); - if (ImGui::CollapsingHeader("Session", ImGuiTreeNodeFlags_DefaultOpen)) { + 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("Load session", ImVec2(-1, 0))) loadSession(s); + 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(); @@ -1035,16 +1288,25 @@ int main(int argc, char* argv[]) { 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)"); - if (ImGui::Button("Load cloud", ImVec2(-1, 0))) loadCloud(s); + if (ImGui::Button("Load cloud", ImVec2(-1, 0))) + loadCloud(s); ImGui::PopItemWidth(); } - if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) { + if (ImGui::CollapsingHeader("Calibration", ImGuiTreeNodeFlags_DefaultOpen)) + { ImGui::PushItemWidth(-1); ImGui::Text("Calibration JSON:"); ImGui::InputText("##cal", s.calibBuf, sizeof(s.calibBuf)); - if (ImGui::Button("Load calibration", ImVec2(-1,0))) loadCalib(s); - if (s.calibLoaded) { + 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); ImGui::InputInt("Image W", &s.imgW); @@ -1053,13 +1315,15 @@ int main(int argc, char* argv[]) { ImGui::PopItemWidth(); } - if (ImGui::CollapsingHeader("Visualization", ImGuiTreeNodeFlags_DefaultOpen)) { - ImGui::Checkbox("Show path", &s.showPath); + 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::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::Checkbox("Color by image (RGB)", &s.useImageColor); if (ImGui::IsItemHovered()) @@ -1067,17 +1331,22 @@ int main(int argc, char* argv[]) { } } - if (ImGui::CollapsingHeader("Image Preview", ImGuiTreeNodeFlags_DefaultOpen)) { - if (s.imageTsNs.empty()) { + if (ImGui::CollapsingHeader("Image Preview", ImGuiTreeNodeFlags_DefaultOpen)) + { + if (s.imageTsNs.empty()) + { ImGui::TextDisabled("Load session first"); - } else { + } + 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) { + if (moved) + { s.imgViewIdx = std::clamp(s.imgViewIdx, 0, nImgs - 1); s.imgViewRequest.store(s.imgViewIdx); } @@ -1086,33 +1355,47 @@ int main(int argc, char* argv[]) { 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..."); + 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); + ImGui::TextColored(ImVec4(0, 1, 0, 1), "%dx%d", s.imgViewTex.width, s.imgViewTex.height); } } - if (ImGui::CollapsingHeader("Export", ImGuiTreeNodeFlags_DefaultOpen)) { + 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("Export colored LAZ", ImVec2(-1, 0))) exportLAZ(s); + 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")) { + 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")); ImGui::Combo("Storage", &s.rosStorageIdx, "mcap\0sqlite3\0"); ImGui::Separator(); ImGui::Checkbox("TF + static TF", &s.ros.exportTf); ImGui::Checkbox("Camera", &s.ros.exportCamera); - if (s.ros.exportCamera) { + if (s.ros.exportCamera) + { ImGui::Indent(); ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); if (ImGui::IsItemHovered()) @@ -1123,7 +1406,7 @@ int main(int argc, char* argv[]) { ImGui::Unindent(); } ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); - ImGui::Checkbox("LiDAR raw (sensor frame)", &s.ros.exportLidarRaw); + 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)."); @@ -1133,11 +1416,14 @@ int main(int argc, char* argv[]) { ImGui::InputInt("LiDAR decimation", &s.ros.lidarDecim); s.ros.lidarDecim = std::max(1, s.ros.lidarDecim); - if (s.rosBusy.load()) { + 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))) { + } + else if (ImGui::Button("Export ROS 2 bag", ImVec2(-1, 0))) + { exportRos(s); } ImGui::PopItemWidth(); @@ -1147,24 +1433,30 @@ int main(int argc, char* argv[]) { #endif } - if (ImGui::CollapsingHeader("COLMAP Export")) { + 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); ImGui::InputInt("Point decimation", &s.colmapPtDecim); s.colmapPtDecim = std::max(1, s.colmapPtDecim); - if (ImGui::Button("Export COLMAP model", ImVec2(-1, 0))) exportColmap(s); + 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::SetTooltip( + "Needs calibration + a loaded cloud (for points3D).\n" + "Point COLMAP image_path at the images dir."); ImGui::PopItemWidth(); } - if (!s.status.empty()) { + if (!s.status.empty()) + { ImGui::Separator(); - ImGui::TextColored(ImVec4(1,1,0,1), "%s", s.status.c_str()); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "%s", s.status.c_str()); } ImGui::Separator(); @@ -1173,7 +1465,8 @@ int main(int argc, char* argv[]) { ImGui::End(); // ── floating image viewer window ────────────────────────────────────── - if (s.imgViewTexValid) { + if (s.imgViewTexValid) + { ImGui::SetNextWindowPos(ImVec2(8, 8), ImGuiCond_Once); ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_Once); ImGui::Begin("Image##viewer", nullptr, ImGuiWindowFlags_NoScrollbar); @@ -1181,7 +1474,11 @@ int main(int argc, char* argv[]) { 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); } + if (dispH > (int)avail.y) + { + dispH = (int)avail.y; + dispW = (int)(avail.y / aspect); + } rlImGuiImageSize(&s.imgViewTex, dispW, dispH); ImGui::End(); } @@ -1192,11 +1489,14 @@ int main(int argc, char* argv[]) { s.imgViewStop = true; s.imgViewThread.join(); - if (s.rosThread.joinable()) s.rosThread.join(); - if (s.imgViewTexValid) UnloadTexture(s.imgViewTex); + if (s.rosThread.joinable()) + s.rosThread.join(); + if (s.imgViewTexValid) + UnloadTexture(s.imgViewTex); s.cloud.unload(); - if (s.shaderOk) UnloadShader(s.shader); + if (s.shaderOk) + UnloadShader(s.shader); rlImGuiShutdown(); CloseWindow(); return 0; diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 7035ca45..cb8a26e6 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(calib_core STATIC src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp + src/FileDialog.cpp ) target_include_directories(calib_core PUBLIC @@ -35,6 +36,7 @@ target_include_directories(calib_core PRIVATE # classes instead and never needed this path, so nothing else in the repo # wires it up. ${CMAKE_BINARY_DIR}/3rdparty/LASzip/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) 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/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 From cbc41669a6844938e0f148301539e75fa0c5e3b6 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Sun, 2 Aug 2026 01:33:22 +0200 Subject: [PATCH 06/13] Port ROI feature from mp/roi and fix window/UX bugs in camera_lidar_* apps Re-ports camera_lidar_trajectory_viewer's TrajectoryViewer.cpp from mandeye-colors' mp/roi branch (the initial import was mistakenly based on master, before the ROI work landed there): adds a Roi struct (calib_core's Camera.h), SLERP pose interpolation instead of nearest-neighbor lookups, distortion-aware point projection, a "Geometry" coloring strategy (closest camera by depth) alongside the existing temporal one, ROI-filtered colorization with a Camera-ID/In-ROI render mode and an ROI overlay on the image preview, and colored/uncolored point-count stats. Also fixes three real bugs surfaced by manually running the apps: - Fixed-size windows (1400x900 etc.) could be taller than the screen once the OS menu bar + title bar are accounted for, silently pushing the top of the control panel off-screen. Added fitWindowToScreen() (monitor-aware resize/reposition) and SetWindowMinSize() to all three apps. - fitWindowToScreen() itself was buggy: GetMonitorWidth/Height return the monitor's native pixel resolution while GetScreenWidth/Height and SetWindowSize/SetWindowPosition operate in logical points, a 2x mismatch on Retina displays that pushed the window mostly off-screen. Fixed by dividing by GetWindowScaleDPI(). Also added a PollInputEvents() call so raylib's cached mouse/window geometry is refreshed before rlImGuiSetup() reads it. - Several ImGui widgets with trailing labels (InputInt, Combo, InputDouble) were wrapped in PushItemWidth(-1), which gives the widget box the entire row and clips the label off the right edge of the panel. Scoped a narrower width around each affected widget. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- apps/camera_lidar_calibration/App.cpp | 412 ++++++++++----- apps/camera_lidar_calibration/UI.cpp | 5 +- .../IntrinsicsCalib.cpp | 49 ++ .../TrajectoryViewer.cpp | 497 ++++++++++++++++-- calib_core/include/CalibCore/Camera.h | 79 +-- 5 files changed, 810 insertions(+), 232 deletions(-) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 34985927..ed12dc02 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -1,58 +1,66 @@ #include "App.h" -#include "rlImGui.h" #include "imgui.h" -#include -#include -#include -#include -#include +#include "rlImGui.h" +#include #include #include #include -#include +#include +#include +#include +#include +#include #include // ── AppState::rebuildImageTexture ───────────────────────────────────────────── -void AppState::rebuildImageTexture() { - if (originalImage.empty()) return; +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); + 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 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::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); + if (imageLoaded) + UnloadTexture(imageTexture); Image rimg = {}; - rimg.data = display.data; - rimg.width = display.cols; - rimg.height = display.rows; + 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; + rimg.format = PIXELFORMAT_UNCOMPRESSED_R8G8B8; + imageTexture = LoadTextureFromImage(rimg); // copies pixels to GPU + imageLoaded = true; } // ── AppState::loadImage ─────────────────────────────────────────────────────── -void AppState::loadImage(const char* path) { +void AppState::loadImage(const char* path) +{ cv::Mat bgr = cv::imread(path, cv::IMREAD_COLOR); - if (bgr.empty()) { + if (bgr.empty()) + { statusMsg = std::string("Failed to load image: ") + path; return; } @@ -66,39 +74,41 @@ void AppState::loadImage(const char* path) { } // ── 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}); +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)) { +void AppState::loadCloud(const char* path) +{ + if (!cloud.load(path)) + { statusMsg = std::string("Failed to load cloud: ") + path; return; } - cloudPaths = {path}; + cloudPaths = { path }; renderer.uploadCloud(cloud); centerOrbitOnCloud(*this); statusMsg = ""; } -void AppState::addCloud(const char* path) { +void AppState::addCloud(const char* path) +{ PointCloud extra; - if (!extra.load(path)) { + if (!extra.load(path)) + { statusMsg = std::string("Failed to load: ") + path; return; } // merge bounding box - if (cloud.empty()) { + if (cloud.empty()) + { cloud = std::move(extra); - } else { + } + 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); @@ -120,45 +130,67 @@ void AppState::addCloud(const char* path) { // - 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) { +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 == '.') { + 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; } + 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) { +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; } + if (!f) + { + err = "cannot open file"; + return false; + } std::vector camMat, dist; - std::vector* active = nullptr; // section whose data we collect + std::vector* active = nullptr; // section whose data we collect std::vector* collecting = nullptr; bool inFlow = false; std::string line; - while (std::getline(f, line)) { + while (std::getline(f, line)) + { std::string trimmed = line; trimmed.erase(0, trimmed.find_first_not_of(" \t")); - if (inFlow) { + if (inFlow) + { extractNumbers(trimmed, *collecting); - if (trimmed.find(']') != std::string::npos) { inFlow = false; collecting = nullptr; } + if (trimmed.find(']') != std::string::npos) + { + inFlow = false; + collecting = nullptr; + } continue; } bool topLevel = !line.empty() && line[0] != ' ' && line[0] != '\t' && line[0] != '-'; - if (topLevel) { + if (topLevel) + { collecting = nullptr; - if (trimmed.rfind("camera_matrix:", 0) == 0) active = &camMat; - else if (trimmed.rfind("distortion_coefficients:", 0) == 0) active = &dist; - else { + 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); @@ -168,29 +200,39 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, continue; } - if (active && trimmed.rfind("data:", 0) == 0) { + if (active && trimmed.rfind("data:", 0) == 0) + { auto bracket = trimmed.find('['); - if (bracket != std::string::npos) { + if (bracket != std::string::npos) + { extractNumbers(trimmed.substr(bracket), *active); - if (trimmed.find(']') == std::string::npos) { + if (trimmed.find(']') == std::string::npos) + { collecting = active; inFlow = true; } - } else { - collecting = active; // block list follows + } + else + { + collecting = active; // block list follows } continue; } - if (collecting) { + 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 + collecting = nullptr; // rows:/cols: or another key ends the list } } - if (camMat.size() < 9) { err = "camera_matrix needs 9 values"; return false; } + 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]); @@ -198,33 +240,46 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, 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); + 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); + K.k4 = d(5); + K.k5 = d(6); + K.k6 = d(7); return true; } // ── AppState::loadIntrinsics ────────────────────────────────────────────────── -void AppState::loadIntrinsics(const char* path) { +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)); + for (auto& c : ext) + c = static_cast(tolower(c)); - if (ext == "yml" || ext == "yaml") { + if (ext == "yml" || ext == "yaml") + { int imgW = 0, imgH = 0; std::string err; - if (!parseOpenCVYaml(path, intrinsics, imgW, imgH, 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 + rebuildImageTexture(); // re-rectify with the new coefficients statusMsg = "Intrinsics loaded"; - if (imageRectified) statusMsg += ", image rectified"; - if (imgW > 0) { + 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); @@ -233,7 +288,11 @@ void AppState::loadIntrinsics(const char* path) { } std::ifstream f(path); - if (!f) { statusMsg = std::string("Cannot open: ") + path; return; } + if (!f) + { + statusMsg = std::string("Cannot open: ") + path; + return; + } nlohmann::json j; f >> j; intrinsics.fx = j.value("fx", intrinsics.fx); @@ -254,16 +313,28 @@ void AppState::loadIntrinsics(const char* path) { } // ── AppState::loadCalibration ───────────────────────────────────────────────── -void AppState::loadCalibration(const char* path) { +void AppState::loadCalibration(const char* path) +{ std::ifstream f(path); - if (!f) { statusMsg = std::string("Cannot open: ") + path; return; } + if (!f) + { + statusMsg = std::string("Cannot open: ") + path; + return; + } nlohmann::json j; - try { f >> j; } - catch (...) { statusMsg = std::string("JSON parse error: ") + path; return; } + try + { + f >> j; + } catch (...) + { + statusMsg = std::string("JSON parse error: ") + path; + return; + } bool gotIntrinsics = false, gotExtrinsics = false; - if (j.contains("intrinsics")) { + if (j.contains("intrinsics")) + { auto& ji = j["intrinsics"]; intrinsics.fx = ji.value("fx", intrinsics.fx); intrinsics.fy = ji.value("fy", intrinsics.fy); @@ -281,19 +352,20 @@ void AppState::loadCalibration(const char* path) { gotIntrinsics = true; } - if (j.contains("extrinsics")) { + 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) { + 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) { + 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(); @@ -302,7 +374,8 @@ void AppState::loadCalibration(const char* path) { gotExtrinsics = true; } - if (!gotIntrinsics && !gotExtrinsics) { + if (!gotIntrinsics && !gotExtrinsics) + { statusMsg = std::string("No intrinsics/extrinsics found in: ") + path; return; } @@ -311,54 +384,104 @@ void AppState::loadCalibration(const char* path) { rebuildImageTexture(); statusMsg = "Loaded"; - if (gotIntrinsics) statusMsg += " intrinsics"; - if (gotIntrinsics && gotExtrinsics) statusMsg += " +"; - if (gotExtrinsics) statusMsg += " extrinsics"; + 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) { +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::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 + 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["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} + { 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; } + 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() { +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 @@ -366,14 +489,17 @@ void App::run() { state.renderer.initPointShader(); // Load files passed as command-line arguments - if (!pendingImage.empty()) state.loadImage(pendingImage.c_str()); + 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()) { + (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(); } @@ -386,29 +512,28 @@ void App::run() { } // ── App::update ─────────────────────────────────────────────────────────────── -void App::update() { +void App::update() +{ bool imguiWantMouse = ImGui::GetIO().WantCaptureMouse; state.orbit.update(!imguiWantMouse); } // ── App::draw ───────────────────────────────────────────────────────────────── -void App::draw() { +void App::draw() +{ float panelW = 340.f; - float viewW = (float)GetScreenWidth() - panelW; - float viewH = (float)GetScreenHeight(); + 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); + 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}); + ClearBackground(Color{ 30, 30, 30, 255 }); // Clipping for 3D region (bottom-left) // Note: raylib scissor is in screen coords (y-down) @@ -417,13 +542,17 @@ void App::draw() { 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); + 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.drawCameraFrustum(state.intrinsics, state.extrinsics, state.imageW, state.imageH); state.renderer.drawAxes(2.f); // Grid on ground plane @@ -433,11 +562,10 @@ void App::draw() { EndScissorMode(); // ── 3D label - DrawText("3D View [LMB: orbit | RMB: pan | Scroll: zoom]", - 8, (int)view3DY + 4, 14, LIGHTGRAY); + 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); + DrawLineEx(Vector2{ 0, view3DY }, Vector2{ viewW, view3DY }, 1.f, GRAY); // ── ImGui on top ───────────────────────────────────────────────────────── rlImGuiBegin(); diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 999535c8..1b6a87e2 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -300,7 +300,10 @@ void UI::panelVisualization(AppState& state) { VisualizationParams& vp = state.vizParams; - ImGui::PushItemWidth(-1); + // -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); diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index db18ef01..fd27dbaa 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -35,6 +35,51 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) 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 { @@ -322,6 +367,10 @@ int main(int argc, char* argv[]) 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); diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index ccfd6a2b..72ccff29 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -33,20 +33,154 @@ 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 +// 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); @@ -62,6 +196,7 @@ void main() { fragIntensity = lidarIntensity; vertColor = vec4(r, g, b, 1.0); fragColorCameraId = colorCameraId; + fragInRoi = inRoi; } )"; static const char* kFS = R"( @@ -69,6 +204,7 @@ static const char* kFS = R"( 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; @@ -78,20 +214,45 @@ vec3 jet(float t) { 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 (selectedCamera < 0) - { - finalColor = vertColor; - } + 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 - { - if (selectedCamera == int(fragColorCameraId)) - finalColor = vertColor; - else - discard; // render only points colored from the selected camera - } + 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); } @@ -112,7 +273,7 @@ struct GpuCloud vao = rlLoadVertexArray(); rlEnableVertexArray(vao); vbo = rlLoadVertexBuffer(data.data(), (int)(data.size() * sizeof(float)), false); - const int stride = 6 * sizeof(float); + 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)); @@ -121,8 +282,10 @@ struct GpuCloud 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() / 6); + count = (int)(data.size() / 7); } void unload() { @@ -201,6 +364,7 @@ struct State std::vector imageTsNs; Intrinsics K; Extrinsics E; + Roi roi; bool calibLoaded = false; int imgW = 4656, imgH = 3496; @@ -220,10 +384,20 @@ struct State bool isolateCamera = false; // render only points colored by the selected (preview) image float frustumScale = 0.5f; float pointSize = 2.f; - int cloudDecim = 1; + int cloudDecim = 5; int drawDecim = 1; bool multiImgColoring = true; // false = single image per chunk (midpoint) - bool useImageColor = false; + // 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] = {}; @@ -261,16 +435,6 @@ struct State }; // ── helpers ─────────────────────────────────────────────────────────────────── -// 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'; -} - static Vector3 toRL(float x, float y, float z) { return { x, z, -y }; @@ -441,6 +605,20 @@ static void loadCloud(State& s) 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 { @@ -454,17 +632,21 @@ static void loadCloud(State& s) struct ImgEntry { int64_t ts; - const TrajPose* pose; + 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) { @@ -512,8 +694,8 @@ static void loadCloud(State& s) auto fnIt = s.imagesFilenamesInTime.find(imgTs); if (fnIt == s.imagesFilenamesInTime.end()) continue; - const TrajPose* pose = s.traj.nearest(imgTs); - if (!pose) + Eigen::Affine3f pose; + if (!interpPose(trajMap, imgTs, pose)) continue; cv::Mat img = cv::imread(fnIt->second); if (img.empty()) @@ -525,7 +707,7 @@ static void loadCloud(State& s) else { // legacy: single image nearest to chunk midpoint - int64_t mid = (chunkFirst + chunkLast) / 2; + int64_t mid = chunkFirst; auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), mid); if (it == s.imageTsNs.end()) --it; @@ -537,8 +719,8 @@ static void loadCloud(State& s) } int64_t imgTs = *it; auto fnIt = s.imagesFilenamesInTime.find(imgTs); - const TrajPose* pose = s.traj.nearest(imgTs); - if (fnIt != s.imagesFilenamesInTime.end() && pose) + 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()); @@ -575,6 +757,7 @@ static void loadCloud(State& s) 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) { @@ -600,43 +783,117 @@ static void loadCloud(State& s) } startIdx = (int)(it - chunkImgs.begin()); } - - // try images expanding outward from startIdx; first frustum hit wins - auto tryImg = [&](int idx) -> bool + // 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 false; + return h; auto& e = chunkImgs[idx]; - Eigen::Vector3f pl = e.pose->T.inverse() * pw; + Eigen::Vector3f pl = e.pose.inverse() * pw; Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); if (pc_.z() <= 0.05f) - return false; - int iu = (int)std::round(K_fx * pc_.x() / pc_.z() + K_cx); - int iv = (int)std::round(K_fy * pc_.y() / pc_.z() + K_cy); + 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 false; + 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(&colorF, &p, 4); - camIdF = (float)e.globalIdx; - return true; + 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 (!tryImg(startIdx)) + if (s.colorStrategy == 1) { - for (int delta = 1; delta < nImgs; ++delta) + // 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) { - if (tryImg(startIdx + delta)) - break; - if (tryImg(startIdx - delta)) - break; + 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); @@ -661,6 +918,10 @@ static void loadCloud(State& s) // 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) { @@ -671,6 +932,12 @@ static void loadCloud(State& s) 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) @@ -716,6 +983,19 @@ static void loadCalib(State& s) 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"; } @@ -855,13 +1135,14 @@ static void exportColmap(State& s) 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) { - const TrajPose* pose = s.traj.nearest(ts); - if (!pose) + Eigen::Affine3f pose; + if (!interpPose(trajMap, ts, pose)) continue; - Eigen::Affine3f T_wc = pose->T * T_lc; // camera in world + 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(); @@ -1074,10 +1355,9 @@ static void drawScene(State& s) rlEnableShader(s.shader.id); rlSetUniformMatrix(s.locMVP, mvp); rlSetUniform(s.locPS, &s.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); - int cm = s.useImageColor ? 1 : 0; - rlSetUniform(s.locCM, &cm, RL_SHADER_UNIFORM_INT, 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.useImageColor && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) ? s.imgViewIdx : -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); @@ -1132,6 +1412,9 @@ int main(int argc, char* argv[]) 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); @@ -1208,7 +1491,7 @@ int main(int argc, char* argv[]) if (!ImGui::GetIO().WantCaptureKeyboard) { if (IsKeyPressed(KEY_LEFT_CONTROL) || IsKeyPressed(KEY_RIGHT_CONTROL)) - s.useImageColor = !s.useImageColor; + s.colorMode = (s.colorMode == 1) ? 0 : 1; if (IsKeyPressed(KEY_LEFT)) { @@ -1283,11 +1566,38 @@ int main(int argc, char* argv[]) 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(); @@ -1309,8 +1619,38 @@ int main(int argc, char* argv[]) { 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(); } @@ -1325,9 +1665,20 @@ int main(int argc, char* argv[]) if (!s.imagesFilenamesInTime.empty()) { ImGui::Separator(); - ImGui::Checkbox("Color by image (RGB)", &s.useImageColor); + 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."); } } @@ -1389,7 +1740,12 @@ int main(int argc, char* argv[]) 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); @@ -1411,10 +1767,15 @@ int main(int argc, char* argv[]) 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()) { @@ -1441,7 +1802,12 @@ int main(int argc, char* argv[]) 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); @@ -1479,7 +1845,22 @@ int main(int argc, char* argv[]) 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(); } @@ -1500,4 +1881,4 @@ int main(int argc, char* argv[]) rlImGuiShutdown(); CloseWindow(); return 0; -} \ No newline at end of file +} diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 9cc11383..ef315a12 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -1,40 +1,57 @@ #pragma once -#include #include #include +#include + +namespace calib +{ -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 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; + }; -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); + // 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); + // 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 +} // namespace calib \ No newline at end of file From 0c7f1dc872dffc3673b3f8b4c45cdc91081f8ae2 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 3 Aug 2026 01:14:42 +0200 Subject: [PATCH 07/13] Deduplicate file-dialog wrapper between calib_core and core calib::fd (calib_core/src/FileDialog.cpp) was a byte-for-byte duplicate of core's mandeye::fd (core/src/pfd_wrapper.cpp), kept separate only because pfd_wrapper.cpp was compiled straight into the monolithic `core` target, which drags in core_math/PROJ/spdlog/vqf/Fusion/plycpp/WGS84toCartesian/ imgui/ImGuizmo/freeglut -- none of which file dialogs need. Split pfd_wrapper.cpp out into its own minimal `core_pfd` static lib (portable-file-dialogs + std only). `core` links it publicly so all existing apps keep working unchanged via #include . The three camera_lidar_* apps now link core_pfd directly and use mandeye::fd instead of their own copy; calib_core drops FileDialog entirely, since file dialogs are a GUI concern rather than calibration logic. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- apps/camera_lidar_calibration/CMakeLists.txt | 1 + apps/camera_lidar_calibration/UI.cpp | 10 +-- .../CMakeLists.txt | 7 +- .../IntrinsicsCalib.cpp | 6 +- .../CMakeLists.txt | 1 + .../TrajectoryViewer.cpp | 14 ++-- calib_core/CMakeLists.txt | 4 +- calib_core/include/CalibCore/FileDialog.h | 46 ------------- calib_core/src/FileDialog.cpp | 65 ------------------- core/CMakeLists.txt | 13 +++- core/include/Core/pfd_wrapper.hpp | 3 + core/src/pfd_wrapper.cpp | 6 +- 12 files changed, 42 insertions(+), 134 deletions(-) delete mode 100644 calib_core/include/CalibCore/FileDialog.h delete mode 100644 calib_core/src/FileDialog.cpp diff --git a/apps/camera_lidar_calibration/CMakeLists.txt b/apps/camera_lidar_calibration/CMakeLists.txt index 326244eb..bad928d5 100644 --- a/apps/camera_lidar_calibration/CMakeLists.txt +++ b/apps/camera_lidar_calibration/CMakeLists.txt @@ -27,6 +27,7 @@ target_compile_definitions(camera_lidar_calibration PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_calibration PRIVATE calib_core + core_pfd raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 1b6a87e2..d7dc2cef 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -2,7 +2,7 @@ #include "App.h" #include "imgui.h" #include "rlImGui.h" -#include +#include #include #include #include @@ -182,7 +182,7 @@ void UI::panelFiles(AppState& state) 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)); + setBuf(imagePathBuf, sizeof(imagePathBuf), mandeye::fd::OpenFileDialogOneFile("Select camera image", mandeye::fd::ImageFilter)); if (ImGui::Button("Load Image##btn", ImVec2(-1, 0))) state.loadImage(imagePathBuf); @@ -190,7 +190,7 @@ void UI::panelFiles(AppState& state) 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)); + setBuf(cloudPathBuf, sizeof(cloudPathBuf), mandeye::fd::OpenFileDialogOneFile("Select point cloud", mandeye::fd::LazFilter)); { float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; if (ImGui::Button("Load##laz", ImVec2(hw, 0))) @@ -204,7 +204,7 @@ void UI::panelFiles(AppState& state) 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)); + setBuf(intrPathBuf, sizeof(intrPathBuf), mandeye::fd::OpenFileDialogOneFile("Select intrinsics file", mandeye::fd::IntrinsicsFilter)); if (ImGui::Button("Load Intrinsics##btn", ImVec2(-1, 0))) state.loadIntrinsics(intrPathBuf); @@ -212,7 +212,7 @@ void UI::panelFiles(AppState& state) 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)); + setBuf(savePath, sizeof(savePath), mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter)); float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; if (ImGui::Button("Load##calib", ImVec2(hw, 0))) state.loadCalibration(savePath); diff --git a/apps/camera_lidar_intrinsics_calib/CMakeLists.txt b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt index 9f51bf54..375a2872 100644 --- a/apps/camera_lidar_intrinsics_calib/CMakeLists.txt +++ b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt @@ -4,9 +4,9 @@ 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. +# clouds at all, so it only needs calib_core for CliArgs and core_pfd for +# file dialogs, 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 ) @@ -20,6 +20,7 @@ target_compile_definitions(camera_lidar_intrinsics_calib PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_intrinsics_calib PRIVATE calib_core + core_pfd raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index fd27dbaa..903d8cb7 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -2,7 +2,7 @@ #include "raylib.h" #include "rlImGui.h" #include -#include +#include #include #include #include @@ -434,7 +434,7 @@ int main(int argc, char* argv[]) 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")); + setBuf(state.dirBuf, sizeof(state.dirBuf), mandeye::fd::SelectFolder("Select checkerboard image directory")); if (ImGui::Button("Load", ImVec2(-1, 0))) loadDir(state); ImGui::Text("%zu images", state.images.size()); @@ -554,7 +554,7 @@ int main(int argc, char* argv[]) setBuf( state.outPath, sizeof(state.outPath), - calib::fd::SaveFileDialog("Save intrinsics JSON", calib::fd::CalibJsonFilter, ".json", defaultName)); + mandeye::fd::SaveFileDialog("Save intrinsics JSON", mandeye::fd::json_filter, ".json", defaultName)); } if (ImGui::Button("Save JSON", ImVec2(-1, 0))) { diff --git a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt index 5f8125ea..fab862b8 100644 --- a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt +++ b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt @@ -41,6 +41,7 @@ target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_trajectory_viewer PRIVATE calib_core + core_pfd raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 72ccff29..df6f14d7 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -7,7 +7,7 @@ #include "rlgl.h" #include #include -#include +#include #include #include #include @@ -1556,11 +1556,11 @@ int main(int argc, char* argv[]) 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")); + setBuf(s.sessionBuf, sizeof(s.sessionBuf), mandeye::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")); + setBuf(s.cameraBuf, sizeof(s.cameraBuf), mandeye::fd::SelectFolder("Select CAMERA_0 directory")); if (ImGui::Button("Load session", ImVec2(-1, 0))) loadSession(s); if (!s.imagesFilenamesInTime.empty()) @@ -1612,7 +1612,7 @@ int main(int argc, char* argv[]) setBuf( s.calibBuf, sizeof(s.calibBuf), - calib::fd::OpenFileDialogOneFile("Select calibration file", calib::fd::CalibJsonFilter)); + mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter)); if (ImGui::Button("Load calibration", ImVec2(-1, 0))) loadCalib(s); if (s.calibLoaded) @@ -1723,7 +1723,7 @@ int main(int argc, char* argv[]) setBuf( s.exportBuf, sizeof(s.exportBuf), - calib::fd::SaveFileDialog("Export colored point cloud", calib::fd::LazFilter, ".laz", defaultName)); + mandeye::fd::SaveFileDialog("Export colored point cloud", mandeye::fd::LazFilter, ".laz", defaultName)); } if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) exportLAZ(s); @@ -1739,7 +1739,7 @@ int main(int argc, char* argv[]) 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")); + setBuf(s.rosOutBuf, sizeof(s.rosOutBuf), mandeye::fd::SelectFolder("Select ROS 2 bag output directory")); // Scoped narrower width -- see the "Load decimation" comment above. ImGui::PopItemWidth(); ImGui::PushItemWidth(-140.f); @@ -1800,7 +1800,7 @@ int main(int argc, char* argv[]) 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")); + setBuf(s.colmapBuf, sizeof(s.colmapBuf), mandeye::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(); diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index cb8a26e6..cdc90526 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -9,6 +9,8 @@ project(calib_core) # 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. +# File dialogs are a GUI concern, not calibration logic, so they live in +# core's core_pfd target (mandeye::fd) instead -- apps link it directly. # # All public types live in namespace calib (see include/CalibCore/*.h) to # avoid colliding with core's own global (non-namespaced) PointCloud @@ -19,7 +21,6 @@ add_library(calib_core STATIC src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp - src/FileDialog.cpp ) target_include_directories(calib_core PUBLIC @@ -36,7 +37,6 @@ target_include_directories(calib_core PRIVATE # classes instead and never needed this path, so nothing else in the repo # wires it up. ${CMAKE_BINARY_DIR}/3rdparty/LASzip/include - ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) diff --git a/calib_core/include/CalibCore/FileDialog.h b/calib_core/include/CalibCore/FileDialog.h deleted file mode 100644 index 3c8479b9..00000000 --- a/calib_core/include/CalibCore/FileDialog.h +++ /dev/null @@ -1,46 +0,0 @@ -#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/src/FileDialog.cpp b/calib_core/src/FileDialog.cpp deleted file mode 100644 index 77dfed70..00000000 --- a/calib_core/src/FileDialog.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#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/core/CMakeLists.txt b/core/CMakeLists.txt index bc4d409a..88589b29 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -54,9 +54,19 @@ endif() set(CORE_GUI_SOURCES src/manual_pose_graph_loop_closure.cpp src/observation_picking.cpp - src/pfd_wrapper.cpp ) +# core_pfd -- native file/folder picker dialogs (mandeye::fd, wrapping +# portable-file-dialogs), split out of CORE_GUI_SOURCES into its own minimal +# target so non-core GUI consumers (the camera_lidar_* apps, via calib_core) +# can reuse it without linking core_math/PROJ/spdlog/vqf/Fusion/plycpp/ +# WGS84toCartesian/imgui/ImGuizmo/freeglut, none of which pfd_wrapper.cpp +# actually needs. +add_library(core_pfd STATIC src/pfd_wrapper.cpp) +target_include_directories(core_pfd PUBLIC include) +target_include_directories(core_pfd PRIVATE ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master) +set_target_properties(core_pfd PROPERTIES POSITION_INDEPENDENT_CODE ON) + function(add_core_target target_name with_gui) if(${with_gui}) set(SOURCES ${CORE_BASE_SOURCES} ${CORE_GUI_SOURCES}) @@ -93,6 +103,7 @@ endfunction() add_core_target(core_no_gui FALSE) add_core_target(core TRUE) +target_link_libraries(core PUBLIC core_pfd) target_precompile_headers(core_no_gui PRIVATE include/pch/pch.h diff --git a/core/include/Core/pfd_wrapper.hpp b/core/include/Core/pfd_wrapper.hpp index 029b0432..2c33a25f 100644 --- a/core/include/Core/pfd_wrapper.hpp +++ b/core/include/Core/pfd_wrapper.hpp @@ -44,6 +44,9 @@ namespace mandeye::fd const std::vector json_filter = { "Calibration file (*.json)", "*.json", "All files", "*" }; const std::vector sn_filter = { "SN file (*.sn)", "*.sn", "All files", "*" }; + const std::vector IntrinsicsFilter = { + "Camera intrinsics (*.json, *.yml, *.yaml)", "*.json *.yml *.yaml", "All files", "*" + }; std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter); std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect); diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index c34357d0..bb832fd0 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -1,9 +1,11 @@ -#include - #include #include +#include +#include +#include + namespace mandeye::fd { std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter) From 56edc05bc810791f10cb7d8415d6d63d4a54c24b Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 3 Aug 2026 01:23:13 +0200 Subject: [PATCH 08/13] UX-unify camera_lidar_* apps with the rest of HDMapping Window titles now follow the project-wide " " + HDMAPPING_VERSION_STRING convention used by every other app (hd_mapper, manual_color, trajectory viewers, etc.) instead of bare, unversioned strings -- these three apps had neither the version suffix nor the include. Also document the three tools in README.md, which didn't mention them at all despite covering every other app in the suite. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- README.md | 8 ++++++++ apps/camera_lidar_calibration/App.cpp | 3 ++- apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp | 3 ++- apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp | 3 ++- 4 files changed, 14 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 77ea4f2e..b7763249 100644 --- a/README.md +++ b/README.md @@ -132,6 +132,14 @@ More information can be found here: - Info for Windows users: please use the latest release https://github.com/MapsHD/HDMapping/releases - Contact email: januszbedkowski@gmail.com +# Camera-LiDAR calibration tools + +Three tools (`apps/camera_lidar_*`) supporting the spherical-camera-to-3D-LiDAR calibration/coloring workflow described in Będkowski et al., "Method for spherical camera to 3D LiDAR calibration and synchronization with example on Insta360 X4 and LiVOX MID 360" (2025, EuroCOW, [[PDF]](https://isprs-archives.copernicus.org/articles/XLVIII-1-W4-2025/13/2025/isprs-archives-XLVIII-1-W4-2025-13-2025.pdf)): + +- **camera_lidar_intrinsics_calib** -- checkerboard-based camera intrinsic calibration (OpenCV rational distortion model). +- **camera_lidar_calibration** -- interactive LiDAR-camera extrinsic calibration: aligns a LAZ/LAS point cloud to a camera image with live GPU-shader reprojection feedback. +- **camera_lidar_trajectory_viewer** -- multi-camera trajectory/point-cloud viewer that assigns per-point "which camera colored this point" RGB, with LAZ export, plus optional COLMAP sparse-model and ROS 2 bag export. + # GNSS with RTK A portable NTRIP (Networked Transport of RTCM via Internet Protocol) client for M5Stack devices that receives RTK correction data from NTRIP casters and forwards it to u-blox GNSS receivers for high-precision positioning https://github.com/michalpelka/M5NtripClient. diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index ed12dc02..0b13a4a7 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -1,6 +1,7 @@ #include "App.h" #include "imgui.h" #include "rlImGui.h" +#include #include #include #include @@ -477,7 +478,7 @@ void App::run() { const int W = 1400, H = 900; SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); - InitWindow(W, H, "LiDAR-Camera Calibration"); + InitWindow(W, H, ("LiDAR-Camera Calibration " HDMAPPING_VERSION_STRING)); fitWindowToScreen(); // The 340px-wide side panel is fixed-width; below this the 3D/image // views and the panel start overlapping instead of scrolling. diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index 903d8cb7..3ffa7a1b 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -3,6 +3,7 @@ #include "rlImGui.h" #include #include +#include #include #include #include @@ -366,7 +367,7 @@ int main(int argc, char* argv[]) } SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); - InitWindow(1280, 800, "Intrinsics Calibration"); + InitWindow(1280, 800, ("Intrinsics Calibration " HDMAPPING_VERSION_STRING)); fitWindowToScreen(); // PANEL_W below (330) is fixed; below this the image view and panel // start overlapping instead of scrolling. diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index df6f14d7..09a9c6ba 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -1411,7 +1412,7 @@ int main(int argc, char* argv[]) strncpy(s.calibBuf, calib.c_str(), sizeof(s.calibBuf) - 1); SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); - InitWindow(1400, 900, "Trajectory Viewer"); + InitWindow(1400, 900, ("Trajectory Viewer " HDMAPPING_VERSION_STRING)); fitWindowToScreen(); // panelW below is user-resizable but the 3D view still needs room. SetWindowMinSize(900, 500); From 8ac533226a2f030781048b7410a19e9681bdee98 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 3 Aug 2026 22:28:21 +0200 Subject: [PATCH 09/13] Add top menu bars, shortcuts, and shared raylib_widgets across camera_lidar_* and step2 UX/code harmonization across camera_lidar_calibration, camera_lidar_trajectory_viewer, camera_lidar_intrinsics_calib, and multi_view_tls_registration_step_2: - Top menu bars (File/View/Help) replacing ad-hoc side-panel Browse buttons, matching the convention used by the rest of HDMapping's apps. - Keyboard shortcuts for File actions and view toggles, following each file's existing shortcut-handling idiom (ImGui::IsKeyPressed+io.AddKeyEvent for the calibration app, raylib IsKeyPressed for the trajectory viewer). Chords that collided in meaning with step2's existing bindings were re-lettered (Ctrl+L->Ctrl+Shift+C, Ctrl+E->Ctrl+S, bare F->bare V); Ctrl+O and bare C/P were kept aligned since they already matched step2's intent. - New raylib_widgets/ static lib (raylib+imgui_raylib only, no Eigen/core coupling) holding code that was duplicated or near-duplicated across these apps: the compass/ruler 3D overlay, the DPI-aware fitWindowToScreen window positioning (previously byte-identical in 3 apps; step2's own inline copy lacked the DPI-scale correction, now fixed), and a simplified ShortcutEntry/ShowShortcutsTable shortcuts-help table. The trajectory viewer gets an in-app shortcuts reference (Help menu) for the first time. - Merging step2's two-list shortcut-table indirection (rl_utils.cpp's generic scaffold + multi_view_tls_registration_gui.cpp's per-app overrides) into one list surfaced two pre-existing bugs, now fixed: a missing "Ctrl+J" entry was silently shifting every shortcut description after "J" by one row, and "Right click + drag" had a stray "n" instead of its real "camera pan" text. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01DiH2pr8ruiHu6k7Y2wSXS2 --- CMakeLists.txt | 1 + apps/camera_lidar_calibration/App.cpp | 66 +- apps/camera_lidar_calibration/App.h | 1 + apps/camera_lidar_calibration/CMakeLists.txt | 1 + apps/camera_lidar_calibration/UI.cpp | 157 ++++- apps/camera_lidar_calibration/UI.h | 11 + .../CMakeLists.txt | 1 + .../IntrinsicsCalib.cpp | 47 +- .../CMakeLists.txt | 1 + .../TrajectoryViewer.cpp | 312 ++++++--- .../CMakeLists.txt | 1 + .../multi_view_tls_registration_gui.cpp | 343 +++++----- apps/multi_view_tls_registration/rl_utils.cpp | 630 ++++++------------ apps/multi_view_tls_registration/rl_utils.h | 124 ++-- raylib_widgets/CMakeLists.txt | 23 + .../include/RaylibWidgets/CompassRuler.h | 32 + .../include/RaylibWidgets/ShortcutsTable.h | 21 + .../include/RaylibWidgets/WindowFit.h | 17 + raylib_widgets/src/CompassRuler.cpp | 60 ++ raylib_widgets/src/ShortcutsTable.cpp | 46 ++ raylib_widgets/src/WindowFit.cpp | 46 ++ 21 files changed, 1081 insertions(+), 860 deletions(-) create mode 100644 raylib_widgets/CMakeLists.txt create mode 100644 raylib_widgets/include/RaylibWidgets/CompassRuler.h create mode 100644 raylib_widgets/include/RaylibWidgets/ShortcutsTable.h create mode 100644 raylib_widgets/include/RaylibWidgets/WindowFit.h create mode 100644 raylib_widgets/src/CompassRuler.cpp create mode 100644 raylib_widgets/src/ShortcutsTable.cpp create mode 100644 raylib_widgets/src/WindowFit.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b7b89bb..c0907910 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,6 +102,7 @@ include(cmake/raylib.cmake) # ============================================================================ add_subdirectory(core) add_subdirectory(calib_core) +add_subdirectory(raylib_widgets) set(CORE_LIBRARIES core) set(GUI_LIBRARIES imgui imguizmo implot) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 0b13a4a7..b79e0402 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -1,7 +1,10 @@ #include "App.h" #include "imgui.h" +#include "raymath.h" #include "rlImGui.h" #include +#include +#include #include #include #include @@ -427,59 +430,13 @@ void AppState::saveCalibration(const char* 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 " HDMAPPING_VERSION_STRING)); - fitWindowToScreen(); + raylib_widgets::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); @@ -522,10 +479,11 @@ void App::update() // ── App::draw ───────────────────────────────────────────────────────────────── void App::draw() { + float menuBarH = ImGui::GetFrameHeight(); float panelW = 340.f; float viewW = (float)GetScreenWidth() - panelW; - float viewH = (float)GetScreenHeight(); - float view3DY = viewH * 0.5f; // 3D starts at middle + float viewH = (float)GetScreenHeight() - menuBarH; + float view3DY = menuBarH + viewH * 0.5f; // 3D starts at middle, below the menu bar // ── Image + projection overlay (GPU, into render texture) if (state.imageLoaded) @@ -538,7 +496,7 @@ void App::draw() // Clipping for 3D region (bottom-left) // Note: raylib scissor is in screen coords (y-down) - BeginScissorMode(0, (int)view3DY, (int)viewW, (int)(viewH - view3DY)); + BeginScissorMode(0, (int)view3DY, (int)viewW, (int)(viewH * 0.5f)); Camera3D cam3d = state.orbit.toRaylib(); BeginMode3D(cam3d); @@ -562,6 +520,14 @@ void App::draw() EndMode3D(); EndScissorMode(); + if (state.showCompassRuler) + { + Vector3 fwd = Vector3Normalize(Vector3Subtract(cam3d.target, cam3d.position)); + Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam3d.up)); + Vector3 up = Vector3CrossProduct(right, fwd); + raylib_widgets::drawCompassRuler(right, up, state.orbit.distance, LIGHTGRAY); + } + // ── 3D label DrawText("3D View [LMB: orbit | RMB: pan | Scroll: zoom]", 8, (int)view3DY + 4, 14, LIGHTGRAY); diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index 071d16e3..8da95f73 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -28,6 +28,7 @@ struct AppState { // ── visualization ───────────────────────────────────────────────────────── VisualizationParams vizParams; + bool showCompassRuler = true; // ── 3D camera ───────────────────────────────────────────────────────────── OrbitCamera orbit; diff --git a/apps/camera_lidar_calibration/CMakeLists.txt b/apps/camera_lidar_calibration/CMakeLists.txt index bad928d5..ce28ef36 100644 --- a/apps/camera_lidar_calibration/CMakeLists.txt +++ b/apps/camera_lidar_calibration/CMakeLists.txt @@ -28,6 +28,7 @@ target_compile_definitions(camera_lidar_calibration PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_calibration PRIVATE calib_core core_pfd + raylib_widgets raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index d7dc2cef..67bc8086 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -42,11 +42,15 @@ static void helpMarker(const char* desc) // ── Main draw ──────────────────────────────────────────────────────────────── void UI::draw(AppState& state) { + panelMenuBar(state); + handleShortcuts(state); + ImGuiIO& io = ImGui::GetIO(); + float menuBarH = ImGui::GetFrameHeight(); float panelW = 340.f; - float panelH = (float)GetScreenHeight(); + float panelH = (float)GetScreenHeight() - menuBarH; - ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, 0), ImGuiCond_Always); + ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, menuBarH), ImGuiCond_Always); ImGui::SetNextWindowSize(ImVec2(panelW, panelH), ImGuiCond_Always); ImGui::Begin( "Controls", @@ -84,8 +88,8 @@ void UI::draw(AppState& state) if (state.renderer.imageTexValid) { float viewW = io.DisplaySize.x - panelW; - float viewH = io.DisplaySize.y * 0.5f; - ImGui::SetNextWindowPos(ImVec2(0, 0), ImGuiCond_Always); + float viewH = (io.DisplaySize.y - menuBarH) * 0.5f; + ImGui::SetNextWindowPos(ImVec2(0, menuBarH), ImGuiCond_Always); ImGui::SetNextWindowSize(ImVec2(viewW, viewH), ImGuiCond_Always); ImGui::Begin( "Image View", @@ -174,6 +178,141 @@ void UI::drawImageView(AppState& state) ImGui::TextColored(ImVec4(1, 1, 0, 0.8f), "%.0f%% [wheel: zoom | drag: pan | dbl-click: reset]", zoom2D * fitScale * 100.f); } +// ── Menu bar ───────────────────────────────────────────────────────────────── +// File actions moved here from the side panel's Browse buttons, matching the +// File-menu convention used by the rest of HDMapping's apps (e.g. +// mandeye_single_session_viewer). The side panel keeps its path textboxes and +// Load/Add/Save buttons for the manual-path-entry workflow. +// ── File actions ───────────────────────────────────────────────────────────── +// Factored out of panelMenuBar so the File menu items and their keyboard +// shortcuts (handleShortcuts) call the exact same code, matching the +// openSession()-style convention used by mandeye_single_session_viewer etc. +void UI::actionOpenImage(AppState& state) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select camera image", mandeye::fd::ImageFilter); + if (!path.empty()) + { + setBuf(imagePathBuf, sizeof(imagePathBuf), path); + state.loadImage(imagePathBuf); + } +} + +void UI::actionOpenPointCloud(AppState& state) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select point cloud", mandeye::fd::LazFilter); + if (!path.empty()) + { + setBuf(cloudPathBuf, sizeof(cloudPathBuf), path); + state.loadCloud(cloudPathBuf); + } +} + +void UI::actionAddPointCloud(AppState& state) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select point cloud", mandeye::fd::LazFilter); + if (!path.empty()) + { + setBuf(cloudPathBuf, sizeof(cloudPathBuf), path); + state.addCloud(cloudPathBuf); + } +} + +void UI::actionOpenIntrinsics(AppState& state) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select intrinsics file", mandeye::fd::IntrinsicsFilter); + if (!path.empty()) + { + setBuf(intrPathBuf, sizeof(intrPathBuf), path); + state.loadIntrinsics(intrPathBuf); + } +} + +void UI::actionOpenCalibration(AppState& state) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter); + if (!path.empty()) + { + setBuf(savePath, sizeof(savePath), path); + state.loadCalibration(savePath); + } +} + +void UI::actionSaveCalibration(AppState& state) +{ + std::string path = mandeye::fd::SaveFileDialog("Save calibration file", mandeye::fd::json_filter, ".json", "calibration.json"); + if (!path.empty()) + { + setBuf(savePath, sizeof(savePath), path); + state.saveCalibration(savePath); + } +} + +// ── Keyboard shortcuts ─────────────────────────────────────────────────────── +// Ctrl-combos follow the io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_X, false) +// + io.AddKeyEvent(...) reset convention used by mandeye_single_session_viewer +// and multi_view_tls_registration_gui.cpp. Guarded by !WantCaptureKeyboard so +// they don't fire while a path textbox has focus. +void UI::handleShortcuts(AppState& state) +{ + ImGuiIO& io = ImGui::GetIO(); + if (io.WantCaptureKeyboard) + return; + + auto ctrlPressed = [&io](ImGuiKey key) + { + bool pressed = io.KeyCtrl && ImGui::IsKeyPressed(key, false); + if (pressed) + { + io.AddKeyEvent(key, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + return pressed; + }; + + if (ctrlPressed(ImGuiKey_I)) + actionOpenImage(state); + if (io.KeyShift && ctrlPressed(ImGuiKey_O)) + actionAddPointCloud(state); + else if (ctrlPressed(ImGuiKey_O)) + actionOpenPointCloud(state); + if (ctrlPressed(ImGuiKey_K)) + actionOpenIntrinsics(state); + if (ctrlPressed(ImGuiKey_L)) + actionOpenCalibration(state); + if (ctrlPressed(ImGuiKey_S)) + actionSaveCalibration(state); + + if (ImGui::IsKeyPressed(ImGuiKey_C, false)) + state.showCompassRuler = !state.showCompassRuler; +} + +void UI::panelMenuBar(AppState& state) +{ + if (!ImGui::BeginMainMenuBar()) + return; + + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Open Image...", "Ctrl+I")) + actionOpenImage(state); + if (ImGui::MenuItem("Open Point Cloud...", "Ctrl+O")) + actionOpenPointCloud(state); + if (ImGui::MenuItem("Add Point Cloud...", "Ctrl+Shift+O")) + actionAddPointCloud(state); + ImGui::Separator(); + if (ImGui::MenuItem("Open Intrinsics...", "Ctrl+K")) + actionOpenIntrinsics(state); + ImGui::Separator(); + if (ImGui::MenuItem("Open Calibration...", "Ctrl+L")) + actionOpenCalibration(state); + if (ImGui::MenuItem("Save Calibration...", "Ctrl+S")) + actionSaveCalibration(state); + ImGui::EndMenu(); + } + + ImGui::EndMainMenuBar(); +} + // ── Files ──────────────────────────────────────────────────────────────────── void UI::panelFiles(AppState& state) { @@ -181,16 +320,12 @@ void UI::panelFiles(AppState& state) ImGui::Text("JPG image:"); ImGui::InputText("##img", imagePathBuf, sizeof(imagePathBuf)); - if (ImGui::Button("Browse...##img", ImVec2(-1, 0))) - setBuf(imagePathBuf, sizeof(imagePathBuf), mandeye::fd::OpenFileDialogOneFile("Select camera image", mandeye::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), mandeye::fd::OpenFileDialogOneFile("Select point cloud", mandeye::fd::LazFilter)); { float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; if (ImGui::Button("Load##laz", ImVec2(hw, 0))) @@ -203,16 +338,12 @@ void UI::panelFiles(AppState& state) 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), mandeye::fd::OpenFileDialogOneFile("Select intrinsics file", mandeye::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), mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter)); float hw = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.x) * 0.5f; if (ImGui::Button("Load##calib", ImVec2(hw, 0))) state.loadCalibration(savePath); @@ -312,6 +443,8 @@ void UI::panelVisualization(AppState& state) const char* modes[] = { "Jet (depth)", "Jet (intensity)", "Jet (height)", "Camera RGB" }; ImGui::Combo("Color mode", &vp.colorMode, modes, 4); ImGui::PopItemWidth(); + + ImGui::Checkbox("Show compass/ruler (C)", &state.showCompassRuler); } // ── Status bar ──────────────────────────────────────────────────────────────── diff --git a/apps/camera_lidar_calibration/UI.h b/apps/camera_lidar_calibration/UI.h index 73bdf24e..146cbad4 100644 --- a/apps/camera_lidar_calibration/UI.h +++ b/apps/camera_lidar_calibration/UI.h @@ -24,9 +24,20 @@ class UI { int viewImgW = 0, viewImgH = 0; void drawImageView(AppState& state); + void panelMenuBar(AppState& state); void panelFiles(AppState& state); void panelIntrinsics(AppState& state); void panelExtrinsics(AppState& state); void panelVisualization(AppState& state); void panelStatus(const AppState& state); + + // File actions -- shared by the File menu items and their keyboard + // shortcuts (handleShortcuts). + void actionOpenImage(AppState& state); + void actionOpenPointCloud(AppState& state); + void actionAddPointCloud(AppState& state); + void actionOpenIntrinsics(AppState& state); + void actionOpenCalibration(AppState& state); + void actionSaveCalibration(AppState& state); + void handleShortcuts(AppState& state); }; diff --git a/apps/camera_lidar_intrinsics_calib/CMakeLists.txt b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt index 375a2872..9765bcc6 100644 --- a/apps/camera_lidar_intrinsics_calib/CMakeLists.txt +++ b/apps/camera_lidar_intrinsics_calib/CMakeLists.txt @@ -21,6 +21,7 @@ target_compile_definitions(camera_lidar_intrinsics_calib PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_intrinsics_calib PRIVATE calib_core core_pfd + raylib_widgets raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index 3ffa7a1b..9bab769d 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -36,50 +37,6 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) 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 @@ -368,7 +325,7 @@ int main(int argc, char* argv[]) SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); InitWindow(1280, 800, ("Intrinsics Calibration " HDMAPPING_VERSION_STRING)); - fitWindowToScreen(); + raylib_widgets::fitWindowToScreen(); // PANEL_W below (330) is fixed; below this the image view and panel // start overlapping instead of scrolling. SetWindowMinSize(800, 500); diff --git a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt index fab862b8..9824bf92 100644 --- a/apps/camera_lidar_trajectory_viewer/CMakeLists.txt +++ b/apps/camera_lidar_trajectory_viewer/CMakeLists.txt @@ -42,6 +42,7 @@ target_compile_definitions(camera_lidar_trajectory_viewer PRIVATE WITH_GUI=1) target_link_libraries(camera_lidar_trajectory_viewer PRIVATE calib_core core_pfd + raylib_widgets raylib imgui_raylib rlimgui diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 09a9c6ba..d5746620 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -9,6 +9,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -34,6 +37,25 @@ using namespace calib; namespace fs = std::filesystem; +// Shortcuts help table (Help menu). Only lists this app's actual bindings -- +// no A-Z scaffold like multi_view_tls_registration_step_2's, since +// ShowShortcutsTable() just renders whatever it's given. +static const std::vector appShortcuts = { + { "Normal keys", "C", "Toggle compass/ruler" }, + { "", "P", "Toggle show path" }, + { "", "V", "Toggle show frustums" }, + { "", "Ctrl (tap)", "Toggle Intensity <-> RGB point color" }, + { "", "Ctrl+O", "Select LIO result directory" }, + { "", "Ctrl+Shift+O", "Select CAMERA_0 directory" }, + { "", "Ctrl+Shift+C", "Open calibration" }, + { "", "Ctrl+S", "Export colored point cloud" }, + { "Special keys", "Left arrow", "Previous image (image preview)" }, + { "", "Right arrow", "Next image (image preview)" }, + { "Mouse related", "Left click + drag", "Orbit camera" }, + { "", "Right click + drag", "Pan camera" }, + { "", "Scroll", "Zoom camera" }, +}; + // 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) @@ -44,51 +66,6 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) 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) { @@ -350,6 +327,7 @@ struct Orbit } }; + struct ColorPt { float x, y, z; @@ -382,6 +360,8 @@ struct State // controls bool showPath = true; bool showFrustums = true; + bool showCompassRuler = true; + bool showHelp = false; bool isolateCamera = false; // render only points colored by the selected (preview) image float frustumScale = 0.5f; float pointSize = 2.f; @@ -1087,6 +1067,51 @@ static void exportLAZ(State& s) s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; } +// ── File actions ───────────────────────────────────────────────────────────── +// Factored out so the File menu items and their keyboard shortcuts (in the +// main loop below) call the exact same code, matching the openSession()-style +// convention used by mandeye_single_session_viewer/multi_view_tls_registration. +static void actionSelectLioResultDir(State& s) +{ + setBuf(s.sessionBuf, sizeof(s.sessionBuf), mandeye::fd::SelectFolder("Select LIO result directory")); +} + +static void actionSelectCamera0Dir(State& s) +{ + setBuf(s.cameraBuf, sizeof(s.cameraBuf), mandeye::fd::SelectFolder("Select CAMERA_0 directory")); +} + +static void actionOpenCalibration(State& s) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter); + if (!path.empty()) + { + setBuf(s.calibBuf, sizeof(s.calibBuf), path); + loadCalib(s); + } +} + +static void actionExportColoredPointCloud(State& s) +{ + std::string defaultName = fs::path(s.exportBuf).filename().string(); + std::string path = mandeye::fd::SaveFileDialog("Export colored point cloud", mandeye::fd::LazFilter, ".laz", defaultName); + if (!path.empty()) + { + setBuf(s.exportBuf, sizeof(s.exportBuf), path); + exportLAZ(s); + } +} + +static void actionSelectRosOutputDir(State& s) +{ + setBuf(s.rosOutBuf, sizeof(s.rosOutBuf), mandeye::fd::SelectFolder("Select ROS 2 bag output directory")); +} + +static void actionSelectColmapOutputDir(State& s) +{ + setBuf(s.colmapBuf, sizeof(s.colmapBuf), mandeye::fd::SelectFolder("Select COLMAP output directory")); +} + // 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) @@ -1413,7 +1438,7 @@ int main(int argc, char* argv[]) SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); InitWindow(1400, 900, ("Trajectory Viewer " HDMAPPING_VERSION_STRING)); - fitWindowToScreen(); + raylib_widgets::fitWindowToScreen(); // panelW below is user-resizable but the 3D view still needs room. SetWindowMinSize(900, 500); SetTargetFPS(60); @@ -1494,6 +1519,30 @@ int main(int argc, char* argv[]) if (IsKeyPressed(KEY_LEFT_CONTROL) || IsKeyPressed(KEY_RIGHT_CONTROL)) s.colorMode = (s.colorMode == 1) ? 0 : 1; + // Chord choices avoid colliding in MEANING with + // multi_view_tls_registration_step_2's shortcuts (Ctrl+L there + // is manual loop closure, Ctrl+E is the lio segments editor; + // bare F there is the "camera Front" preset). Ctrl+O and bare + // C/P are kept aligned with step2 (Ctrl+O = open/load session, + // C = compass/ruler). + bool ctrlDown = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL); + bool shiftDown = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); + if (ctrlDown && shiftDown && IsKeyPressed(KEY_O)) + actionSelectCamera0Dir(s); + else if (ctrlDown && IsKeyPressed(KEY_O)) + actionSelectLioResultDir(s); + if (ctrlDown && shiftDown && IsKeyPressed(KEY_C)) + actionOpenCalibration(s); + if (ctrlDown && IsKeyPressed(KEY_S)) + actionExportColoredPointCloud(s); + + if (!ctrlDown && IsKeyPressed(KEY_P)) + s.showPath = !s.showPath; + if (!ctrlDown && IsKeyPressed(KEY_V)) + s.showFrustums = !s.showFrustums; + if (!ctrlDown && IsKeyPressed(KEY_C)) + s.showCompassRuler = !s.showCompassRuler; + if (IsKeyPressed(KEY_LEFT)) { s.imgViewIdx = std::max(s.imgViewIdx - 1, 0); @@ -1519,6 +1568,14 @@ int main(int argc, char* argv[]) DrawLine3D({ 0, 0, 0 }, { 0, 0, -2 }, BLUE); EndMode3D(); + if (s.showCompassRuler) + { + Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); + Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); + Vector3 up = Vector3CrossProduct(right, fwd); + raylib_widgets::drawCompassRuler(right, up, s.orbit.dist, LIGHTGRAY); + } + // ── upload image viewer texture if worker produced one ──────────────── { cv::Mat toUpload; @@ -1542,9 +1599,111 @@ int main(int argc, char* argv[]) // ── ImGui panel ─────────────────────────────────────────────────────── rlImGuiBegin(); + + if (ImGui::BeginMainMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Select LIO Result Directory...", "Ctrl+O")) + actionSelectLioResultDir(s); + if (ImGui::MenuItem("Select CAMERA_0 Directory...", "Ctrl+Shift+O")) + actionSelectCamera0Dir(s); + ImGui::Separator(); + if (ImGui::MenuItem("Open Calibration...", "Ctrl+Shift+C")) + actionOpenCalibration(s); + ImGui::Separator(); + if (ImGui::MenuItem("Export Colored Point Cloud...", "Ctrl+S")) + actionExportColoredPointCloud(s); + ImGui::Separator(); + if (ImGui::MenuItem("Select ROS 2 Bag Output Directory...")) + actionSelectRosOutputDir(s); + ImGui::Separator(); + if (ImGui::MenuItem("Select COLMAP Output Directory...")) + actionSelectColmapOutputDir(s); + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("View")) + { + ImGui::MenuItem("Show path", "P", &s.showPath); + ImGui::MenuItem("Show frustums", "V", &s.showFrustums); + ImGui::MenuItem("Show compass/ruler", "C", &s.showCompassRuler); + ImGui::Separator(); + ImGui::SetNextItemWidth(140.f); + ImGui::SliderFloat("Frustum scale", &s.frustumScale, 0.05f, 5.f, "%.2f"); + ImGui::SetNextItemWidth(140.f); + ImGui::SliderFloat("Point size", &s.pointSize, 1.f, 20.f, "%.1f"); + ImGui::SetNextItemWidth(140.f); + ImGui::SliderInt("Draw decimation", &s.drawDecim, 1, 64); + if (!s.imagesFilenamesInTime.empty()) + { + ImGui::Separator(); + ImGui::TextDisabled("Point color:"); + if (ImGui::MenuItem("Intensity", nullptr, s.colorMode == 0)) + s.colorMode = 0; + if (ImGui::MenuItem("RGB (image)", "Ctrl", s.colorMode == 1)) + s.colorMode = 1; + if (ImGui::MenuItem("Camera ID", nullptr, s.colorMode == 2)) + s.colorMode = 2; + if (ImGui::MenuItem("In ROI", nullptr, s.colorMode == 3)) + s.colorMode = 3; + } + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Help")) + { + ImGui::MenuItem("Shortcuts...", nullptr, &s.showHelp); + ImGui::EndMenu(); + } + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + constexpr float ImGuiNumberWidth = 120.0f; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + + ImGui::InputInt("Points render downsampling", &s.drawDecim, 2, 10); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); + ImGui::SameLine(); + + // fps_avg = fps_avg * 0.7f + ImGui::GetIO().Framerate * 0.3f; // exponential smoothing + + // double now = ImGui::GetTime(); // ImGui’s built-in timer (in seconds) + + // ImGui::Checkbox("dynamic", &dynamicSubsampling); + // if (ImGui::IsItemHovered()) + // ImGui::SetTooltip("automatically control subsampling vs FPS: increase bellow 10, decrease above 60"); + // if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) + //{ + // app_state.viewer_decimate_point_cloud += 1; + // lastAdjustTime = now; + //} + // ImGui::SameLine(); + // ImGui::Text("(avg %.1f)", fps_avg); + + if (s.drawDecim < 1) + s.drawDecim = 1; + + ImGui::SameLine(); + // GetFPS()/point-cloud draw-call/vertex count via raylib/ScanRenderer, + // rather than ImGui's own Framerate tracker -- raylib doesn't + // expose a general "draw calls" counter (rlgl's own internal one + // only tracks its immediate-mode batch renderer, not custom + // glDrawArrays calls like ScanRenderer's), so these are scan_renderer's + // own per-frame counts of the calls/points it issued in draw(). + ImGui::Text( + "(%d FPS)", GetFPS()); + + ImGui::EndMainMenuBar(); + } + ImGuiIO& io = ImGui::GetIO(); - ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, 0), ImGuiCond_Always); - ImGui::SetNextWindowSize(ImVec2(panelW, io.DisplaySize.y), ImGuiCond_Always); + float menuBarH = ImGui::GetFrameHeight(); + ImGui::SetNextWindowPos(ImVec2(io.DisplaySize.x - panelW, menuBarH), ImGuiCond_Always); + ImGui::SetNextWindowSize(ImVec2(panelW, io.DisplaySize.y - menuBarH), ImGuiCond_Always); ImGui::Begin("##panel", nullptr, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse); panelW = ImGui::GetWindowWidth(); @@ -1556,12 +1715,8 @@ int main(int argc, char* argv[]) 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), mandeye::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), mandeye::fd::SelectFolder("Select CAMERA_0 directory")); if (ImGui::Button("Load session", ImVec2(-1, 0))) loadSession(s); if (!s.imagesFilenamesInTime.empty()) @@ -1609,11 +1764,6 @@ int main(int argc, char* argv[]) 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), - mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter)); if (ImGui::Button("Load calibration", ImVec2(-1, 0))) loadCalib(s); if (s.calibLoaded) @@ -1656,33 +1806,6 @@ int main(int argc, char* argv[]) 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()) @@ -1718,14 +1841,6 @@ int main(int argc, char* argv[]) 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), - mandeye::fd::SaveFileDialog("Export colored point cloud", mandeye::fd::LazFilter, ".laz", defaultName)); - } if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) exportLAZ(s); if (!s.exportCloud.empty()) @@ -1739,8 +1854,6 @@ int main(int argc, char* argv[]) 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), mandeye::fd::SelectFolder("Select ROS 2 bag output directory")); // Scoped narrower width -- see the "Load decimation" comment above. ImGui::PopItemWidth(); ImGui::PushItemWidth(-140.f); @@ -1800,8 +1913,6 @@ int main(int argc, char* argv[]) 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), mandeye::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(); @@ -1831,6 +1942,15 @@ int main(int argc, char* argv[]) ImGui::End(); + // ── shortcuts help window ─────────────────────────────────────────────── + if (s.showHelp) + { + ImGui::SetNextWindowSize(ImVec2(420, 320), ImGuiCond_FirstUseEver); + if (ImGui::Begin("Shortcuts", &s.showHelp)) + raylib_widgets::ShowShortcutsTable(appShortcuts); + ImGui::End(); + } + // ── floating image viewer window ────────────────────────────────────── if (s.imgViewTexValid) { diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 58e7e19e..ff460d6b 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -68,6 +68,7 @@ target_include_directories( # ordered relative to libcore.a on the final link line -- see the # comment there for why. core_raylib + raylib_widgets imgui_raylib rlimgui imguizmo_raylib diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 76ee6e65..a77fadf0 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #ifdef _WIN32 // portable-file-dialogs.h pulls in real windows.h, whose CloseWindow(HWND)/ @@ -167,87 +168,94 @@ std::vector infoLines = { "LAZ files are the product of MANDEYE process (open them with Cloud Compare)", }; -// App specific shortcuts (Type and Shortcut are just for easy reference) +// App specific shortcuts. Used to be two overlaid lists (this one, plus a +// "generic" scaffold in rl_utils.cpp that ShowShortcutsTable() fell back to +// for blank descriptions) -- merged into one here since raylib_widgets' +// shared ShowShortcutsTable() just takes a single complete list. The merge +// also fixes two bugs the old indirection was hiding: this list was missing +// a "Ctrl+J" entry, silently shifting every entry after "J" by one row +// against the generic list's descriptions; and "Right click + drag" had a +// stray "n" instead of its real "camera pan" description. static const std::vector appShortcuts = { { "Normal keys", "A", "" }, { "", "Ctrl+A", "point cloud Alignment" }, - { "", "B", "" }, + { "", "B", "camera Back" }, { "", "Ctrl+B", "" }, - { "", "C", "" }, + { "", "C", "Compass/ruler" }, { "", "Ctrl+C", "Control points" }, { "", "D", "" }, { "", "Ctrl+D", "" }, { "", "E", "" }, { "", "Ctrl+E", "lio segments Editor" }, - { "", "F", "" }, + { "", "F", "camera Front" }, { "", "Ctrl+F", "" }, { "", "G", "" }, { "", "Ctrl+G", "Ground control points" }, { "", "H", "" }, { "", "Ctrl+H", "" }, - { "", "I", "" }, + { "", "I", "camera Isometric" }, { "", "Ctrl+I", "" }, { "", "J", "" }, - { "", "Ctrl+K", "" }, + { "", "Ctrl+J", "" }, { "", "K", "" }, { "", "Ctrl+K", "" }, - { "", "L", "" }, + { "", "L", "camera Left" }, { "", "Ctrl+L", "manual Loop closure" }, { "", "M", "" }, { "", "Ctrl+M", "" }, { "", "N", "" }, { "", "Ctrl+N", "" }, - { "", "O", "" }, - { "", "Ctrl+O", "Open session" }, + { "", "O", "Ortographic view" }, + { "", "Ctrl+O", "Open/load session/data" }, { "", "P", "" }, { "", "Ctrl+P", "Pose graph slam" }, { "", "Q", "" }, { "", "Ctrl+Q", "" }, - { "", "R", "" }, + { "", "R", "camera Right" }, { "", "Ctrl+R", "Random cloud colors" }, - { "", "Shift+R", "" }, + { "", "Shift+R", "Rotation center" }, { "", "S", "" }, { "", "Ctrl+S", "Save session" }, { "", "Ctrl+Shift+S", "Save subsession" }, - { "", "T", "" }, + { "", "T", "camera Top" }, { "", "Ctrl+T", "Solid cloud color" }, - { "", "U", "" }, + { "", "U", "camera bottom (Under)" }, { "", "Ctrl+U", "" }, { "", "V", "" }, { "", "Ctrl+V", "" }, { "", "W", "" }, { "", "Ctrl+W", "" }, - { "", "X", "" }, + { "", "X", "show aXes" }, { "", "Ctrl+X", "" }, { "", "Y", "" }, { "", "Ctrl+Y", "" }, - { "", "Z", "" }, + { "", "Z", "camera reset" }, { "", "Ctrl+Z", "" }, - { "", "Shift+Z", "" }, - { "", "1-9", "" }, + { "", "Shift+Z", "Lock Z" }, + { "", "1-9", "point size" }, { "Special keys", "Up arrow", "" }, - { "", "Shift + up arrow", "" }, + { "", "Shift + up arrow", "camera translate Up" }, { "", "Ctrl + up arrow", "" }, { "", "Down arrow", "" }, - { "", "Shift + down arrow", "" }, + { "", "Shift + down arrow", "camera translate Down" }, { "", "Ctrl + down arrow", "" }, { "", "Left arrow", "" }, - { "", "Shift + left arrow", "" }, + { "", "Shift + left arrow", "camera translate Left" }, { "", "Ctrl + left arrow", "" }, { "", "Right arrow", "" }, - { "", "Shift + right arrow", "" }, + { "", "Shift + right arrow", "camera translate Right" }, { "", "Ctrl + right arrow", "" }, { "", "Pg down", "" }, { "", "Pg up", "" }, { "", "- key", "" }, { "", "+ key", "" }, - { "Mouse related", "Left click + drag", "" }, - { "", "Right click + drag", "n" }, - { "", "Scroll", "" }, - { "", "Shift + scroll", "" }, - { "", "Shift + drag", "" }, + { "Mouse related", "Left click + drag", "camera rotate" }, + { "", "Right click + drag", "camera pan" }, + { "", "Scroll", "camera zoom" }, + { "", "Shift + scroll", "camera 5x zoom" }, + { "", "Shift + drag", "Dock window to screen edges" }, { "", "Ctrl + left click", "" }, - { "", "Ctrl + right click", "" }, - { "", "Ctrl + middle click", "" } }; + { "", "Ctrl + right click", "change center of rotation" }, + { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; namespace fs = std::filesystem; @@ -1021,13 +1029,13 @@ void observation_picking_gui() if (ImGui::Button("Reset view")) { - new_rotation_center = rotation_center; - new_rotate_x = 0.0; - new_rotate_y = 0.0; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_rotation_center = app_state.rotation_center; + app_state.new_rotate_x = 0.0; + app_state.new_rotate_y = 0.0; + app_state.new_translate_x = app_state.translate_x; + app_state.new_translate_y = app_state.translate_y; + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } } ImGui::EndDisabled(); @@ -1253,9 +1261,9 @@ void lio_segments_gui() if (index_end < 0) index_end = 0; - rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); - rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); - rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + app_state.rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + app_state.rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + app_state.rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); session.point_clouds_container.show_all_from_range(index_begin, index_end); } ImGui::SameLine(); @@ -1270,9 +1278,9 @@ void lio_segments_gui() if (index_end > session.point_clouds_container.point_clouds.size() - 1) index_end = session.point_clouds_container.point_clouds.size() - 1; - rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); - rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); - rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + app_state.rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + app_state.rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + app_state.rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); session.point_clouds_container.show_all_from_range(index_begin, index_end); } ImGui::SameLine(); @@ -1992,18 +2000,18 @@ void settings_gui() ImGui::NewLine(); - ImGui::InputFloat("camera_x", &new_rotation_center.x()); - ImGui::InputFloat("camera_y", &new_rotation_center.y()); - ImGui::InputFloat("camera_z", &new_rotation_center.z()); + ImGui::InputFloat("camera_x", &app_state.new_rotation_center.x()); + ImGui::InputFloat("camera_y", &app_state.new_rotation_center.y()); + ImGui::InputFloat("camera_z", &app_state.new_rotation_center.z()); if (ImGui::Button("set camera")) { - // new_rotate_x = rotate_x; - // new_rotate_y = rotate_y; - // new_translate_x = -new_rotation_center.x(); - // new_translate_y = -new_rotation_center.y(); - // new_translate_z = -new_rotation_center.z(); - camera_transition_active = true; + // app_state.new_rotate_x = app_state.rotate_x; + // app_state.new_rotate_y = app_state.rotate_y; + // app_state.new_translate_x = -app_state.new_rotation_center.x(); + // app_state.new_translate_y = -app_state.new_rotation_center.y(); + // app_state.new_translate_z = -app_state.new_rotation_center.z(); + app_state.camera_transition_active = true; } if (ImGui::Button("Set initial pose to Identity and update other poses")) @@ -2498,18 +2506,18 @@ void renderLoopClosure( // active-edge scans, at their normal stored pose). scan_renderer.draw( pointClouds, - static_cast(point_size), + static_cast(app_state.point_size), scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), 1); // Pose-sequence trail across the whole session, as a chain of thick // green cylinders (sphere at each joint), sized relative to the current - // zoom (translate_z) so it stays visible next to the point cloud. - const float tubeRadius = std::max(0.005f, fabsf(translate_z) * 0.001f); + // zoom (app_state.translate_z) so it stays visible next to the point cloud. + const float tubeRadius = std::max(0.005f, fabsf(app_state.translate_z) * 0.001f); bool first = true; Vector3 prev{}; for (const auto& pc : pointClouds) @@ -2763,11 +2771,11 @@ void renderControlPoints(const ControlPoints& control_points, PointClouds& point scan_renderer.draw( pointClouds, - static_cast(point_size), + static_cast(app_state.point_size), ScanColorMode::Intensity, static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), 1); @@ -3038,7 +3046,7 @@ void display() // window, the rest showing just the clear color. rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - ClearBackground(ColorFromNormalized(Vector4{ bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w })); + ClearBackground(ColorFromNormalized(Vector4{ app_state.bg_color.x * app_state.bg_color.w, app_state.bg_color.y * app_state.bg_color.w, app_state.bg_color.z * app_state.bg_color.w, app_state.bg_color.w })); rlEnableDepthTest(); rlMatrixMode(RL_PROJECTION); @@ -3047,9 +3055,9 @@ void display() updateCameraTransition(); - viewLocal = Eigen::Affine3f::Identity(); + app_state.viewLocal = Eigen::Affine3f::Identity(); - if (!is_ortho) + if (!app_state.is_ortho) { reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); @@ -3060,16 +3068,16 @@ void display() { // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) //{ - // new_rotation_center.x() = + // app_state.new_rotation_center.x() = // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); - // new_rotation_center.y() = + // app_state.new_rotation_center.y() = // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); - // new_rotation_center.z() = + // app_state.new_rotation_center.z() = // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); // - // new_translate_x = -new_rotation_center.x(); - // new_translate_y = -new_rotation_center.y(); - // camera_transition_active = true; + // app_state.new_translate_x = -app_state.new_rotation_center.x(); + // app_state.new_translate_y = -app_state.new_rotation_center.y(); + // app_state.camera_transition_active = true; //} if (session.pose_graph_loop_closure.manipulate_active_edge) @@ -3078,19 +3086,19 @@ void display() { if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) { - new_rotation_center.x() = + app_state.new_rotation_center.x() = session.point_clouds_container .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] .index_from] .m_pose.translation() .x(); - new_rotation_center.y() = + app_state.new_rotation_center.y() = session.point_clouds_container .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] .index_from] .m_pose.translation() .y(); - new_rotation_center.z() = + app_state.new_rotation_center.z() = session.point_clouds_container .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] .index_from] @@ -3099,37 +3107,37 @@ void display() } } - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } new_loop_closure_index = false; } } - viewLocal.translate(rotation_center); + app_state.viewLocal.translate(app_state.rotation_center); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - if (!lock_z) - viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + app_state.viewLocal.translate(Eigen::Vector3f(app_state.translate_x, app_state.translate_y, app_state.translate_z)); + if (!app_state.lock_z) + app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else - viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + app_state.viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + app_state.viewLocal.rotate(Eigen::AngleAxisf(app_state.rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - viewLocal.translate(-rotation_center); + app_state.viewLocal.translate(-app_state.rotation_center); - rlMultMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(app_state.viewLocal.matrix().data()); } else updateOrthoView(); - frame_view_3d = rlGetMatrixModelview(); - frame_proj_3d = rlGetMatrixProjection(); - frame_mvp_3d = MatrixMultiply(frame_view_3d, frame_proj_3d); + app_state.frame_view_3d = rlGetMatrixModelview(); + app_state.frame_proj_3d = rlGetMatrixProjection(); + frame_mvp_3d = MatrixMultiply(app_state.frame_view_3d, app_state.frame_proj_3d); showAxes(); @@ -3177,24 +3185,24 @@ void display() { session.control_points.index_picked_point = -1; // reset picked point when pose changes - new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); - new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); - new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); + app_state.new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); + app_state.new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); + app_state.new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; if (session.control_points.track_pose_with_camera) { - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); } else { - new_translate_x = translate_x; - new_translate_y = translate_y; + app_state.new_translate_x = app_state.translate_x; + app_state.new_translate_y = app_state.translate_y; } - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } // rlImGuiBegin() only polls raylib input into ImGui's IO and calls @@ -3210,7 +3218,7 @@ void display() ShowMainDockSpace(); if (session.control_points.is_imgui) - session.control_points.imgui(session.point_clouds_container, rotation_center); + session.control_points.imgui(session.point_clouds_container, app_state.rotation_center); if (session.ground_control_points.is_imgui) session.ground_control_points.imgui(session.point_clouds_container); @@ -3235,7 +3243,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!is_ortho) + if (!app_state.is_ortho) { // Named-field copy (not a raw struct memcpy): Matrix's // declared field order isn't guaranteed to match the @@ -3259,8 +3267,8 @@ void display() } else ImGuizmo::Manipulate( - m_ortho_gizmo_view, - m_ortho_projection, + app_state.m_ortho_gizmo_view, + app_state.m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, @@ -3333,13 +3341,13 @@ void display() // sites and each frame (see main()/loadSession() below). scan_renderer.draw( session.point_clouds_container.point_clouds, - static_cast(point_size), + static_cast(app_state.point_size), scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), static_cast(session_dims.z_max), - Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + Eigen::Vector3d(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()), static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), - viewer_decimate_point_cloud); + app_state.viewer_decimate_point_cloud); scan_renderer.drawTrajectories( session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff); @@ -3426,7 +3434,7 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (!is_ortho) + if (!app_state.is_ortho) { Matrix projMat = rlGetMatrixProjection(); Matrix modelMat = rlGetMatrixModelview(); @@ -3447,8 +3455,8 @@ void display() } else ImGuizmo::Manipulate( - m_ortho_gizmo_view, - m_ortho_projection, + app_state.m_ortho_gizmo_view, + app_state.m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, @@ -4356,19 +4364,19 @@ void display() { if (ImGui::BeginMenu("Point cloud")) { - auto tmp = point_size; + auto tmp = app_state.point_size; ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points size", &point_size); + ImGui::InputInt("Points size", &app_state.point_size); if (ImGui::IsItemHovered()) ImGui::SetTooltip("keyboard 1-9 keys"); - if (point_size < 1) - point_size = 1; - else if (point_size > 10) - point_size = 10; + if (app_state.point_size < 1) + app_state.point_size = 1; + else if (app_state.point_size > 10) + app_state.point_size = 10; - if (tmp != point_size) + if (tmp != app_state.point_size) for (auto& point_cloud : session.point_clouds_container.point_clouds) - point_cloud.point_size = point_size; + point_cloud.point_size = app_state.point_size; ImGui::Separator(); @@ -4453,7 +4461,7 @@ void display() { auto tmp = session.point_clouds_container.point_clouds[0].line_width; - ImGui::BeginDisabled(!glLineWidthSupport); + ImGui::BeginDisabled(!app_state.glLineWidthSupport); { ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputInt("Line width", &tmp); @@ -4526,7 +4534,7 @@ void display() ImGui::EndMenu(); } - ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background color", (float*)&app_state.bg_color, ImGuiColorEditFlags_NoInputs); ImGui::BeginDisabled(tls_registration.gnss.gnss_poses.size() <= 0); { @@ -4538,26 +4546,26 @@ void display() } ImGui::EndDisabled(); - if (ImGui::MenuItem("Orthographic", "key O", &is_ortho)) + if (ImGui::MenuItem("Orthographic", "key O", &app_state.is_ortho)) { - if (is_ortho) + if (app_state.is_ortho) { - new_rotation_center = rotation_center; - new_rotate_x = 0.0; - new_rotate_y = 0.0; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_rotation_center = app_state.rotation_center; + app_state.new_rotate_x = 0.0; + app_state.new_rotate_y = 0.0; + app_state.new_translate_x = app_state.translate_x; + app_state.new_translate_y = app_state.translate_y; + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); - ImGui::MenuItem("Show axes", "key X", &show_axes); - ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); + ImGui::MenuItem("Show axes", "key X", &app_state.show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &app_state.compass_ruler); - ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); + ImGui::MenuItem("Lock Z", "Shift + Z", &app_state.lock_z, !app_state.is_ortho); ImGui::Separator(); @@ -4613,7 +4621,7 @@ void display() ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 2, 10); + ImGui::InputInt("Points render downsampling", &app_state.viewer_decimate_point_cloud, 2, 10); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); ImGui::SameLine(); @@ -4627,14 +4635,14 @@ void display() // ImGui::SetTooltip("automatically control subsampling vs FPS: increase bellow 10, decrease above 60"); // if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) //{ - // viewer_decimate_point_cloud += 1; + // app_state.viewer_decimate_point_cloud += 1; // lastAdjustTime = now; //} // ImGui::SameLine(); // ImGui::Text("(avg %.1f)", fps_avg); - if (viewer_decimate_point_cloud < 1) - viewer_decimate_point_cloud = 1; + if (app_state.viewer_decimate_point_cloud < 1) + app_state.viewer_decimate_point_cloud = 1; ImGui::SameLine(); // GetFPS()/point-cloud draw-call/vertex count via raylib/ScanRenderer, @@ -4666,7 +4674,7 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - info_gui = !info_gui; + app_state.info_gui = !app_state.info_gui; ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -4725,7 +4733,7 @@ void display() renderControlPointsLabels(session.control_points, session.point_clouds_container); - if (compass_ruler) + if (app_state.compass_ruler) drawMiniCompassWithRuler(); rlImGuiEnd(); @@ -4831,14 +4839,14 @@ void translate_gui() translate_tool.has_transform = false; translate_tool.transform = Eigen::Affine3d::Identity(); - is_ortho = true; - new_rotation_center = rotation_center; - new_rotate_x = 0.0; - new_rotate_y = 0.0; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; - camera_transition_active = true; + app_state.is_ortho = true; + app_state.new_rotation_center = app_state.rotation_center; + app_state.new_rotate_x = 0.0; + app_state.new_rotate_y = 0.0; + app_state.new_translate_x = app_state.translate_x; + app_state.new_translate_y = app_state.translate_y; + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; SetMouseCursor(MOUSE_CURSOR_CROSSHAIR); } @@ -4995,8 +5003,8 @@ void mouse(int glut_button, int state, int x, int y) break; } - mouse_old_x = x; - mouse_old_y = y; + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; return; } @@ -5031,20 +5039,20 @@ void mouse(int glut_button, int state, int x, int y) { min_distance = dist; - new_rotation_center.x() = vp.x(); - new_rotation_center.y() = vp.y(); - new_rotation_center.z() = vp.z(); + app_state.new_rotation_center.x() = vp.x(); + app_state.new_rotation_center.y() = vp.y(); + app_state.new_rotation_center.z() = vp.z(); session.control_points.index_picked_point = j; } } - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } } else @@ -5082,7 +5090,7 @@ void mouse(int glut_button, int state, int x, int y) if (state == GLUT_DOWN) { - mouse_buttons |= 1 << glut_button; + app_state.mouse_buttons |= 1 << glut_button; if (observation_picking.is_observation_picking_mode) { @@ -5102,10 +5110,10 @@ void mouse(int glut_button, int state, int x, int y) } } else if (state == GLUT_UP) - mouse_buttons = 0; + app_state.mouse_buttons = 0; - mouse_old_x = x; - mouse_old_y = y; + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; } } @@ -5151,22 +5159,9 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), // very top of the content area (where ImGui's main menu bar lives) // behind the OS menu bar/title bar instead of below it. Shrinking to // fit the monitor's work area and recentering avoids that; on screens - // that already fit the default size this is a no-op. - { - const int monitor = GetCurrentMonitor(); - const int monitorWidth = GetMonitorWidth(monitor); - const int monitorHeight = GetMonitorHeight(monitor); - const int margin = 100; // room for the OS title bar, menu bar and dock - const int fitWidth = - (monitorWidth > 0) ? std::min(static_cast(window_width), monitorWidth - margin) : static_cast(window_width); - const int fitHeight = - (monitorHeight > 0) ? std::min(static_cast(window_height), monitorHeight - margin) : static_cast(window_height); - if (fitWidth != static_cast(window_width) || fitHeight != static_cast(window_height)) - { - SetWindowSize(fitWidth, fitHeight); - SetWindowPosition((monitorWidth - fitWidth) / 2, (monitorHeight - fitHeight) / 2); - } - } + // that already fit the default size this is a no-op. DPI-aware and + // shared with the camera_lidar_* apps -- see raylib_widgets. + raylib_widgets::fitWindowToScreen(/*marginW=*/100, /*marginH=*/100, /*centerVertically=*/true); rlImGuiSetup(true); ImGuiIO& io = ImGui::GetIO(); @@ -5222,7 +5217,7 @@ int main(int argc, char* argv[]) // directly here instead: mouse() on raylib button-state transitions // (mirroring glutMouseFunc's fire-on-transition semantics), motion() // every frame (mirroring glutMotionFunc -- motion() itself only acts - // when mouse_buttons is set, so this is safe unconditionally), wheel() + // when app_state.mouse_buttons is set, so this is safe unconditionally), wheel() // when GetMouseWheelMove() is nonzero, and display() once per frame. while (!WindowShouldClose()) { diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index 10841d91..bd36ff33 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include @@ -44,50 +45,12 @@ #endif /////////////////////////////////////////////////////////////////////////////////// -// Formerly 's extern globals -- defined here now (see -// rl_utils.h's top comment for why). +// Formerly 's extern globals -- now members of AppStateBase, +// defined in rl_utils.h (see its top comment for why). /////////////////////////////////////////////////////////////////////////////////// -int viewer_decimate_point_cloud = 2; - -int mouse_old_x, mouse_old_y; -int mouse_buttons = 0; -float mouse_sensitivity = 1.0; - -bool is_ortho = false; -bool lock_z = false; -bool show_axes = true; -ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); -int point_size = 1; - -bool info_gui = false; -bool compass_ruler = true; - -Eigen::Affine3f viewLocal; - -Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); -float rotate_x = -35.264f, rotate_y = 135.0f; -float translate_x, translate_y = 0.0; -float translate_z = -50.0; - -double camera_ortho_xy_view_zoom = 10; -double camera_ortho_xy_view_shift_x = 0.0; -double camera_ortho_xy_view_shift_y = 0.0; -double camera_mode_ortho_z_center_h = 0.0; - -// Target camera state for smooth transitions -Eigen::Vector3f new_rotation_center = rotation_center; -float new_rotate_x = rotate_x; -float new_rotate_y = rotate_y; -float new_translate_x = translate_x; -float new_translate_y = translate_y; -float new_translate_z = translate_z; - bool cor_gui = false; -// Transition timing -bool camera_transition_active = false; - bool scroll_hint_enabled = true; bool scroll_hint_active = false; int scroll_hint_count = 0; @@ -96,14 +59,6 @@ double scroll_hint_lastT = 0.0; bool show_about = false; -bool glLineWidthSupport = true; - -float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - -Matrix frame_view_3d = MatrixIdentity(); -Matrix frame_proj_3d = MatrixIdentity(); - // ============================================================================ // Formerly core/src/utils.cpp -- local now (see the big comment at the top // of this file for why). Everywhere the original was pure ImGui/Eigen/GLM @@ -138,38 +93,38 @@ void wheel(int button, int dir, int x, int y) { if (dir > 0) { - if (is_ortho) + if (app_state.is_ortho) { - camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; + app_state.camera_ortho_xy_view_zoom -= 0.1f * app_state.camera_ortho_xy_view_zoom; - if (camera_ortho_xy_view_zoom < 0.1) + if (app_state.camera_ortho_xy_view_zoom < 0.1) { - camera_ortho_xy_view_zoom = 0.1; + app_state.camera_ortho_xy_view_zoom = 0.1; } } else { if (io.KeyShift) - translate_z += 5.0f; + app_state.translate_z += 5.0f; else - translate_z += 1.0f; + app_state.translate_z += 1.0f; } } else { - if (is_ortho) - camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + if (app_state.is_ortho) + app_state.camera_ortho_xy_view_zoom += 0.1 * app_state.camera_ortho_xy_view_zoom; else { if (io.KeyShift) - translate_z -= 5.0f; + app_state.translate_z -= 5.0f; else - translate_z -= 1.0f; + app_state.translate_z -= 1.0f; } } - mouse_sensitivity = fabs(translate_z) / 100; // 1 for translate_z 50 (default zoom) - camera_transition_active = false; + app_state.mouse_sensitivity = fabs(app_state.translate_z) / 100; // 1 for app_state.translate_z 50 (default zoom) + app_state.camera_transition_active = false; if (scroll_hint_enabled) { @@ -211,7 +166,7 @@ void reshape(int w, int h) rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); rlMatrixMode(RL_PROJECTION); rlLoadIdentity(); - if (!is_ortho) + if (!app_state.is_ortho) { const double fovy = 60.0; const double aspect = (double)w / (double)h; @@ -228,10 +183,10 @@ void reshape(int w, int h) float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); rlOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, + -app_state.camera_ortho_xy_view_zoom, + app_state.camera_ortho_xy_view_zoom, + -app_state.camera_ortho_xy_view_zoom / ratio, + app_state.camera_ortho_xy_view_zoom / ratio, -100000, 100000); } @@ -249,24 +204,24 @@ void motion(int x, int y) if (!io.WantCaptureMouse) { float dx, dy; - dx = (float)(x - mouse_old_x); - dy = (float)(y - mouse_old_y); + dx = (float)(x - app_state.mouse_old_x); + dy = (float)(y - app_state.mouse_old_y); - if (mouse_buttons & 1) // left button + if (app_state.mouse_buttons & 1) // left button { - rotate_x += dy * 0.2f; - rotate_y += dx * 0.2f; + app_state.rotate_x += dy * 0.2f; + app_state.rotate_y += dx * 0.2f; breakCameraTransition(); } - if (mouse_buttons & 4) // right button + if (app_state.mouse_buttons & 4) // right button { - if (is_ortho) + if (app_state.is_ortho) { float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); Eigen::Vector3d v( - dx * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), - dy * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), + dx * (app_state.camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), + dy * (app_state.camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), 0); TaitBryanPose pose_tb; pose_tb.px = 0.0; @@ -274,22 +229,22 @@ void motion(int x, int y) pose_tb.pz = 0.0; pose_tb.om = 0.0; pose_tb.fi = 0.0; - pose_tb.ka = (rotate_x + rotate_y) * M_PI / 180.0; + pose_tb.ka = (app_state.rotate_x + app_state.rotate_y) * M_PI / 180.0; auto m = affine_matrix_from_pose_tait_bryan(pose_tb); Eigen::Vector3d v_t = m * v; - camera_ortho_xy_view_shift_x += v_t.x(); - camera_ortho_xy_view_shift_y += v_t.y(); + app_state.camera_ortho_xy_view_shift_x += v_t.x(); + app_state.camera_ortho_xy_view_shift_y += v_t.y(); } else { - translate_x += dx * 0.1f * mouse_sensitivity; - translate_y -= dy * 0.1f * mouse_sensitivity; + app_state.translate_x += dx * 0.1f * app_state.mouse_sensitivity; + app_state.translate_y -= dy * 0.1f * app_state.mouse_sensitivity; breakCameraTransition(); } } - mouse_old_x = x; - mouse_old_y = y; + app_state.mouse_old_x = x; + app_state.mouse_old_y = y; } } @@ -335,26 +290,26 @@ void ShowMainDockSpace() // Was glBegin(GL_LINES)/glColor3f/glVertex3f/glEnd -- rl* rename. void showAxes() { - if (show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes { rlBegin(RL_LINES); rlColor3f(1.f, 1.f, 1.f); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x() + 1.f, app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x() - 1.f, app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y() - 1.f, app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y() + 1.f, app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z() - 1.f); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z()); + rlVertex3f(app_state.rotation_center.x(), app_state.rotation_center.y(), app_state.rotation_center.z() + 1.f); rlEnd(); } - if (show_axes || ImGui::GetIO().KeyCtrl) // origin axes + if (app_state.show_axes || ImGui::GetIO().KeyCtrl) // origin axes { rlBegin(RL_LINES); rlColor3f(1.0f, 0.0f, 0.0f); @@ -375,57 +330,57 @@ void showAxes() // GL-free -- copied verbatim. void updateCameraTransition() { - if (!camera_transition_active) + if (!app_state.camera_transition_active) return; float t = 1.0f - powf(1.0f - std::min(ImGui::GetIO().DeltaTime * camera_transition_speed, 1.0f), 3.0f); - bool doneXrc = fabs(new_rotation_center.x() - rotation_center.x()) < 0.01f; - bool doneYrc = fabs(new_rotation_center.y() - rotation_center.y()) < 0.01f; - bool doneZrc = fabs(new_rotation_center.z() - rotation_center.z()) < 0.01f; - bool doneXr = fabs(new_rotate_x - rotate_x) < 0.01f; - bool doneYr = fabs(new_rotate_y - rotate_y) < 0.01f; - bool doneXt = fabs(new_translate_x - translate_x) < 0.01f; - bool doneYt = fabs(new_translate_y - translate_y) < 0.01f; - bool doneZt = fabs(new_translate_z - translate_z) < 0.01f; + bool doneXrc = fabs(app_state.new_rotation_center.x() - app_state.rotation_center.x()) < 0.01f; + bool doneYrc = fabs(app_state.new_rotation_center.y() - app_state.rotation_center.y()) < 0.01f; + bool doneZrc = fabs(app_state.new_rotation_center.z() - app_state.rotation_center.z()) < 0.01f; + bool doneXr = fabs(app_state.new_rotate_x - app_state.rotate_x) < 0.01f; + bool doneYr = fabs(app_state.new_rotate_y - app_state.rotate_y) < 0.01f; + bool doneXt = fabs(app_state.new_translate_x - app_state.translate_x) < 0.01f; + bool doneYt = fabs(app_state.new_translate_y - app_state.translate_y) < 0.01f; + bool doneZt = fabs(app_state.new_translate_z - app_state.translate_z) < 0.01f; if (!doneXrc) - rotation_center.x() += (new_rotation_center.x() - rotation_center.x()) * t; + app_state.rotation_center.x() += (app_state.new_rotation_center.x() - app_state.rotation_center.x()) * t; if (!doneYrc) - rotation_center.y() += (new_rotation_center.y() - rotation_center.y()) * t; + app_state.rotation_center.y() += (app_state.new_rotation_center.y() - app_state.rotation_center.y()) * t; if (!doneZrc) - rotation_center.z() += (new_rotation_center.z() - rotation_center.z()) * t; + app_state.rotation_center.z() += (app_state.new_rotation_center.z() - app_state.rotation_center.z()) * t; if (!doneXr) - rotate_x += (new_rotate_x - rotate_x) * t; + app_state.rotate_x += (app_state.new_rotate_x - app_state.rotate_x) * t; if (!doneYr) - rotate_y += (new_rotate_y - rotate_y) * t; + app_state.rotate_y += (app_state.new_rotate_y - app_state.rotate_y) * t; if (!doneXt) - translate_x += (new_translate_x - translate_x) * t; + app_state.translate_x += (app_state.new_translate_x - app_state.translate_x) * t; if (!doneYt) - translate_y += (new_translate_y - translate_y) * t; + app_state.translate_y += (app_state.new_translate_y - app_state.translate_y) * t; if (!doneZt) - translate_z += (new_translate_z - translate_z) * t; + app_state.translate_z += (app_state.new_translate_z - app_state.translate_z) * t; - camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); + app_state.camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); - if (!camera_transition_active) + if (!app_state.camera_transition_active) { - rotation_center = new_rotation_center; - rotate_x = new_rotate_x; - rotate_y = new_rotate_y; - translate_x = new_translate_x; - translate_y = new_translate_y; - translate_z = new_translate_z; + app_state.rotation_center = app_state.new_rotation_center; + app_state.rotate_x = app_state.new_rotate_x; + app_state.rotate_y = app_state.new_rotate_y; + app_state.translate_x = app_state.new_translate_x; + app_state.translate_y = app_state.new_translate_y; + app_state.translate_z = app_state.new_translate_z; } } // GL-free -- copied verbatim. void breakCameraTransition() { - if (camera_transition_active == false) + if (app_state.camera_transition_active == false) return; - rotation_center = new_rotation_center; - camera_transition_active = false; + app_state.rotation_center = app_state.new_rotation_center; + app_state.camera_transition_active = false; } // GL-free -- copied verbatim. @@ -436,68 +391,68 @@ void setCameraPreset(CameraPreset preset) switch (preset) { case CAMERA_FRONT: - new_rotate_x = -90.0f; - new_rotate_y = +90.0f; + app_state.new_rotate_x = -90.0f; + app_state.new_rotate_y = +90.0f; triggered = true; break; case CAMERA_BACK: - new_rotate_x = -90.0f; - new_rotate_y = -90.0f; + app_state.new_rotate_x = -90.0f; + app_state.new_rotate_y = -90.0f; triggered = true; break; case CAMERA_LEFT: - new_rotate_x = -90.0f; - new_rotate_y = 180.0f; + app_state.new_rotate_x = -90.0f; + app_state.new_rotate_y = 180.0f; triggered = true; break; case CAMERA_RIGHT: - new_rotate_x = -90.0f; - new_rotate_y = 0.0f; + app_state.new_rotate_x = -90.0f; + app_state.new_rotate_y = 0.0f; triggered = true; break; case CAMERA_TOP: - new_rotate_x = 0.0f; - new_rotate_y = 90.0f; + app_state.new_rotate_x = 0.0f; + app_state.new_rotate_y = 90.0f; triggered = true; break; case CAMERA_BOTTOM: - new_rotate_x = 180.0f; - new_rotate_y = -90.0f; + app_state.new_rotate_x = 180.0f; + app_state.new_rotate_y = -90.0f; triggered = true; break; case CAMERA_ISO: - new_rotate_x = -35.264f; - new_rotate_y = 135.0f; + app_state.new_rotate_x = -35.264f; + app_state.new_rotate_y = 135.0f; triggered = true; break; case CAMERA_RESET: - new_rotation_center = Eigen::Vector3f::Zero(); - new_rotate_x = 0; - new_rotate_y = 0; - new_translate_x = 0; - new_translate_y = 0; - new_translate_z = -50.0f; - mouse_sensitivity = fabs(translate_z) / 100; - - camera_ortho_xy_view_zoom = 10; - camera_ortho_xy_view_shift_x = 0.0; - camera_ortho_xy_view_shift_y = 0.0; - camera_mode_ortho_z_center_h = 0.0; - - viewer_decimate_point_cloud = 1000; + app_state.new_rotation_center = Eigen::Vector3f::Zero(); + app_state.new_rotate_x = 0; + app_state.new_rotate_y = 0; + app_state.new_translate_x = 0; + app_state.new_translate_y = 0; + app_state.new_translate_z = -50.0f; + app_state.mouse_sensitivity = fabs(app_state.translate_z) / 100; + + app_state.camera_ortho_xy_view_zoom = 10; + app_state.camera_ortho_xy_view_shift_x = 0.0; + app_state.camera_ortho_xy_view_shift_y = 0.0; + app_state.camera_mode_ortho_z_center_h = 0.0; + + app_state.viewer_decimate_point_cloud = 1000; triggered = false; break; } if (triggered) { - new_rotation_center = rotation_center; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; + app_state.new_rotation_center = app_state.rotation_center; + app_state.new_translate_x = app_state.translate_x; + app_state.new_translate_y = app_state.translate_y; + app_state.new_translate_z = app_state.translate_z; } - camera_transition_active = true; + app_state.camera_transition_active = true; } // GL-free -- copied verbatim. @@ -548,11 +503,11 @@ void camMenu() ImGui::Text("X"); ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", rotate_x); + ImGui::Text("%.3f", app_state.rotate_x); ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_x); + ImGui::Text("%.3f", app_state.translate_x); ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.x()); + ImGui::Text("%.3f", app_state.rotation_center.x()); ImGui::TableNextRow(); ImGui::TableSetColumnIndex(0); @@ -560,11 +515,11 @@ void camMenu() ImGui::Text("Y"); ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", rotate_y); + ImGui::Text("%.3f", app_state.rotate_y); ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_y); + ImGui::Text("%.3f", app_state.translate_y); ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.y()); + ImGui::Text("%.3f", app_state.rotation_center.y()); ImGui::TableNextRow(); ImGui::TableSetColumnIndex(0); @@ -572,13 +527,13 @@ void camMenu() ImGui::Text("Z"); ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_z); + ImGui::Text("%.3f", app_state.translate_z); ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.y()); + ImGui::Text("%.3f", app_state.rotation_center.y()); ImGui::EndTable(); } - ImGui::Text("Mouse sensitivity: %.4f", mouse_sensitivity); + ImGui::Text("Mouse sensitivity: %.4f", app_state.mouse_sensitivity); ImGui::EndTooltip(); } @@ -607,53 +562,53 @@ void view_kbd_shortcuts() if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) { - translate_x += 0.5f * mouse_sensitivity; + app_state.translate_x += 0.5f * app_state.mouse_sensitivity; breakCameraTransition(); } if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) { - translate_x -= 0.5f * mouse_sensitivity; + app_state.translate_x -= 0.5f * app_state.mouse_sensitivity; breakCameraTransition(); } if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) { - translate_y += 0.5f * mouse_sensitivity; + app_state.translate_y += 0.5f * app_state.mouse_sensitivity; breakCameraTransition(); } if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) { - translate_y -= 0.5f * mouse_sensitivity; + app_state.translate_y -= 0.5f * app_state.mouse_sensitivity; breakCameraTransition(); } if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) { - rotate_y -= 0.6; + app_state.rotate_y -= 0.6; breakCameraTransition(); } if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) { - rotate_y += 0.6; + app_state.rotate_y += 0.6; breakCameraTransition(); } if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) { - rotate_x -= 0.6; + app_state.rotate_x -= 0.6; breakCameraTransition(); } if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) { - rotate_x += 0.6; + app_state.rotate_x += 0.6; breakCameraTransition(); } if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) cor_gui = true; - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !is_ortho) - lock_z = !lock_z; + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !app_state.is_ortho) + app_state.lock_z = !app_state.lock_z; if (io.KeyCtrl || io.KeyAlt || io.KeyShift) return; @@ -676,30 +631,30 @@ void view_kbd_shortcuts() setCameraPreset(CAMERA_RESET); if (ImGui::IsKeyPressed(ImGuiKey_C, false)) - compass_ruler = !compass_ruler; + app_state.compass_ruler = !app_state.compass_ruler; if (ImGui::IsKeyPressed(ImGuiKey_O, false)) - is_ortho = !is_ortho; + app_state.is_ortho = !app_state.is_ortho; if (ImGui::IsKeyPressed(ImGuiKey_X, false)) - show_axes = !show_axes; + app_state.show_axes = !app_state.show_axes; if (ImGui::IsKeyPressed(ImGuiKey_1)) - point_size = 1; + app_state.point_size = 1; if (ImGui::IsKeyPressed(ImGuiKey_2)) - point_size = 2; + app_state.point_size = 2; if (ImGui::IsKeyPressed(ImGuiKey_3)) - point_size = 3; + app_state.point_size = 3; if (ImGui::IsKeyPressed(ImGuiKey_4)) - point_size = 4; + app_state.point_size = 4; if (ImGui::IsKeyPressed(ImGuiKey_5)) - point_size = 5; + app_state.point_size = 5; if (ImGui::IsKeyPressed(ImGuiKey_6)) - point_size = 6; + app_state.point_size = 6; if (ImGui::IsKeyPressed(ImGuiKey_7)) - point_size = 7; + app_state.point_size = 7; if (ImGui::IsKeyPressed(ImGuiKey_8)) - point_size = 8; + app_state.point_size = 8; if (ImGui::IsKeyPressed(ImGuiKey_9)) - point_size = 9; + app_state.point_size = 9; } // GL-free -- copied verbatim. @@ -715,15 +670,15 @@ void cor_window() { ImGui::Text("Select new center of rotation [m]:"); ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); + ImGui::InputFloat("X", &app_state.new_rotation_center.x(), 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); ImGui::SameLine(); - ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); + ImGui::InputFloat("Y", &app_state.new_rotation_center.y(), 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(yText); ImGui::SameLine(); - ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); + ImGui::InputFloat("Z", &app_state.new_rotation_center.z(), 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(zText); ImGui::PopItemWidth(); @@ -732,13 +687,13 @@ void cor_window() if (ImGui::Button("Set")) { - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); + app_state.new_translate_z = app_state.translate_z; - camera_transition_active = true; + app_state.camera_transition_active = true; ImGui::CloseCurrentPopup(); } @@ -786,147 +741,23 @@ void ImGuiHyperlink(const char* url, ImVec4 color) } } -// General shortcuts applicable to any app -- GL-free, copied verbatim. -static const std::vector shortcuts = { { "Normal keys", "A", "" }, - { "", "Ctrl+A", "" }, - { "", "B", "camera Back" }, - { "", "Ctrl+B", "" }, - { "", "C", "Compass/ruler" }, - { "", "Ctrl+C", "" }, - { "", "D", "" }, - { "", "Ctrl+D", "" }, - { "", "E", "" }, - { "", "Ctrl+E", "" }, - { "", "F", "camera Front" }, - { "", "Ctrl+F", "" }, - { "", "G", "" }, - { "", "Ctrl+G", "" }, - { "", "H", "" }, - { "", "Ctrl+H", "" }, - { "", "I", "camera Isometric" }, - { "", "Ctrl+I", "" }, - { "", "J", "" }, - { "", "Ctrl+J", "" }, - { "", "K", "" }, - { "", "Ctrl+K", "" }, - { "", "L", "camera Left" }, - { "", "Ctrl+L", "" }, - { "", "M", "" }, - { "", "Ctrl+M", "" }, - { "", "N", "" }, - { "", "Ctrl+N", "" }, - { "", "O", "Ortographic view" }, - { "", "Ctrl+O", "Open/load session/data" }, - { "", "P", "" }, - { "", "Ctrl+P", "" }, - { "", "Q", "" }, - { "", "Ctrl+Q", "" }, - { "", "R", "camera Right" }, - { "", "Ctrl+R", "" }, - { "", "Shift+R", "Rotation center" }, - { "", "S", "" }, - { "", "Ctrl+S", "" }, - { "", "Ctrl+Shift+S", "" }, - { "", "T", "camera Top" }, - { "", "Ctrl+T", "" }, - { "", "U", "camera bottom (Under)" }, - { "", "Ctrl+U", "" }, - { "", "V", "" }, - { "", "Ctrl+V", "" }, - { "", "W", "" }, - { "", "Ctrl+W", "" }, - { "", "X", "show aXes" }, - { "", "Ctrl+X", "" }, - { "", "Y", "" }, - { "", "Ctrl+Y", "" }, - { "", "Z", "camera reset" }, - { "", "Ctrl+Z", "" }, - { "", "Shift+Z", "Lock Z" }, - { "", "1-9", "point size" }, - { "Special keys", "Up arrow", "" }, - { "", "Shift + up arrow", "camera translate Up" }, - { "", "Ctrl + up arrow", "" }, - { "", "Down arrow", "" }, - { "", "Shift + down arrow", "camera translate Down" }, - { "", "Ctrl + down arrow", "" }, - { "", "Left arrow", "" }, - { "", "Shift + left arrow", "camera translate Left" }, - { "", "Ctrl + left arrow", "" }, - { "", "Right arrow", "" }, - { "", "Shift + right arrow", "camera translate Right" }, - { "", "Ctrl + right arrow", "" }, - { "", "Pg down", "" }, - { "", "Pg up", "" }, - { "", "- key", "" }, - { "", "+ key", "" }, - { "Mouse related", "Left click + drag", "camera rotate" }, - { "", "Right click + drag", "camera pan" }, - { "", "Scroll", "camera zoom" }, - { "", "Shift + scroll", "camera 5x zoom" }, - { "", "Shift + drag", "Dock window to screen edges" }, - { "", "Ctrl + left click", "" }, - { "", "Ctrl + right click", "change center of rotation" }, - { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; - -// GL-free -- copied verbatim. -void ShowShortcutsTable(const std::vector appShortcuts) -{ - if (ImGui::BeginTable( - "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) - { - ImGui::TableSetupScrollFreeze(0, 1); - ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); - ImGui::TableSetupColumn("Description"); - ImGui::TableHeadersRow(); - - std::string lastType; - - for (size_t i = 0; i < shortcuts.size(); ++i) - { - const auto& s = shortcuts[i]; - - if (!s.type.empty() && s.type != lastType) - { - lastType = s.type; - ImGui::TableNextRow(); - - ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, IM_COL32(70, 70, 140, 255)); - - ImGui::TableSetColumnIndex(0); - ImGui::TextColored(ImVec4(0.8f, 0.8f, 1.0f, 1.0f), "%s", lastType.c_str()); - ImGui::TableSetColumnIndex(1); - ImGui::TextUnformatted(""); - } - - auto description = s.description; - - if (description.empty()) - description = appShortcuts[i].description; - - if (!description.empty()) - { - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::TextUnformatted(s.shortcut.c_str()); - ImGui::TableSetColumnIndex(1); - ImGui::TextUnformatted(description.c_str()); - } - } - - ImGui::EndTable(); - } -} +// ShortcutEntry/ShowShortcutsTable moved to raylib_widgets (shared with the +// camera_lidar_* apps) -- the generic shortcut-label scaffolding that used to +// live here was merged directly into gui.cpp's appShortcuts (see the comment +// there), removing the two-list indirection (and a pre-existing off-by-one: +// gui.cpp's list was missing a "Ctrl+J" entry, silently misaligning every +// entry after "J" against this list's descriptions). // GL-free -- copied verbatim (glGetString(GL_RENDERER/...) is a plain // string query, still valid under a core-profile context). void info_window(const std::vector& infoLines, const std::vector& appShortcuts) { - if (!info_gui) + if (!app_state.info_gui) return; if (ImGui::Begin( "Info", - &info_gui, + &app_state.info_gui, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) { bool firstLine = true; @@ -976,7 +807,7 @@ void info_window(const std::vector& infoLines, const std::vector& infoLines, const std::vector(GetScreenHeight()) - compassSize * 0.5f; - const float axisPixelLength = compassSize * 0.35f; - - struct Axis - { - Eigen::Vector3f dir; - const char* label; - Color color; - }; - const Axis axes[3] = { - { Eigen::Vector3f::UnitX(), "X (long.)", RED }, - { Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN }, - { Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE }, - }; - - for (const auto& axis : axes) - { - Eigen::Vector3f eyeDir = viewLocal.rotation() * axis.dir; - Vector2 tip = { originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength }; - DrawLineEx(Vector2{ originX, originY }, tip, 2.f, axis.color); - DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); - } - - // Ruler: "nice" (1/2/5 x 10^n) length, mirroring the original's - // 0.1 * fabs(translate_z) heuristic (translate_z is this app's - // zoom/dolly distance). - float rawUnit = std::max(0.001f, 0.1f * fabsf(translate_z)); - float base = powf(10.0f, floorf(log10f(rawUnit))); - float normalized = rawUnit / base; - float niceUnit = normalized < 2.0f ? 1.0f : (normalized < 5.0f ? 2.0f : 5.0f); - float worldLength = niceUnit * base; - - char label[32]; - if (worldLength >= 1000.0f) - snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); - else if (worldLength >= 1.0f) - snprintf(label, sizeof(label), "%.0f [m]", worldLength); - else if (worldLength >= 0.01f) - snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); - else - snprintf(label, sizeof(label), "<1 [cm]"); - - float rulerY = originY + compassSize * 0.45f; - Color rulerColor = ColorFromNormalized(Vector4{ 1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f }); - DrawLineEx(Vector2{ originX - 40.f, rulerY }, Vector2{ originX + 40.f, rulerY }, 2.f, rulerColor); - DrawLineEx(Vector2{ originX - 40.f, rulerY - 5.f }, Vector2{ originX - 40.f, rulerY + 5.f }, 2.f, rulerColor); - DrawLineEx(Vector2{ originX + 40.f, rulerY - 5.f }, Vector2{ originX + 40.f, rulerY + 5.f }, 2.f, rulerColor); - DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); + const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); + Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; + Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; + Color rulerColor = ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); + raylib_widgets::drawCompassRuler( + right, up, app_state.translate_z, rulerColor, raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); } // GL-free -- copied verbatim. @@ -1095,8 +881,8 @@ LaserBeam GetLaserBeam(int x, int y) float ndcX = (2.0f * (float)x) / (float)width - 1.0f; float ndcY = 1.0f - (2.0f * (float)y) / (float)height; - Matrix matView = frame_view_3d; - Matrix matProj = frame_proj_3d; + Matrix matView = app_state.frame_view_3d; + Matrix matProj = app_state.frame_proj_3d; Vector3 nearPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 0.0f }, matProj, matView); Vector3 farPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 1.0f }, matProj, matView); @@ -1140,9 +926,9 @@ void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, index_i = i; index_j = j; - new_rotation_center.x() = static_cast(vp.x()); - new_rotation_center.y() = static_cast(vp.y()); - new_rotation_center.z() = static_cast(vp.z()); + app_state.new_rotation_center.x() = static_cast(vp.x()); + app_state.new_rotation_center.y() = static_cast(vp.y()); + app_state.new_rotation_center.z() = static_cast(vp.z()); if (gcpPicking) { @@ -1155,12 +941,12 @@ void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, } } - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); + app_state.new_translate_z = app_state.translate_z; + app_state.camera_transition_active = true; } // GL-free -- copied verbatim. @@ -1174,17 +960,17 @@ void setNewRotationCenter(int x, int y) pl.b = 0; pl.c = 1; pl.d = 0; - new_rotation_center = rayIntersection(laser_beam, pl).cast(); + app_state.new_rotation_center = rayIntersection(laser_beam, pl).cast(); - std::cout << "Setting new rotation center to:\n" << new_rotation_center << std::endl; + std::cout << "Setting new rotation center to:\n" << app_state.new_rotation_center << std::endl; - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; + app_state.new_rotate_x = app_state.rotate_x; + app_state.new_rotate_y = app_state.rotate_y; + app_state.new_translate_x = -app_state.new_rotation_center.x(); + app_state.new_translate_y = -app_state.new_rotation_center.y(); + app_state.new_translate_z = app_state.translate_z; - camera_transition_active = true; + app_state.camera_transition_active = true; } // GL-free -- copied verbatim. @@ -1205,35 +991,35 @@ bool checkClHelp(int argc, char** argv) // Was glOrtho + gluLookAt (folded into GL_PROJECTION, matching the // original's call order -- gluLookAt ran before the GL_MODELVIEW switch // below) -- rewritten as rlOrtho + rlMultMatrixf with the same lookAt -// matrix already computed via GLM for m_ortho_gizmo_view just above it. +// matrix already computed via GLM for app_state.m_ortho_gizmo_view just above it. void updateOrthoView() { - // still updating viewLocal for compass - viewLocal.rotate(Eigen::AngleAxisf((rotate_x + rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + // still updating app_state.viewLocal for compass + app_state.viewLocal.rotate(Eigen::AngleAxisf((app_state.rotate_x + app_state.rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); ImGuiIO& io = ImGui::GetIO(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); rlOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, + -app_state.camera_ortho_xy_view_zoom, + app_state.camera_ortho_xy_view_zoom, + -app_state.camera_ortho_xy_view_zoom / ratio, + app_state.camera_ortho_xy_view_zoom / ratio, -100000, 100000); glm::mat4 proj = glm::orthoLH_ZO( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, + -app_state.camera_ortho_xy_view_zoom, + app_state.camera_ortho_xy_view_zoom, + -app_state.camera_ortho_xy_view_zoom / ratio, + app_state.camera_ortho_xy_view_zoom / ratio, -100, 100); - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + std::copy(&proj[0][0], &proj[3][3], app_state.m_ortho_projection); - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v_eye_t(-app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h); Eigen::Vector3d v(0, 1, 0); TaitBryanPose pose_tb; @@ -1242,7 +1028,7 @@ void updateOrthoView() pose_tb.pz = 0.0; pose_tb.om = 0.0; pose_tb.fi = 0.0; - pose_tb.ka = -(rotate_x + rotate_y) * DEG_TO_RAD; + pose_tb.ka = -(app_state.rotate_x + app_state.rotate_y) * DEG_TO_RAD; auto m = affine_matrix_from_pose_tait_bryan(pose_tb); Eigen::Vector3d v_t = m * v; @@ -1251,7 +1037,7 @@ void updateOrthoView() glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); - std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); + std::copy(&lookat[0][0], &lookat[3][3], app_state.m_ortho_gizmo_view); rlMultMatrixf(&lookat[0][0]); diff --git a/apps/multi_view_tls_registration/rl_utils.h b/apps/multi_view_tls_registration/rl_utils.h index 32d1a323..ca03ccda 100644 --- a/apps/multi_view_tls_registration/rl_utils.h +++ b/apps/multi_view_tls_registration/rl_utils.h @@ -19,6 +19,8 @@ #include +#include + #include #include #include @@ -73,69 +75,70 @@ enum ColorScheme }; /////////////////////////////////////////////////////////////////////////////////// - -extern int viewer_decimate_point_cloud; - -extern int mouse_old_x, mouse_old_y; -extern int mouse_buttons; -extern float mouse_sensitivity; - -extern bool is_ortho; -extern bool lock_z; -extern bool show_axes; -extern ImVec4 bg_color; -extern int point_size; - -extern bool info_gui; -extern bool compass_ruler; - -extern Eigen::Affine3f viewLocal; - -extern Eigen::Vector3f rotation_center; -extern float rotate_x, rotate_y; -extern float translate_x, translate_y, translate_z; - -extern double camera_ortho_xy_view_zoom; -extern double camera_ortho_xy_view_shift_x; -extern double camera_ortho_xy_view_shift_y; -extern double camera_mode_ortho_z_center_h; - -// Target camera state for smooth transitions -extern Eigen::Vector3f new_rotation_center; -extern float new_rotate_x; -extern float new_rotate_y; -extern float new_translate_x; -extern float new_translate_y; -extern float new_translate_z; - -// Transition timing -extern bool camera_transition_active; - -// The 3D view/projection rlgl had active during this frame's scene render, -// cached by display() right before end3DMatrixStack() resets rlgl's matrix -// stack to the 2D screen-space ortho used for the mini-compass/ImGui pass. -// GetLaserBeam() (called from mouse(), which runs *before* display() each -// frame -- see main()) needs these: querying rlGetMatrixModelview()/ -// rlGetMatrixProjection() live at that point would still see the previous -// frame's post-end3DMatrixStack() state (identity modelview, 2D ortho -// projection), not the 3D camera, producing a meaningless pick ray. -extern Matrix frame_view_3d; -extern Matrix frame_proj_3d; - -// Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width -// support is uniform enough here not to need a runtime check -- always true. -extern bool glLineWidthSupport; - -extern float m_ortho_projection[]; -extern float m_ortho_gizmo_view[]; - -struct ShortcutEntry +struct AppStateBase { - std::string type; - std::string shortcut; - std::string description; + int viewer_decimate_point_cloud = 2; + + int mouse_old_x = 0, mouse_old_y = 0; + int mouse_buttons = 0; + float mouse_sensitivity = 1.0f; + bool is_ortho = false; + bool lock_z = false; + bool show_axes = true; + ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); + int point_size = 1; + + bool info_gui = false; + bool compass_ruler = true; + + Eigen::Affine3f viewLocal; + + Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); + float rotate_x = -35.264f, rotate_y = 135.0f; + float translate_x = 0.0f, translate_y = 0.0f, translate_z = -50.0f; + + double camera_ortho_xy_view_zoom = 10; + double camera_ortho_xy_view_shift_x = 0.0; + double camera_ortho_xy_view_shift_y = 0.0; + double camera_mode_ortho_z_center_h = 0.0; + + // Target camera state for smooth transitions + Eigen::Vector3f new_rotation_center = rotation_center; + float new_rotate_x = rotate_x; + float new_rotate_y = rotate_y; + float new_translate_x = translate_x; + float new_translate_y = translate_y; + float new_translate_z = translate_z; + + // Transition timing + bool camera_transition_active = false; + + // The 3D view/projection rlgl had active during this frame's scene render, + // cached by display() right before end3DMatrixStack() resets rlgl's matrix + // stack to the 2D screen-space ortho used for the mini-compass/ImGui pass. + // GetLaserBeam() (called from mouse(), which runs *before* display() each + // frame -- see main()) needs these: querying rlGetMatrixModelview()/ + // rlGetMatrixProjection() live at that point would still see the previous + // frame's post-end3DMatrixStack() state (identity modelview, 2D ortho + // projection), not the 3D camera, producing a meaningless pick ray. + Matrix frame_view_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + Matrix frame_proj_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + + // Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width + // support is uniform enough here not to need a runtime check -- always true. + bool glLineWidthSupport = true; + + float m_ortho_projection[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + float m_ortho_gizmo_view[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; }; +inline AppStateBase app_state; + +// Now shared with the camera_lidar_* apps -- see raylib_widgets/include/RaylibWidgets/ShortcutsTable.h. +using raylib_widgets::ShortcutEntry; + /////////////////////////////////////////////////////////////////////////////////// std::string truncPath(const std::string& fullPath); @@ -154,7 +157,6 @@ void view_kbd_shortcuts(); void cor_window(); void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); -void ShowShortcutsTable(const std::vector appShortcuts); void info_window(const std::vector& infoLines, const std::vector& appShortcuts); void drawMiniCompassWithRuler(); diff --git a/raylib_widgets/CMakeLists.txt b/raylib_widgets/CMakeLists.txt new file mode 100644 index 00000000..3ee4ee9d --- /dev/null +++ b/raylib_widgets/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(raylib_widgets) + +# Small UI overlay helpers (compass/ruler, DPI-aware window fit-to-screen, +# a generic shortcuts-help table) shared between +# apps/multi_view_tls_registration and the camera_lidar_* apps. Depends on +# raylib + imgui_raylib only -- no Eigen, no core, no calib_core -- so it's +# linkable from both the core_raylib side (already coupled to core/core_math) +# and the calib_core side (deliberately not) without adding coupling either +# way; every current/planned consumer is a raylib+ImGui app already, so +# depending on imgui_raylib (not the separate GLUT-backed `imgui` target) +# doesn't add anything new for them. +add_library(raylib_widgets STATIC + src/CompassRuler.cpp + src/WindowFit.cpp + src/ShortcutsTable.cpp +) + +target_include_directories(raylib_widgets PUBLIC include) +target_link_libraries(raylib_widgets PUBLIC raylib imgui_raylib) + +set_target_properties(raylib_widgets PROPERTIES POSITION_INDEPENDENT_CODE ON) diff --git a/raylib_widgets/include/RaylibWidgets/CompassRuler.h b/raylib_widgets/include/RaylibWidgets/CompassRuler.h new file mode 100644 index 00000000..dcc4739f --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/CompassRuler.h @@ -0,0 +1,32 @@ +#pragma once +#include "raylib.h" + +// Bottom-left axis compass + zoom-tied ruler overlay, shared between +// apps/multi_view_tls_registration (rl_utils.cpp's drawMiniCompassWithRuler) +// and the camera_lidar_* apps. Depends on nothing but raylib -- no Eigen, no +// core -- so it stays linkable from both the core_raylib side (which already +// pulls in core/core_math) and the calib_core side (which deliberately +// doesn't) without adding coupling either way. +namespace raylib_widgets { + +struct CompassAxisLabels { + const char* x = "X"; + const char* y = "Y"; + const char* z = "Z"; +}; + +// right/up: the current camera's screen-right/up directions in world space +// (e.g. cross(forward, camUp) / cross(right, forward) for a raylib Camera3D, +// or the first two rows of a world-to-eye rotation matrix). +// zoomDistance: current camera distance/zoom, used to size the ruler. +// rulerColor: contrast color for the ruler ticks/label (axis lines are +// always drawn in RGB = X/Y/Z). Call after 3D drawing ends (2D screen-space +// overlay). +void drawCompassRuler( + Vector3 right, + Vector3 up, + float zoomDistance, + Color rulerColor, + CompassAxisLabels labels = {}); + +} // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/ShortcutsTable.h b/raylib_widgets/include/RaylibWidgets/ShortcutsTable.h new file mode 100644 index 00000000..f3b8063e --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/ShortcutsTable.h @@ -0,0 +1,21 @@ +#pragma once +#include +#include + +// A generic ImGui table for documenting an app's keyboard/mouse shortcuts, +// shared between apps/multi_view_tls_registration (rl_utils.cpp's +// ShowShortcutsTable/ShortcutEntry) and the camera_lidar_* apps. Depends on +// raylib_widgets' existing imgui_raylib link, no Eigen/core coupling. +namespace raylib_widgets { + +struct ShortcutEntry { + std::string type; // group header (e.g. "Normal keys"), shown once when it changes from the previous entry ("" = no header) + std::string shortcut; // key combo label, e.g. "Ctrl+O" + std::string description; // shown next to shortcut; entries with an empty description are skipped (but still contribute a header if `type` is set) +}; + +// Renders `entries` as a bordered, scrollable two-column table (Shortcut | +// Description) inside the current ImGui window. +void ShowShortcutsTable(const std::vector& entries); + +} // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/WindowFit.h b/raylib_widgets/include/RaylibWidgets/WindowFit.h new file mode 100644 index 00000000..cd1545bf --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/WindowFit.h @@ -0,0 +1,17 @@ +#pragma once + +// Shrinks/repositions the just-created window so it fits within the current +// monitor's usable area, DPI-aware. Was duplicated byte-for-byte across +// apps/camera_lidar_calibration, apps/camera_lidar_trajectory_viewer and +// apps/camera_lidar_intrinsics_calib; multi_view_tls_registration_step_2 had +// its own inline version without the DPI-scale correction (fine on a 1x +// display, but centers/sizes wrong on a 2x Retina one). +namespace raylib_widgets { + +// marginW/marginH: side/top+bottom breathing room (OS menu bar, title bar, +// dock) subtracted from the monitor's usable area. +// centerVertically: false positions the window near the top (Y=30, the +// calib apps' behavior); true centers it vertically (step2's behavior). +void fitWindowToScreen(int marginW = 40, int marginH = 100, bool centerVertically = false); + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/CompassRuler.cpp b/raylib_widgets/src/CompassRuler.cpp new file mode 100644 index 00000000..8431168e --- /dev/null +++ b/raylib_widgets/src/CompassRuler.cpp @@ -0,0 +1,60 @@ +#include +#include "raymath.h" + +#include +#include +#include + +namespace raylib_widgets { + +void drawCompassRuler(Vector3 right, Vector3 up, float zoomDistance, Color rulerColor, CompassAxisLabels labels) +{ + const float compassSize = 200.0f; + const float originX = compassSize * 0.5f; + const float originY = static_cast(GetScreenHeight()) - compassSize * 0.5f; + const float axisPixelLength = compassSize * 0.35f; + + struct Axis { + Vector3 dir; + const char* label; + Color color; + }; + const Axis axes[3] = { + { Vector3{ 1.f, 0.f, 0.f }, labels.x, RED }, + { Vector3{ 0.f, 1.f, 0.f }, labels.y, GREEN }, + { Vector3{ 0.f, 0.f, 1.f }, labels.z, BLUE }, + }; + for (const auto& axis : axes) + { + float ex = Vector3DotProduct(axis.dir, right); + float ey = Vector3DotProduct(axis.dir, up); + Vector2 tip = { originX + ex * axisPixelLength, originY - ey * axisPixelLength }; + DrawLineEx(Vector2{ originX, originY }, tip, 2.f, axis.color); + DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); + } + + // "Nice" (1/2/5 x 10^n) ruler length tied to the current camera zoom. + float rawUnit = std::max(0.001f, 0.1f * std::fabs(zoomDistance)); + float base = std::pow(10.0f, std::floor(std::log10(rawUnit))); + float normalized = rawUnit / base; + float niceUnit = normalized < 2.0f ? 1.0f : (normalized < 5.0f ? 2.0f : 5.0f); + float worldLength = niceUnit * base; + + char label[32]; + if (worldLength >= 1000.0f) + snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); + else if (worldLength >= 1.0f) + snprintf(label, sizeof(label), "%.0f [m]", worldLength); + else if (worldLength >= 0.01f) + snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); + else + snprintf(label, sizeof(label), "<1 [cm]"); + + float rulerY = originY + compassSize * 0.45f; + DrawLineEx(Vector2{ originX - 40.f, rulerY }, Vector2{ originX + 40.f, rulerY }, 2.f, rulerColor); + DrawLineEx(Vector2{ originX - 40.f, rulerY - 5.f }, Vector2{ originX - 40.f, rulerY + 5.f }, 2.f, rulerColor); + DrawLineEx(Vector2{ originX + 40.f, rulerY - 5.f }, Vector2{ originX + 40.f, rulerY + 5.f }, 2.f, rulerColor); + DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); +} + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/ShortcutsTable.cpp b/raylib_widgets/src/ShortcutsTable.cpp new file mode 100644 index 00000000..55717905 --- /dev/null +++ b/raylib_widgets/src/ShortcutsTable.cpp @@ -0,0 +1,46 @@ +#include +#include + +#include + +namespace raylib_widgets { + +void ShowShortcutsTable(const std::vector& entries) +{ + if (!ImGui::BeginTable( + "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) + return; + + ImGui::TableSetupScrollFreeze(0, 1); + ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); + ImGui::TableSetupColumn("Description"); + ImGui::TableHeadersRow(); + + std::string lastType; + for (const auto& e : entries) + { + if (!e.type.empty() && e.type != lastType) + { + lastType = e.type; + ImGui::TableNextRow(); + ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, IM_COL32(70, 70, 140, 255)); + ImGui::TableSetColumnIndex(0); + ImGui::TextColored(ImVec4(0.8f, 0.8f, 1.0f, 1.0f), "%s", lastType.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(""); + } + + if (!e.description.empty()) + { + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::TextUnformatted(e.shortcut.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(e.description.c_str()); + } + } + + ImGui::EndTable(); +} + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/WindowFit.cpp b/raylib_widgets/src/WindowFit.cpp new file mode 100644 index 00000000..fcd353f5 --- /dev/null +++ b/raylib_widgets/src/WindowFit.cpp @@ -0,0 +1,46 @@ +#include +#include "raylib.h" + +#include + +namespace raylib_widgets { + +void fitWindowToScreen(int marginW, int marginH, bool centerVertically) +{ + 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 + + int w = std::min(GetScreenWidth(), monW - marginW); + int h = std::min(GetScreenHeight(), monH - marginH); + if (w != GetScreenWidth() || h != GetScreenHeight()) + SetWindowSize(w, h); + + int posY = centerVertically ? std::max(0, (monH - h) / 2) : 30; + SetWindowPosition(std::max(0, (monW - w) / 2), posY); + + // 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(); +} + +} // namespace raylib_widgets From e01f0ede91c9644ace0027cb3ee3627f9399e138 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 4 Aug 2026 01:35:54 +0200 Subject: [PATCH 10/13] Port center-of-rotation UX to camera_lidar_trajectory_viewer's OrbitCamera raylib_widgets::OrbitCamera (shared with camera_lidar_calibration) gains an eased target transition, ground-plane ray picking (Ctrl+Right-click or middle-click), a shared "Center of rotation" dialog, and a 3D crosshair marker at the current center -- porting multi_view_tls_registration's center-of-rotation UX without replacing OrbitCamera's Camera3D-based rendering pipeline. camera_lidar_trajectory_viewer's local Orbit struct is replaced by the shared OrbitCamera, and its State struct is renamed to AppState. Also finishes/fixes the in-progress raylib_widgets refactor found along the way: builds OrbitCamera.cpp into the library (was listed on disk but never compiled), removes camera_lidar_calibration's now-duplicate OrbitCamera definitions (would have been a link error once OrbitCamera.cpp actually built), and reverts an unfinished move of multi_view_tls_registration's rl_utils.h/.cpp into raylib_widgets -- it stays local to that app per its own "deliberately not shared" comment, and the move had left TrajectoryViewer.cpp with a duplicate app_state definition (compile error) plus missing Core/Eigen include paths. Co-Authored-By: Claude Sonnet 5 --- apps/camera_lidar_calibration/Renderer.cpp | 198 +---------------- apps/camera_lidar_calibration/Renderer.h | 13 +- .../RendererShaders.h | 136 ++++++++++++ apps/camera_lidar_calibration/UI.cpp | 11 +- .../TrajectoryViewer.cpp | 206 ++++-------------- .../TrajectoryViewerShaders.h | 99 +++++++++ .../multi_view_tls_registration_gui.cpp | 13 -- core/CMakeLists.txt | 5 + core/src/raylib_render.cpp | 75 +------ core/src/raylib_render_shaders.hpp | 73 +++++++ raylib_widgets/CMakeLists.txt | 18 +- .../RaylibWidgets/CenterOfRotationWindow.h | 19 ++ .../include/RaylibWidgets/OrbitCamera.h | 54 +++++ .../include/RaylibWidgets/Shaders.h | 30 +++ raylib_widgets/src/CenterOfRotationWindow.cpp | 46 ++++ raylib_widgets/src/OrbitCamera.cpp | 106 +++++++++ 16 files changed, 649 insertions(+), 453 deletions(-) create mode 100644 apps/camera_lidar_calibration/RendererShaders.h create mode 100644 apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h create mode 100644 core/src/raylib_render_shaders.hpp create mode 100644 raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h create mode 100644 raylib_widgets/include/RaylibWidgets/OrbitCamera.h create mode 100644 raylib_widgets/include/RaylibWidgets/Shaders.h create mode 100644 raylib_widgets/src/CenterOfRotationWindow.cpp create mode 100644 raylib_widgets/src/OrbitCamera.cpp diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp index 590a97c3..3f794610 100644 --- a/apps/camera_lidar_calibration/Renderer.cpp +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -3,8 +3,10 @@ #include "raymath.h" // glad function pointers are compiled into raylib; the header only declares them #include "external/glad.h" +#include "RendererShaders.h" #include #include +#include #include // ── Jet colormap ───────────────────────────────────────────────────────────── @@ -21,52 +23,10 @@ Color jetColor(float t) { }; } -// ── 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); - } -} +// OrbitCamera::toRaylib()/update() now live in raylib_widgets/src/OrbitCamera.cpp +// (Renderer.h aliases raylib_widgets::OrbitCamera) -- this used to duplicate +// those definitions, which became a duplicate-symbol link error once +// raylib_widgets/CMakeLists.txt started actually compiling that .cpp. // World-frame convention: E.rx/ry/rz = camera orientation in world (R_wc, ZYX Euler). // E.tx/ty/tz = camera position in world. p_cam = R_wc^T * (p_lidar - C). @@ -105,147 +65,13 @@ void Renderer::shutdown() { } } -// ── 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); -} -)"; +using renderer_shaders::kPointVS; +using renderer_shaders::kPointFS; +using renderer_shaders::kProjVS; +using renderer_shaders::kProjFS; void Renderer::initPointShader() { - pointShader = LoadShaderFromMemory(kPointVS, kPointFS); + pointShader = LoadShaderFromMemory(kPointVS, kPointFS.c_str()); shaderValid = pointShader.id > 0; if (!shaderValid) { TraceLog(LOG_ERROR, "Point cloud shader failed to compile"); @@ -262,7 +88,7 @@ void Renderer::initPointShader() { locCamTex = rlGetLocationUniform(pointShader.id, "imageTex"); } - projShader = LoadShaderFromMemory(kProjVS, kProjFS); + projShader = LoadShaderFromMemory(kProjVS, kProjFS.c_str()); projShaderValid = projShader.id > 0; if (!projShaderValid) { TraceLog(LOG_ERROR, "Projection shader failed to compile"); diff --git a/apps/camera_lidar_calibration/Renderer.h b/apps/camera_lidar_calibration/Renderer.h index e6b527dd..e6f3a215 100644 --- a/apps/camera_lidar_calibration/Renderer.h +++ b/apps/camera_lidar_calibration/Renderer.h @@ -1,21 +1,12 @@ #pragma once #include "raylib.h" +#include #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); -}; +using raylib_widgets::OrbitCamera; struct VisualizationParams { float pointSize = 2.f; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h new file mode 100644 index 00000000..175bc2e4 --- /dev/null +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -0,0 +1,136 @@ +#pragma once + +// GLSL source for Renderer's point/projection shaders. Split out of +// Renderer.cpp to keep the .cpp free of embedded shader text; included only +// there. +#include + +#include + +namespace renderer_shaders +{ +// ── GPU point cloud shaders ────────────────────────────────────────────────── +// Explicit attribute locations so one VAO works with both programs: +// location 0 = position (raylib coords), location 1 = intensity. +inline constexpr 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; +} +)"; + +inline const std::string kPointFS = std::string(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; +)") + raylib_widgets::kJetColormapGLSL + R"( +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). +inline constexpr 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; +} +)"; + +inline const std::string kProjFS = std::string(R"( +#version 330 +in float fragDepth; +in float fragIntensity; +uniform vec2 depthRange; +uniform float opacity; +uniform int colorMode; +out vec4 finalColor; +)") + raylib_widgets::kJetColormapGLSL + R"( +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); +} +)"; +} // namespace renderer_shaders diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 67bc8086..0895b2d6 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -60,8 +60,11 @@ void UI::draw(AppState& state) 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)) + // Alt/Cmd = toggle Camera RGB ↔ Intensity (works anywhere in the window). + // Cmd (Super) alongside Alt for macOS, where Option is awkward to use as + // a modifier (it composes special characters). + if (ImGui::IsKeyPressed(ImGuiKey_LeftAlt) || ImGui::IsKeyPressed(ImGuiKey_RightAlt) || + ImGui::IsKeyPressed(ImGuiKey_LeftSuper) || ImGui::IsKeyPressed(ImGuiKey_RightSuper)) { auto& cm = state.vizParams.colorMode; if (cm == 3) @@ -209,8 +212,8 @@ void UI::actionOpenPointCloud(AppState& state) void UI::actionAddPointCloud(AppState& state) { - std::string path = mandeye::fd::OpenFileDialogOneFile("Select point cloud", mandeye::fd::LazFilter); - if (!path.empty()) + std::vector paths = mandeye::fd::OpenFileDialog("Select point cloud(s)", mandeye::fd::LazFilter, true); + for (const std::string& path : paths) { setBuf(cloudPathBuf, sizeof(cloudPathBuf), path); state.addCloud(cloudPathBuf); diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index d5746620..796453ac 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -9,11 +9,14 @@ #include #include #include +#include #include +#include #include #include #include #include +#include "TrajectoryViewerShaders.h" #include #include #include @@ -33,7 +36,6 @@ #include #include #include - using namespace calib; namespace fs = std::filesystem; @@ -54,6 +56,9 @@ static const std::vector appShortcuts = { { "Mouse related", "Left click + drag", "Orbit camera" }, { "", "Right click + drag", "Pan camera" }, { "", "Scroll", "Zoom camera" }, + { "", "Ctrl+Right click", "Set center of rotation (ground plane)" }, + { "", "Middle click", "Set center of rotation (ground plane)" }, + { "", "Shift+R", "Open 'Center of rotation' dialog" }, }; // Copies `path` into `buf` (truncating to fit), for wiring a native-dialog @@ -143,98 +148,8 @@ static bool interpPose(const std::map& trajMap, int64_t 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); -} -)"; +using trajectory_viewer_shaders::kVS; +using trajectory_viewer_shaders::kFS; struct GpuCloud { @@ -281,53 +196,6 @@ struct GpuCloud } }; -// ── 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; @@ -337,7 +205,7 @@ struct ColorPt }; // ── Application state ───────────────────────────────────────────────────────── -struct State +struct AppState { Trajectory traj; std::vector imageTsNs; @@ -355,7 +223,8 @@ struct State bool shaderOk = false; int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; - Orbit orbit; + raylib_widgets::OrbitCamera orbit; + bool showCenterOfRotationWindow = false; // controls bool showPath = true; @@ -426,7 +295,7 @@ static Vector3 toRL(const Eigen::Vector3f& v) } // Load all cam0_*.jpg from CAMERA_0 (sibling of session dir) into s.images, resized by s.imgScale. -static void loadImages(State& s) +static void loadImages(AppState& s) { s.imagesFilenamesInTime.clear(); fs::path camDir; @@ -492,7 +361,7 @@ static std::map parseMRP(const fs::path& mrpPath) return result; } -static void loadSession(State& s) +static void loadSession(AppState& s) { s.traj.poses.clear(); s.imageTsNs.clear(); @@ -556,7 +425,7 @@ static void loadSession(State& s) (mrp.empty() ? " (no MRP)" : " +MRP") + " — press Load cloud"; } -static void loadCloud(State& s) +static void loadCloud(AppState& s) { s.exportCloud.clear(); s.cloud.unload(); @@ -908,7 +777,7 @@ static void loadCloud(State& s) { 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.orbit.distance = std::max(5.f, mx * 0.3f); } s.status = "Pts: " + std::to_string(s.cloud.count) + " Poses: " + std::to_string(s.traj.poses.size()) + @@ -921,7 +790,7 @@ static void loadCloud(State& s) } } -static void loadCalib(State& s) +static void loadCalib(AppState& s) { std::ifstream f(s.calibBuf); if (!f) @@ -981,7 +850,7 @@ static void loadCalib(State& s) s.status = "Calibration loaded"; } -static void exportLAZ(State& s) +static void exportLAZ(AppState& s) { if (s.exportCloud.empty()) { @@ -1071,17 +940,17 @@ static void exportLAZ(State& s) // Factored out so the File menu items and their keyboard shortcuts (in the // main loop below) call the exact same code, matching the openSession()-style // convention used by mandeye_single_session_viewer/multi_view_tls_registration. -static void actionSelectLioResultDir(State& s) +static void actionSelectLioResultDir(AppState& s) { setBuf(s.sessionBuf, sizeof(s.sessionBuf), mandeye::fd::SelectFolder("Select LIO result directory")); } -static void actionSelectCamera0Dir(State& s) +static void actionSelectCamera0Dir(AppState& s) { setBuf(s.cameraBuf, sizeof(s.cameraBuf), mandeye::fd::SelectFolder("Select CAMERA_0 directory")); } -static void actionOpenCalibration(State& s) +static void actionOpenCalibration(AppState& s) { std::string path = mandeye::fd::OpenFileDialogOneFile("Select calibration file", mandeye::fd::json_filter); if (!path.empty()) @@ -1091,7 +960,7 @@ static void actionOpenCalibration(State& s) } } -static void actionExportColoredPointCloud(State& s) +static void actionExportColoredPointCloud(AppState& s) { std::string defaultName = fs::path(s.exportBuf).filename().string(); std::string path = mandeye::fd::SaveFileDialog("Export colored point cloud", mandeye::fd::LazFilter, ".laz", defaultName); @@ -1102,19 +971,19 @@ static void actionExportColoredPointCloud(State& s) } } -static void actionSelectRosOutputDir(State& s) +static void actionSelectRosOutputDir(AppState& s) { setBuf(s.rosOutBuf, sizeof(s.rosOutBuf), mandeye::fd::SelectFolder("Select ROS 2 bag output directory")); } -static void actionSelectColmapOutputDir(State& s) +static void actionSelectColmapOutputDir(AppState& s) { setBuf(s.colmapBuf, sizeof(s.colmapBuf), mandeye::fd::SelectFolder("Select COLMAP output directory")); } // 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) +static void exportColmap(AppState& s) { if (!s.calibLoaded) { @@ -1231,7 +1100,7 @@ static void exportColmap(State& s) } // Gather everything the ROS exporter needs from current viewer state. -static void buildRosInput(State& s, RosExportInput& in) +static void buildRosInput(AppState& s, RosExportInput& in) { in.traj = s.traj; in.imageFiles = s.imagesFilenamesInTime; @@ -1269,7 +1138,7 @@ static void buildRosInput(State& s, RosExportInput& in) } } -static void exportRos(State& s) +static void exportRos(AppState& s) { if (s.rosBusy.load()) return; @@ -1300,7 +1169,7 @@ static void exportRos(State& s) }); } -static void drawScene(State& s) +static void drawScene(AppState& s) { // ── trajectory path ─────────────────────────────────────────────────────── if (s.showPath) @@ -1410,7 +1279,7 @@ int main(int argc, char* argv[]) return 1; } - State s; + AppState s; // --mjs gives the session manifest; the session directory is its parent. std::string sessionDir; if (args.has("mjs")) @@ -1444,7 +1313,7 @@ int main(int argc, char* argv[]) SetTargetFPS(60); rlImGuiSetup(true); - s.shader = LoadShaderFromMemory(kVS, kFS); + s.shader = LoadShaderFromMemory(kVS, kFS.c_str()); s.shaderOk = s.shader.id > 0; if (s.shaderOk) { @@ -1502,6 +1371,8 @@ int main(int argc, char* argv[]) { bool imguiWants = ImGui::GetIO().WantCaptureMouse; s.orbit.update(!imguiWants); + s.orbit.updateTransition(GetFrameTime()); + Camera3D cam = s.orbit.toRaylib(); // pick up the ROS export result from the worker thread (if any) { @@ -1543,6 +1414,13 @@ int main(int argc, char* argv[]) if (!ctrlDown && IsKeyPressed(KEY_C)) s.showCompassRuler = !s.showCompassRuler; + if (shiftDown && IsKeyPressed(KEY_R)) + s.showCenterOfRotationWindow = true; + if (!imguiWants && ctrlDown && IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) + s.orbit.pickGroundPlaneTarget(GetMousePosition(), cam); + if (!imguiWants && IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) + s.orbit.pickGroundPlaneTarget(GetMousePosition(), cam); + if (IsKeyPressed(KEY_LEFT)) { s.imgViewIdx = std::max(s.imgViewIdx - 1, 0); @@ -1558,7 +1436,6 @@ int main(int argc, char* argv[]) BeginDrawing(); ClearBackground(Color{ 25, 25, 25, 255 }); - Camera3D cam = s.orbit.toRaylib(); BeginMode3D(cam); drawScene(s); DrawGrid(20, 1.f); @@ -1566,6 +1443,7 @@ int main(int argc, char* argv[]) DrawLine3D({ 0, 0, 0 }, { 2, 0, 0 }, RED); DrawLine3D({ 0, 0, 0 }, { 0, 2, 0 }, GREEN); DrawLine3D({ 0, 0, 0 }, { 0, 0, -2 }, BLUE); + raylib_widgets::drawRotationCenterCross(s.orbit.target, s.orbit.distance * 0.05f, WHITE); EndMode3D(); if (s.showCompassRuler) @@ -1573,7 +1451,7 @@ int main(int argc, char* argv[]) Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); Vector3 up = Vector3CrossProduct(right, fwd); - raylib_widgets::drawCompassRuler(right, up, s.orbit.dist, LIGHTGRAY); + raylib_widgets::drawCompassRuler(right, up, s.orbit.distance, LIGHTGRAY); } // ── upload image viewer texture if worker produced one ──────────────── @@ -1628,6 +1506,8 @@ int main(int argc, char* argv[]) ImGui::MenuItem("Show path", "P", &s.showPath); ImGui::MenuItem("Show frustums", "V", &s.showFrustums); ImGui::MenuItem("Show compass/ruler", "C", &s.showCompassRuler); + if (ImGui::MenuItem("Center of rotation...", "Shift+R")) + s.showCenterOfRotationWindow = true; ImGui::Separator(); ImGui::SetNextItemWidth(140.f); ImGui::SliderFloat("Frustum scale", &s.frustumScale, 0.05f, 5.f, "%.2f"); @@ -1942,6 +1822,8 @@ int main(int argc, char* argv[]) ImGui::End(); + raylib_widgets::showCenterOfRotationWindow(s.showCenterOfRotationWindow, s.orbit); + // ── shortcuts help window ─────────────────────────────────────────────── if (s.showHelp) { diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h new file mode 100644 index 00000000..95c9c950 --- /dev/null +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h @@ -0,0 +1,99 @@ +#pragma once + +// GLSL source for the trajectory viewer's point shader. Split out of +// TrajectoryViewer.cpp to keep the .cpp free of embedded shader text; +// included only there. +#include + +#include + +namespace trajectory_viewer_shaders +{ +// colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI +inline constexpr 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; +} +)"; + +inline const std::string kFS = std::string(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; +)") + raylib_widgets::kJetColormapGLSL + R"( +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); +} +)"; +} // namespace trajectory_viewer_shaders diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index a77fadf0..761fb09d 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -4952,19 +4952,6 @@ constexpr int GLUT_UP = 1; void mouse(int glut_button, int state, int x, int y) { ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - - int button = -1; - if (glut_button == GLUT_LEFT_BUTTON) - button = 0; - if (glut_button == GLUT_RIGHT_BUTTON) - button = 1; - if (glut_button == GLUT_MIDDLE_BUTTON) - button = 2; - if (button != -1 && state == GLUT_DOWN) - io.MouseDown[button] = true; - if (button != -1 && state == GLUT_UP) - io.MouseDown[button] = false; // The GLUT-version-gated legacy mouse-wheel-as-button-3/4 fallback is // dropped -- raylib's GetMouseWheelMove() (polled in main()'s loop, diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 88589b29..66d88641 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -136,6 +136,11 @@ target_include_directories(core_raylib PRIVATE ${EXTERNAL_LIBRARIES_DIRECTORY}/include ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + # raylib_widgets/include/RaylibWidgets/Shaders.h -- shared GLSL colormap + # snippet spliced into raylib_render.cpp's fragment shader. Header-only, + # so just the include dir is needed here, not a full raylib_widgets link + # (which would add imgui_raylib as a new transitive core_raylib dependency). + ${REPOSITORY_DIRECTORY}/raylib_widgets/include ) # PUBLIC: consumers (apps/multi_view_tls_registration) get core's # Session/PointCloud types and raylib/rlgl/raymath transitively just by diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 8231ab90..75a4e3f0 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -5,10 +5,13 @@ #include "raymath.h" #include "rlgl.h" +#include "raylib_render_shaders.hpp" + #include #include #include #include +#include namespace { @@ -25,74 +28,8 @@ namespace // ============================================================================ namespace { - // vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per - // scan at upload time (see ScanRenderer::rebuild). vertexPosition is - // already world-space (rebuild() applies pc.m_pose before uploading), so - // it doubles as the Elevation/Distance color modes' input with no extra - // per-vertex data needed. colorMode selects between the flat per-scan - // pointColor (0, the original app's only mode) and the ScanColorMode - // gradients (1/2/3), matching the enum's Intensity/Elevation/Distance - // ordering exactly (see ScanRenderer::draw()'s static_cast below). - const char* kPointVS = R"( -#version 330 -in vec3 vertexPosition; -in float vertexIntensity; -uniform mat4 mvp; -uniform float pointSize; -out float fragIntensity; -out vec3 fragWorldPos; -void main() -{ - gl_Position = mvp * vec4(vertexPosition, 1.0); - gl_PointSize = pointSize; - fragIntensity = vertexIntensity; - fragWorldPos = vertexPosition; -} -)"; - - const char* kPointFS = R"( -#version 330 -uniform vec4 pointColor; -uniform int colorMode; -uniform float elevMin; -uniform float elevMax; -uniform vec3 distCenter; -uniform float distMax; -in float fragIntensity; -in vec3 fragWorldPos; -out vec4 finalColor; - -vec3 jet(float t) -{ - t = clamp(t, 0.0, 1.0); - float r = clamp(1.5 - abs(4.0 * t - 3.0), 0.0, 1.0); - float g = clamp(1.5 - abs(4.0 * t - 2.0), 0.0, 1.0); - float b = clamp(1.5 - abs(4.0 * t - 1.0), 0.0, 1.0); - return vec3(r, g, b); -} - -void main() -{ - if (colorMode == 1) - { - finalColor = vec4(jet(fragIntensity), pointColor.a); - } - else if (colorMode == 2) - { - float range = max(elevMax - elevMin, 1e-6); - finalColor = vec4(jet((fragWorldPos.z - elevMin) / range), pointColor.a); - } - else if (colorMode == 3) - { - float d = length(fragWorldPos - distCenter); - finalColor = vec4(jet(d / max(distMax, 1e-6)), pointColor.a); - } - else - { - finalColor = pointColor; - } -} -)"; + using raylib_render_shaders::kPointVS; + using raylib_render_shaders::kPointFS; // Bytes per vertex in the uploaded buffer (xyz position + intensity). constexpr int kVertexStride = 4 * sizeof(float); @@ -105,7 +42,7 @@ ScanRenderer::~ScanRenderer() void ScanRenderer::init() { - shader_ = LoadShaderFromMemory(kPointVS, kPointFS); + shader_ = LoadShaderFromMemory(kPointVS, kPointFS.c_str()); shaderValid_ = shader_.id > 0; if (shaderValid_) { diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp new file mode 100644 index 00000000..60701dda --- /dev/null +++ b/core/src/raylib_render_shaders.hpp @@ -0,0 +1,73 @@ +#pragma once + +// GLSL source for ScanRenderer's point shader. Split out of raylib_render.cpp +// to keep the .cpp free of embedded shader text; included only there. +#include + +#include + +namespace raylib_render_shaders +{ +// vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per +// scan at upload time (see ScanRenderer::rebuild). vertexPosition is +// already world-space (rebuild() applies pc.m_pose before uploading), so +// it doubles as the Elevation/Distance color modes' input with no extra +// per-vertex data needed. colorMode selects between the flat per-scan +// pointColor (0, the original app's only mode) and the ScanColorMode +// gradients (1/2/3), matching the enum's Intensity/Elevation/Distance +// ordering exactly (see ScanRenderer::draw()'s static_cast below). +inline constexpr const char* kPointVS = R"( +#version 330 +in vec3 vertexPosition; +in float vertexIntensity; +uniform mat4 mvp; +uniform float pointSize; +out float fragIntensity; +out vec3 fragWorldPos; +void main() +{ + gl_Position = mvp * vec4(vertexPosition, 1.0); + gl_PointSize = pointSize; + fragIntensity = vertexIntensity; + fragWorldPos = vertexPosition; +} +)"; + +// jet() is shared (raylib_widgets/Shaders.h) with camera_lidar_calibration's +// and camera_lidar_trajectory_viewer's point shaders -- was byte-for-byte +// duplicated here. +inline const std::string kPointFS = std::string(R"( +#version 330 +uniform vec4 pointColor; +uniform int colorMode; +uniform float elevMin; +uniform float elevMax; +uniform vec3 distCenter; +uniform float distMax; +in float fragIntensity; +in vec3 fragWorldPos; +out vec4 finalColor; +)") + raylib_widgets::kJetColormapGLSL + R"( +void main() +{ + if (colorMode == 1) + { + finalColor = vec4(jet(fragIntensity), pointColor.a); + } + else if (colorMode == 2) + { + float range = max(elevMax - elevMin, 1e-6); + finalColor = vec4(jet((fragWorldPos.z - elevMin) / range), pointColor.a); + } + else if (colorMode == 3) + { + float d = length(fragWorldPos - distCenter); + finalColor = vec4(jet(d / max(distMax, 1e-6)), pointColor.a); + } + else + { + finalColor = pointColor; + } +} +)"; +} // namespace raylib_render_shaders diff --git a/raylib_widgets/CMakeLists.txt b/raylib_widgets/CMakeLists.txt index 3ee4ee9d..abeb56b9 100644 --- a/raylib_widgets/CMakeLists.txt +++ b/raylib_widgets/CMakeLists.txt @@ -3,18 +3,20 @@ cmake_minimum_required(VERSION 4.0.0) project(raylib_widgets) # Small UI overlay helpers (compass/ruler, DPI-aware window fit-to-screen, -# a generic shortcuts-help table) shared between -# apps/multi_view_tls_registration and the camera_lidar_* apps. Depends on -# raylib + imgui_raylib only -- no Eigen, no core, no calib_core -- so it's -# linkable from both the core_raylib side (already coupled to core/core_math) -# and the calib_core side (deliberately not) without adding coupling either -# way; every current/planned consumer is a raylib+ImGui app already, so -# depending on imgui_raylib (not the separate GLUT-backed `imgui` target) -# doesn't add anything new for them. +# a generic shortcuts-help table, an orbit camera + its center-of-rotation +# dialog) shared between apps/multi_view_tls_registration and the +# camera_lidar_* apps. Depends on raylib + imgui_raylib only -- no Eigen, no +# core, no calib_core -- so it's linkable from both the core_raylib side +# (already coupled to core/core_math) and the calib_core side (deliberately +# not) without adding coupling either way; every current/planned consumer is +# a raylib+ImGui app already, so depending on imgui_raylib (not the separate +# GLUT-backed `imgui` target) doesn't add anything new for them. add_library(raylib_widgets STATIC src/CompassRuler.cpp src/WindowFit.cpp src/ShortcutsTable.cpp + src/OrbitCamera.cpp + src/CenterOfRotationWindow.cpp ) target_include_directories(raylib_widgets PUBLIC include) diff --git a/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h b/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h new file mode 100644 index 00000000..2969b40b --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/CenterOfRotationWindow.h @@ -0,0 +1,19 @@ +#pragma once +#include + +// "Center of rotation" modal dialog, shared between the camera_lidar_* apps +// (mirrors multi_view_tls_registration's cor_window(), adapted from its +// Eigen rotation_center to OrbitCamera's Vector3 target). Depends on raylib +// + ImGui + OrbitCamera.h only -- no Eigen, no core -- same dependency story +// as OrbitCamera.h itself. +namespace raylib_widgets { + +// Call once per frame. `open` is an edge-triggered request: the caller sets +// it true (e.g. from a keyboard shortcut or menu item) to pop the dialog; +// this function opens the ImGui popup and immediately clears `open` back to +// false (the popup then manages its own visibility until Set/Cancel). On +// "Set", starts an eased transition of camera.target to the typed X/Y/Z via +// camera.moveTargetTo(). +void showCenterOfRotationWindow(bool& open, OrbitCamera& camera); + +} // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/OrbitCamera.h b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h new file mode 100644 index 00000000..22eaff18 --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/OrbitCamera.h @@ -0,0 +1,54 @@ +#pragma once +#include "raylib.h" + +// Mouse-driven orbit/pan/zoom camera, shared between +// apps/camera_lidar_calibration and apps/camera_lidar_trajectory_viewer (was +// byte-for-byte duplicated as camera_lidar_calibration's OrbitCamera and +// camera_lidar_trajectory_viewer's Orbit). Depends on nothing but raylib -- +// no Eigen, no core -- so it stays linkable from both the core_raylib side +// (which already pulls in core/core_math) and the calib_core side (which +// deliberately doesn't) without adding coupling either way. +namespace raylib_widgets { + +struct OrbitCamera { + float azimuth = 30.f; // degrees + float elevation = 25.f; // degrees + float distance = 30.f; + Vector3 target = {0.f, 0.f, 0.f}; + + // Center-of-rotation ("target") smooth-transition state, mirroring + // multi_view_tls_registration's rotation_center/new_rotation_center/ + // camera_transition_active animation -- only target eases, azimuth/ + // elevation/distance are left as-is (matching that app's actual + // click-picked/typed-center behavior). + Vector3 transitionTarget = target; + bool transitionActive = false; + float transitionSpeed = 1.f; + + Camera3D toRaylib() const; + // Processes mouse input when active (mouse not over ImGui): left-drag + // orbits, right-drag pans, wheel zooms. A manual drag cancels any + // in-flight transition. + void update(bool active); + + // Starts (or retargets) an eased transition of `target` to newTarget. + void moveTargetTo(Vector3 newTarget); + // Eases `target` toward `transitionTarget`; call once per frame with the + // frame's delta time. No-op when no transition is active. + void updateTransition(float dt); + // Immediately snaps `target` to `transitionTarget` and ends the transition. + void cancelTransition(); + + // Casts a ray from `mouse` through `cam` and intersects the y = groundY + // plane, starting a transition of `target` to the hit point on success. + // Returns false (no-op) when the ray is ~parallel to the plane. + bool pickGroundPlaneTarget(Vector2 mouse, Camera3D cam, float groundY = 0.f); +}; + +// Small 3-axis crosshair marking the current center of rotation. Call inside +// BeginMode3D/EndMode3D, every frame -- drawing at camera.target (which +// updateTransition() eases each frame) makes the cross visibly slide to a +// newly-picked/typed center along with the camera. +void drawRotationCenterCross(Vector3 center, float size, Color color); + +} // namespace raylib_widgets diff --git a/raylib_widgets/include/RaylibWidgets/Shaders.h b/raylib_widgets/include/RaylibWidgets/Shaders.h new file mode 100644 index 00000000..2246cf42 --- /dev/null +++ b/raylib_widgets/include/RaylibWidgets/Shaders.h @@ -0,0 +1,30 @@ +#pragma once + +// Shared GLSL fragment-shader snippets, string-spliced into each app's own +// otherwise-distinct point-cloud shaders. Header-only (no .cpp, nothing to +// link) -- consumers just need this directory on their include path, which +// core_raylib gets via a plain target_include_directories (not a full +// raylib_widgets link, to avoid pulling imgui_raylib into core_raylib as a +// new transitive dependency for what's just a compile-time string constant). +namespace raylib_widgets { + +// Standard "jet" colormap (blue -> cyan -> yellow -> red), t in [0,1]. Was +// byte-for-byte duplicated in core/src/raylib_render.cpp's ScanRenderer, +// apps/camera_lidar_calibration/Renderer.cpp's two point shaders, and +// apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp's point shader. +// Splice into a fragment shader's source via string concatenation, e.g.: +// static const std::string kMyFS = std::string(R"(#version 330 +// ...uniforms/varyings... +// )") + raylib_widgets::kJetColormapGLSL + R"( +// void main() { ... } +// )"; +inline constexpr const char* kJetColormapGLSL = R"( +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); +} +)"; + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/CenterOfRotationWindow.cpp b/raylib_widgets/src/CenterOfRotationWindow.cpp new file mode 100644 index 00000000..017acd61 --- /dev/null +++ b/raylib_widgets/src/CenterOfRotationWindow.cpp @@ -0,0 +1,46 @@ +#include + +#include + +namespace raylib_widgets { + +void showCenterOfRotationWindow(bool& open, OrbitCamera& camera) +{ + static Vector3 pending = { 0.f, 0.f, 0.f }; + + if (open) + { + pending = camera.target; + ImGui::OpenPopup("Center of rotation"); + open = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(120.f); + ImGui::InputFloat("X", &pending.x, 0.0f, 0.0f, "%.3f"); + ImGui::SameLine(); + ImGui::InputFloat("Y", &pending.y, 0.0f, 0.0f, "%.3f"); + ImGui::SameLine(); + ImGui::InputFloat("Z", &pending.z, 0.0f, 0.0f, "%.3f"); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + camera.moveTargetTo(pending); + ImGui::CloseCurrentPopup(); + } + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } +} + +} // namespace raylib_widgets diff --git a/raylib_widgets/src/OrbitCamera.cpp b/raylib_widgets/src/OrbitCamera.cpp new file mode 100644 index 00000000..0984242c --- /dev/null +++ b/raylib_widgets/src/OrbitCamera.cpp @@ -0,0 +1,106 @@ +#include "RaylibWidgets/OrbitCamera.h" + +#include "raymath.h" + +#include +#include + +namespace raylib_widgets { + +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)); + cancelTransition(); + } + // 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)); + cancelTransition(); + } + // Scroll → zoom + float wheel = GetMouseWheelMove(); + if (wheel != 0.f) { + distance -= wheel * distance * 0.1f; + distance = std::max(0.5f, distance); + } +} + +void OrbitCamera::moveTargetTo(Vector3 newTarget) { + transitionTarget = newTarget; + transitionActive = true; +} + +void OrbitCamera::updateTransition(float dt) { + if (!transitionActive) return; + + float t = 1.f - std::pow(1.f - std::min(dt * transitionSpeed, 1.f), 3.f); + + bool doneX = std::fabs(transitionTarget.x - target.x) < 0.01f; + bool doneY = std::fabs(transitionTarget.y - target.y) < 0.01f; + bool doneZ = std::fabs(transitionTarget.z - target.z) < 0.01f; + + if (!doneX) target.x += (transitionTarget.x - target.x) * t; + if (!doneY) target.y += (transitionTarget.y - target.y) * t; + if (!doneZ) target.z += (transitionTarget.z - target.z) * t; + + transitionActive = !(doneX && doneY && doneZ); + if (!transitionActive) + target = transitionTarget; +} + +void OrbitCamera::cancelTransition() { + if (!transitionActive) return; + target = transitionTarget; + transitionActive = false; +} + +bool OrbitCamera::pickGroundPlaneTarget(Vector2 mouse, Camera3D cam, float groundY) { + Ray ray = GetScreenToWorldRay(mouse, cam); + + const float kTolerance = 0.0001f; + if (ray.direction.y > -kTolerance && ray.direction.y < kTolerance) + return false; // ray ~parallel to the ground plane + + float t = (groundY - ray.position.y) / ray.direction.y; + Vector3 hit = Vector3Add(ray.position, Vector3Scale(ray.direction, t)); + moveTargetTo(hit); + return true; +} + +void drawRotationCenterCross(Vector3 center, float size, Color color) { + DrawLine3D(Vector3{ center.x - size, center.y, center.z }, Vector3{ center.x + size, center.y, center.z }, color); + DrawLine3D(Vector3{ center.x, center.y - size, center.z }, Vector3{ center.x, center.y + size, center.z }, color); + DrawLine3D(Vector3{ center.x, center.y, center.z - size }, Vector3{ center.x, center.y, center.z + size }, color); +} + +} // namespace raylib_widgets From a399068b271122361059070443b8bd42fce5907d Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 4 Aug 2026 02:42:35 +0200 Subject: [PATCH 11/13] Deduplicate getInterpolatedPose into shared/ and add its first unit tests getInterpolatedPose was byte-for-byte duplicated between lidar_odometry_utils.h/.cpp (compiled into lidar_odometry_step_1, drag_folder_with_mandeye_data_and_drop_here-precision_forestry, mandeye_compare_trajectories, and multi_view_tls_registration_step_2) and camera_lidar_trajectory_viewer's TrajectoryViewer.cpp. Moved it to shared/include/HDMapping/PoseInterpolation.h -- header-only, Eigen/std-only, in the global namespace like the original, so it needs no new include dir or library link anywhere (shared/include is already on every target's include path via the root CMakeLists.txt). Also adds this project's first unit test infrastructure: doctest (vendored as its own 3rdparty/doctest, MIT) since neither GoogleTest nor Catch2 is available offline in this repo (the only existing GTest reference, 3rdparty/manif/test/gtest, fetches it live from GitHub), gated behind a new opt-in `-DBUILD_TESTING=ON` option so it doesn't affect the default build. shared/tests/test_pose_interpolation.cpp exercises this "very important function" and caught a real, pre-existing bug in the process: the translation lerp adds its delta onto the *later* sample instead of the earlier one (`it_next.translation + diff * res` instead of `it_lower.translation + diff * res`), so it extrapolates past the later pose rather than interpolating between the two -- present in every call site since this function was first written. The corresponding test case is left deliberately failing (asserts the mathematically correct result) as a visible marker; fixing the formula itself is intentionally deferred to a separate change. `ctest`/running hdmapping_shared_tests today reports 6 passed, 1 failed -- that one failure is expected. Co-Authored-By: Claude Sonnet 5 --- 3rdparty/doctest/LICENSE.txt | 21 + 3rdparty/doctest/doctest.h | 7106 +++++++++++++++++ CMakeLists.txt | 9 + .../TrajectoryViewer.cpp | 65 +- .../lidar_odometry_utils.cpp | 56 - .../lidar_odometry_utils.h | 4 +- shared/include/HDMapping/PoseInterpolation.h | 71 + shared/tests/CMakeLists.txt | 26 + shared/tests/test_pose_interpolation.cpp | 118 + 9 files changed, 7354 insertions(+), 122 deletions(-) create mode 100644 3rdparty/doctest/LICENSE.txt create mode 100644 3rdparty/doctest/doctest.h create mode 100644 shared/include/HDMapping/PoseInterpolation.h create mode 100644 shared/tests/CMakeLists.txt create mode 100644 shared/tests/test_pose_interpolation.cpp diff --git a/3rdparty/doctest/LICENSE.txt b/3rdparty/doctest/LICENSE.txt new file mode 100644 index 00000000..6bf7d949 --- /dev/null +++ b/3rdparty/doctest/LICENSE.txt @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2016-2023 Viktor Kirilov + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/3rdparty/doctest/doctest.h b/3rdparty/doctest/doctest.h new file mode 100644 index 00000000..5c754cde --- /dev/null +++ b/3rdparty/doctest/doctest.h @@ -0,0 +1,7106 @@ +// ====================================================================== lgtm [cpp/missing-header-guard] +// == DO NOT MODIFY THIS FILE BY HAND - IT IS AUTO GENERATED BY CMAKE! == +// ====================================================================== +// +// doctest.h - the lightest feature-rich C++ single-header testing framework for unit tests and TDD +// +// Copyright (c) 2016-2023 Viktor Kirilov +// +// Distributed under the MIT Software License +// See accompanying file LICENSE.txt or copy at +// https://opensource.org/licenses/MIT +// +// The documentation can be found at the library's page: +// https://github.com/doctest/doctest/blob/master/doc/markdown/readme.md +// +// ================================================================================================= +// ================================================================================================= +// ================================================================================================= +// +// The library is heavily influenced by Catch - https://github.com/catchorg/Catch2 +// which uses the Boost Software License - Version 1.0 +// see here - https://github.com/catchorg/Catch2/blob/master/LICENSE.txt +// +// The concept of subcases (sections in Catch) and expression decomposition are from there. +// Some parts of the code are taken directly: +// - stringification - the detection of "ostream& operator<<(ostream&, const T&)" and StringMaker<> +// - the Approx() helper class for floating point comparison +// - colors in the console +// - breaking into a debugger +// - signal / SEH handling +// - timer +// - XmlWriter class - thanks to Phil Nash for allowing the direct reuse (AKA copy/paste) +// +// The expression decomposing templates are taken from lest - https://github.com/martinmoene/lest +// which uses the Boost Software License - Version 1.0 +// see here - https://github.com/martinmoene/lest/blob/master/LICENSE.txt +// +// ================================================================================================= +// ================================================================================================= +// ================================================================================================= + +#ifndef DOCTEST_LIBRARY_INCLUDED +#define DOCTEST_LIBRARY_INCLUDED + +// ================================================================================================= +// == VERSION ====================================================================================== +// ================================================================================================= + +#define DOCTEST_VERSION_MAJOR 2 +#define DOCTEST_VERSION_MINOR 4 +#define DOCTEST_VERSION_PATCH 11 + +// util we need here +#define DOCTEST_TOSTR_IMPL(x) #x +#define DOCTEST_TOSTR(x) DOCTEST_TOSTR_IMPL(x) + +#define DOCTEST_VERSION_STR \ + DOCTEST_TOSTR(DOCTEST_VERSION_MAJOR) "." \ + DOCTEST_TOSTR(DOCTEST_VERSION_MINOR) "." \ + DOCTEST_TOSTR(DOCTEST_VERSION_PATCH) + +#define DOCTEST_VERSION \ + (DOCTEST_VERSION_MAJOR * 10000 + DOCTEST_VERSION_MINOR * 100 + DOCTEST_VERSION_PATCH) + +// ================================================================================================= +// == COMPILER VERSION ============================================================================= +// ================================================================================================= + +// ideas for the version stuff are taken from here: https://github.com/cxxstuff/cxx_detect + +#ifdef _MSC_VER +#define DOCTEST_CPLUSPLUS _MSVC_LANG +#else +#define DOCTEST_CPLUSPLUS __cplusplus +#endif + +#define DOCTEST_COMPILER(MAJOR, MINOR, PATCH) ((MAJOR)*10000000 + (MINOR)*100000 + (PATCH)) + +// GCC/Clang and GCC/MSVC are mutually exclusive, but Clang/MSVC are not because of clang-cl... +#if defined(_MSC_VER) && defined(_MSC_FULL_VER) +#if _MSC_VER == _MSC_FULL_VER / 10000 +#define DOCTEST_MSVC DOCTEST_COMPILER(_MSC_VER / 100, _MSC_VER % 100, _MSC_FULL_VER % 10000) +#else // MSVC +#define DOCTEST_MSVC \ + DOCTEST_COMPILER(_MSC_VER / 100, (_MSC_FULL_VER / 100000) % 100, _MSC_FULL_VER % 100000) +#endif // MSVC +#endif // MSVC +#if defined(__clang__) && defined(__clang_minor__) && defined(__clang_patchlevel__) +#define DOCTEST_CLANG DOCTEST_COMPILER(__clang_major__, __clang_minor__, __clang_patchlevel__) +#elif defined(__GNUC__) && defined(__GNUC_MINOR__) && defined(__GNUC_PATCHLEVEL__) && \ + !defined(__INTEL_COMPILER) +#define DOCTEST_GCC DOCTEST_COMPILER(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__) +#endif // GCC +#if defined(__INTEL_COMPILER) +#define DOCTEST_ICC DOCTEST_COMPILER(__INTEL_COMPILER / 100, __INTEL_COMPILER % 100, 0) +#endif // ICC + +#ifndef DOCTEST_MSVC +#define DOCTEST_MSVC 0 +#endif // DOCTEST_MSVC +#ifndef DOCTEST_CLANG +#define DOCTEST_CLANG 0 +#endif // DOCTEST_CLANG +#ifndef DOCTEST_GCC +#define DOCTEST_GCC 0 +#endif // DOCTEST_GCC +#ifndef DOCTEST_ICC +#define DOCTEST_ICC 0 +#endif // DOCTEST_ICC + +// ================================================================================================= +// == COMPILER WARNINGS HELPERS ==================================================================== +// ================================================================================================= + +#if DOCTEST_CLANG && !DOCTEST_ICC +#define DOCTEST_PRAGMA_TO_STR(x) _Pragma(#x) +#define DOCTEST_CLANG_SUPPRESS_WARNING_PUSH _Pragma("clang diagnostic push") +#define DOCTEST_CLANG_SUPPRESS_WARNING(w) DOCTEST_PRAGMA_TO_STR(clang diagnostic ignored w) +#define DOCTEST_CLANG_SUPPRESS_WARNING_POP _Pragma("clang diagnostic pop") +#define DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH(w) \ + DOCTEST_CLANG_SUPPRESS_WARNING_PUSH DOCTEST_CLANG_SUPPRESS_WARNING(w) +#else // DOCTEST_CLANG +#define DOCTEST_CLANG_SUPPRESS_WARNING_PUSH +#define DOCTEST_CLANG_SUPPRESS_WARNING(w) +#define DOCTEST_CLANG_SUPPRESS_WARNING_POP +#define DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH(w) +#endif // DOCTEST_CLANG + +#if DOCTEST_GCC +#define DOCTEST_PRAGMA_TO_STR(x) _Pragma(#x) +#define DOCTEST_GCC_SUPPRESS_WARNING_PUSH _Pragma("GCC diagnostic push") +#define DOCTEST_GCC_SUPPRESS_WARNING(w) DOCTEST_PRAGMA_TO_STR(GCC diagnostic ignored w) +#define DOCTEST_GCC_SUPPRESS_WARNING_POP _Pragma("GCC diagnostic pop") +#define DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH(w) \ + DOCTEST_GCC_SUPPRESS_WARNING_PUSH DOCTEST_GCC_SUPPRESS_WARNING(w) +#else // DOCTEST_GCC +#define DOCTEST_GCC_SUPPRESS_WARNING_PUSH +#define DOCTEST_GCC_SUPPRESS_WARNING(w) +#define DOCTEST_GCC_SUPPRESS_WARNING_POP +#define DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH(w) +#endif // DOCTEST_GCC + +#if DOCTEST_MSVC +#define DOCTEST_MSVC_SUPPRESS_WARNING_PUSH __pragma(warning(push)) +#define DOCTEST_MSVC_SUPPRESS_WARNING(w) __pragma(warning(disable : w)) +#define DOCTEST_MSVC_SUPPRESS_WARNING_POP __pragma(warning(pop)) +#define DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(w) \ + DOCTEST_MSVC_SUPPRESS_WARNING_PUSH DOCTEST_MSVC_SUPPRESS_WARNING(w) +#else // DOCTEST_MSVC +#define DOCTEST_MSVC_SUPPRESS_WARNING_PUSH +#define DOCTEST_MSVC_SUPPRESS_WARNING(w) +#define DOCTEST_MSVC_SUPPRESS_WARNING_POP +#define DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(w) +#endif // DOCTEST_MSVC + +// ================================================================================================= +// == COMPILER WARNINGS ============================================================================ +// ================================================================================================= + +// both the header and the implementation suppress all of these, +// so it only makes sense to aggregate them like so +#define DOCTEST_SUPPRESS_COMMON_WARNINGS_PUSH \ + DOCTEST_CLANG_SUPPRESS_WARNING_PUSH \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wunknown-pragmas") \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wweak-vtables") \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wpadded") \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wmissing-prototypes") \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wc++98-compat") \ + DOCTEST_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") \ + \ + DOCTEST_GCC_SUPPRESS_WARNING_PUSH \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wunknown-pragmas") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wpragmas") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Weffc++") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wstrict-overflow") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wstrict-aliasing") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wmissing-declarations") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wuseless-cast") \ + DOCTEST_GCC_SUPPRESS_WARNING("-Wnoexcept") \ + \ + DOCTEST_MSVC_SUPPRESS_WARNING_PUSH \ + /* these 4 also disabled globally via cmake: */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4514) /* unreferenced inline function has been removed */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4571) /* SEH related */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4710) /* function not inlined */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4711) /* function selected for inline expansion*/ \ + /* common ones */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4616) /* invalid compiler warning */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4619) /* invalid compiler warning */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4996) /* The compiler encountered a deprecated declaration */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4706) /* assignment within conditional expression */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4512) /* 'class' : assignment operator could not be generated */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4127) /* conditional expression is constant */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4820) /* padding */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4625) /* copy constructor was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4626) /* assignment operator was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5027) /* move assignment operator implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5026) /* move constructor was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4640) /* construction of local static object not thread-safe */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5045) /* Spectre mitigation for memory load */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5264) /* 'variable-name': 'const' variable is not used */ \ + /* static analysis */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(26439) /* Function may not throw. Declare it 'noexcept' */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(26495) /* Always initialize a member variable */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(26451) /* Arithmetic overflow ... */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(26444) /* Avoid unnamed objects with custom ctor and dtor... */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(26812) /* Prefer 'enum class' over 'enum' */ + +#define DOCTEST_SUPPRESS_COMMON_WARNINGS_POP \ + DOCTEST_CLANG_SUPPRESS_WARNING_POP \ + DOCTEST_GCC_SUPPRESS_WARNING_POP \ + DOCTEST_MSVC_SUPPRESS_WARNING_POP + +DOCTEST_SUPPRESS_COMMON_WARNINGS_PUSH + +DOCTEST_CLANG_SUPPRESS_WARNING_PUSH +DOCTEST_CLANG_SUPPRESS_WARNING("-Wnon-virtual-dtor") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wdeprecated") + +DOCTEST_GCC_SUPPRESS_WARNING_PUSH +DOCTEST_GCC_SUPPRESS_WARNING("-Wctor-dtor-privacy") +DOCTEST_GCC_SUPPRESS_WARNING("-Wnon-virtual-dtor") +DOCTEST_GCC_SUPPRESS_WARNING("-Wsign-promo") + +DOCTEST_MSVC_SUPPRESS_WARNING_PUSH +DOCTEST_MSVC_SUPPRESS_WARNING(4623) // default constructor was implicitly defined as deleted + +#define DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_BEGIN \ + DOCTEST_MSVC_SUPPRESS_WARNING_PUSH \ + DOCTEST_MSVC_SUPPRESS_WARNING(4548) /* before comma no effect; expected side - effect */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4265) /* virtual functions, but destructor is not virtual */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4986) /* exception specification does not match previous */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4350) /* 'member1' called instead of 'member2' */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4668) /* not defined as a preprocessor macro */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4365) /* signed/unsigned mismatch */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4774) /* format string not a string literal */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4820) /* padding */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4625) /* copy constructor was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4626) /* assignment operator was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5027) /* move assignment operator implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5026) /* move constructor was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4623) /* default constructor was implicitly deleted */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5039) /* pointer to pot. throwing function passed to extern C */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5045) /* Spectre mitigation for memory load */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5105) /* macro producing 'defined' has undefined behavior */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(4738) /* storing float result in memory, loss of performance */ \ + DOCTEST_MSVC_SUPPRESS_WARNING(5262) /* implicit fall-through */ + +#define DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_END DOCTEST_MSVC_SUPPRESS_WARNING_POP + +// ================================================================================================= +// == FEATURE DETECTION ============================================================================ +// ================================================================================================= + +// general compiler feature support table: https://en.cppreference.com/w/cpp/compiler_support +// MSVC C++11 feature support table: https://msdn.microsoft.com/en-us/library/hh567368.aspx +// GCC C++11 feature support table: https://gcc.gnu.org/projects/cxx-status.html +// MSVC version table: +// https://en.wikipedia.org/wiki/Microsoft_Visual_C%2B%2B#Internal_version_numbering +// MSVC++ 14.3 (17) _MSC_VER == 1930 (Visual Studio 2022) +// MSVC++ 14.2 (16) _MSC_VER == 1920 (Visual Studio 2019) +// MSVC++ 14.1 (15) _MSC_VER == 1910 (Visual Studio 2017) +// MSVC++ 14.0 _MSC_VER == 1900 (Visual Studio 2015) +// MSVC++ 12.0 _MSC_VER == 1800 (Visual Studio 2013) +// MSVC++ 11.0 _MSC_VER == 1700 (Visual Studio 2012) +// MSVC++ 10.0 _MSC_VER == 1600 (Visual Studio 2010) +// MSVC++ 9.0 _MSC_VER == 1500 (Visual Studio 2008) +// MSVC++ 8.0 _MSC_VER == 1400 (Visual Studio 2005) + +// Universal Windows Platform support +#if defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_APP) +#define DOCTEST_CONFIG_NO_WINDOWS_SEH +#endif // WINAPI_FAMILY +#if DOCTEST_MSVC && !defined(DOCTEST_CONFIG_WINDOWS_SEH) +#define DOCTEST_CONFIG_WINDOWS_SEH +#endif // MSVC +#if defined(DOCTEST_CONFIG_NO_WINDOWS_SEH) && defined(DOCTEST_CONFIG_WINDOWS_SEH) +#undef DOCTEST_CONFIG_WINDOWS_SEH +#endif // DOCTEST_CONFIG_NO_WINDOWS_SEH + +#if !defined(_WIN32) && !defined(__QNX__) && !defined(DOCTEST_CONFIG_POSIX_SIGNALS) && \ + !defined(__EMSCRIPTEN__) && !defined(__wasi__) +#define DOCTEST_CONFIG_POSIX_SIGNALS +#endif // _WIN32 +#if defined(DOCTEST_CONFIG_NO_POSIX_SIGNALS) && defined(DOCTEST_CONFIG_POSIX_SIGNALS) +#undef DOCTEST_CONFIG_POSIX_SIGNALS +#endif // DOCTEST_CONFIG_NO_POSIX_SIGNALS + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) \ + || defined(__wasi__) +#define DOCTEST_CONFIG_NO_EXCEPTIONS +#endif // no exceptions +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + +#ifdef DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS +#define DOCTEST_CONFIG_NO_EXCEPTIONS +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS + +#if defined(DOCTEST_CONFIG_NO_EXCEPTIONS) && !defined(DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS) +#define DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS && !DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS + +#ifdef __wasi__ +#define DOCTEST_CONFIG_NO_MULTITHREADING +#endif + +#if defined(DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN) && !defined(DOCTEST_CONFIG_IMPLEMENT) +#define DOCTEST_CONFIG_IMPLEMENT +#endif // DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN + +#if defined(_WIN32) || defined(__CYGWIN__) +#if DOCTEST_MSVC +#define DOCTEST_SYMBOL_EXPORT __declspec(dllexport) +#define DOCTEST_SYMBOL_IMPORT __declspec(dllimport) +#else // MSVC +#define DOCTEST_SYMBOL_EXPORT __attribute__((dllexport)) +#define DOCTEST_SYMBOL_IMPORT __attribute__((dllimport)) +#endif // MSVC +#else // _WIN32 +#define DOCTEST_SYMBOL_EXPORT __attribute__((visibility("default"))) +#define DOCTEST_SYMBOL_IMPORT +#endif // _WIN32 + +#ifdef DOCTEST_CONFIG_IMPLEMENTATION_IN_DLL +#ifdef DOCTEST_CONFIG_IMPLEMENT +#define DOCTEST_INTERFACE DOCTEST_SYMBOL_EXPORT +#else // DOCTEST_CONFIG_IMPLEMENT +#define DOCTEST_INTERFACE DOCTEST_SYMBOL_IMPORT +#endif // DOCTEST_CONFIG_IMPLEMENT +#else // DOCTEST_CONFIG_IMPLEMENTATION_IN_DLL +#define DOCTEST_INTERFACE +#endif // DOCTEST_CONFIG_IMPLEMENTATION_IN_DLL + +// needed for extern template instantiations +// see https://github.com/fmtlib/fmt/issues/2228 +#if DOCTEST_MSVC +#define DOCTEST_INTERFACE_DECL +#define DOCTEST_INTERFACE_DEF DOCTEST_INTERFACE +#else // DOCTEST_MSVC +#define DOCTEST_INTERFACE_DECL DOCTEST_INTERFACE +#define DOCTEST_INTERFACE_DEF +#endif // DOCTEST_MSVC + +#define DOCTEST_EMPTY + +#if DOCTEST_MSVC +#define DOCTEST_NOINLINE __declspec(noinline) +#define DOCTEST_UNUSED +#define DOCTEST_ALIGNMENT(x) +#elif DOCTEST_CLANG && DOCTEST_CLANG < DOCTEST_COMPILER(3, 5, 0) +#define DOCTEST_NOINLINE +#define DOCTEST_UNUSED +#define DOCTEST_ALIGNMENT(x) +#else +#define DOCTEST_NOINLINE __attribute__((noinline)) +#define DOCTEST_UNUSED __attribute__((unused)) +#define DOCTEST_ALIGNMENT(x) __attribute__((aligned(x))) +#endif + +#ifdef DOCTEST_CONFIG_NO_CONTRADICTING_INLINE +#define DOCTEST_INLINE_NOINLINE inline +#else +#define DOCTEST_INLINE_NOINLINE inline DOCTEST_NOINLINE +#endif + +#ifndef DOCTEST_NORETURN +#if DOCTEST_MSVC && (DOCTEST_MSVC < DOCTEST_COMPILER(19, 0, 0)) +#define DOCTEST_NORETURN +#else // DOCTEST_MSVC +#define DOCTEST_NORETURN [[noreturn]] +#endif // DOCTEST_MSVC +#endif // DOCTEST_NORETURN + +#ifndef DOCTEST_NOEXCEPT +#if DOCTEST_MSVC && (DOCTEST_MSVC < DOCTEST_COMPILER(19, 0, 0)) +#define DOCTEST_NOEXCEPT +#else // DOCTEST_MSVC +#define DOCTEST_NOEXCEPT noexcept +#endif // DOCTEST_MSVC +#endif // DOCTEST_NOEXCEPT + +#ifndef DOCTEST_CONSTEXPR +#if DOCTEST_MSVC && (DOCTEST_MSVC < DOCTEST_COMPILER(19, 0, 0)) +#define DOCTEST_CONSTEXPR const +#define DOCTEST_CONSTEXPR_FUNC inline +#else // DOCTEST_MSVC +#define DOCTEST_CONSTEXPR constexpr +#define DOCTEST_CONSTEXPR_FUNC constexpr +#endif // DOCTEST_MSVC +#endif // DOCTEST_CONSTEXPR + +#ifndef DOCTEST_NO_SANITIZE_INTEGER +#if DOCTEST_CLANG >= DOCTEST_COMPILER(3, 7, 0) +#define DOCTEST_NO_SANITIZE_INTEGER __attribute__((no_sanitize("integer"))) +#else +#define DOCTEST_NO_SANITIZE_INTEGER +#endif +#endif // DOCTEST_NO_SANITIZE_INTEGER + +// ================================================================================================= +// == FEATURE DETECTION END ======================================================================== +// ================================================================================================= + +#define DOCTEST_DECLARE_INTERFACE(name) \ + virtual ~name(); \ + name() = default; \ + name(const name&) = delete; \ + name(name&&) = delete; \ + name& operator=(const name&) = delete; \ + name& operator=(name&&) = delete; + +#define DOCTEST_DEFINE_INTERFACE(name) \ + name::~name() = default; + +// internal macros for string concatenation and anonymous variable name generation +#define DOCTEST_CAT_IMPL(s1, s2) s1##s2 +#define DOCTEST_CAT(s1, s2) DOCTEST_CAT_IMPL(s1, s2) +#ifdef __COUNTER__ // not standard and may be missing for some compilers +#define DOCTEST_ANONYMOUS(x) DOCTEST_CAT(x, __COUNTER__) +#else // __COUNTER__ +#define DOCTEST_ANONYMOUS(x) DOCTEST_CAT(x, __LINE__) +#endif // __COUNTER__ + +#ifndef DOCTEST_CONFIG_ASSERTION_PARAMETERS_BY_VALUE +#define DOCTEST_REF_WRAP(x) x& +#else // DOCTEST_CONFIG_ASSERTION_PARAMETERS_BY_VALUE +#define DOCTEST_REF_WRAP(x) x +#endif // DOCTEST_CONFIG_ASSERTION_PARAMETERS_BY_VALUE + +// not using __APPLE__ because... this is how Catch does it +#ifdef __MAC_OS_X_VERSION_MIN_REQUIRED +#define DOCTEST_PLATFORM_MAC +#elif defined(__IPHONE_OS_VERSION_MIN_REQUIRED) +#define DOCTEST_PLATFORM_IPHONE +#elif defined(_WIN32) +#define DOCTEST_PLATFORM_WINDOWS +#elif defined(__wasi__) +#define DOCTEST_PLATFORM_WASI +#else // DOCTEST_PLATFORM +#define DOCTEST_PLATFORM_LINUX +#endif // DOCTEST_PLATFORM + +namespace doctest { namespace detail { + static DOCTEST_CONSTEXPR int consume(const int*, int) noexcept { return 0; } +}} + +#define DOCTEST_GLOBAL_NO_WARNINGS(var, ...) \ + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wglobal-constructors") \ + static const int var = doctest::detail::consume(&var, __VA_ARGS__); \ + DOCTEST_CLANG_SUPPRESS_WARNING_POP + +#ifndef DOCTEST_BREAK_INTO_DEBUGGER +// should probably take a look at https://github.com/scottt/debugbreak +#ifdef DOCTEST_PLATFORM_LINUX +#if defined(__GNUC__) && (defined(__i386) || defined(__x86_64)) +// Break at the location of the failing check if possible +#define DOCTEST_BREAK_INTO_DEBUGGER() __asm__("int $3\n" : :) // NOLINT(hicpp-no-assembler) +#else +#include +#define DOCTEST_BREAK_INTO_DEBUGGER() raise(SIGTRAP) +#endif +#elif defined(DOCTEST_PLATFORM_MAC) +#if defined(__x86_64) || defined(__x86_64__) || defined(__amd64__) || defined(__i386) +#define DOCTEST_BREAK_INTO_DEBUGGER() __asm__("int $3\n" : :) // NOLINT(hicpp-no-assembler) +#elif defined(__ppc__) || defined(__ppc64__) +// https://www.cocoawithlove.com/2008/03/break-into-debugger.html +#define DOCTEST_BREAK_INTO_DEBUGGER() __asm__("li r0, 20\nsc\nnop\nli r0, 37\nli r4, 2\nsc\nnop\n": : : "memory","r0","r3","r4") // NOLINT(hicpp-no-assembler) +#else +#define DOCTEST_BREAK_INTO_DEBUGGER() __asm__("brk #0"); // NOLINT(hicpp-no-assembler) +#endif +#elif DOCTEST_MSVC +#define DOCTEST_BREAK_INTO_DEBUGGER() __debugbreak() +#elif defined(__MINGW32__) +DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wredundant-decls") +extern "C" __declspec(dllimport) void __stdcall DebugBreak(); +DOCTEST_GCC_SUPPRESS_WARNING_POP +#define DOCTEST_BREAK_INTO_DEBUGGER() ::DebugBreak() +#else // linux +#define DOCTEST_BREAK_INTO_DEBUGGER() (static_cast(0)) +#endif // linux +#endif // DOCTEST_BREAK_INTO_DEBUGGER + +// this is kept here for backwards compatibility since the config option was changed +#ifdef DOCTEST_CONFIG_USE_IOSFWD +#ifndef DOCTEST_CONFIG_USE_STD_HEADERS +#define DOCTEST_CONFIG_USE_STD_HEADERS +#endif +#endif // DOCTEST_CONFIG_USE_IOSFWD + +// for clang - always include ciso646 (which drags some std stuff) because +// we want to check if we are using libc++ with the _LIBCPP_VERSION macro in +// which case we don't want to forward declare stuff from std - for reference: +// https://github.com/doctest/doctest/issues/126 +// https://github.com/doctest/doctest/issues/356 +#if DOCTEST_CLANG +#include +#endif // clang + +#ifdef _LIBCPP_VERSION +#ifndef DOCTEST_CONFIG_USE_STD_HEADERS +#define DOCTEST_CONFIG_USE_STD_HEADERS +#endif +#endif // _LIBCPP_VERSION + +#ifdef DOCTEST_CONFIG_USE_STD_HEADERS +#ifndef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS +#define DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS +DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_BEGIN +#include +#include +#include +DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_END +#else // DOCTEST_CONFIG_USE_STD_HEADERS + +// Forward declaring 'X' in namespace std is not permitted by the C++ Standard. +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4643) + +namespace std { // NOLINT(cert-dcl58-cpp) +typedef decltype(nullptr) nullptr_t; // NOLINT(modernize-use-using) +typedef decltype(sizeof(void*)) size_t; // NOLINT(modernize-use-using) +template +struct char_traits; +template <> +struct char_traits; +template +class basic_ostream; // NOLINT(fuchsia-virtual-inheritance) +typedef basic_ostream> ostream; // NOLINT(modernize-use-using) +template +// NOLINTNEXTLINE +basic_ostream& operator<<(basic_ostream&, const char*); +template +class basic_istream; +typedef basic_istream> istream; // NOLINT(modernize-use-using) +template +class tuple; +#if DOCTEST_MSVC >= DOCTEST_COMPILER(19, 20, 0) +// see this issue on why this is needed: https://github.com/doctest/doctest/issues/183 +template +class allocator; +template +class basic_string; +using string = basic_string, allocator>; +#endif // VS 2019 +} // namespace std + +DOCTEST_MSVC_SUPPRESS_WARNING_POP + +#endif // DOCTEST_CONFIG_USE_STD_HEADERS + +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS +#include +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + +namespace doctest { + +using std::size_t; + +DOCTEST_INTERFACE extern bool is_running_in_test; + +#ifndef DOCTEST_CONFIG_STRING_SIZE_TYPE +#define DOCTEST_CONFIG_STRING_SIZE_TYPE unsigned +#endif + +// A 24 byte string class (can be as small as 17 for x64 and 13 for x86) that can hold strings with length +// of up to 23 chars on the stack before going on the heap - the last byte of the buffer is used for: +// - "is small" bit - the highest bit - if "0" then it is small - otherwise its "1" (128) +// - if small - capacity left before going on the heap - using the lowest 5 bits +// - if small - 2 bits are left unused - the second and third highest ones +// - if small - acts as a null terminator if strlen() is 23 (24 including the null terminator) +// and the "is small" bit remains "0" ("as well as the capacity left") so its OK +// Idea taken from this lecture about the string implementation of facebook/folly - fbstring +// https://www.youtube.com/watch?v=kPR8h4-qZdk +// TODO: +// - optimizations - like not deleting memory unnecessarily in operator= and etc. +// - resize/reserve/clear +// - replace +// - back/front +// - iterator stuff +// - find & friends +// - push_back/pop_back +// - assign/insert/erase +// - relational operators as free functions - taking const char* as one of the params +class DOCTEST_INTERFACE String +{ +public: + using size_type = DOCTEST_CONFIG_STRING_SIZE_TYPE; + +private: + static DOCTEST_CONSTEXPR size_type len = 24; //!OCLINT avoid private static members + static DOCTEST_CONSTEXPR size_type last = len - 1; //!OCLINT avoid private static members + + struct view // len should be more than sizeof(view) - because of the final byte for flags + { + char* ptr; + size_type size; + size_type capacity; + }; + + union + { + char buf[len]; // NOLINT(*-avoid-c-arrays) + view data; + }; + + char* allocate(size_type sz); + + bool isOnStack() const noexcept { return (buf[last] & 128) == 0; } + void setOnHeap() noexcept; + void setLast(size_type in = last) noexcept; + void setSize(size_type sz) noexcept; + + void copy(const String& other); + +public: + static DOCTEST_CONSTEXPR size_type npos = static_cast(-1); + + String() noexcept; + ~String(); + + // cppcheck-suppress noExplicitConstructor + String(const char* in); + String(const char* in, size_type in_size); + + String(std::istream& in, size_type in_size); + + String(const String& other); + String& operator=(const String& other); + + String& operator+=(const String& other); + + String(String&& other) noexcept; + String& operator=(String&& other) noexcept; + + char operator[](size_type i) const; + char& operator[](size_type i); + + // the only functions I'm willing to leave in the interface - available for inlining + const char* c_str() const { return const_cast(this)->c_str(); } // NOLINT + char* c_str() { + if (isOnStack()) { + return reinterpret_cast(buf); + } + return data.ptr; + } + + size_type size() const; + size_type capacity() const; + + String substr(size_type pos, size_type cnt = npos) &&; + String substr(size_type pos, size_type cnt = npos) const &; + + size_type find(char ch, size_type pos = 0) const; + size_type rfind(char ch, size_type pos = npos) const; + + int compare(const char* other, bool no_case = false) const; + int compare(const String& other, bool no_case = false) const; + +friend DOCTEST_INTERFACE std::ostream& operator<<(std::ostream& s, const String& in); +}; + +DOCTEST_INTERFACE String operator+(const String& lhs, const String& rhs); + +DOCTEST_INTERFACE bool operator==(const String& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator!=(const String& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator<(const String& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator>(const String& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator<=(const String& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator>=(const String& lhs, const String& rhs); + +class DOCTEST_INTERFACE Contains { +public: + explicit Contains(const String& string); + + bool checkWith(const String& other) const; + + String string; +}; + +DOCTEST_INTERFACE String toString(const Contains& in); + +DOCTEST_INTERFACE bool operator==(const String& lhs, const Contains& rhs); +DOCTEST_INTERFACE bool operator==(const Contains& lhs, const String& rhs); +DOCTEST_INTERFACE bool operator!=(const String& lhs, const Contains& rhs); +DOCTEST_INTERFACE bool operator!=(const Contains& lhs, const String& rhs); + +namespace Color { + enum Enum + { + None = 0, + White, + Red, + Green, + Blue, + Cyan, + Yellow, + Grey, + + Bright = 0x10, + + BrightRed = Bright | Red, + BrightGreen = Bright | Green, + LightGrey = Bright | Grey, + BrightWhite = Bright | White + }; + + DOCTEST_INTERFACE std::ostream& operator<<(std::ostream& s, Color::Enum code); +} // namespace Color + +namespace assertType { + enum Enum + { + // macro traits + + is_warn = 1, + is_check = 2 * is_warn, + is_require = 2 * is_check, + + is_normal = 2 * is_require, + is_throws = 2 * is_normal, + is_throws_as = 2 * is_throws, + is_throws_with = 2 * is_throws_as, + is_nothrow = 2 * is_throws_with, + + is_false = 2 * is_nothrow, + is_unary = 2 * is_false, // not checked anywhere - used just to distinguish the types + + is_eq = 2 * is_unary, + is_ne = 2 * is_eq, + + is_lt = 2 * is_ne, + is_gt = 2 * is_lt, + + is_ge = 2 * is_gt, + is_le = 2 * is_ge, + + // macro types + + DT_WARN = is_normal | is_warn, + DT_CHECK = is_normal | is_check, + DT_REQUIRE = is_normal | is_require, + + DT_WARN_FALSE = is_normal | is_false | is_warn, + DT_CHECK_FALSE = is_normal | is_false | is_check, + DT_REQUIRE_FALSE = is_normal | is_false | is_require, + + DT_WARN_THROWS = is_throws | is_warn, + DT_CHECK_THROWS = is_throws | is_check, + DT_REQUIRE_THROWS = is_throws | is_require, + + DT_WARN_THROWS_AS = is_throws_as | is_warn, + DT_CHECK_THROWS_AS = is_throws_as | is_check, + DT_REQUIRE_THROWS_AS = is_throws_as | is_require, + + DT_WARN_THROWS_WITH = is_throws_with | is_warn, + DT_CHECK_THROWS_WITH = is_throws_with | is_check, + DT_REQUIRE_THROWS_WITH = is_throws_with | is_require, + + DT_WARN_THROWS_WITH_AS = is_throws_with | is_throws_as | is_warn, + DT_CHECK_THROWS_WITH_AS = is_throws_with | is_throws_as | is_check, + DT_REQUIRE_THROWS_WITH_AS = is_throws_with | is_throws_as | is_require, + + DT_WARN_NOTHROW = is_nothrow | is_warn, + DT_CHECK_NOTHROW = is_nothrow | is_check, + DT_REQUIRE_NOTHROW = is_nothrow | is_require, + + DT_WARN_EQ = is_normal | is_eq | is_warn, + DT_CHECK_EQ = is_normal | is_eq | is_check, + DT_REQUIRE_EQ = is_normal | is_eq | is_require, + + DT_WARN_NE = is_normal | is_ne | is_warn, + DT_CHECK_NE = is_normal | is_ne | is_check, + DT_REQUIRE_NE = is_normal | is_ne | is_require, + + DT_WARN_GT = is_normal | is_gt | is_warn, + DT_CHECK_GT = is_normal | is_gt | is_check, + DT_REQUIRE_GT = is_normal | is_gt | is_require, + + DT_WARN_LT = is_normal | is_lt | is_warn, + DT_CHECK_LT = is_normal | is_lt | is_check, + DT_REQUIRE_LT = is_normal | is_lt | is_require, + + DT_WARN_GE = is_normal | is_ge | is_warn, + DT_CHECK_GE = is_normal | is_ge | is_check, + DT_REQUIRE_GE = is_normal | is_ge | is_require, + + DT_WARN_LE = is_normal | is_le | is_warn, + DT_CHECK_LE = is_normal | is_le | is_check, + DT_REQUIRE_LE = is_normal | is_le | is_require, + + DT_WARN_UNARY = is_normal | is_unary | is_warn, + DT_CHECK_UNARY = is_normal | is_unary | is_check, + DT_REQUIRE_UNARY = is_normal | is_unary | is_require, + + DT_WARN_UNARY_FALSE = is_normal | is_false | is_unary | is_warn, + DT_CHECK_UNARY_FALSE = is_normal | is_false | is_unary | is_check, + DT_REQUIRE_UNARY_FALSE = is_normal | is_false | is_unary | is_require, + }; +} // namespace assertType + +DOCTEST_INTERFACE const char* assertString(assertType::Enum at); +DOCTEST_INTERFACE const char* failureString(assertType::Enum at); +DOCTEST_INTERFACE const char* skipPathFromFilename(const char* file); + +struct DOCTEST_INTERFACE TestCaseData +{ + String m_file; // the file in which the test was registered (using String - see #350) + unsigned m_line; // the line where the test was registered + const char* m_name; // name of the test case + const char* m_test_suite; // the test suite in which the test was added + const char* m_description; + bool m_skip; + bool m_no_breaks; + bool m_no_output; + bool m_may_fail; + bool m_should_fail; + int m_expected_failures; + double m_timeout; +}; + +struct DOCTEST_INTERFACE AssertData +{ + // common - for all asserts + const TestCaseData* m_test_case; + assertType::Enum m_at; + const char* m_file; + int m_line; + const char* m_expr; + bool m_failed; + + // exception-related - for all asserts + bool m_threw; + String m_exception; + + // for normal asserts + String m_decomp; + + // for specific exception-related asserts + bool m_threw_as; + const char* m_exception_type; + + class DOCTEST_INTERFACE StringContains { + private: + Contains content; + bool isContains; + + public: + StringContains(const String& str) : content(str), isContains(false) { } + StringContains(Contains cntn) : content(static_cast(cntn)), isContains(true) { } + + bool check(const String& str) { return isContains ? (content == str) : (content.string == str); } + + operator const String&() const { return content.string; } + + const char* c_str() const { return content.string.c_str(); } + } m_exception_string; + + AssertData(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type, const StringContains& exception_string); +}; + +struct DOCTEST_INTERFACE MessageData +{ + String m_string; + const char* m_file; + int m_line; + assertType::Enum m_severity; +}; + +struct DOCTEST_INTERFACE SubcaseSignature +{ + String m_name; + const char* m_file; + int m_line; + + bool operator==(const SubcaseSignature& other) const; + bool operator<(const SubcaseSignature& other) const; +}; + +struct DOCTEST_INTERFACE IContextScope +{ + DOCTEST_DECLARE_INTERFACE(IContextScope) + virtual void stringify(std::ostream*) const = 0; +}; + +namespace detail { + struct DOCTEST_INTERFACE TestCase; +} // namespace detail + +struct ContextOptions //!OCLINT too many fields +{ + std::ostream* cout = nullptr; // stdout stream + String binary_name; // the test binary name + + const detail::TestCase* currentTest = nullptr; + + // == parameters from the command line + String out; // output filename + String order_by; // how tests should be ordered + unsigned rand_seed; // the seed for rand ordering + + unsigned first; // the first (matching) test to be executed + unsigned last; // the last (matching) test to be executed + + int abort_after; // stop tests after this many failed assertions + int subcase_filter_levels; // apply the subcase filters for the first N levels + + bool success; // include successful assertions in output + bool case_sensitive; // if filtering should be case sensitive + bool exit; // if the program should be exited after the tests are ran/whatever + bool duration; // print the time duration of each test case + bool minimal; // minimal console output (only test failures) + bool quiet; // no console output + bool no_throw; // to skip exceptions-related assertion macros + bool no_exitcode; // if the framework should return 0 as the exitcode + bool no_run; // to not run the tests at all (can be done with an "*" exclude) + bool no_intro; // to not print the intro of the framework + bool no_version; // to not print the version of the framework + bool no_colors; // if output to the console should be colorized + bool force_colors; // forces the use of colors even when a tty cannot be detected + bool no_breaks; // to not break into the debugger + bool no_skip; // don't skip test cases which are marked to be skipped + bool gnu_file_line; // if line numbers should be surrounded with :x: and not (x): + bool no_path_in_filenames; // if the path to files should be removed from the output + bool no_line_numbers; // if source code line numbers should be omitted from the output + bool no_debug_output; // no output in the debug console when a debugger is attached + bool no_skipped_summary; // don't print "skipped" in the summary !!! UNDOCUMENTED !!! + bool no_time_in_output; // omit any time/timestamps from output !!! UNDOCUMENTED !!! + + bool help; // to print the help + bool version; // to print the version + bool count; // if only the count of matching tests is to be retrieved + bool list_test_cases; // to list all tests matching the filters + bool list_test_suites; // to list all suites matching the filters + bool list_reporters; // lists all registered reporters +}; + +namespace detail { + namespace types { +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + using namespace std; +#else + template + struct enable_if { }; + + template + struct enable_if { using type = T; }; + + struct true_type { static DOCTEST_CONSTEXPR bool value = true; }; + struct false_type { static DOCTEST_CONSTEXPR bool value = false; }; + + template struct remove_reference { using type = T; }; + template struct remove_reference { using type = T; }; + template struct remove_reference { using type = T; }; + + template struct is_rvalue_reference : false_type { }; + template struct is_rvalue_reference : true_type { }; + + template struct remove_const { using type = T; }; + template struct remove_const { using type = T; }; + + // Compiler intrinsics + template struct is_enum { static DOCTEST_CONSTEXPR bool value = __is_enum(T); }; + template struct underlying_type { using type = __underlying_type(T); }; + + template struct is_pointer : false_type { }; + template struct is_pointer : true_type { }; + + template struct is_array : false_type { }; + // NOLINTNEXTLINE(*-avoid-c-arrays) + template struct is_array : true_type { }; +#endif + } + + // + template + T&& declval(); + + template + DOCTEST_CONSTEXPR_FUNC T&& forward(typename types::remove_reference::type& t) DOCTEST_NOEXCEPT { + return static_cast(t); + } + + template + DOCTEST_CONSTEXPR_FUNC T&& forward(typename types::remove_reference::type&& t) DOCTEST_NOEXCEPT { + return static_cast(t); + } + + template + struct deferred_false : types::false_type { }; + +// MSVS 2015 :( +#if !DOCTEST_CLANG && defined(_MSC_VER) && _MSC_VER <= 1900 + template + struct has_global_insertion_operator : types::false_type { }; + + template + struct has_global_insertion_operator(), declval()), void())> : types::true_type { }; + + template + struct has_insertion_operator { static DOCTEST_CONSTEXPR bool value = has_global_insertion_operator::value; }; + + template + struct insert_hack; + + template + struct insert_hack { + static void insert(std::ostream& os, const T& t) { ::operator<<(os, t); } + }; + + template + struct insert_hack { + static void insert(std::ostream& os, const T& t) { operator<<(os, t); } + }; + + template + using insert_hack_t = insert_hack::value>; +#else + template + struct has_insertion_operator : types::false_type { }; +#endif + + template + struct has_insertion_operator(), declval()), void())> : types::true_type { }; + + template + struct should_stringify_as_underlying_type { + static DOCTEST_CONSTEXPR bool value = detail::types::is_enum::value && !doctest::detail::has_insertion_operator::value; + }; + + DOCTEST_INTERFACE std::ostream* tlssPush(); + DOCTEST_INTERFACE String tlssPop(); + + template + struct StringMakerBase { + template + static String convert(const DOCTEST_REF_WRAP(T)) { +#ifdef DOCTEST_CONFIG_REQUIRE_STRINGIFICATION_FOR_ALL_USED_TYPES + static_assert(deferred_false::value, "No stringification detected for type T. See string conversion manual"); +#endif + return "{?}"; + } + }; + + template + struct filldata; + + template + void filloss(std::ostream* stream, const T& in) { + filldata::fill(stream, in); + } + + template + void filloss(std::ostream* stream, const T (&in)[N]) { // NOLINT(*-avoid-c-arrays) + // T[N], T(&)[N], T(&&)[N] have same behaviour. + // Hence remove reference. + filloss::type>(stream, in); + } + + template + String toStream(const T& in) { + std::ostream* stream = tlssPush(); + filloss(stream, in); + return tlssPop(); + } + + template <> + struct StringMakerBase { + template + static String convert(const DOCTEST_REF_WRAP(T) in) { + return toStream(in); + } + }; +} // namespace detail + +template +struct StringMaker : public detail::StringMakerBase< + detail::has_insertion_operator::value || detail::types::is_pointer::value || detail::types::is_array::value> +{}; + +#ifndef DOCTEST_STRINGIFY +#ifdef DOCTEST_CONFIG_DOUBLE_STRINGIFY +#define DOCTEST_STRINGIFY(...) toString(toString(__VA_ARGS__)) +#else +#define DOCTEST_STRINGIFY(...) toString(__VA_ARGS__) +#endif +#endif + +template +String toString() { +#if DOCTEST_CLANG == 0 && DOCTEST_GCC == 0 && DOCTEST_ICC == 0 + String ret = __FUNCSIG__; // class doctest::String __cdecl doctest::toString(void) + String::size_type beginPos = ret.find('<'); + return ret.substr(beginPos + 1, ret.size() - beginPos - static_cast(sizeof(">(void)"))); +#else + String ret = __PRETTY_FUNCTION__; // doctest::String toString() [with T = TYPE] + String::size_type begin = ret.find('=') + 2; + return ret.substr(begin, ret.size() - begin - 1); +#endif +} + +template ::value, bool>::type = true> +String toString(const DOCTEST_REF_WRAP(T) value) { + return StringMaker::convert(value); +} + +#ifdef DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +DOCTEST_INTERFACE String toString(const char* in); +#endif // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + +#if DOCTEST_MSVC >= DOCTEST_COMPILER(19, 20, 0) +// see this issue on why this is needed: https://github.com/doctest/doctest/issues/183 +DOCTEST_INTERFACE String toString(const std::string& in); +#endif // VS 2019 + +DOCTEST_INTERFACE String toString(String in); + +DOCTEST_INTERFACE String toString(std::nullptr_t); + +DOCTEST_INTERFACE String toString(bool in); + +DOCTEST_INTERFACE String toString(float in); +DOCTEST_INTERFACE String toString(double in); +DOCTEST_INTERFACE String toString(double long in); + +DOCTEST_INTERFACE String toString(char in); +DOCTEST_INTERFACE String toString(char signed in); +DOCTEST_INTERFACE String toString(char unsigned in); +DOCTEST_INTERFACE String toString(short in); +DOCTEST_INTERFACE String toString(short unsigned in); +DOCTEST_INTERFACE String toString(signed in); +DOCTEST_INTERFACE String toString(unsigned in); +DOCTEST_INTERFACE String toString(long in); +DOCTEST_INTERFACE String toString(long unsigned in); +DOCTEST_INTERFACE String toString(long long in); +DOCTEST_INTERFACE String toString(long long unsigned in); + +template ::value, bool>::type = true> +String toString(const DOCTEST_REF_WRAP(T) value) { + using UT = typename detail::types::underlying_type::type; + return (DOCTEST_STRINGIFY(static_cast(value))); +} + +namespace detail { + template + struct filldata + { + static void fill(std::ostream* stream, const T& in) { +#if defined(_MSC_VER) && _MSC_VER <= 1900 + insert_hack_t::insert(*stream, in); +#else + operator<<(*stream, in); +#endif + } + }; + +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4866) +// NOLINTBEGIN(*-avoid-c-arrays) + template + struct filldata { + static void fill(std::ostream* stream, const T(&in)[N]) { + *stream << "["; + for (size_t i = 0; i < N; i++) { + if (i != 0) { *stream << ", "; } + *stream << (DOCTEST_STRINGIFY(in[i])); + } + *stream << "]"; + } + }; +// NOLINTEND(*-avoid-c-arrays) +DOCTEST_MSVC_SUPPRESS_WARNING_POP + + // Specialized since we don't want the terminating null byte! +// NOLINTBEGIN(*-avoid-c-arrays) + template + struct filldata { + static void fill(std::ostream* stream, const char (&in)[N]) { + *stream << String(in, in[N - 1] ? N : N - 1); + } // NOLINT(clang-analyzer-cplusplus.NewDeleteLeaks) + }; +// NOLINTEND(*-avoid-c-arrays) + + template <> + struct filldata { + static void fill(std::ostream* stream, const void* in); + }; + + template + struct filldata { +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4180) + static void fill(std::ostream* stream, const T* in) { +DOCTEST_MSVC_SUPPRESS_WARNING_POP +DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wmicrosoft-cast") + filldata::fill(stream, +#if DOCTEST_GCC == 0 || DOCTEST_GCC >= DOCTEST_COMPILER(4, 9, 0) + reinterpret_cast(in) +#else + *reinterpret_cast(&in) +#endif + ); +DOCTEST_CLANG_SUPPRESS_WARNING_POP + } + }; +} + +struct DOCTEST_INTERFACE Approx +{ + Approx(double value); + + Approx operator()(double value) const; + +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + template + explicit Approx(const T& value, + typename detail::types::enable_if::value>::type* = + static_cast(nullptr)) { + *this = static_cast(value); + } +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + + Approx& epsilon(double newEpsilon); + +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + template + typename std::enable_if::value, Approx&>::type epsilon( + const T& newEpsilon) { + m_epsilon = static_cast(newEpsilon); + return *this; + } +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + + Approx& scale(double newScale); + +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + template + typename std::enable_if::value, Approx&>::type scale( + const T& newScale) { + m_scale = static_cast(newScale); + return *this; + } +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + + // clang-format off + DOCTEST_INTERFACE friend bool operator==(double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator==(const Approx & lhs, double rhs); + DOCTEST_INTERFACE friend bool operator!=(double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator!=(const Approx & lhs, double rhs); + DOCTEST_INTERFACE friend bool operator<=(double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator<=(const Approx & lhs, double rhs); + DOCTEST_INTERFACE friend bool operator>=(double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator>=(const Approx & lhs, double rhs); + DOCTEST_INTERFACE friend bool operator< (double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator< (const Approx & lhs, double rhs); + DOCTEST_INTERFACE friend bool operator> (double lhs, const Approx & rhs); + DOCTEST_INTERFACE friend bool operator> (const Approx & lhs, double rhs); + +#ifdef DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS +#define DOCTEST_APPROX_PREFIX \ + template friend typename std::enable_if::value, bool>::type + + DOCTEST_APPROX_PREFIX operator==(const T& lhs, const Approx& rhs) { return operator==(static_cast(lhs), rhs); } + DOCTEST_APPROX_PREFIX operator==(const Approx& lhs, const T& rhs) { return operator==(rhs, lhs); } + DOCTEST_APPROX_PREFIX operator!=(const T& lhs, const Approx& rhs) { return !operator==(lhs, rhs); } + DOCTEST_APPROX_PREFIX operator!=(const Approx& lhs, const T& rhs) { return !operator==(rhs, lhs); } + DOCTEST_APPROX_PREFIX operator<=(const T& lhs, const Approx& rhs) { return static_cast(lhs) < rhs.m_value || lhs == rhs; } + DOCTEST_APPROX_PREFIX operator<=(const Approx& lhs, const T& rhs) { return lhs.m_value < static_cast(rhs) || lhs == rhs; } + DOCTEST_APPROX_PREFIX operator>=(const T& lhs, const Approx& rhs) { return static_cast(lhs) > rhs.m_value || lhs == rhs; } + DOCTEST_APPROX_PREFIX operator>=(const Approx& lhs, const T& rhs) { return lhs.m_value > static_cast(rhs) || lhs == rhs; } + DOCTEST_APPROX_PREFIX operator< (const T& lhs, const Approx& rhs) { return static_cast(lhs) < rhs.m_value && lhs != rhs; } + DOCTEST_APPROX_PREFIX operator< (const Approx& lhs, const T& rhs) { return lhs.m_value < static_cast(rhs) && lhs != rhs; } + DOCTEST_APPROX_PREFIX operator> (const T& lhs, const Approx& rhs) { return static_cast(lhs) > rhs.m_value && lhs != rhs; } + DOCTEST_APPROX_PREFIX operator> (const Approx& lhs, const T& rhs) { return lhs.m_value > static_cast(rhs) && lhs != rhs; } +#undef DOCTEST_APPROX_PREFIX +#endif // DOCTEST_CONFIG_INCLUDE_TYPE_TRAITS + + // clang-format on + + double m_epsilon; + double m_scale; + double m_value; +}; + +DOCTEST_INTERFACE String toString(const Approx& in); + +DOCTEST_INTERFACE const ContextOptions* getContextOptions(); + +template +struct DOCTEST_INTERFACE_DECL IsNaN +{ + F value; bool flipped; + IsNaN(F f, bool flip = false) : value(f), flipped(flip) { } + IsNaN operator!() const { return { value, !flipped }; } + operator bool() const; +}; +#ifndef __MINGW32__ +extern template struct DOCTEST_INTERFACE_DECL IsNaN; +extern template struct DOCTEST_INTERFACE_DECL IsNaN; +extern template struct DOCTEST_INTERFACE_DECL IsNaN; +#endif +DOCTEST_INTERFACE String toString(IsNaN in); +DOCTEST_INTERFACE String toString(IsNaN in); +DOCTEST_INTERFACE String toString(IsNaN in); + +#ifndef DOCTEST_CONFIG_DISABLE + +namespace detail { + // clang-format off +#ifdef DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + template struct decay_array { using type = T; }; + template struct decay_array { using type = T*; }; + template struct decay_array { using type = T*; }; + + template struct not_char_pointer { static DOCTEST_CONSTEXPR int value = 1; }; + template<> struct not_char_pointer { static DOCTEST_CONSTEXPR int value = 0; }; + template<> struct not_char_pointer { static DOCTEST_CONSTEXPR int value = 0; }; + + template struct can_use_op : public not_char_pointer::type> {}; +#endif // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + // clang-format on + + struct DOCTEST_INTERFACE TestFailureException + { + }; + + DOCTEST_INTERFACE bool checkIfShouldThrow(assertType::Enum at); + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + DOCTEST_NORETURN +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + DOCTEST_INTERFACE void throwException(); + + struct DOCTEST_INTERFACE Subcase + { + SubcaseSignature m_signature; + bool m_entered = false; + + Subcase(const String& name, const char* file, int line); + Subcase(const Subcase&) = delete; + Subcase(Subcase&&) = delete; + Subcase& operator=(const Subcase&) = delete; + Subcase& operator=(Subcase&&) = delete; + ~Subcase(); + + operator bool() const; + + private: + bool checkFilters(); + }; + + template + String stringifyBinaryExpr(const DOCTEST_REF_WRAP(L) lhs, const char* op, + const DOCTEST_REF_WRAP(R) rhs) { + return (DOCTEST_STRINGIFY(lhs)) + op + (DOCTEST_STRINGIFY(rhs)); + } + +#if DOCTEST_CLANG && DOCTEST_CLANG < DOCTEST_COMPILER(3, 6, 0) +DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wunused-comparison") +#endif + +// This will check if there is any way it could find a operator like member or friend and uses it. +// If not it doesn't find the operator or if the operator at global scope is defined after +// this template, the template won't be instantiated due to SFINAE. Once the template is not +// instantiated it can look for global operator using normal conversions. +#ifdef __NVCC__ +#define SFINAE_OP(ret,op) ret +#else +#define SFINAE_OP(ret,op) decltype((void)(doctest::detail::declval() op doctest::detail::declval()),ret{}) +#endif + +#define DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(op, op_str, op_macro) \ + template \ + DOCTEST_NOINLINE SFINAE_OP(Result,op) operator op(R&& rhs) { \ + bool res = op_macro(doctest::detail::forward(lhs), doctest::detail::forward(rhs)); \ + if(m_at & assertType::is_false) \ + res = !res; \ + if(!res || doctest::getContextOptions()->success) \ + return Result(res, stringifyBinaryExpr(lhs, op_str, rhs)); \ + return Result(res); \ + } + + // more checks could be added - like in Catch: + // https://github.com/catchorg/Catch2/pull/1480/files + // https://github.com/catchorg/Catch2/pull/1481/files +#define DOCTEST_FORBIT_EXPRESSION(rt, op) \ + template \ + rt& operator op(const R&) { \ + static_assert(deferred_false::value, \ + "Expression Too Complex Please Rewrite As Binary Comparison!"); \ + return *this; \ + } + + struct DOCTEST_INTERFACE Result // NOLINT(*-member-init) + { + bool m_passed; + String m_decomp; + + Result() = default; // TODO: Why do we need this? (To remove NOLINT) + Result(bool passed, const String& decomposition = String()); + + // forbidding some expressions based on this table: https://en.cppreference.com/w/cpp/language/operator_precedence + DOCTEST_FORBIT_EXPRESSION(Result, &) + DOCTEST_FORBIT_EXPRESSION(Result, ^) + DOCTEST_FORBIT_EXPRESSION(Result, |) + DOCTEST_FORBIT_EXPRESSION(Result, &&) + DOCTEST_FORBIT_EXPRESSION(Result, ||) + DOCTEST_FORBIT_EXPRESSION(Result, ==) + DOCTEST_FORBIT_EXPRESSION(Result, !=) + DOCTEST_FORBIT_EXPRESSION(Result, <) + DOCTEST_FORBIT_EXPRESSION(Result, >) + DOCTEST_FORBIT_EXPRESSION(Result, <=) + DOCTEST_FORBIT_EXPRESSION(Result, >=) + DOCTEST_FORBIT_EXPRESSION(Result, =) + DOCTEST_FORBIT_EXPRESSION(Result, +=) + DOCTEST_FORBIT_EXPRESSION(Result, -=) + DOCTEST_FORBIT_EXPRESSION(Result, *=) + DOCTEST_FORBIT_EXPRESSION(Result, /=) + DOCTEST_FORBIT_EXPRESSION(Result, %=) + DOCTEST_FORBIT_EXPRESSION(Result, <<=) + DOCTEST_FORBIT_EXPRESSION(Result, >>=) + DOCTEST_FORBIT_EXPRESSION(Result, &=) + DOCTEST_FORBIT_EXPRESSION(Result, ^=) + DOCTEST_FORBIT_EXPRESSION(Result, |=) + }; + +#ifndef DOCTEST_CONFIG_NO_COMPARISON_WARNING_SUPPRESSION + + DOCTEST_CLANG_SUPPRESS_WARNING_PUSH + DOCTEST_CLANG_SUPPRESS_WARNING("-Wsign-conversion") + DOCTEST_CLANG_SUPPRESS_WARNING("-Wsign-compare") + //DOCTEST_CLANG_SUPPRESS_WARNING("-Wdouble-promotion") + //DOCTEST_CLANG_SUPPRESS_WARNING("-Wconversion") + //DOCTEST_CLANG_SUPPRESS_WARNING("-Wfloat-equal") + + DOCTEST_GCC_SUPPRESS_WARNING_PUSH + DOCTEST_GCC_SUPPRESS_WARNING("-Wsign-conversion") + DOCTEST_GCC_SUPPRESS_WARNING("-Wsign-compare") + //DOCTEST_GCC_SUPPRESS_WARNING("-Wdouble-promotion") + //DOCTEST_GCC_SUPPRESS_WARNING("-Wconversion") + //DOCTEST_GCC_SUPPRESS_WARNING("-Wfloat-equal") + + DOCTEST_MSVC_SUPPRESS_WARNING_PUSH + // https://stackoverflow.com/questions/39479163 what's the difference between 4018 and 4389 + DOCTEST_MSVC_SUPPRESS_WARNING(4388) // signed/unsigned mismatch + DOCTEST_MSVC_SUPPRESS_WARNING(4389) // 'operator' : signed/unsigned mismatch + DOCTEST_MSVC_SUPPRESS_WARNING(4018) // 'expression' : signed/unsigned mismatch + //DOCTEST_MSVC_SUPPRESS_WARNING(4805) // 'operation' : unsafe mix of type 'type' and type 'type' in operation + +#endif // DOCTEST_CONFIG_NO_COMPARISON_WARNING_SUPPRESSION + + // clang-format off +#ifndef DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +#define DOCTEST_COMPARISON_RETURN_TYPE bool +#else // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +#define DOCTEST_COMPARISON_RETURN_TYPE typename types::enable_if::value || can_use_op::value, bool>::type + inline bool eq(const char* lhs, const char* rhs) { return String(lhs) == String(rhs); } + inline bool ne(const char* lhs, const char* rhs) { return String(lhs) != String(rhs); } + inline bool lt(const char* lhs, const char* rhs) { return String(lhs) < String(rhs); } + inline bool gt(const char* lhs, const char* rhs) { return String(lhs) > String(rhs); } + inline bool le(const char* lhs, const char* rhs) { return String(lhs) <= String(rhs); } + inline bool ge(const char* lhs, const char* rhs) { return String(lhs) >= String(rhs); } +#endif // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + // clang-format on + +#define DOCTEST_RELATIONAL_OP(name, op) \ + template \ + DOCTEST_COMPARISON_RETURN_TYPE name(const DOCTEST_REF_WRAP(L) lhs, \ + const DOCTEST_REF_WRAP(R) rhs) { \ + return lhs op rhs; \ + } + + DOCTEST_RELATIONAL_OP(eq, ==) + DOCTEST_RELATIONAL_OP(ne, !=) + DOCTEST_RELATIONAL_OP(lt, <) + DOCTEST_RELATIONAL_OP(gt, >) + DOCTEST_RELATIONAL_OP(le, <=) + DOCTEST_RELATIONAL_OP(ge, >=) + +#ifndef DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +#define DOCTEST_CMP_EQ(l, r) l == r +#define DOCTEST_CMP_NE(l, r) l != r +#define DOCTEST_CMP_GT(l, r) l > r +#define DOCTEST_CMP_LT(l, r) l < r +#define DOCTEST_CMP_GE(l, r) l >= r +#define DOCTEST_CMP_LE(l, r) l <= r +#else // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +#define DOCTEST_CMP_EQ(l, r) eq(l, r) +#define DOCTEST_CMP_NE(l, r) ne(l, r) +#define DOCTEST_CMP_GT(l, r) gt(l, r) +#define DOCTEST_CMP_LT(l, r) lt(l, r) +#define DOCTEST_CMP_GE(l, r) ge(l, r) +#define DOCTEST_CMP_LE(l, r) le(l, r) +#endif // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + + template + // cppcheck-suppress copyCtorAndEqOperator + struct Expression_lhs + { + L lhs; + assertType::Enum m_at; + + explicit Expression_lhs(L&& in, assertType::Enum at) + : lhs(static_cast(in)) + , m_at(at) {} + + DOCTEST_NOINLINE operator Result() { +// this is needed only for MSVC 2015 +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4800) // 'int': forcing value to bool + bool res = static_cast(lhs); +DOCTEST_MSVC_SUPPRESS_WARNING_POP + if(m_at & assertType::is_false) { //!OCLINT bitwise operator in conditional + res = !res; + } + + if(!res || getContextOptions()->success) { + return { res, (DOCTEST_STRINGIFY(lhs)) }; + } + return { res }; + } + + /* This is required for user-defined conversions from Expression_lhs to L */ + operator L() const { return lhs; } + + // clang-format off + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(==, " == ", DOCTEST_CMP_EQ) //!OCLINT bitwise operator in conditional + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(!=, " != ", DOCTEST_CMP_NE) //!OCLINT bitwise operator in conditional + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(>, " > ", DOCTEST_CMP_GT) //!OCLINT bitwise operator in conditional + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(<, " < ", DOCTEST_CMP_LT) //!OCLINT bitwise operator in conditional + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(>=, " >= ", DOCTEST_CMP_GE) //!OCLINT bitwise operator in conditional + DOCTEST_DO_BINARY_EXPRESSION_COMPARISON(<=, " <= ", DOCTEST_CMP_LE) //!OCLINT bitwise operator in conditional + // clang-format on + + // forbidding some expressions based on this table: https://en.cppreference.com/w/cpp/language/operator_precedence + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, &) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, ^) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, |) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, &&) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, ||) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, =) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, +=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, -=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, *=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, /=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, %=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, <<=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, >>=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, &=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, ^=) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, |=) + // these 2 are unfortunate because they should be allowed - they have higher precedence over the comparisons, but the + // ExpressionDecomposer class uses the left shift operator to capture the left operand of the binary expression... + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, <<) + DOCTEST_FORBIT_EXPRESSION(Expression_lhs, >>) + }; + +#ifndef DOCTEST_CONFIG_NO_COMPARISON_WARNING_SUPPRESSION + + DOCTEST_CLANG_SUPPRESS_WARNING_POP + DOCTEST_MSVC_SUPPRESS_WARNING_POP + DOCTEST_GCC_SUPPRESS_WARNING_POP + +#endif // DOCTEST_CONFIG_NO_COMPARISON_WARNING_SUPPRESSION + +#if DOCTEST_CLANG && DOCTEST_CLANG < DOCTEST_COMPILER(3, 6, 0) +DOCTEST_CLANG_SUPPRESS_WARNING_POP +#endif + + struct DOCTEST_INTERFACE ExpressionDecomposer + { + assertType::Enum m_at; + + ExpressionDecomposer(assertType::Enum at); + + // The right operator for capturing expressions is "<=" instead of "<<" (based on the operator precedence table) + // but then there will be warnings from GCC about "-Wparentheses" and since "_Pragma()" is problematic this will stay for now... + // https://github.com/catchorg/Catch2/issues/870 + // https://github.com/catchorg/Catch2/issues/565 + template + Expression_lhs operator<<(L&& operand) { + return Expression_lhs(static_cast(operand), m_at); + } + + template ::value,void >::type* = nullptr> + Expression_lhs operator<<(const L &operand) { + return Expression_lhs(operand, m_at); + } + }; + + struct DOCTEST_INTERFACE TestSuite + { + const char* m_test_suite = nullptr; + const char* m_description = nullptr; + bool m_skip = false; + bool m_no_breaks = false; + bool m_no_output = false; + bool m_may_fail = false; + bool m_should_fail = false; + int m_expected_failures = 0; + double m_timeout = 0; + + TestSuite& operator*(const char* in); + + template + TestSuite& operator*(const T& in) { + in.fill(*this); + return *this; + } + }; + + using funcType = void (*)(); + + struct DOCTEST_INTERFACE TestCase : public TestCaseData + { + funcType m_test; // a function pointer to the test case + + String m_type; // for templated test cases - gets appended to the real name + int m_template_id; // an ID used to distinguish between the different versions of a templated test case + String m_full_name; // contains the name (only for templated test cases!) + the template type + + TestCase(funcType test, const char* file, unsigned line, const TestSuite& test_suite, + const String& type = String(), int template_id = -1); + + TestCase(const TestCase& other); + TestCase(TestCase&&) = delete; + + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(26434) // hides a non-virtual function + TestCase& operator=(const TestCase& other); + DOCTEST_MSVC_SUPPRESS_WARNING_POP + + TestCase& operator=(TestCase&&) = delete; + + TestCase& operator*(const char* in); + + template + TestCase& operator*(const T& in) { + in.fill(*this); + return *this; + } + + bool operator<(const TestCase& other) const; + + ~TestCase() = default; + }; + + // forward declarations of functions used by the macros + DOCTEST_INTERFACE int regTest(const TestCase& tc); + DOCTEST_INTERFACE int setTestSuite(const TestSuite& ts); + DOCTEST_INTERFACE bool isDebuggerActive(); + + template + int instantiationHelper(const T&) { return 0; } + + namespace binaryAssertComparison { + enum Enum + { + eq = 0, + ne, + gt, + lt, + ge, + le + }; + } // namespace binaryAssertComparison + + // clang-format off + template struct RelationalComparator { bool operator()(const DOCTEST_REF_WRAP(L), const DOCTEST_REF_WRAP(R) ) const { return false; } }; + +#define DOCTEST_BINARY_RELATIONAL_OP(n, op) \ + template struct RelationalComparator { bool operator()(const DOCTEST_REF_WRAP(L) lhs, const DOCTEST_REF_WRAP(R) rhs) const { return op(lhs, rhs); } }; + // clang-format on + + DOCTEST_BINARY_RELATIONAL_OP(0, doctest::detail::eq) + DOCTEST_BINARY_RELATIONAL_OP(1, doctest::detail::ne) + DOCTEST_BINARY_RELATIONAL_OP(2, doctest::detail::gt) + DOCTEST_BINARY_RELATIONAL_OP(3, doctest::detail::lt) + DOCTEST_BINARY_RELATIONAL_OP(4, doctest::detail::ge) + DOCTEST_BINARY_RELATIONAL_OP(5, doctest::detail::le) + + struct DOCTEST_INTERFACE ResultBuilder : public AssertData + { + ResultBuilder(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type = "", const String& exception_string = ""); + + ResultBuilder(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type, const Contains& exception_string); + + void setResult(const Result& res); + + template + DOCTEST_NOINLINE bool binary_assert(const DOCTEST_REF_WRAP(L) lhs, + const DOCTEST_REF_WRAP(R) rhs) { + m_failed = !RelationalComparator()(lhs, rhs); + if (m_failed || getContextOptions()->success) { + m_decomp = stringifyBinaryExpr(lhs, ", ", rhs); + } + return !m_failed; + } + + template + DOCTEST_NOINLINE bool unary_assert(const DOCTEST_REF_WRAP(L) val) { + m_failed = !val; + + if (m_at & assertType::is_false) { //!OCLINT bitwise operator in conditional + m_failed = !m_failed; + } + + if (m_failed || getContextOptions()->success) { + m_decomp = (DOCTEST_STRINGIFY(val)); + } + + return !m_failed; + } + + void translateException(); + + bool log(); + void react() const; + }; + + namespace assertAction { + enum Enum + { + nothing = 0, + dbgbreak = 1, + shouldthrow = 2 + }; + } // namespace assertAction + + DOCTEST_INTERFACE void failed_out_of_a_testing_context(const AssertData& ad); + + DOCTEST_INTERFACE bool decomp_assert(assertType::Enum at, const char* file, int line, + const char* expr, const Result& result); + +#define DOCTEST_ASSERT_OUT_OF_TESTS(decomp) \ + do { \ + if(!is_running_in_test) { \ + if(failed) { \ + ResultBuilder rb(at, file, line, expr); \ + rb.m_failed = failed; \ + rb.m_decomp = decomp; \ + failed_out_of_a_testing_context(rb); \ + if(isDebuggerActive() && !getContextOptions()->no_breaks) \ + DOCTEST_BREAK_INTO_DEBUGGER(); \ + if(checkIfShouldThrow(at)) \ + throwException(); \ + } \ + return !failed; \ + } \ + } while(false) + +#define DOCTEST_ASSERT_IN_TESTS(decomp) \ + ResultBuilder rb(at, file, line, expr); \ + rb.m_failed = failed; \ + if(rb.m_failed || getContextOptions()->success) \ + rb.m_decomp = decomp; \ + if(rb.log()) \ + DOCTEST_BREAK_INTO_DEBUGGER(); \ + if(rb.m_failed && checkIfShouldThrow(at)) \ + throwException() + + template + DOCTEST_NOINLINE bool binary_assert(assertType::Enum at, const char* file, int line, + const char* expr, const DOCTEST_REF_WRAP(L) lhs, + const DOCTEST_REF_WRAP(R) rhs) { + bool failed = !RelationalComparator()(lhs, rhs); + + // ################################################################################### + // IF THE DEBUGGER BREAKS HERE - GO 1 LEVEL UP IN THE CALLSTACK FOR THE FAILING ASSERT + // THIS IS THE EFFECT OF HAVING 'DOCTEST_CONFIG_SUPER_FAST_ASSERTS' DEFINED + // ################################################################################### + DOCTEST_ASSERT_OUT_OF_TESTS(stringifyBinaryExpr(lhs, ", ", rhs)); + DOCTEST_ASSERT_IN_TESTS(stringifyBinaryExpr(lhs, ", ", rhs)); + return !failed; + } + + template + DOCTEST_NOINLINE bool unary_assert(assertType::Enum at, const char* file, int line, + const char* expr, const DOCTEST_REF_WRAP(L) val) { + bool failed = !val; + + if(at & assertType::is_false) //!OCLINT bitwise operator in conditional + failed = !failed; + + // ################################################################################### + // IF THE DEBUGGER BREAKS HERE - GO 1 LEVEL UP IN THE CALLSTACK FOR THE FAILING ASSERT + // THIS IS THE EFFECT OF HAVING 'DOCTEST_CONFIG_SUPER_FAST_ASSERTS' DEFINED + // ################################################################################### + DOCTEST_ASSERT_OUT_OF_TESTS((DOCTEST_STRINGIFY(val))); + DOCTEST_ASSERT_IN_TESTS((DOCTEST_STRINGIFY(val))); + return !failed; + } + + struct DOCTEST_INTERFACE IExceptionTranslator + { + DOCTEST_DECLARE_INTERFACE(IExceptionTranslator) + virtual bool translate(String&) const = 0; + }; + + template + class ExceptionTranslator : public IExceptionTranslator //!OCLINT destructor of virtual class + { + public: + explicit ExceptionTranslator(String (*translateFunction)(T)) + : m_translateFunction(translateFunction) {} + + bool translate(String& res) const override { +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + try { + throw; // lgtm [cpp/rethrow-no-exception] + // cppcheck-suppress catchExceptionByValue + } catch(const T& ex) { + res = m_translateFunction(ex); //!OCLINT parameter reassignment + return true; + } catch(...) {} //!OCLINT - empty catch statement +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + static_cast(res); // to silence -Wunused-parameter + return false; + } + + private: + String (*m_translateFunction)(T); + }; + + DOCTEST_INTERFACE void registerExceptionTranslatorImpl(const IExceptionTranslator* et); + + // ContextScope base class used to allow implementing methods of ContextScope + // that don't depend on the template parameter in doctest.cpp. + struct DOCTEST_INTERFACE ContextScopeBase : public IContextScope { + ContextScopeBase(const ContextScopeBase&) = delete; + + ContextScopeBase& operator=(const ContextScopeBase&) = delete; + ContextScopeBase& operator=(ContextScopeBase&&) = delete; + + ~ContextScopeBase() override = default; + + protected: + ContextScopeBase(); + ContextScopeBase(ContextScopeBase&& other) noexcept; + + void destroy(); + bool need_to_destroy{true}; + }; + + template class ContextScope : public ContextScopeBase + { + L lambda_; + + public: + explicit ContextScope(const L &lambda) : lambda_(lambda) {} + explicit ContextScope(L&& lambda) : lambda_(static_cast(lambda)) { } + + ContextScope(const ContextScope&) = delete; + ContextScope(ContextScope&&) noexcept = default; + + ContextScope& operator=(const ContextScope&) = delete; + ContextScope& operator=(ContextScope&&) = delete; + + void stringify(std::ostream* s) const override { lambda_(s); } + + ~ContextScope() override { + if (need_to_destroy) { + destroy(); + } + } + }; + + struct DOCTEST_INTERFACE MessageBuilder : public MessageData + { + std::ostream* m_stream; + bool logged = false; + + MessageBuilder(const char* file, int line, assertType::Enum severity); + + MessageBuilder(const MessageBuilder&) = delete; + MessageBuilder(MessageBuilder&&) = delete; + + MessageBuilder& operator=(const MessageBuilder&) = delete; + MessageBuilder& operator=(MessageBuilder&&) = delete; + + ~MessageBuilder(); + + // the preferred way of chaining parameters for stringification +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4866) + template + MessageBuilder& operator,(const T& in) { + *m_stream << (DOCTEST_STRINGIFY(in)); + return *this; + } +DOCTEST_MSVC_SUPPRESS_WARNING_POP + + // kept here just for backwards-compatibility - the comma operator should be preferred now + template + MessageBuilder& operator<<(const T& in) { return this->operator,(in); } + + // the `,` operator has the lowest operator precedence - if `<<` is used by the user then + // the `,` operator will be called last which is not what we want and thus the `*` operator + // is used first (has higher operator precedence compared to `<<`) so that we guarantee that + // an operator of the MessageBuilder class is called first before the rest of the parameters + template + MessageBuilder& operator*(const T& in) { return this->operator,(in); } + + bool log(); + void react(); + }; + + template + ContextScope MakeContextScope(const L &lambda) { + return ContextScope(lambda); + } +} // namespace detail + +#define DOCTEST_DEFINE_DECORATOR(name, type, def) \ + struct name \ + { \ + type data; \ + name(type in = def) \ + : data(in) {} \ + void fill(detail::TestCase& state) const { state.DOCTEST_CAT(m_, name) = data; } \ + void fill(detail::TestSuite& state) const { state.DOCTEST_CAT(m_, name) = data; } \ + } + +DOCTEST_DEFINE_DECORATOR(test_suite, const char*, ""); +DOCTEST_DEFINE_DECORATOR(description, const char*, ""); +DOCTEST_DEFINE_DECORATOR(skip, bool, true); +DOCTEST_DEFINE_DECORATOR(no_breaks, bool, true); +DOCTEST_DEFINE_DECORATOR(no_output, bool, true); +DOCTEST_DEFINE_DECORATOR(timeout, double, 0); +DOCTEST_DEFINE_DECORATOR(may_fail, bool, true); +DOCTEST_DEFINE_DECORATOR(should_fail, bool, true); +DOCTEST_DEFINE_DECORATOR(expected_failures, int, 0); + +template +int registerExceptionTranslator(String (*translateFunction)(T)) { + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wexit-time-destructors") + static detail::ExceptionTranslator exceptionTranslator(translateFunction); + DOCTEST_CLANG_SUPPRESS_WARNING_POP + detail::registerExceptionTranslatorImpl(&exceptionTranslator); + return 0; +} + +} // namespace doctest + +// in a separate namespace outside of doctest because the DOCTEST_TEST_SUITE macro +// introduces an anonymous namespace in which getCurrentTestSuite gets overridden +namespace doctest_detail_test_suite_ns { +DOCTEST_INTERFACE doctest::detail::TestSuite& getCurrentTestSuite(); +} // namespace doctest_detail_test_suite_ns + +namespace doctest { +#else // DOCTEST_CONFIG_DISABLE +template +int registerExceptionTranslator(String (*)(T)) { + return 0; +} +#endif // DOCTEST_CONFIG_DISABLE + +namespace detail { + using assert_handler = void (*)(const AssertData&); + struct ContextState; +} // namespace detail + +class DOCTEST_INTERFACE Context +{ + detail::ContextState* p; + + void parseArgs(int argc, const char* const* argv, bool withDefaults = false); + +public: + explicit Context(int argc = 0, const char* const* argv = nullptr); + + Context(const Context&) = delete; + Context(Context&&) = delete; + + Context& operator=(const Context&) = delete; + Context& operator=(Context&&) = delete; + + ~Context(); // NOLINT(performance-trivially-destructible) + + void applyCommandLine(int argc, const char* const* argv); + + void addFilter(const char* filter, const char* value); + void clearFilters(); + void setOption(const char* option, bool value); + void setOption(const char* option, int value); + void setOption(const char* option, const char* value); + + bool shouldExit(); + + void setAsDefaultForAssertsOutOfTestCases(); + + void setAssertHandler(detail::assert_handler ah); + + void setCout(std::ostream* out); + + int run(); +}; + +namespace TestCaseFailureReason { + enum Enum + { + None = 0, + AssertFailure = 1, // an assertion has failed in the test case + Exception = 2, // test case threw an exception + Crash = 4, // a crash... + TooManyFailedAsserts = 8, // the abort-after option + Timeout = 16, // see the timeout decorator + ShouldHaveFailedButDidnt = 32, // see the should_fail decorator + ShouldHaveFailedAndDid = 64, // see the should_fail decorator + DidntFailExactlyNumTimes = 128, // see the expected_failures decorator + FailedExactlyNumTimes = 256, // see the expected_failures decorator + CouldHaveFailedAndDid = 512 // see the may_fail decorator + }; +} // namespace TestCaseFailureReason + +struct DOCTEST_INTERFACE CurrentTestCaseStats +{ + int numAssertsCurrentTest; + int numAssertsFailedCurrentTest; + double seconds; + int failure_flags; // use TestCaseFailureReason::Enum + bool testCaseSuccess; +}; + +struct DOCTEST_INTERFACE TestCaseException +{ + String error_string; + bool is_crash; +}; + +struct DOCTEST_INTERFACE TestRunStats +{ + unsigned numTestCases; + unsigned numTestCasesPassingFilters; + unsigned numTestSuitesPassingFilters; + unsigned numTestCasesFailed; + int numAsserts; + int numAssertsFailed; +}; + +struct QueryData +{ + const TestRunStats* run_stats = nullptr; + const TestCaseData** data = nullptr; + unsigned num_data = 0; +}; + +struct DOCTEST_INTERFACE IReporter +{ + // The constructor has to accept "const ContextOptions&" as a single argument + // which has most of the options for the run + a pointer to the stdout stream + // Reporter(const ContextOptions& in) + + // called when a query should be reported (listing test cases, printing the version, etc.) + virtual void report_query(const QueryData&) = 0; + + // called when the whole test run starts + virtual void test_run_start() = 0; + // called when the whole test run ends (caching a pointer to the input doesn't make sense here) + virtual void test_run_end(const TestRunStats&) = 0; + + // called when a test case is started (safe to cache a pointer to the input) + virtual void test_case_start(const TestCaseData&) = 0; + // called when a test case is reentered because of unfinished subcases (safe to cache a pointer to the input) + virtual void test_case_reenter(const TestCaseData&) = 0; + // called when a test case has ended + virtual void test_case_end(const CurrentTestCaseStats&) = 0; + + // called when an exception is thrown from the test case (or it crashes) + virtual void test_case_exception(const TestCaseException&) = 0; + + // called whenever a subcase is entered (don't cache pointers to the input) + virtual void subcase_start(const SubcaseSignature&) = 0; + // called whenever a subcase is exited (don't cache pointers to the input) + virtual void subcase_end() = 0; + + // called for each assert (don't cache pointers to the input) + virtual void log_assert(const AssertData&) = 0; + // called for each message (don't cache pointers to the input) + virtual void log_message(const MessageData&) = 0; + + // called when a test case is skipped either because it doesn't pass the filters, has a skip decorator + // or isn't in the execution range (between first and last) (safe to cache a pointer to the input) + virtual void test_case_skipped(const TestCaseData&) = 0; + + DOCTEST_DECLARE_INTERFACE(IReporter) + + // can obtain all currently active contexts and stringify them if one wishes to do so + static int get_num_active_contexts(); + static const IContextScope* const* get_active_contexts(); + + // can iterate through contexts which have been stringified automatically in their destructors when an exception has been thrown + static int get_num_stringified_contexts(); + static const String* get_stringified_contexts(); +}; + +namespace detail { + using reporterCreatorFunc = IReporter* (*)(const ContextOptions&); + + DOCTEST_INTERFACE void registerReporterImpl(const char* name, int prio, reporterCreatorFunc c, bool isReporter); + + template + IReporter* reporterCreator(const ContextOptions& o) { + return new Reporter(o); + } +} // namespace detail + +template +int registerReporter(const char* name, int priority, bool isReporter) { + detail::registerReporterImpl(name, priority, detail::reporterCreator, isReporter); + return 0; +} +} // namespace doctest + +#ifdef DOCTEST_CONFIG_ASSERTS_RETURN_VALUES +#define DOCTEST_FUNC_EMPTY [] { return false; }() +#else +#define DOCTEST_FUNC_EMPTY (void)0 +#endif + +// if registering is not disabled +#ifndef DOCTEST_CONFIG_DISABLE + +#ifdef DOCTEST_CONFIG_ASSERTS_RETURN_VALUES +#define DOCTEST_FUNC_SCOPE_BEGIN [&] +#define DOCTEST_FUNC_SCOPE_END () +#define DOCTEST_FUNC_SCOPE_RET(v) return v +#else +#define DOCTEST_FUNC_SCOPE_BEGIN do +#define DOCTEST_FUNC_SCOPE_END while(false) +#define DOCTEST_FUNC_SCOPE_RET(v) (void)0 +#endif + +// common code in asserts - for convenience +#define DOCTEST_ASSERT_LOG_REACT_RETURN(b) \ + if(b.log()) DOCTEST_BREAK_INTO_DEBUGGER(); \ + b.react(); \ + DOCTEST_FUNC_SCOPE_RET(!b.m_failed) + +#ifdef DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS +#define DOCTEST_WRAP_IN_TRY(x) x; +#else // DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS +#define DOCTEST_WRAP_IN_TRY(x) \ + try { \ + x; \ + } catch(...) { DOCTEST_RB.translateException(); } +#endif // DOCTEST_CONFIG_NO_TRY_CATCH_IN_ASSERTS + +#ifdef DOCTEST_CONFIG_VOID_CAST_EXPRESSIONS +#define DOCTEST_CAST_TO_VOID(...) \ + DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wuseless-cast") \ + static_cast(__VA_ARGS__); \ + DOCTEST_GCC_SUPPRESS_WARNING_POP +#else // DOCTEST_CONFIG_VOID_CAST_EXPRESSIONS +#define DOCTEST_CAST_TO_VOID(...) __VA_ARGS__; +#endif // DOCTEST_CONFIG_VOID_CAST_EXPRESSIONS + +// registers the test by initializing a dummy var with a function +#define DOCTEST_REGISTER_FUNCTION(global_prefix, f, decorators) \ + global_prefix DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_VAR_), /* NOLINT */ \ + doctest::detail::regTest( \ + doctest::detail::TestCase( \ + f, __FILE__, __LINE__, \ + doctest_detail_test_suite_ns::getCurrentTestSuite()) * \ + decorators)) + +#define DOCTEST_IMPLEMENT_FIXTURE(der, base, func, decorators) \ + namespace { /* NOLINT */ \ + struct der : public base \ + { \ + void f(); \ + }; \ + static DOCTEST_INLINE_NOINLINE void func() { \ + der v; \ + v.f(); \ + } \ + DOCTEST_REGISTER_FUNCTION(DOCTEST_EMPTY, func, decorators) \ + } \ + DOCTEST_INLINE_NOINLINE void der::f() // NOLINT(misc-definitions-in-headers) + +#define DOCTEST_CREATE_AND_REGISTER_FUNCTION(f, decorators) \ + static void f(); \ + DOCTEST_REGISTER_FUNCTION(DOCTEST_EMPTY, f, decorators) \ + static void f() + +#define DOCTEST_CREATE_AND_REGISTER_FUNCTION_IN_CLASS(f, proxy, decorators) \ + static doctest::detail::funcType proxy() { return f; } \ + DOCTEST_REGISTER_FUNCTION(inline, proxy(), decorators) \ + static void f() + +// for registering tests +#define DOCTEST_TEST_CASE(decorators) \ + DOCTEST_CREATE_AND_REGISTER_FUNCTION(DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), decorators) + +// for registering tests in classes - requires C++17 for inline variables! +#if DOCTEST_CPLUSPLUS >= 201703L +#define DOCTEST_TEST_CASE_CLASS(decorators) \ + DOCTEST_CREATE_AND_REGISTER_FUNCTION_IN_CLASS(DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), \ + DOCTEST_ANONYMOUS(DOCTEST_ANON_PROXY_), \ + decorators) +#else // DOCTEST_TEST_CASE_CLASS +#define DOCTEST_TEST_CASE_CLASS(...) \ + TEST_CASES_CAN_BE_REGISTERED_IN_CLASSES_ONLY_IN_CPP17_MODE_OR_WITH_VS_2017_OR_NEWER +#endif // DOCTEST_TEST_CASE_CLASS + +// for registering tests with a fixture +#define DOCTEST_TEST_CASE_FIXTURE(c, decorators) \ + DOCTEST_IMPLEMENT_FIXTURE(DOCTEST_ANONYMOUS(DOCTEST_ANON_CLASS_), c, \ + DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), decorators) + +// for converting types to strings without the header and demangling +#define DOCTEST_TYPE_TO_STRING_AS(str, ...) \ + namespace doctest { \ + template <> \ + inline String toString<__VA_ARGS__>() { \ + return str; \ + } \ + } \ + static_assert(true, "") + +#define DOCTEST_TYPE_TO_STRING(...) DOCTEST_TYPE_TO_STRING_AS(#__VA_ARGS__, __VA_ARGS__) + +#define DOCTEST_TEST_CASE_TEMPLATE_DEFINE_IMPL(dec, T, iter, func) \ + template \ + static void func(); \ + namespace { /* NOLINT */ \ + template \ + struct iter; \ + template \ + struct iter> \ + { \ + iter(const char* file, unsigned line, int index) { \ + doctest::detail::regTest(doctest::detail::TestCase(func, file, line, \ + doctest_detail_test_suite_ns::getCurrentTestSuite(), \ + doctest::toString(), \ + int(line) * 1000 + index) \ + * dec); \ + iter>(file, line, index + 1); \ + } \ + }; \ + template <> \ + struct iter> \ + { \ + iter(const char*, unsigned, int) {} \ + }; \ + } \ + template \ + static void func() + +#define DOCTEST_TEST_CASE_TEMPLATE_DEFINE(dec, T, id) \ + DOCTEST_TEST_CASE_TEMPLATE_DEFINE_IMPL(dec, T, DOCTEST_CAT(id, ITERATOR), \ + DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_)) + +#define DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE_IMPL(id, anon, ...) \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_CAT(anon, DUMMY), /* NOLINT(cert-err58-cpp, fuchsia-statically-constructed-objects) */ \ + doctest::detail::instantiationHelper( \ + DOCTEST_CAT(id, ITERATOR)<__VA_ARGS__>(__FILE__, __LINE__, 0))) + +#define DOCTEST_TEST_CASE_TEMPLATE_INVOKE(id, ...) \ + DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE_IMPL(id, DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_), std::tuple<__VA_ARGS__>) \ + static_assert(true, "") + +#define DOCTEST_TEST_CASE_TEMPLATE_APPLY(id, ...) \ + DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE_IMPL(id, DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_), __VA_ARGS__) \ + static_assert(true, "") + +#define DOCTEST_TEST_CASE_TEMPLATE_IMPL(dec, T, anon, ...) \ + DOCTEST_TEST_CASE_TEMPLATE_DEFINE_IMPL(dec, T, DOCTEST_CAT(anon, ITERATOR), anon); \ + DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE_IMPL(anon, anon, std::tuple<__VA_ARGS__>) \ + template \ + static void anon() + +#define DOCTEST_TEST_CASE_TEMPLATE(dec, T, ...) \ + DOCTEST_TEST_CASE_TEMPLATE_IMPL(dec, T, DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_), __VA_ARGS__) + +// for subcases +#define DOCTEST_SUBCASE(name) \ + if(const doctest::detail::Subcase & DOCTEST_ANONYMOUS(DOCTEST_ANON_SUBCASE_) DOCTEST_UNUSED = \ + doctest::detail::Subcase(name, __FILE__, __LINE__)) + +// for grouping tests in test suites by using code blocks +#define DOCTEST_TEST_SUITE_IMPL(decorators, ns_name) \ + namespace ns_name { namespace doctest_detail_test_suite_ns { \ + static DOCTEST_NOINLINE doctest::detail::TestSuite& getCurrentTestSuite() noexcept { \ + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4640) \ + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wexit-time-destructors") \ + DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wmissing-field-initializers") \ + static doctest::detail::TestSuite data{}; \ + static bool inited = false; \ + DOCTEST_MSVC_SUPPRESS_WARNING_POP \ + DOCTEST_CLANG_SUPPRESS_WARNING_POP \ + DOCTEST_GCC_SUPPRESS_WARNING_POP \ + if(!inited) { \ + data* decorators; \ + inited = true; \ + } \ + return data; \ + } \ + } \ + } \ + namespace ns_name + +#define DOCTEST_TEST_SUITE(decorators) \ + DOCTEST_TEST_SUITE_IMPL(decorators, DOCTEST_ANONYMOUS(DOCTEST_ANON_SUITE_)) + +// for starting a testsuite block +#define DOCTEST_TEST_SUITE_BEGIN(decorators) \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_VAR_), /* NOLINT(cert-err58-cpp) */ \ + doctest::detail::setTestSuite(doctest::detail::TestSuite() * decorators)) \ + static_assert(true, "") + +// for ending a testsuite block +#define DOCTEST_TEST_SUITE_END \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_VAR_), /* NOLINT(cert-err58-cpp) */ \ + doctest::detail::setTestSuite(doctest::detail::TestSuite() * "")) \ + using DOCTEST_ANONYMOUS(DOCTEST_ANON_FOR_SEMICOLON_) = int + +// for registering exception translators +#define DOCTEST_REGISTER_EXCEPTION_TRANSLATOR_IMPL(translatorName, signature) \ + inline doctest::String translatorName(signature); \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_TRANSLATOR_), /* NOLINT(cert-err58-cpp) */ \ + doctest::registerExceptionTranslator(translatorName)) \ + doctest::String translatorName(signature) + +#define DOCTEST_REGISTER_EXCEPTION_TRANSLATOR(signature) \ + DOCTEST_REGISTER_EXCEPTION_TRANSLATOR_IMPL(DOCTEST_ANONYMOUS(DOCTEST_ANON_TRANSLATOR_), \ + signature) + +// for registering reporters +#define DOCTEST_REGISTER_REPORTER(name, priority, reporter) \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_REPORTER_), /* NOLINT(cert-err58-cpp) */ \ + doctest::registerReporter(name, priority, true)) \ + static_assert(true, "") + +// for registering listeners +#define DOCTEST_REGISTER_LISTENER(name, priority, reporter) \ + DOCTEST_GLOBAL_NO_WARNINGS(DOCTEST_ANONYMOUS(DOCTEST_ANON_REPORTER_), /* NOLINT(cert-err58-cpp) */ \ + doctest::registerReporter(name, priority, false)) \ + static_assert(true, "") + +// clang-format off +// for logging - disabling formatting because it's important to have these on 2 separate lines - see PR #557 +#define DOCTEST_INFO(...) \ + DOCTEST_INFO_IMPL(DOCTEST_ANONYMOUS(DOCTEST_CAPTURE_), \ + DOCTEST_ANONYMOUS(DOCTEST_CAPTURE_OTHER_), \ + __VA_ARGS__) +// clang-format on + +#define DOCTEST_INFO_IMPL(mb_name, s_name, ...) \ + auto DOCTEST_ANONYMOUS(DOCTEST_CAPTURE_) = doctest::detail::MakeContextScope( \ + [&](std::ostream* s_name) { \ + doctest::detail::MessageBuilder mb_name(__FILE__, __LINE__, doctest::assertType::is_warn); \ + mb_name.m_stream = s_name; \ + mb_name * __VA_ARGS__; \ + }) + +#define DOCTEST_CAPTURE(x) DOCTEST_INFO(#x " := ", x) + +#define DOCTEST_ADD_AT_IMPL(type, file, line, mb, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + doctest::detail::MessageBuilder mb(file, line, doctest::assertType::type); \ + mb * __VA_ARGS__; \ + if(mb.log()) \ + DOCTEST_BREAK_INTO_DEBUGGER(); \ + mb.react(); \ + } DOCTEST_FUNC_SCOPE_END + +// clang-format off +#define DOCTEST_ADD_MESSAGE_AT(file, line, ...) DOCTEST_ADD_AT_IMPL(is_warn, file, line, DOCTEST_ANONYMOUS(DOCTEST_MESSAGE_), __VA_ARGS__) +#define DOCTEST_ADD_FAIL_CHECK_AT(file, line, ...) DOCTEST_ADD_AT_IMPL(is_check, file, line, DOCTEST_ANONYMOUS(DOCTEST_MESSAGE_), __VA_ARGS__) +#define DOCTEST_ADD_FAIL_AT(file, line, ...) DOCTEST_ADD_AT_IMPL(is_require, file, line, DOCTEST_ANONYMOUS(DOCTEST_MESSAGE_), __VA_ARGS__) +// clang-format on + +#define DOCTEST_MESSAGE(...) DOCTEST_ADD_MESSAGE_AT(__FILE__, __LINE__, __VA_ARGS__) +#define DOCTEST_FAIL_CHECK(...) DOCTEST_ADD_FAIL_CHECK_AT(__FILE__, __LINE__, __VA_ARGS__) +#define DOCTEST_FAIL(...) DOCTEST_ADD_FAIL_AT(__FILE__, __LINE__, __VA_ARGS__) + +#define DOCTEST_TO_LVALUE(...) __VA_ARGS__ // Not removed to keep backwards compatibility. + +#ifndef DOCTEST_CONFIG_SUPER_FAST_ASSERTS + +#define DOCTEST_ASSERT_IMPLEMENT_2(assert_type, ...) \ + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Woverloaded-shift-op-parentheses") \ + /* NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) */ \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, #__VA_ARGS__); \ + DOCTEST_WRAP_IN_TRY(DOCTEST_RB.setResult( \ + doctest::detail::ExpressionDecomposer(doctest::assertType::assert_type) \ + << __VA_ARGS__)) /* NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) */ \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB) \ + DOCTEST_CLANG_SUPPRESS_WARNING_POP + +#define DOCTEST_ASSERT_IMPLEMENT_1(assert_type, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + DOCTEST_ASSERT_IMPLEMENT_2(assert_type, __VA_ARGS__); \ + } DOCTEST_FUNC_SCOPE_END // NOLINT(clang-analyzer-cplusplus.NewDeleteLeaks) + +#define DOCTEST_BINARY_ASSERT(assert_type, comp, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, #__VA_ARGS__); \ + DOCTEST_WRAP_IN_TRY( \ + DOCTEST_RB.binary_assert( \ + __VA_ARGS__)) \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB); \ + } DOCTEST_FUNC_SCOPE_END + +#define DOCTEST_UNARY_ASSERT(assert_type, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, #__VA_ARGS__); \ + DOCTEST_WRAP_IN_TRY(DOCTEST_RB.unary_assert(__VA_ARGS__)) \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB); \ + } DOCTEST_FUNC_SCOPE_END + +#else // DOCTEST_CONFIG_SUPER_FAST_ASSERTS + +// necessary for _MESSAGE +#define DOCTEST_ASSERT_IMPLEMENT_2 DOCTEST_ASSERT_IMPLEMENT_1 + +#define DOCTEST_ASSERT_IMPLEMENT_1(assert_type, ...) \ + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Woverloaded-shift-op-parentheses") \ + doctest::detail::decomp_assert( \ + doctest::assertType::assert_type, __FILE__, __LINE__, #__VA_ARGS__, \ + doctest::detail::ExpressionDecomposer(doctest::assertType::assert_type) \ + << __VA_ARGS__) DOCTEST_CLANG_SUPPRESS_WARNING_POP + +#define DOCTEST_BINARY_ASSERT(assert_type, comparison, ...) \ + doctest::detail::binary_assert( \ + doctest::assertType::assert_type, __FILE__, __LINE__, #__VA_ARGS__, __VA_ARGS__) + +#define DOCTEST_UNARY_ASSERT(assert_type, ...) \ + doctest::detail::unary_assert(doctest::assertType::assert_type, __FILE__, __LINE__, \ + #__VA_ARGS__, __VA_ARGS__) + +#endif // DOCTEST_CONFIG_SUPER_FAST_ASSERTS + +#define DOCTEST_WARN(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_WARN, __VA_ARGS__) +#define DOCTEST_CHECK(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_CHECK, __VA_ARGS__) +#define DOCTEST_REQUIRE(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_REQUIRE, __VA_ARGS__) +#define DOCTEST_WARN_FALSE(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_WARN_FALSE, __VA_ARGS__) +#define DOCTEST_CHECK_FALSE(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_CHECK_FALSE, __VA_ARGS__) +#define DOCTEST_REQUIRE_FALSE(...) DOCTEST_ASSERT_IMPLEMENT_1(DT_REQUIRE_FALSE, __VA_ARGS__) + +// clang-format off +#define DOCTEST_WARN_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_WARN, cond); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_CHECK, cond); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_REQUIRE, cond); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_WARN_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_WARN_FALSE, cond); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_CHECK_FALSE, cond); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_ASSERT_IMPLEMENT_2(DT_REQUIRE_FALSE, cond); } DOCTEST_FUNC_SCOPE_END +// clang-format on + +#define DOCTEST_WARN_EQ(...) DOCTEST_BINARY_ASSERT(DT_WARN_EQ, eq, __VA_ARGS__) +#define DOCTEST_CHECK_EQ(...) DOCTEST_BINARY_ASSERT(DT_CHECK_EQ, eq, __VA_ARGS__) +#define DOCTEST_REQUIRE_EQ(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_EQ, eq, __VA_ARGS__) +#define DOCTEST_WARN_NE(...) DOCTEST_BINARY_ASSERT(DT_WARN_NE, ne, __VA_ARGS__) +#define DOCTEST_CHECK_NE(...) DOCTEST_BINARY_ASSERT(DT_CHECK_NE, ne, __VA_ARGS__) +#define DOCTEST_REQUIRE_NE(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_NE, ne, __VA_ARGS__) +#define DOCTEST_WARN_GT(...) DOCTEST_BINARY_ASSERT(DT_WARN_GT, gt, __VA_ARGS__) +#define DOCTEST_CHECK_GT(...) DOCTEST_BINARY_ASSERT(DT_CHECK_GT, gt, __VA_ARGS__) +#define DOCTEST_REQUIRE_GT(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_GT, gt, __VA_ARGS__) +#define DOCTEST_WARN_LT(...) DOCTEST_BINARY_ASSERT(DT_WARN_LT, lt, __VA_ARGS__) +#define DOCTEST_CHECK_LT(...) DOCTEST_BINARY_ASSERT(DT_CHECK_LT, lt, __VA_ARGS__) +#define DOCTEST_REQUIRE_LT(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_LT, lt, __VA_ARGS__) +#define DOCTEST_WARN_GE(...) DOCTEST_BINARY_ASSERT(DT_WARN_GE, ge, __VA_ARGS__) +#define DOCTEST_CHECK_GE(...) DOCTEST_BINARY_ASSERT(DT_CHECK_GE, ge, __VA_ARGS__) +#define DOCTEST_REQUIRE_GE(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_GE, ge, __VA_ARGS__) +#define DOCTEST_WARN_LE(...) DOCTEST_BINARY_ASSERT(DT_WARN_LE, le, __VA_ARGS__) +#define DOCTEST_CHECK_LE(...) DOCTEST_BINARY_ASSERT(DT_CHECK_LE, le, __VA_ARGS__) +#define DOCTEST_REQUIRE_LE(...) DOCTEST_BINARY_ASSERT(DT_REQUIRE_LE, le, __VA_ARGS__) + +#define DOCTEST_WARN_UNARY(...) DOCTEST_UNARY_ASSERT(DT_WARN_UNARY, __VA_ARGS__) +#define DOCTEST_CHECK_UNARY(...) DOCTEST_UNARY_ASSERT(DT_CHECK_UNARY, __VA_ARGS__) +#define DOCTEST_REQUIRE_UNARY(...) DOCTEST_UNARY_ASSERT(DT_REQUIRE_UNARY, __VA_ARGS__) +#define DOCTEST_WARN_UNARY_FALSE(...) DOCTEST_UNARY_ASSERT(DT_WARN_UNARY_FALSE, __VA_ARGS__) +#define DOCTEST_CHECK_UNARY_FALSE(...) DOCTEST_UNARY_ASSERT(DT_CHECK_UNARY_FALSE, __VA_ARGS__) +#define DOCTEST_REQUIRE_UNARY_FALSE(...) DOCTEST_UNARY_ASSERT(DT_REQUIRE_UNARY_FALSE, __VA_ARGS__) + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + +#define DOCTEST_ASSERT_THROWS_AS(expr, assert_type, message, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + if(!doctest::getContextOptions()->no_throw) { \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, #expr, #__VA_ARGS__, message); \ + try { \ + DOCTEST_CAST_TO_VOID(expr) \ + } catch(const typename doctest::detail::types::remove_const< \ + typename doctest::detail::types::remove_reference<__VA_ARGS__>::type>::type&) {\ + DOCTEST_RB.translateException(); \ + DOCTEST_RB.m_threw_as = true; \ + } catch(...) { DOCTEST_RB.translateException(); } \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB); \ + } else { /* NOLINT(*-else-after-return) */ \ + DOCTEST_FUNC_SCOPE_RET(false); \ + } \ + } DOCTEST_FUNC_SCOPE_END + +#define DOCTEST_ASSERT_THROWS_WITH(expr, expr_str, assert_type, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + if(!doctest::getContextOptions()->no_throw) { \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, expr_str, "", __VA_ARGS__); \ + try { \ + DOCTEST_CAST_TO_VOID(expr) \ + } catch(...) { DOCTEST_RB.translateException(); } \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB); \ + } else { /* NOLINT(*-else-after-return) */ \ + DOCTEST_FUNC_SCOPE_RET(false); \ + } \ + } DOCTEST_FUNC_SCOPE_END + +#define DOCTEST_ASSERT_NOTHROW(assert_type, ...) \ + DOCTEST_FUNC_SCOPE_BEGIN { \ + doctest::detail::ResultBuilder DOCTEST_RB(doctest::assertType::assert_type, __FILE__, \ + __LINE__, #__VA_ARGS__); \ + try { \ + DOCTEST_CAST_TO_VOID(__VA_ARGS__) \ + } catch(...) { DOCTEST_RB.translateException(); } \ + DOCTEST_ASSERT_LOG_REACT_RETURN(DOCTEST_RB); \ + } DOCTEST_FUNC_SCOPE_END + +// clang-format off +#define DOCTEST_WARN_THROWS(...) DOCTEST_ASSERT_THROWS_WITH((__VA_ARGS__), #__VA_ARGS__, DT_WARN_THROWS, "") +#define DOCTEST_CHECK_THROWS(...) DOCTEST_ASSERT_THROWS_WITH((__VA_ARGS__), #__VA_ARGS__, DT_CHECK_THROWS, "") +#define DOCTEST_REQUIRE_THROWS(...) DOCTEST_ASSERT_THROWS_WITH((__VA_ARGS__), #__VA_ARGS__, DT_REQUIRE_THROWS, "") + +#define DOCTEST_WARN_THROWS_AS(expr, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_WARN_THROWS_AS, "", __VA_ARGS__) +#define DOCTEST_CHECK_THROWS_AS(expr, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_CHECK_THROWS_AS, "", __VA_ARGS__) +#define DOCTEST_REQUIRE_THROWS_AS(expr, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_REQUIRE_THROWS_AS, "", __VA_ARGS__) + +#define DOCTEST_WARN_THROWS_WITH(expr, ...) DOCTEST_ASSERT_THROWS_WITH(expr, #expr, DT_WARN_THROWS_WITH, __VA_ARGS__) +#define DOCTEST_CHECK_THROWS_WITH(expr, ...) DOCTEST_ASSERT_THROWS_WITH(expr, #expr, DT_CHECK_THROWS_WITH, __VA_ARGS__) +#define DOCTEST_REQUIRE_THROWS_WITH(expr, ...) DOCTEST_ASSERT_THROWS_WITH(expr, #expr, DT_REQUIRE_THROWS_WITH, __VA_ARGS__) + +#define DOCTEST_WARN_THROWS_WITH_AS(expr, message, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_WARN_THROWS_WITH_AS, message, __VA_ARGS__) +#define DOCTEST_CHECK_THROWS_WITH_AS(expr, message, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_CHECK_THROWS_WITH_AS, message, __VA_ARGS__) +#define DOCTEST_REQUIRE_THROWS_WITH_AS(expr, message, ...) DOCTEST_ASSERT_THROWS_AS(expr, DT_REQUIRE_THROWS_WITH_AS, message, __VA_ARGS__) + +#define DOCTEST_WARN_NOTHROW(...) DOCTEST_ASSERT_NOTHROW(DT_WARN_NOTHROW, __VA_ARGS__) +#define DOCTEST_CHECK_NOTHROW(...) DOCTEST_ASSERT_NOTHROW(DT_CHECK_NOTHROW, __VA_ARGS__) +#define DOCTEST_REQUIRE_NOTHROW(...) DOCTEST_ASSERT_NOTHROW(DT_REQUIRE_NOTHROW, __VA_ARGS__) + +#define DOCTEST_WARN_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_WARN_THROWS(expr); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_CHECK_THROWS(expr); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_REQUIRE_THROWS(expr); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_WARN_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_WARN_THROWS_AS(expr, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_CHECK_THROWS_AS(expr, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_REQUIRE_THROWS_AS(expr, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_WARN_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_WARN_THROWS_WITH(expr, with); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_CHECK_THROWS_WITH(expr, with); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_REQUIRE_THROWS_WITH(expr, with); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_WARN_THROWS_WITH_AS(expr, with, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_CHECK_THROWS_WITH_AS(expr, with, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_REQUIRE_THROWS_WITH_AS(expr, with, ex); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_WARN_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_WARN_NOTHROW(expr); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_CHECK_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_CHECK_NOTHROW(expr); } DOCTEST_FUNC_SCOPE_END +#define DOCTEST_REQUIRE_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_SCOPE_BEGIN { DOCTEST_INFO(__VA_ARGS__); DOCTEST_REQUIRE_NOTHROW(expr); } DOCTEST_FUNC_SCOPE_END +// clang-format on + +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + +// ================================================================================================= +// == WHAT FOLLOWS IS VERSIONS OF THE MACROS THAT DO NOT DO ANY REGISTERING! == +// == THIS CAN BE ENABLED BY DEFINING DOCTEST_CONFIG_DISABLE GLOBALLY! == +// ================================================================================================= +#else // DOCTEST_CONFIG_DISABLE + +#define DOCTEST_IMPLEMENT_FIXTURE(der, base, func, name) \ + namespace /* NOLINT */ { \ + template \ + struct der : public base \ + { void f(); }; \ + } \ + template \ + inline void der::f() + +#define DOCTEST_CREATE_AND_REGISTER_FUNCTION(f, name) \ + template \ + static inline void f() + +// for registering tests +#define DOCTEST_TEST_CASE(name) \ + DOCTEST_CREATE_AND_REGISTER_FUNCTION(DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), name) + +// for registering tests in classes +#define DOCTEST_TEST_CASE_CLASS(name) \ + DOCTEST_CREATE_AND_REGISTER_FUNCTION(DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), name) + +// for registering tests with a fixture +#define DOCTEST_TEST_CASE_FIXTURE(x, name) \ + DOCTEST_IMPLEMENT_FIXTURE(DOCTEST_ANONYMOUS(DOCTEST_ANON_CLASS_), x, \ + DOCTEST_ANONYMOUS(DOCTEST_ANON_FUNC_), name) + +// for converting types to strings without the header and demangling +#define DOCTEST_TYPE_TO_STRING_AS(str, ...) static_assert(true, "") +#define DOCTEST_TYPE_TO_STRING(...) static_assert(true, "") + +// for typed tests +#define DOCTEST_TEST_CASE_TEMPLATE(name, type, ...) \ + template \ + inline void DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_)() + +#define DOCTEST_TEST_CASE_TEMPLATE_DEFINE(name, type, id) \ + template \ + inline void DOCTEST_ANONYMOUS(DOCTEST_ANON_TMP_)() + +#define DOCTEST_TEST_CASE_TEMPLATE_INVOKE(id, ...) static_assert(true, "") +#define DOCTEST_TEST_CASE_TEMPLATE_APPLY(id, ...) static_assert(true, "") + +// for subcases +#define DOCTEST_SUBCASE(name) + +// for a testsuite block +#define DOCTEST_TEST_SUITE(name) namespace // NOLINT + +// for starting a testsuite block +#define DOCTEST_TEST_SUITE_BEGIN(name) static_assert(true, "") + +// for ending a testsuite block +#define DOCTEST_TEST_SUITE_END using DOCTEST_ANONYMOUS(DOCTEST_ANON_FOR_SEMICOLON_) = int + +#define DOCTEST_REGISTER_EXCEPTION_TRANSLATOR(signature) \ + template \ + static inline doctest::String DOCTEST_ANONYMOUS(DOCTEST_ANON_TRANSLATOR_)(signature) + +#define DOCTEST_REGISTER_REPORTER(name, priority, reporter) +#define DOCTEST_REGISTER_LISTENER(name, priority, reporter) + +#define DOCTEST_INFO(...) (static_cast(0)) +#define DOCTEST_CAPTURE(x) (static_cast(0)) +#define DOCTEST_ADD_MESSAGE_AT(file, line, ...) (static_cast(0)) +#define DOCTEST_ADD_FAIL_CHECK_AT(file, line, ...) (static_cast(0)) +#define DOCTEST_ADD_FAIL_AT(file, line, ...) (static_cast(0)) +#define DOCTEST_MESSAGE(...) (static_cast(0)) +#define DOCTEST_FAIL_CHECK(...) (static_cast(0)) +#define DOCTEST_FAIL(...) (static_cast(0)) + +#if defined(DOCTEST_CONFIG_EVALUATE_ASSERTS_EVEN_WHEN_DISABLED) \ + && defined(DOCTEST_CONFIG_ASSERTS_RETURN_VALUES) + +#define DOCTEST_WARN(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_CHECK(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_REQUIRE(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_WARN_FALSE(...) [&] { return !(__VA_ARGS__); }() +#define DOCTEST_CHECK_FALSE(...) [&] { return !(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_FALSE(...) [&] { return !(__VA_ARGS__); }() + +#define DOCTEST_WARN_MESSAGE(cond, ...) [&] { return cond; }() +#define DOCTEST_CHECK_MESSAGE(cond, ...) [&] { return cond; }() +#define DOCTEST_REQUIRE_MESSAGE(cond, ...) [&] { return cond; }() +#define DOCTEST_WARN_FALSE_MESSAGE(cond, ...) [&] { return !(cond); }() +#define DOCTEST_CHECK_FALSE_MESSAGE(cond, ...) [&] { return !(cond); }() +#define DOCTEST_REQUIRE_FALSE_MESSAGE(cond, ...) [&] { return !(cond); }() + +namespace doctest { +namespace detail { +#define DOCTEST_RELATIONAL_OP(name, op) \ + template \ + bool name(const DOCTEST_REF_WRAP(L) lhs, const DOCTEST_REF_WRAP(R) rhs) { return lhs op rhs; } + + DOCTEST_RELATIONAL_OP(eq, ==) + DOCTEST_RELATIONAL_OP(ne, !=) + DOCTEST_RELATIONAL_OP(lt, <) + DOCTEST_RELATIONAL_OP(gt, >) + DOCTEST_RELATIONAL_OP(le, <=) + DOCTEST_RELATIONAL_OP(ge, >=) +} // namespace detail +} // namespace doctest + +#define DOCTEST_WARN_EQ(...) [&] { return doctest::detail::eq(__VA_ARGS__); }() +#define DOCTEST_CHECK_EQ(...) [&] { return doctest::detail::eq(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_EQ(...) [&] { return doctest::detail::eq(__VA_ARGS__); }() +#define DOCTEST_WARN_NE(...) [&] { return doctest::detail::ne(__VA_ARGS__); }() +#define DOCTEST_CHECK_NE(...) [&] { return doctest::detail::ne(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_NE(...) [&] { return doctest::detail::ne(__VA_ARGS__); }() +#define DOCTEST_WARN_LT(...) [&] { return doctest::detail::lt(__VA_ARGS__); }() +#define DOCTEST_CHECK_LT(...) [&] { return doctest::detail::lt(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_LT(...) [&] { return doctest::detail::lt(__VA_ARGS__); }() +#define DOCTEST_WARN_GT(...) [&] { return doctest::detail::gt(__VA_ARGS__); }() +#define DOCTEST_CHECK_GT(...) [&] { return doctest::detail::gt(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_GT(...) [&] { return doctest::detail::gt(__VA_ARGS__); }() +#define DOCTEST_WARN_LE(...) [&] { return doctest::detail::le(__VA_ARGS__); }() +#define DOCTEST_CHECK_LE(...) [&] { return doctest::detail::le(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_LE(...) [&] { return doctest::detail::le(__VA_ARGS__); }() +#define DOCTEST_WARN_GE(...) [&] { return doctest::detail::ge(__VA_ARGS__); }() +#define DOCTEST_CHECK_GE(...) [&] { return doctest::detail::ge(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_GE(...) [&] { return doctest::detail::ge(__VA_ARGS__); }() +#define DOCTEST_WARN_UNARY(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_CHECK_UNARY(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_REQUIRE_UNARY(...) [&] { return __VA_ARGS__; }() +#define DOCTEST_WARN_UNARY_FALSE(...) [&] { return !(__VA_ARGS__); }() +#define DOCTEST_CHECK_UNARY_FALSE(...) [&] { return !(__VA_ARGS__); }() +#define DOCTEST_REQUIRE_UNARY_FALSE(...) [&] { return !(__VA_ARGS__); }() + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + +#define DOCTEST_WARN_THROWS_WITH(expr, with, ...) [] { static_assert(false, "Exception translation is not available when doctest is disabled."); return false; }() +#define DOCTEST_CHECK_THROWS_WITH(expr, with, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_REQUIRE_THROWS_WITH(expr, with, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_WARN_THROWS_WITH_AS(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_CHECK_THROWS_WITH_AS(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_REQUIRE_THROWS_WITH_AS(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) + +#define DOCTEST_WARN_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_CHECK_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_REQUIRE_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) +#define DOCTEST_REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH(,,) + +#define DOCTEST_WARN_THROWS(...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_CHECK_THROWS(...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_REQUIRE_THROWS(...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_WARN_THROWS_AS(expr, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_CHECK_THROWS_AS(expr, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_REQUIRE_THROWS_AS(expr, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_WARN_NOTHROW(...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() +#define DOCTEST_CHECK_NOTHROW(...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() +#define DOCTEST_REQUIRE_NOTHROW(...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() + +#define DOCTEST_WARN_THROWS_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_CHECK_THROWS_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_REQUIRE_THROWS_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return false; } catch (...) { return true; } }() +#define DOCTEST_WARN_THROWS_AS_MESSAGE(expr, ex, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_CHECK_THROWS_AS_MESSAGE(expr, ex, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_REQUIRE_THROWS_AS_MESSAGE(expr, ex, ...) [&] { try { expr; } catch (__VA_ARGS__) { return true; } catch (...) { } return false; }() +#define DOCTEST_WARN_NOTHROW_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() +#define DOCTEST_CHECK_NOTHROW_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() +#define DOCTEST_REQUIRE_NOTHROW_MESSAGE(expr, ...) [&] { try { __VA_ARGS__; return true; } catch (...) { return false; } }() + +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + +#else // DOCTEST_CONFIG_EVALUATE_ASSERTS_EVEN_WHEN_DISABLED + +#define DOCTEST_WARN(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_FALSE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_FALSE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_FALSE(...) DOCTEST_FUNC_EMPTY + +#define DOCTEST_WARN_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_FALSE_MESSAGE(cond, ...) DOCTEST_FUNC_EMPTY + +#define DOCTEST_WARN_EQ(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_EQ(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_EQ(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_NE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_NE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_NE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_GT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_GT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_GT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_LT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_LT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_LT(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_GE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_GE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_GE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_LE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_LE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_LE(...) DOCTEST_FUNC_EMPTY + +#define DOCTEST_WARN_UNARY(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_UNARY(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_UNARY(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_UNARY_FALSE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_UNARY_FALSE(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_UNARY_FALSE(...) DOCTEST_FUNC_EMPTY + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + +#define DOCTEST_WARN_THROWS(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_AS(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_AS(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_AS(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_WITH(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_WITH(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_WITH(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_WITH_AS(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_WITH_AS(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_WITH_AS(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_NOTHROW(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_NOTHROW(...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_NOTHROW(...) DOCTEST_FUNC_EMPTY + +#define DOCTEST_WARN_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_WARN_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_CHECK_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY +#define DOCTEST_REQUIRE_NOTHROW_MESSAGE(expr, ...) DOCTEST_FUNC_EMPTY + +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + +#endif // DOCTEST_CONFIG_EVALUATE_ASSERTS_EVEN_WHEN_DISABLED + +#endif // DOCTEST_CONFIG_DISABLE + +#ifdef DOCTEST_CONFIG_NO_EXCEPTIONS + +#ifdef DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS +#define DOCTEST_EXCEPTION_EMPTY_FUNC DOCTEST_FUNC_EMPTY +#else // DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS +#define DOCTEST_EXCEPTION_EMPTY_FUNC [] { static_assert(false, "Exceptions are disabled! " \ + "Use DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS if you want to compile with exceptions disabled."); return false; }() + +#undef DOCTEST_REQUIRE +#undef DOCTEST_REQUIRE_FALSE +#undef DOCTEST_REQUIRE_MESSAGE +#undef DOCTEST_REQUIRE_FALSE_MESSAGE +#undef DOCTEST_REQUIRE_EQ +#undef DOCTEST_REQUIRE_NE +#undef DOCTEST_REQUIRE_GT +#undef DOCTEST_REQUIRE_LT +#undef DOCTEST_REQUIRE_GE +#undef DOCTEST_REQUIRE_LE +#undef DOCTEST_REQUIRE_UNARY +#undef DOCTEST_REQUIRE_UNARY_FALSE + +#define DOCTEST_REQUIRE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_FALSE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_MESSAGE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_FALSE_MESSAGE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_EQ DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_NE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_GT DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_LT DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_GE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_LE DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_UNARY DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_UNARY_FALSE DOCTEST_EXCEPTION_EMPTY_FUNC + +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS_BUT_WITH_ALL_ASSERTS + +#define DOCTEST_WARN_THROWS(...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS(...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS(...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_AS(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_AS(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_AS(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_WITH(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_WITH(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_WITH(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_WITH_AS(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_WITH_AS(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_WITH_AS(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_NOTHROW(...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_NOTHROW(...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_NOTHROW(...) DOCTEST_EXCEPTION_EMPTY_FUNC + +#define DOCTEST_WARN_THROWS_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_WARN_NOTHROW_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_CHECK_NOTHROW_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC +#define DOCTEST_REQUIRE_NOTHROW_MESSAGE(expr, ...) DOCTEST_EXCEPTION_EMPTY_FUNC + +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + +// clang-format off +// KEPT FOR BACKWARDS COMPATIBILITY - FORWARDING TO THE RIGHT MACROS +#define DOCTEST_FAST_WARN_EQ DOCTEST_WARN_EQ +#define DOCTEST_FAST_CHECK_EQ DOCTEST_CHECK_EQ +#define DOCTEST_FAST_REQUIRE_EQ DOCTEST_REQUIRE_EQ +#define DOCTEST_FAST_WARN_NE DOCTEST_WARN_NE +#define DOCTEST_FAST_CHECK_NE DOCTEST_CHECK_NE +#define DOCTEST_FAST_REQUIRE_NE DOCTEST_REQUIRE_NE +#define DOCTEST_FAST_WARN_GT DOCTEST_WARN_GT +#define DOCTEST_FAST_CHECK_GT DOCTEST_CHECK_GT +#define DOCTEST_FAST_REQUIRE_GT DOCTEST_REQUIRE_GT +#define DOCTEST_FAST_WARN_LT DOCTEST_WARN_LT +#define DOCTEST_FAST_CHECK_LT DOCTEST_CHECK_LT +#define DOCTEST_FAST_REQUIRE_LT DOCTEST_REQUIRE_LT +#define DOCTEST_FAST_WARN_GE DOCTEST_WARN_GE +#define DOCTEST_FAST_CHECK_GE DOCTEST_CHECK_GE +#define DOCTEST_FAST_REQUIRE_GE DOCTEST_REQUIRE_GE +#define DOCTEST_FAST_WARN_LE DOCTEST_WARN_LE +#define DOCTEST_FAST_CHECK_LE DOCTEST_CHECK_LE +#define DOCTEST_FAST_REQUIRE_LE DOCTEST_REQUIRE_LE + +#define DOCTEST_FAST_WARN_UNARY DOCTEST_WARN_UNARY +#define DOCTEST_FAST_CHECK_UNARY DOCTEST_CHECK_UNARY +#define DOCTEST_FAST_REQUIRE_UNARY DOCTEST_REQUIRE_UNARY +#define DOCTEST_FAST_WARN_UNARY_FALSE DOCTEST_WARN_UNARY_FALSE +#define DOCTEST_FAST_CHECK_UNARY_FALSE DOCTEST_CHECK_UNARY_FALSE +#define DOCTEST_FAST_REQUIRE_UNARY_FALSE DOCTEST_REQUIRE_UNARY_FALSE + +#define DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE(id, ...) DOCTEST_TEST_CASE_TEMPLATE_INVOKE(id,__VA_ARGS__) +// clang-format on + +// BDD style macros +// clang-format off +#define DOCTEST_SCENARIO(name) DOCTEST_TEST_CASE(" Scenario: " name) +#define DOCTEST_SCENARIO_CLASS(name) DOCTEST_TEST_CASE_CLASS(" Scenario: " name) +#define DOCTEST_SCENARIO_TEMPLATE(name, T, ...) DOCTEST_TEST_CASE_TEMPLATE(" Scenario: " name, T, __VA_ARGS__) +#define DOCTEST_SCENARIO_TEMPLATE_DEFINE(name, T, id) DOCTEST_TEST_CASE_TEMPLATE_DEFINE(" Scenario: " name, T, id) + +#define DOCTEST_GIVEN(name) DOCTEST_SUBCASE(" Given: " name) +#define DOCTEST_WHEN(name) DOCTEST_SUBCASE(" When: " name) +#define DOCTEST_AND_WHEN(name) DOCTEST_SUBCASE("And when: " name) +#define DOCTEST_THEN(name) DOCTEST_SUBCASE(" Then: " name) +#define DOCTEST_AND_THEN(name) DOCTEST_SUBCASE(" And: " name) +// clang-format on + +// == SHORT VERSIONS OF THE MACROS +#ifndef DOCTEST_CONFIG_NO_SHORT_MACRO_NAMES + +#define TEST_CASE(name) DOCTEST_TEST_CASE(name) +#define TEST_CASE_CLASS(name) DOCTEST_TEST_CASE_CLASS(name) +#define TEST_CASE_FIXTURE(x, name) DOCTEST_TEST_CASE_FIXTURE(x, name) +#define TYPE_TO_STRING_AS(str, ...) DOCTEST_TYPE_TO_STRING_AS(str, __VA_ARGS__) +#define TYPE_TO_STRING(...) DOCTEST_TYPE_TO_STRING(__VA_ARGS__) +#define TEST_CASE_TEMPLATE(name, T, ...) DOCTEST_TEST_CASE_TEMPLATE(name, T, __VA_ARGS__) +#define TEST_CASE_TEMPLATE_DEFINE(name, T, id) DOCTEST_TEST_CASE_TEMPLATE_DEFINE(name, T, id) +#define TEST_CASE_TEMPLATE_INVOKE(id, ...) DOCTEST_TEST_CASE_TEMPLATE_INVOKE(id, __VA_ARGS__) +#define TEST_CASE_TEMPLATE_APPLY(id, ...) DOCTEST_TEST_CASE_TEMPLATE_APPLY(id, __VA_ARGS__) +#define SUBCASE(name) DOCTEST_SUBCASE(name) +#define TEST_SUITE(decorators) DOCTEST_TEST_SUITE(decorators) +#define TEST_SUITE_BEGIN(name) DOCTEST_TEST_SUITE_BEGIN(name) +#define TEST_SUITE_END DOCTEST_TEST_SUITE_END +#define REGISTER_EXCEPTION_TRANSLATOR(signature) DOCTEST_REGISTER_EXCEPTION_TRANSLATOR(signature) +#define REGISTER_REPORTER(name, priority, reporter) DOCTEST_REGISTER_REPORTER(name, priority, reporter) +#define REGISTER_LISTENER(name, priority, reporter) DOCTEST_REGISTER_LISTENER(name, priority, reporter) +#define INFO(...) DOCTEST_INFO(__VA_ARGS__) +#define CAPTURE(x) DOCTEST_CAPTURE(x) +#define ADD_MESSAGE_AT(file, line, ...) DOCTEST_ADD_MESSAGE_AT(file, line, __VA_ARGS__) +#define ADD_FAIL_CHECK_AT(file, line, ...) DOCTEST_ADD_FAIL_CHECK_AT(file, line, __VA_ARGS__) +#define ADD_FAIL_AT(file, line, ...) DOCTEST_ADD_FAIL_AT(file, line, __VA_ARGS__) +#define MESSAGE(...) DOCTEST_MESSAGE(__VA_ARGS__) +#define FAIL_CHECK(...) DOCTEST_FAIL_CHECK(__VA_ARGS__) +#define FAIL(...) DOCTEST_FAIL(__VA_ARGS__) +#define TO_LVALUE(...) DOCTEST_TO_LVALUE(__VA_ARGS__) + +#define WARN(...) DOCTEST_WARN(__VA_ARGS__) +#define WARN_FALSE(...) DOCTEST_WARN_FALSE(__VA_ARGS__) +#define WARN_THROWS(...) DOCTEST_WARN_THROWS(__VA_ARGS__) +#define WARN_THROWS_AS(expr, ...) DOCTEST_WARN_THROWS_AS(expr, __VA_ARGS__) +#define WARN_THROWS_WITH(expr, ...) DOCTEST_WARN_THROWS_WITH(expr, __VA_ARGS__) +#define WARN_THROWS_WITH_AS(expr, with, ...) DOCTEST_WARN_THROWS_WITH_AS(expr, with, __VA_ARGS__) +#define WARN_NOTHROW(...) DOCTEST_WARN_NOTHROW(__VA_ARGS__) +#define CHECK(...) DOCTEST_CHECK(__VA_ARGS__) +#define CHECK_FALSE(...) DOCTEST_CHECK_FALSE(__VA_ARGS__) +#define CHECK_THROWS(...) DOCTEST_CHECK_THROWS(__VA_ARGS__) +#define CHECK_THROWS_AS(expr, ...) DOCTEST_CHECK_THROWS_AS(expr, __VA_ARGS__) +#define CHECK_THROWS_WITH(expr, ...) DOCTEST_CHECK_THROWS_WITH(expr, __VA_ARGS__) +#define CHECK_THROWS_WITH_AS(expr, with, ...) DOCTEST_CHECK_THROWS_WITH_AS(expr, with, __VA_ARGS__) +#define CHECK_NOTHROW(...) DOCTEST_CHECK_NOTHROW(__VA_ARGS__) +#define REQUIRE(...) DOCTEST_REQUIRE(__VA_ARGS__) +#define REQUIRE_FALSE(...) DOCTEST_REQUIRE_FALSE(__VA_ARGS__) +#define REQUIRE_THROWS(...) DOCTEST_REQUIRE_THROWS(__VA_ARGS__) +#define REQUIRE_THROWS_AS(expr, ...) DOCTEST_REQUIRE_THROWS_AS(expr, __VA_ARGS__) +#define REQUIRE_THROWS_WITH(expr, ...) DOCTEST_REQUIRE_THROWS_WITH(expr, __VA_ARGS__) +#define REQUIRE_THROWS_WITH_AS(expr, with, ...) DOCTEST_REQUIRE_THROWS_WITH_AS(expr, with, __VA_ARGS__) +#define REQUIRE_NOTHROW(...) DOCTEST_REQUIRE_NOTHROW(__VA_ARGS__) + +#define WARN_MESSAGE(cond, ...) DOCTEST_WARN_MESSAGE(cond, __VA_ARGS__) +#define WARN_FALSE_MESSAGE(cond, ...) DOCTEST_WARN_FALSE_MESSAGE(cond, __VA_ARGS__) +#define WARN_THROWS_MESSAGE(expr, ...) DOCTEST_WARN_THROWS_MESSAGE(expr, __VA_ARGS__) +#define WARN_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_WARN_THROWS_AS_MESSAGE(expr, ex, __VA_ARGS__) +#define WARN_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_WARN_THROWS_WITH_MESSAGE(expr, with, __VA_ARGS__) +#define WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_WARN_THROWS_WITH_AS_MESSAGE(expr, with, ex, __VA_ARGS__) +#define WARN_NOTHROW_MESSAGE(expr, ...) DOCTEST_WARN_NOTHROW_MESSAGE(expr, __VA_ARGS__) +#define CHECK_MESSAGE(cond, ...) DOCTEST_CHECK_MESSAGE(cond, __VA_ARGS__) +#define CHECK_FALSE_MESSAGE(cond, ...) DOCTEST_CHECK_FALSE_MESSAGE(cond, __VA_ARGS__) +#define CHECK_THROWS_MESSAGE(expr, ...) DOCTEST_CHECK_THROWS_MESSAGE(expr, __VA_ARGS__) +#define CHECK_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_CHECK_THROWS_AS_MESSAGE(expr, ex, __VA_ARGS__) +#define CHECK_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_CHECK_THROWS_WITH_MESSAGE(expr, with, __VA_ARGS__) +#define CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_CHECK_THROWS_WITH_AS_MESSAGE(expr, with, ex, __VA_ARGS__) +#define CHECK_NOTHROW_MESSAGE(expr, ...) DOCTEST_CHECK_NOTHROW_MESSAGE(expr, __VA_ARGS__) +#define REQUIRE_MESSAGE(cond, ...) DOCTEST_REQUIRE_MESSAGE(cond, __VA_ARGS__) +#define REQUIRE_FALSE_MESSAGE(cond, ...) DOCTEST_REQUIRE_FALSE_MESSAGE(cond, __VA_ARGS__) +#define REQUIRE_THROWS_MESSAGE(expr, ...) DOCTEST_REQUIRE_THROWS_MESSAGE(expr, __VA_ARGS__) +#define REQUIRE_THROWS_AS_MESSAGE(expr, ex, ...) DOCTEST_REQUIRE_THROWS_AS_MESSAGE(expr, ex, __VA_ARGS__) +#define REQUIRE_THROWS_WITH_MESSAGE(expr, with, ...) DOCTEST_REQUIRE_THROWS_WITH_MESSAGE(expr, with, __VA_ARGS__) +#define REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, ...) DOCTEST_REQUIRE_THROWS_WITH_AS_MESSAGE(expr, with, ex, __VA_ARGS__) +#define REQUIRE_NOTHROW_MESSAGE(expr, ...) DOCTEST_REQUIRE_NOTHROW_MESSAGE(expr, __VA_ARGS__) + +#define SCENARIO(name) DOCTEST_SCENARIO(name) +#define SCENARIO_CLASS(name) DOCTEST_SCENARIO_CLASS(name) +#define SCENARIO_TEMPLATE(name, T, ...) DOCTEST_SCENARIO_TEMPLATE(name, T, __VA_ARGS__) +#define SCENARIO_TEMPLATE_DEFINE(name, T, id) DOCTEST_SCENARIO_TEMPLATE_DEFINE(name, T, id) +#define GIVEN(name) DOCTEST_GIVEN(name) +#define WHEN(name) DOCTEST_WHEN(name) +#define AND_WHEN(name) DOCTEST_AND_WHEN(name) +#define THEN(name) DOCTEST_THEN(name) +#define AND_THEN(name) DOCTEST_AND_THEN(name) + +#define WARN_EQ(...) DOCTEST_WARN_EQ(__VA_ARGS__) +#define CHECK_EQ(...) DOCTEST_CHECK_EQ(__VA_ARGS__) +#define REQUIRE_EQ(...) DOCTEST_REQUIRE_EQ(__VA_ARGS__) +#define WARN_NE(...) DOCTEST_WARN_NE(__VA_ARGS__) +#define CHECK_NE(...) DOCTEST_CHECK_NE(__VA_ARGS__) +#define REQUIRE_NE(...) DOCTEST_REQUIRE_NE(__VA_ARGS__) +#define WARN_GT(...) DOCTEST_WARN_GT(__VA_ARGS__) +#define CHECK_GT(...) DOCTEST_CHECK_GT(__VA_ARGS__) +#define REQUIRE_GT(...) DOCTEST_REQUIRE_GT(__VA_ARGS__) +#define WARN_LT(...) DOCTEST_WARN_LT(__VA_ARGS__) +#define CHECK_LT(...) DOCTEST_CHECK_LT(__VA_ARGS__) +#define REQUIRE_LT(...) DOCTEST_REQUIRE_LT(__VA_ARGS__) +#define WARN_GE(...) DOCTEST_WARN_GE(__VA_ARGS__) +#define CHECK_GE(...) DOCTEST_CHECK_GE(__VA_ARGS__) +#define REQUIRE_GE(...) DOCTEST_REQUIRE_GE(__VA_ARGS__) +#define WARN_LE(...) DOCTEST_WARN_LE(__VA_ARGS__) +#define CHECK_LE(...) DOCTEST_CHECK_LE(__VA_ARGS__) +#define REQUIRE_LE(...) DOCTEST_REQUIRE_LE(__VA_ARGS__) +#define WARN_UNARY(...) DOCTEST_WARN_UNARY(__VA_ARGS__) +#define CHECK_UNARY(...) DOCTEST_CHECK_UNARY(__VA_ARGS__) +#define REQUIRE_UNARY(...) DOCTEST_REQUIRE_UNARY(__VA_ARGS__) +#define WARN_UNARY_FALSE(...) DOCTEST_WARN_UNARY_FALSE(__VA_ARGS__) +#define CHECK_UNARY_FALSE(...) DOCTEST_CHECK_UNARY_FALSE(__VA_ARGS__) +#define REQUIRE_UNARY_FALSE(...) DOCTEST_REQUIRE_UNARY_FALSE(__VA_ARGS__) + +// KEPT FOR BACKWARDS COMPATIBILITY +#define FAST_WARN_EQ(...) DOCTEST_FAST_WARN_EQ(__VA_ARGS__) +#define FAST_CHECK_EQ(...) DOCTEST_FAST_CHECK_EQ(__VA_ARGS__) +#define FAST_REQUIRE_EQ(...) DOCTEST_FAST_REQUIRE_EQ(__VA_ARGS__) +#define FAST_WARN_NE(...) DOCTEST_FAST_WARN_NE(__VA_ARGS__) +#define FAST_CHECK_NE(...) DOCTEST_FAST_CHECK_NE(__VA_ARGS__) +#define FAST_REQUIRE_NE(...) DOCTEST_FAST_REQUIRE_NE(__VA_ARGS__) +#define FAST_WARN_GT(...) DOCTEST_FAST_WARN_GT(__VA_ARGS__) +#define FAST_CHECK_GT(...) DOCTEST_FAST_CHECK_GT(__VA_ARGS__) +#define FAST_REQUIRE_GT(...) DOCTEST_FAST_REQUIRE_GT(__VA_ARGS__) +#define FAST_WARN_LT(...) DOCTEST_FAST_WARN_LT(__VA_ARGS__) +#define FAST_CHECK_LT(...) DOCTEST_FAST_CHECK_LT(__VA_ARGS__) +#define FAST_REQUIRE_LT(...) DOCTEST_FAST_REQUIRE_LT(__VA_ARGS__) +#define FAST_WARN_GE(...) DOCTEST_FAST_WARN_GE(__VA_ARGS__) +#define FAST_CHECK_GE(...) DOCTEST_FAST_CHECK_GE(__VA_ARGS__) +#define FAST_REQUIRE_GE(...) DOCTEST_FAST_REQUIRE_GE(__VA_ARGS__) +#define FAST_WARN_LE(...) DOCTEST_FAST_WARN_LE(__VA_ARGS__) +#define FAST_CHECK_LE(...) DOCTEST_FAST_CHECK_LE(__VA_ARGS__) +#define FAST_REQUIRE_LE(...) DOCTEST_FAST_REQUIRE_LE(__VA_ARGS__) + +#define FAST_WARN_UNARY(...) DOCTEST_FAST_WARN_UNARY(__VA_ARGS__) +#define FAST_CHECK_UNARY(...) DOCTEST_FAST_CHECK_UNARY(__VA_ARGS__) +#define FAST_REQUIRE_UNARY(...) DOCTEST_FAST_REQUIRE_UNARY(__VA_ARGS__) +#define FAST_WARN_UNARY_FALSE(...) DOCTEST_FAST_WARN_UNARY_FALSE(__VA_ARGS__) +#define FAST_CHECK_UNARY_FALSE(...) DOCTEST_FAST_CHECK_UNARY_FALSE(__VA_ARGS__) +#define FAST_REQUIRE_UNARY_FALSE(...) DOCTEST_FAST_REQUIRE_UNARY_FALSE(__VA_ARGS__) + +#define TEST_CASE_TEMPLATE_INSTANTIATE(id, ...) DOCTEST_TEST_CASE_TEMPLATE_INSTANTIATE(id, __VA_ARGS__) + +#endif // DOCTEST_CONFIG_NO_SHORT_MACRO_NAMES + +#ifndef DOCTEST_CONFIG_DISABLE + +// this is here to clear the 'current test suite' for the current translation unit - at the top +DOCTEST_TEST_SUITE_END(); + +#endif // DOCTEST_CONFIG_DISABLE + +DOCTEST_CLANG_SUPPRESS_WARNING_POP +DOCTEST_MSVC_SUPPRESS_WARNING_POP +DOCTEST_GCC_SUPPRESS_WARNING_POP + +DOCTEST_SUPPRESS_COMMON_WARNINGS_POP + +#endif // DOCTEST_LIBRARY_INCLUDED + +#ifndef DOCTEST_SINGLE_HEADER +#define DOCTEST_SINGLE_HEADER +#endif // DOCTEST_SINGLE_HEADER + +#if defined(DOCTEST_CONFIG_IMPLEMENT) || !defined(DOCTEST_SINGLE_HEADER) + +#ifndef DOCTEST_SINGLE_HEADER +#include "doctest_fwd.h" +#endif // DOCTEST_SINGLE_HEADER + +DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wunused-macros") + +#ifndef DOCTEST_LIBRARY_IMPLEMENTATION +#define DOCTEST_LIBRARY_IMPLEMENTATION + +DOCTEST_CLANG_SUPPRESS_WARNING_POP + +DOCTEST_SUPPRESS_COMMON_WARNINGS_PUSH + +DOCTEST_CLANG_SUPPRESS_WARNING_PUSH +DOCTEST_CLANG_SUPPRESS_WARNING("-Wglobal-constructors") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wexit-time-destructors") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wsign-conversion") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wshorten-64-to-32") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wmissing-variable-declarations") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wswitch") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wswitch-enum") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wcovered-switch-default") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wmissing-noreturn") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wdisabled-macro-expansion") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wmissing-braces") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wmissing-field-initializers") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wunused-member-function") +DOCTEST_CLANG_SUPPRESS_WARNING("-Wnonportable-system-include-path") + +DOCTEST_GCC_SUPPRESS_WARNING_PUSH +DOCTEST_GCC_SUPPRESS_WARNING("-Wconversion") +DOCTEST_GCC_SUPPRESS_WARNING("-Wsign-conversion") +DOCTEST_GCC_SUPPRESS_WARNING("-Wmissing-field-initializers") +DOCTEST_GCC_SUPPRESS_WARNING("-Wmissing-braces") +DOCTEST_GCC_SUPPRESS_WARNING("-Wswitch") +DOCTEST_GCC_SUPPRESS_WARNING("-Wswitch-enum") +DOCTEST_GCC_SUPPRESS_WARNING("-Wswitch-default") +DOCTEST_GCC_SUPPRESS_WARNING("-Wunsafe-loop-optimizations") +DOCTEST_GCC_SUPPRESS_WARNING("-Wold-style-cast") +DOCTEST_GCC_SUPPRESS_WARNING("-Wunused-function") +DOCTEST_GCC_SUPPRESS_WARNING("-Wmultiple-inheritance") +DOCTEST_GCC_SUPPRESS_WARNING("-Wsuggest-attribute") + +DOCTEST_MSVC_SUPPRESS_WARNING_PUSH +DOCTEST_MSVC_SUPPRESS_WARNING(4267) // 'var' : conversion from 'x' to 'y', possible loss of data +DOCTEST_MSVC_SUPPRESS_WARNING(4530) // C++ exception handler used, but unwind semantics not enabled +DOCTEST_MSVC_SUPPRESS_WARNING(4577) // 'noexcept' used with no exception handling mode specified +DOCTEST_MSVC_SUPPRESS_WARNING(4774) // format string expected in argument is not a string literal +DOCTEST_MSVC_SUPPRESS_WARNING(4365) // conversion from 'int' to 'unsigned', signed/unsigned mismatch +DOCTEST_MSVC_SUPPRESS_WARNING(5039) // pointer to potentially throwing function passed to extern C +DOCTEST_MSVC_SUPPRESS_WARNING(4800) // forcing value to bool 'true' or 'false' (performance warning) +DOCTEST_MSVC_SUPPRESS_WARNING(5245) // unreferenced function with internal linkage has been removed + +DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_BEGIN + +// required includes - will go only in one translation unit! +#include +#include +#include +// borland (Embarcadero) compiler requires math.h and not cmath - https://github.com/doctest/doctest/pull/37 +#ifdef __BORLANDC__ +#include +#endif // __BORLANDC__ +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM +#include +#endif // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM +#include +#include +#include +#ifndef DOCTEST_CONFIG_NO_MULTITHREADING +#include +#include +#define DOCTEST_DECLARE_MUTEX(name) std::mutex name; +#define DOCTEST_DECLARE_STATIC_MUTEX(name) static DOCTEST_DECLARE_MUTEX(name) +#define DOCTEST_LOCK_MUTEX(name) std::lock_guard DOCTEST_ANONYMOUS(DOCTEST_ANON_LOCK_)(name); +#else // DOCTEST_CONFIG_NO_MULTITHREADING +#define DOCTEST_DECLARE_MUTEX(name) +#define DOCTEST_DECLARE_STATIC_MUTEX(name) +#define DOCTEST_LOCK_MUTEX(name) +#endif // DOCTEST_CONFIG_NO_MULTITHREADING +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef DOCTEST_PLATFORM_MAC +#include +#include +#include +#endif // DOCTEST_PLATFORM_MAC + +#ifdef DOCTEST_PLATFORM_WINDOWS + +// defines for a leaner windows.h +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN +#define DOCTEST_UNDEF_WIN32_LEAN_AND_MEAN +#endif // WIN32_LEAN_AND_MEAN +#ifndef NOMINMAX +#define NOMINMAX +#define DOCTEST_UNDEF_NOMINMAX +#endif // NOMINMAX + +// not sure what AfxWin.h is for - here I do what Catch does +#ifdef __AFXDLL +#include +#else +#include +#endif +#include + +#else // DOCTEST_PLATFORM_WINDOWS + +#include +#include + +#endif // DOCTEST_PLATFORM_WINDOWS + +// this is a fix for https://github.com/doctest/doctest/issues/348 +// https://mail.gnome.org/archives/xml/2012-January/msg00000.html +#if !defined(HAVE_UNISTD_H) && !defined(STDOUT_FILENO) +#define STDOUT_FILENO fileno(stdout) +#endif // HAVE_UNISTD_H + +DOCTEST_MAKE_STD_HEADERS_CLEAN_FROM_WARNINGS_ON_WALL_END + +// counts the number of elements in a C array +#define DOCTEST_COUNTOF(x) (sizeof(x) / sizeof(x[0])) + +#ifdef DOCTEST_CONFIG_DISABLE +#define DOCTEST_BRANCH_ON_DISABLED(if_disabled, if_not_disabled) if_disabled +#else // DOCTEST_CONFIG_DISABLE +#define DOCTEST_BRANCH_ON_DISABLED(if_disabled, if_not_disabled) if_not_disabled +#endif // DOCTEST_CONFIG_DISABLE + +#ifndef DOCTEST_CONFIG_OPTIONS_PREFIX +#define DOCTEST_CONFIG_OPTIONS_PREFIX "dt-" +#endif + +#ifndef DOCTEST_THREAD_LOCAL +#if defined(DOCTEST_CONFIG_NO_MULTITHREADING) || DOCTEST_MSVC && (DOCTEST_MSVC < DOCTEST_COMPILER(19, 0, 0)) +#define DOCTEST_THREAD_LOCAL +#else // DOCTEST_MSVC +#define DOCTEST_THREAD_LOCAL thread_local +#endif // DOCTEST_MSVC +#endif // DOCTEST_THREAD_LOCAL + +#ifndef DOCTEST_MULTI_LANE_ATOMICS_THREAD_LANES +#define DOCTEST_MULTI_LANE_ATOMICS_THREAD_LANES 32 +#endif + +#ifndef DOCTEST_MULTI_LANE_ATOMICS_CACHE_LINE_SIZE +#define DOCTEST_MULTI_LANE_ATOMICS_CACHE_LINE_SIZE 64 +#endif + +#ifdef DOCTEST_CONFIG_NO_UNPREFIXED_OPTIONS +#define DOCTEST_OPTIONS_PREFIX_DISPLAY DOCTEST_CONFIG_OPTIONS_PREFIX +#else +#define DOCTEST_OPTIONS_PREFIX_DISPLAY "" +#endif + +#if defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_APP) +#define DOCTEST_CONFIG_NO_MULTI_LANE_ATOMICS +#endif + +#ifndef DOCTEST_CDECL +#define DOCTEST_CDECL __cdecl +#endif + +namespace doctest { + +bool is_running_in_test = false; + +namespace { + using namespace detail; + + template + DOCTEST_NORETURN void throw_exception(Ex const& e) { +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + throw e; +#else // DOCTEST_CONFIG_NO_EXCEPTIONS +#ifdef DOCTEST_CONFIG_HANDLE_EXCEPTION + DOCTEST_CONFIG_HANDLE_EXCEPTION(e); +#else // DOCTEST_CONFIG_HANDLE_EXCEPTION +#ifndef DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + std::cerr << "doctest will terminate because it needed to throw an exception.\n" + << "The message was: " << e.what() << '\n'; +#endif // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM +#endif // DOCTEST_CONFIG_HANDLE_EXCEPTION + std::terminate(); +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + } + +#ifndef DOCTEST_INTERNAL_ERROR +#define DOCTEST_INTERNAL_ERROR(msg) \ + throw_exception(std::logic_error( \ + __FILE__ ":" DOCTEST_TOSTR(__LINE__) ": Internal doctest error: " msg)) +#endif // DOCTEST_INTERNAL_ERROR + + // case insensitive strcmp + int stricmp(const char* a, const char* b) { + for(;; a++, b++) { + const int d = tolower(*a) - tolower(*b); + if(d != 0 || !*a) + return d; + } + } + + struct Endianness + { + enum Arch + { + Big, + Little + }; + + static Arch which() { + int x = 1; + // casting any data pointer to char* is allowed + auto ptr = reinterpret_cast(&x); + if(*ptr) + return Little; + return Big; + } + }; +} // namespace + +namespace detail { + DOCTEST_THREAD_LOCAL class + { + std::vector stack; + std::stringstream ss; + + public: + std::ostream* push() { + stack.push_back(ss.tellp()); + return &ss; + } + + String pop() { + if (stack.empty()) + DOCTEST_INTERNAL_ERROR("TLSS was empty when trying to pop!"); + + std::streampos pos = stack.back(); + stack.pop_back(); + unsigned sz = static_cast(ss.tellp() - pos); + ss.rdbuf()->pubseekpos(pos, std::ios::in | std::ios::out); + return String(ss, sz); + } + } g_oss; + + std::ostream* tlssPush() { + return g_oss.push(); + } + + String tlssPop() { + return g_oss.pop(); + } + +#ifndef DOCTEST_CONFIG_DISABLE + +namespace timer_large_integer +{ + +#if defined(DOCTEST_PLATFORM_WINDOWS) + using type = ULONGLONG; +#else // DOCTEST_PLATFORM_WINDOWS + using type = std::uint64_t; +#endif // DOCTEST_PLATFORM_WINDOWS +} + +using ticks_t = timer_large_integer::type; + +#ifdef DOCTEST_CONFIG_GETCURRENTTICKS + ticks_t getCurrentTicks() { return DOCTEST_CONFIG_GETCURRENTTICKS(); } +#elif defined(DOCTEST_PLATFORM_WINDOWS) + ticks_t getCurrentTicks() { + static LARGE_INTEGER hz = { {0} }, hzo = { {0} }; + if(!hz.QuadPart) { + QueryPerformanceFrequency(&hz); + QueryPerformanceCounter(&hzo); + } + LARGE_INTEGER t; + QueryPerformanceCounter(&t); + return ((t.QuadPart - hzo.QuadPart) * LONGLONG(1000000)) / hz.QuadPart; + } +#else // DOCTEST_PLATFORM_WINDOWS + ticks_t getCurrentTicks() { + timeval t; + gettimeofday(&t, nullptr); + return static_cast(t.tv_sec) * 1000000 + static_cast(t.tv_usec); + } +#endif // DOCTEST_PLATFORM_WINDOWS + + struct Timer + { + void start() { m_ticks = getCurrentTicks(); } + unsigned int getElapsedMicroseconds() const { + return static_cast(getCurrentTicks() - m_ticks); + } + //unsigned int getElapsedMilliseconds() const { + // return static_cast(getElapsedMicroseconds() / 1000); + //} + double getElapsedSeconds() const { return static_cast(getCurrentTicks() - m_ticks) / 1000000.0; } + + private: + ticks_t m_ticks = 0; + }; + +#ifdef DOCTEST_CONFIG_NO_MULTITHREADING + template + using Atomic = T; +#else // DOCTEST_CONFIG_NO_MULTITHREADING + template + using Atomic = std::atomic; +#endif // DOCTEST_CONFIG_NO_MULTITHREADING + +#if defined(DOCTEST_CONFIG_NO_MULTI_LANE_ATOMICS) || defined(DOCTEST_CONFIG_NO_MULTITHREADING) + template + using MultiLaneAtomic = Atomic; +#else // DOCTEST_CONFIG_NO_MULTI_LANE_ATOMICS + // Provides a multilane implementation of an atomic variable that supports add, sub, load, + // store. Instead of using a single atomic variable, this splits up into multiple ones, + // each sitting on a separate cache line. The goal is to provide a speedup when most + // operations are modifying. It achieves this with two properties: + // + // * Multiple atomics are used, so chance of congestion from the same atomic is reduced. + // * Each atomic sits on a separate cache line, so false sharing is reduced. + // + // The disadvantage is that there is a small overhead due to the use of TLS, and load/store + // is slower because all atomics have to be accessed. + template + class MultiLaneAtomic + { + struct CacheLineAlignedAtomic + { + Atomic atomic{}; + char padding[DOCTEST_MULTI_LANE_ATOMICS_CACHE_LINE_SIZE - sizeof(Atomic)]; + }; + CacheLineAlignedAtomic m_atomics[DOCTEST_MULTI_LANE_ATOMICS_THREAD_LANES]; + + static_assert(sizeof(CacheLineAlignedAtomic) == DOCTEST_MULTI_LANE_ATOMICS_CACHE_LINE_SIZE, + "guarantee one atomic takes exactly one cache line"); + + public: + T operator++() DOCTEST_NOEXCEPT { return fetch_add(1) + 1; } + + T operator++(int) DOCTEST_NOEXCEPT { return fetch_add(1); } + + T fetch_add(T arg, std::memory_order order = std::memory_order_seq_cst) DOCTEST_NOEXCEPT { + return myAtomic().fetch_add(arg, order); + } + + T fetch_sub(T arg, std::memory_order order = std::memory_order_seq_cst) DOCTEST_NOEXCEPT { + return myAtomic().fetch_sub(arg, order); + } + + operator T() const DOCTEST_NOEXCEPT { return load(); } + + T load(std::memory_order order = std::memory_order_seq_cst) const DOCTEST_NOEXCEPT { + auto result = T(); + for(auto const& c : m_atomics) { + result += c.atomic.load(order); + } + return result; + } + + T operator=(T desired) DOCTEST_NOEXCEPT { // lgtm [cpp/assignment-does-not-return-this] + store(desired); + return desired; + } + + void store(T desired, std::memory_order order = std::memory_order_seq_cst) DOCTEST_NOEXCEPT { + // first value becomes desired", all others become 0. + for(auto& c : m_atomics) { + c.atomic.store(desired, order); + desired = {}; + } + } + + private: + // Each thread has a different atomic that it operates on. If more than NumLanes threads + // use this, some will use the same atomic. So performance will degrade a bit, but still + // everything will work. + // + // The logic here is a bit tricky. The call should be as fast as possible, so that there + // is minimal to no overhead in determining the correct atomic for the current thread. + // + // 1. A global static counter laneCounter counts continuously up. + // 2. Each successive thread will use modulo operation of that counter so it gets an atomic + // assigned in a round-robin fashion. + // 3. This tlsLaneIdx is stored in the thread local data, so it is directly available with + // little overhead. + Atomic& myAtomic() DOCTEST_NOEXCEPT { + static Atomic laneCounter; + DOCTEST_THREAD_LOCAL size_t tlsLaneIdx = + laneCounter++ % DOCTEST_MULTI_LANE_ATOMICS_THREAD_LANES; + + return m_atomics[tlsLaneIdx].atomic; + } + }; +#endif // DOCTEST_CONFIG_NO_MULTI_LANE_ATOMICS + + // this holds both parameters from the command line and runtime data for tests + struct ContextState : ContextOptions, TestRunStats, CurrentTestCaseStats + { + MultiLaneAtomic numAssertsCurrentTest_atomic; + MultiLaneAtomic numAssertsFailedCurrentTest_atomic; + + std::vector> filters = decltype(filters)(9); // 9 different filters + + std::vector reporters_currently_used; + + assert_handler ah = nullptr; + + Timer timer; + + std::vector stringifiedContexts; // logging from INFO() due to an exception + + // stuff for subcases + bool reachedLeaf; + std::vector subcaseStack; + std::vector nextSubcaseStack; + std::unordered_set fullyTraversedSubcases; + size_t currentSubcaseDepth; + Atomic shouldLogCurrentException; + + void resetRunData() { + numTestCases = 0; + numTestCasesPassingFilters = 0; + numTestSuitesPassingFilters = 0; + numTestCasesFailed = 0; + numAsserts = 0; + numAssertsFailed = 0; + numAssertsCurrentTest = 0; + numAssertsFailedCurrentTest = 0; + } + + void finalizeTestCaseData() { + seconds = timer.getElapsedSeconds(); + + // update the non-atomic counters + numAsserts += numAssertsCurrentTest_atomic; + numAssertsFailed += numAssertsFailedCurrentTest_atomic; + numAssertsCurrentTest = numAssertsCurrentTest_atomic; + numAssertsFailedCurrentTest = numAssertsFailedCurrentTest_atomic; + + if(numAssertsFailedCurrentTest) + failure_flags |= TestCaseFailureReason::AssertFailure; + + if(Approx(currentTest->m_timeout).epsilon(DBL_EPSILON) != 0 && + Approx(seconds).epsilon(DBL_EPSILON) > currentTest->m_timeout) + failure_flags |= TestCaseFailureReason::Timeout; + + if(currentTest->m_should_fail) { + if(failure_flags) { + failure_flags |= TestCaseFailureReason::ShouldHaveFailedAndDid; + } else { + failure_flags |= TestCaseFailureReason::ShouldHaveFailedButDidnt; + } + } else if(failure_flags && currentTest->m_may_fail) { + failure_flags |= TestCaseFailureReason::CouldHaveFailedAndDid; + } else if(currentTest->m_expected_failures > 0) { + if(numAssertsFailedCurrentTest == currentTest->m_expected_failures) { + failure_flags |= TestCaseFailureReason::FailedExactlyNumTimes; + } else { + failure_flags |= TestCaseFailureReason::DidntFailExactlyNumTimes; + } + } + + bool ok_to_fail = (TestCaseFailureReason::ShouldHaveFailedAndDid & failure_flags) || + (TestCaseFailureReason::CouldHaveFailedAndDid & failure_flags) || + (TestCaseFailureReason::FailedExactlyNumTimes & failure_flags); + + // if any subcase has failed - the whole test case has failed + testCaseSuccess = !(failure_flags && !ok_to_fail); + if(!testCaseSuccess) + numTestCasesFailed++; + } + }; + + ContextState* g_cs = nullptr; + + // used to avoid locks for the debug output + // TODO: figure out if this is indeed necessary/correct - seems like either there still + // could be a race or that there wouldn't be a race even if using the context directly + DOCTEST_THREAD_LOCAL bool g_no_colors; + +#endif // DOCTEST_CONFIG_DISABLE +} // namespace detail + +char* String::allocate(size_type sz) { + if (sz <= last) { + buf[sz] = '\0'; + setLast(last - sz); + return buf; + } else { + setOnHeap(); + data.size = sz; + data.capacity = data.size + 1; + data.ptr = new char[data.capacity]; + data.ptr[sz] = '\0'; + return data.ptr; + } +} + +void String::setOnHeap() noexcept { *reinterpret_cast(&buf[last]) = 128; } +void String::setLast(size_type in) noexcept { buf[last] = char(in); } +void String::setSize(size_type sz) noexcept { + if (isOnStack()) { buf[sz] = '\0'; setLast(last - sz); } + else { data.ptr[sz] = '\0'; data.size = sz; } +} + +void String::copy(const String& other) { + if(other.isOnStack()) { + memcpy(buf, other.buf, len); + } else { + memcpy(allocate(other.data.size), other.data.ptr, other.data.size); + } +} + +String::String() noexcept { + buf[0] = '\0'; + setLast(); +} + +String::~String() { + if(!isOnStack()) + delete[] data.ptr; +} // NOLINT(clang-analyzer-cplusplus.NewDeleteLeaks) + +String::String(const char* in) + : String(in, strlen(in)) {} + +String::String(const char* in, size_type in_size) { + memcpy(allocate(in_size), in, in_size); +} + +String::String(std::istream& in, size_type in_size) { + in.read(allocate(in_size), in_size); +} + +String::String(const String& other) { copy(other); } + +String& String::operator=(const String& other) { + if(this != &other) { + if(!isOnStack()) + delete[] data.ptr; + + copy(other); + } + + return *this; +} + +String& String::operator+=(const String& other) { + const size_type my_old_size = size(); + const size_type other_size = other.size(); + const size_type total_size = my_old_size + other_size; + if(isOnStack()) { + if(total_size < len) { + // append to the current stack space + memcpy(buf + my_old_size, other.c_str(), other_size + 1); + // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) + setLast(last - total_size); + } else { + // alloc new chunk + char* temp = new char[total_size + 1]; + // copy current data to new location before writing in the union + memcpy(temp, buf, my_old_size); // skip the +1 ('\0') for speed + // update data in union + setOnHeap(); + data.size = total_size; + data.capacity = data.size + 1; + data.ptr = temp; + // transfer the rest of the data + memcpy(data.ptr + my_old_size, other.c_str(), other_size + 1); + } + } else { + if(data.capacity > total_size) { + // append to the current heap block + data.size = total_size; + memcpy(data.ptr + my_old_size, other.c_str(), other_size + 1); + } else { + // resize + data.capacity *= 2; + if(data.capacity <= total_size) + data.capacity = total_size + 1; + // alloc new chunk + char* temp = new char[data.capacity]; + // copy current data to new location before releasing it + memcpy(temp, data.ptr, my_old_size); // skip the +1 ('\0') for speed + // release old chunk + delete[] data.ptr; + // update the rest of the union members + data.size = total_size; + data.ptr = temp; + // transfer the rest of the data + memcpy(data.ptr + my_old_size, other.c_str(), other_size + 1); + } + } + + return *this; +} + +String::String(String&& other) noexcept { + memcpy(buf, other.buf, len); + other.buf[0] = '\0'; + other.setLast(); +} + +String& String::operator=(String&& other) noexcept { + if(this != &other) { + if(!isOnStack()) + delete[] data.ptr; + memcpy(buf, other.buf, len); + other.buf[0] = '\0'; + other.setLast(); + } + return *this; +} + +char String::operator[](size_type i) const { + return const_cast(this)->operator[](i); +} + +char& String::operator[](size_type i) { + if(isOnStack()) + return reinterpret_cast(buf)[i]; + return data.ptr[i]; +} + +DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wmaybe-uninitialized") +String::size_type String::size() const { + if(isOnStack()) + return last - (size_type(buf[last]) & 31); // using "last" would work only if "len" is 32 + return data.size; +} +DOCTEST_GCC_SUPPRESS_WARNING_POP + +String::size_type String::capacity() const { + if(isOnStack()) + return len; + return data.capacity; +} + +String String::substr(size_type pos, size_type cnt) && { + cnt = std::min(cnt, size() - 1 - pos); + char* cptr = c_str(); + memmove(cptr, cptr + pos, cnt); + setSize(cnt); + return std::move(*this); +} + +String String::substr(size_type pos, size_type cnt) const & { + cnt = std::min(cnt, size() - 1 - pos); + return String{ c_str() + pos, cnt }; +} + +String::size_type String::find(char ch, size_type pos) const { + const char* begin = c_str(); + const char* end = begin + size(); + const char* it = begin + pos; + for (; it < end && *it != ch; it++); + if (it < end) { return static_cast(it - begin); } + else { return npos; } +} + +String::size_type String::rfind(char ch, size_type pos) const { + const char* begin = c_str(); + const char* it = begin + std::min(pos, size() - 1); + for (; it >= begin && *it != ch; it--); + if (it >= begin) { return static_cast(it - begin); } + else { return npos; } +} + +int String::compare(const char* other, bool no_case) const { + if(no_case) + return doctest::stricmp(c_str(), other); + return std::strcmp(c_str(), other); +} + +int String::compare(const String& other, bool no_case) const { + return compare(other.c_str(), no_case); +} + +String operator+(const String& lhs, const String& rhs) { return String(lhs) += rhs; } + +bool operator==(const String& lhs, const String& rhs) { return lhs.compare(rhs) == 0; } +bool operator!=(const String& lhs, const String& rhs) { return lhs.compare(rhs) != 0; } +bool operator< (const String& lhs, const String& rhs) { return lhs.compare(rhs) < 0; } +bool operator> (const String& lhs, const String& rhs) { return lhs.compare(rhs) > 0; } +bool operator<=(const String& lhs, const String& rhs) { return (lhs != rhs) ? lhs.compare(rhs) < 0 : true; } +bool operator>=(const String& lhs, const String& rhs) { return (lhs != rhs) ? lhs.compare(rhs) > 0 : true; } + +std::ostream& operator<<(std::ostream& s, const String& in) { return s << in.c_str(); } + +Contains::Contains(const String& str) : string(str) { } + +bool Contains::checkWith(const String& other) const { + return strstr(other.c_str(), string.c_str()) != nullptr; +} + +String toString(const Contains& in) { + return "Contains( " + in.string + " )"; +} + +bool operator==(const String& lhs, const Contains& rhs) { return rhs.checkWith(lhs); } +bool operator==(const Contains& lhs, const String& rhs) { return lhs.checkWith(rhs); } +bool operator!=(const String& lhs, const Contains& rhs) { return !rhs.checkWith(lhs); } +bool operator!=(const Contains& lhs, const String& rhs) { return !lhs.checkWith(rhs); } + +namespace { + void color_to_stream(std::ostream&, Color::Enum) DOCTEST_BRANCH_ON_DISABLED({}, ;) +} // namespace + +namespace Color { + std::ostream& operator<<(std::ostream& s, Color::Enum code) { + color_to_stream(s, code); + return s; + } +} // namespace Color + +// clang-format off +const char* assertString(assertType::Enum at) { + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4061) // enum 'x' in switch of enum 'y' is not explicitly handled + #define DOCTEST_GENERATE_ASSERT_TYPE_CASE(assert_type) case assertType::DT_ ## assert_type: return #assert_type + #define DOCTEST_GENERATE_ASSERT_TYPE_CASES(assert_type) \ + DOCTEST_GENERATE_ASSERT_TYPE_CASE(WARN_ ## assert_type); \ + DOCTEST_GENERATE_ASSERT_TYPE_CASE(CHECK_ ## assert_type); \ + DOCTEST_GENERATE_ASSERT_TYPE_CASE(REQUIRE_ ## assert_type) + switch(at) { + DOCTEST_GENERATE_ASSERT_TYPE_CASE(WARN); + DOCTEST_GENERATE_ASSERT_TYPE_CASE(CHECK); + DOCTEST_GENERATE_ASSERT_TYPE_CASE(REQUIRE); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(FALSE); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(THROWS); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(THROWS_AS); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(THROWS_WITH); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(THROWS_WITH_AS); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(NOTHROW); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(EQ); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(NE); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(GT); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(LT); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(GE); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(LE); + + DOCTEST_GENERATE_ASSERT_TYPE_CASES(UNARY); + DOCTEST_GENERATE_ASSERT_TYPE_CASES(UNARY_FALSE); + + default: DOCTEST_INTERNAL_ERROR("Tried stringifying invalid assert type!"); + } + DOCTEST_MSVC_SUPPRESS_WARNING_POP +} +// clang-format on + +const char* failureString(assertType::Enum at) { + if(at & assertType::is_warn) //!OCLINT bitwise operator in conditional + return "WARNING"; + if(at & assertType::is_check) //!OCLINT bitwise operator in conditional + return "ERROR"; + if(at & assertType::is_require) //!OCLINT bitwise operator in conditional + return "FATAL ERROR"; + return ""; +} + +DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wnull-dereference") +DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wnull-dereference") +// depending on the current options this will remove the path of filenames +const char* skipPathFromFilename(const char* file) { +#ifndef DOCTEST_CONFIG_DISABLE + if(getContextOptions()->no_path_in_filenames) { + auto back = std::strrchr(file, '\\'); + auto forward = std::strrchr(file, '/'); + if(back || forward) { + if(back > forward) + forward = back; + return forward + 1; + } + } +#endif // DOCTEST_CONFIG_DISABLE + return file; +} +DOCTEST_CLANG_SUPPRESS_WARNING_POP +DOCTEST_GCC_SUPPRESS_WARNING_POP + +bool SubcaseSignature::operator==(const SubcaseSignature& other) const { + return m_line == other.m_line + && std::strcmp(m_file, other.m_file) == 0 + && m_name == other.m_name; +} + +bool SubcaseSignature::operator<(const SubcaseSignature& other) const { + if(m_line != other.m_line) + return m_line < other.m_line; + if(std::strcmp(m_file, other.m_file) != 0) + return std::strcmp(m_file, other.m_file) < 0; + return m_name.compare(other.m_name) < 0; +} + +DOCTEST_DEFINE_INTERFACE(IContextScope) + +namespace detail { + void filldata::fill(std::ostream* stream, const void* in) { + if (in) { *stream << in; } + else { *stream << "nullptr"; } + } + + template + String toStreamLit(T t) { + std::ostream* os = tlssPush(); + os->operator<<(t); + return tlssPop(); + } +} + +#ifdef DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING +String toString(const char* in) { return String("\"") + (in ? in : "{null string}") + "\""; } +#endif // DOCTEST_CONFIG_TREAT_CHAR_STAR_AS_STRING + +#if DOCTEST_MSVC >= DOCTEST_COMPILER(19, 20, 0) +// see this issue on why this is needed: https://github.com/doctest/doctest/issues/183 +String toString(const std::string& in) { return in.c_str(); } +#endif // VS 2019 + +String toString(String in) { return in; } + +String toString(std::nullptr_t) { return "nullptr"; } + +String toString(bool in) { return in ? "true" : "false"; } + +String toString(float in) { return toStreamLit(in); } +String toString(double in) { return toStreamLit(in); } +String toString(double long in) { return toStreamLit(in); } + +String toString(char in) { return toStreamLit(static_cast(in)); } +String toString(char signed in) { return toStreamLit(static_cast(in)); } +String toString(char unsigned in) { return toStreamLit(static_cast(in)); } +String toString(short in) { return toStreamLit(in); } +String toString(short unsigned in) { return toStreamLit(in); } +String toString(signed in) { return toStreamLit(in); } +String toString(unsigned in) { return toStreamLit(in); } +String toString(long in) { return toStreamLit(in); } +String toString(long unsigned in) { return toStreamLit(in); } +String toString(long long in) { return toStreamLit(in); } +String toString(long long unsigned in) { return toStreamLit(in); } + +Approx::Approx(double value) + : m_epsilon(static_cast(std::numeric_limits::epsilon()) * 100) + , m_scale(1.0) + , m_value(value) {} + +Approx Approx::operator()(double value) const { + Approx approx(value); + approx.epsilon(m_epsilon); + approx.scale(m_scale); + return approx; +} + +Approx& Approx::epsilon(double newEpsilon) { + m_epsilon = newEpsilon; + return *this; +} +Approx& Approx::scale(double newScale) { + m_scale = newScale; + return *this; +} + +bool operator==(double lhs, const Approx& rhs) { + // Thanks to Richard Harris for his help refining this formula + return std::fabs(lhs - rhs.m_value) < + rhs.m_epsilon * (rhs.m_scale + std::max(std::fabs(lhs), std::fabs(rhs.m_value))); +} +bool operator==(const Approx& lhs, double rhs) { return operator==(rhs, lhs); } +bool operator!=(double lhs, const Approx& rhs) { return !operator==(lhs, rhs); } +bool operator!=(const Approx& lhs, double rhs) { return !operator==(rhs, lhs); } +bool operator<=(double lhs, const Approx& rhs) { return lhs < rhs.m_value || lhs == rhs; } +bool operator<=(const Approx& lhs, double rhs) { return lhs.m_value < rhs || lhs == rhs; } +bool operator>=(double lhs, const Approx& rhs) { return lhs > rhs.m_value || lhs == rhs; } +bool operator>=(const Approx& lhs, double rhs) { return lhs.m_value > rhs || lhs == rhs; } +bool operator<(double lhs, const Approx& rhs) { return lhs < rhs.m_value && lhs != rhs; } +bool operator<(const Approx& lhs, double rhs) { return lhs.m_value < rhs && lhs != rhs; } +bool operator>(double lhs, const Approx& rhs) { return lhs > rhs.m_value && lhs != rhs; } +bool operator>(const Approx& lhs, double rhs) { return lhs.m_value > rhs && lhs != rhs; } + +String toString(const Approx& in) { + return "Approx( " + doctest::toString(in.m_value) + " )"; +} +const ContextOptions* getContextOptions() { return DOCTEST_BRANCH_ON_DISABLED(nullptr, g_cs); } + +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4738) +template +IsNaN::operator bool() const { + return std::isnan(value) ^ flipped; +} +DOCTEST_MSVC_SUPPRESS_WARNING_POP +template struct DOCTEST_INTERFACE_DEF IsNaN; +template struct DOCTEST_INTERFACE_DEF IsNaN; +template struct DOCTEST_INTERFACE_DEF IsNaN; +template +String toString(IsNaN in) { return String(in.flipped ? "! " : "") + "IsNaN( " + doctest::toString(in.value) + " )"; } +String toString(IsNaN in) { return toString(in); } +String toString(IsNaN in) { return toString(in); } +String toString(IsNaN in) { return toString(in); } + +} // namespace doctest + +#ifdef DOCTEST_CONFIG_DISABLE +namespace doctest { +Context::Context(int, const char* const*) {} +Context::~Context() = default; +void Context::applyCommandLine(int, const char* const*) {} +void Context::addFilter(const char*, const char*) {} +void Context::clearFilters() {} +void Context::setOption(const char*, bool) {} +void Context::setOption(const char*, int) {} +void Context::setOption(const char*, const char*) {} +bool Context::shouldExit() { return false; } +void Context::setAsDefaultForAssertsOutOfTestCases() {} +void Context::setAssertHandler(detail::assert_handler) {} +void Context::setCout(std::ostream*) {} +int Context::run() { return 0; } + +int IReporter::get_num_active_contexts() { return 0; } +const IContextScope* const* IReporter::get_active_contexts() { return nullptr; } +int IReporter::get_num_stringified_contexts() { return 0; } +const String* IReporter::get_stringified_contexts() { return nullptr; } + +int registerReporter(const char*, int, IReporter*) { return 0; } + +} // namespace doctest +#else // DOCTEST_CONFIG_DISABLE + +#if !defined(DOCTEST_CONFIG_COLORS_NONE) +#if !defined(DOCTEST_CONFIG_COLORS_WINDOWS) && !defined(DOCTEST_CONFIG_COLORS_ANSI) +#ifdef DOCTEST_PLATFORM_WINDOWS +#define DOCTEST_CONFIG_COLORS_WINDOWS +#else // linux +#define DOCTEST_CONFIG_COLORS_ANSI +#endif // platform +#endif // DOCTEST_CONFIG_COLORS_WINDOWS && DOCTEST_CONFIG_COLORS_ANSI +#endif // DOCTEST_CONFIG_COLORS_NONE + +namespace doctest_detail_test_suite_ns { +// holds the current test suite +doctest::detail::TestSuite& getCurrentTestSuite() { + static doctest::detail::TestSuite data{}; + return data; +} +} // namespace doctest_detail_test_suite_ns + +namespace doctest { +namespace { + // the int (priority) is part of the key for automatic sorting - sadly one can register a + // reporter with a duplicate name and a different priority but hopefully that won't happen often :| + using reporterMap = std::map, reporterCreatorFunc>; + + reporterMap& getReporters() { + static reporterMap data; + return data; + } + reporterMap& getListeners() { + static reporterMap data; + return data; + } +} // namespace +namespace detail { +#define DOCTEST_ITERATE_THROUGH_REPORTERS(function, ...) \ + for(auto& curr_rep : g_cs->reporters_currently_used) \ + curr_rep->function(__VA_ARGS__) + + bool checkIfShouldThrow(assertType::Enum at) { + if(at & assertType::is_require) //!OCLINT bitwise operator in conditional + return true; + + if((at & assertType::is_check) //!OCLINT bitwise operator in conditional + && getContextOptions()->abort_after > 0 && + (g_cs->numAssertsFailed + g_cs->numAssertsFailedCurrentTest_atomic) >= + getContextOptions()->abort_after) + return true; + + return false; + } + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + DOCTEST_NORETURN void throwException() { + g_cs->shouldLogCurrentException = false; + throw TestFailureException(); // NOLINT(hicpp-exception-baseclass) + } +#else // DOCTEST_CONFIG_NO_EXCEPTIONS + void throwException() {} +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS +} // namespace detail + +namespace { + using namespace detail; + // matching of a string against a wildcard mask (case sensitivity configurable) taken from + // https://www.codeproject.com/Articles/1088/Wildcard-string-compare-globbing + int wildcmp(const char* str, const char* wild, bool caseSensitive) { + const char* cp = str; + const char* mp = wild; + + while((*str) && (*wild != '*')) { + if((caseSensitive ? (*wild != *str) : (tolower(*wild) != tolower(*str))) && + (*wild != '?')) { + return 0; + } + wild++; + str++; + } + + while(*str) { + if(*wild == '*') { + if(!*++wild) { + return 1; + } + mp = wild; + cp = str + 1; + } else if((caseSensitive ? (*wild == *str) : (tolower(*wild) == tolower(*str))) || + (*wild == '?')) { + wild++; + str++; + } else { + wild = mp; //!OCLINT parameter reassignment + str = cp++; //!OCLINT parameter reassignment + } + } + + while(*wild == '*') { + wild++; + } + return !*wild; + } + + // checks if the name matches any of the filters (and can be configured what to do when empty) + bool matchesAny(const char* name, const std::vector& filters, bool matchEmpty, + bool caseSensitive) { + if (filters.empty() && matchEmpty) + return true; + for (auto& curr : filters) + if (wildcmp(name, curr.c_str(), caseSensitive)) + return true; + return false; + } + + DOCTEST_NO_SANITIZE_INTEGER + unsigned long long hash(unsigned long long a, unsigned long long b) { + return (a << 5) + b; + } + + // C string hash function (djb2) - taken from http://www.cse.yorku.ca/~oz/hash.html + DOCTEST_NO_SANITIZE_INTEGER + unsigned long long hash(const char* str) { + unsigned long long hash = 5381; + char c; + while ((c = *str++)) + hash = ((hash << 5) + hash) + c; // hash * 33 + c + return hash; + } + + unsigned long long hash(const SubcaseSignature& sig) { + return hash(hash(hash(sig.m_file), hash(sig.m_name.c_str())), sig.m_line); + } + + unsigned long long hash(const std::vector& sigs, size_t count) { + unsigned long long running = 0; + auto end = sigs.begin() + count; + for (auto it = sigs.begin(); it != end; it++) { + running = hash(running, hash(*it)); + } + return running; + } + + unsigned long long hash(const std::vector& sigs) { + unsigned long long running = 0; + for (const SubcaseSignature& sig : sigs) { + running = hash(running, hash(sig)); + } + return running; + } +} // namespace +namespace detail { + bool Subcase::checkFilters() { + if (g_cs->subcaseStack.size() < size_t(g_cs->subcase_filter_levels)) { + if (!matchesAny(m_signature.m_name.c_str(), g_cs->filters[6], true, g_cs->case_sensitive)) + return true; + if (matchesAny(m_signature.m_name.c_str(), g_cs->filters[7], false, g_cs->case_sensitive)) + return true; + } + return false; + } + + Subcase::Subcase(const String& name, const char* file, int line) + : m_signature({name, file, line}) { + if (!g_cs->reachedLeaf) { + if (g_cs->nextSubcaseStack.size() <= g_cs->subcaseStack.size() + || g_cs->nextSubcaseStack[g_cs->subcaseStack.size()] == m_signature) { + // Going down. + if (checkFilters()) { return; } + + g_cs->subcaseStack.push_back(m_signature); + g_cs->currentSubcaseDepth++; + m_entered = true; + DOCTEST_ITERATE_THROUGH_REPORTERS(subcase_start, m_signature); + } + } else { + if (g_cs->subcaseStack[g_cs->currentSubcaseDepth] == m_signature) { + // This subcase is reentered via control flow. + g_cs->currentSubcaseDepth++; + m_entered = true; + DOCTEST_ITERATE_THROUGH_REPORTERS(subcase_start, m_signature); + } else if (g_cs->nextSubcaseStack.size() <= g_cs->currentSubcaseDepth + && g_cs->fullyTraversedSubcases.find(hash(hash(g_cs->subcaseStack, g_cs->currentSubcaseDepth), hash(m_signature))) + == g_cs->fullyTraversedSubcases.end()) { + if (checkFilters()) { return; } + // This subcase is part of the one to be executed next. + g_cs->nextSubcaseStack.clear(); + g_cs->nextSubcaseStack.insert(g_cs->nextSubcaseStack.end(), + g_cs->subcaseStack.begin(), g_cs->subcaseStack.begin() + g_cs->currentSubcaseDepth); + g_cs->nextSubcaseStack.push_back(m_signature); + } + } + } + + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4996) // std::uncaught_exception is deprecated in C++17 + DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wdeprecated-declarations") + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wdeprecated-declarations") + + Subcase::~Subcase() { + if (m_entered) { + g_cs->currentSubcaseDepth--; + + if (!g_cs->reachedLeaf) { + // Leaf. + g_cs->fullyTraversedSubcases.insert(hash(g_cs->subcaseStack)); + g_cs->nextSubcaseStack.clear(); + g_cs->reachedLeaf = true; + } else if (g_cs->nextSubcaseStack.empty()) { + // All children are finished. + g_cs->fullyTraversedSubcases.insert(hash(g_cs->subcaseStack)); + } + +#if defined(__cpp_lib_uncaught_exceptions) && __cpp_lib_uncaught_exceptions >= 201411L && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101200) + if(std::uncaught_exceptions() > 0 +#else + if(std::uncaught_exception() +#endif + && g_cs->shouldLogCurrentException) { + DOCTEST_ITERATE_THROUGH_REPORTERS( + test_case_exception, {"exception thrown in subcase - will translate later " + "when the whole test case has been exited (cannot " + "translate while there is an active exception)", + false}); + g_cs->shouldLogCurrentException = false; + } + + DOCTEST_ITERATE_THROUGH_REPORTERS(subcase_end, DOCTEST_EMPTY); + } + } + + DOCTEST_CLANG_SUPPRESS_WARNING_POP + DOCTEST_GCC_SUPPRESS_WARNING_POP + DOCTEST_MSVC_SUPPRESS_WARNING_POP + + Subcase::operator bool() const { return m_entered; } + + Result::Result(bool passed, const String& decomposition) + : m_passed(passed) + , m_decomp(decomposition) {} + + ExpressionDecomposer::ExpressionDecomposer(assertType::Enum at) + : m_at(at) {} + + TestSuite& TestSuite::operator*(const char* in) { + m_test_suite = in; + return *this; + } + + TestCase::TestCase(funcType test, const char* file, unsigned line, const TestSuite& test_suite, + const String& type, int template_id) { + m_file = file; + m_line = line; + m_name = nullptr; // will be later overridden in operator* + m_test_suite = test_suite.m_test_suite; + m_description = test_suite.m_description; + m_skip = test_suite.m_skip; + m_no_breaks = test_suite.m_no_breaks; + m_no_output = test_suite.m_no_output; + m_may_fail = test_suite.m_may_fail; + m_should_fail = test_suite.m_should_fail; + m_expected_failures = test_suite.m_expected_failures; + m_timeout = test_suite.m_timeout; + + m_test = test; + m_type = type; + m_template_id = template_id; + } + + TestCase::TestCase(const TestCase& other) + : TestCaseData() { + *this = other; + } + + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(26434) // hides a non-virtual function + TestCase& TestCase::operator=(const TestCase& other) { + TestCaseData::operator=(other); + m_test = other.m_test; + m_type = other.m_type; + m_template_id = other.m_template_id; + m_full_name = other.m_full_name; + + if(m_template_id != -1) + m_name = m_full_name.c_str(); + return *this; + } + DOCTEST_MSVC_SUPPRESS_WARNING_POP + + TestCase& TestCase::operator*(const char* in) { + m_name = in; + // make a new name with an appended type for templated test case + if(m_template_id != -1) { + m_full_name = String(m_name) + "<" + m_type + ">"; + // redirect the name to point to the newly constructed full name + m_name = m_full_name.c_str(); + } + return *this; + } + + bool TestCase::operator<(const TestCase& other) const { + // this will be used only to differentiate between test cases - not relevant for sorting + if(m_line != other.m_line) + return m_line < other.m_line; + const int name_cmp = strcmp(m_name, other.m_name); + if(name_cmp != 0) + return name_cmp < 0; + const int file_cmp = m_file.compare(other.m_file); + if(file_cmp != 0) + return file_cmp < 0; + return m_template_id < other.m_template_id; + } + + // all the registered tests + std::set& getRegisteredTests() { + static std::set data; + return data; + } +} // namespace detail +namespace { + using namespace detail; + // for sorting tests by file/line + bool fileOrderComparator(const TestCase* lhs, const TestCase* rhs) { + // this is needed because MSVC gives different case for drive letters + // for __FILE__ when evaluated in a header and a source file + const int res = lhs->m_file.compare(rhs->m_file, bool(DOCTEST_MSVC)); + if(res != 0) + return res < 0; + if(lhs->m_line != rhs->m_line) + return lhs->m_line < rhs->m_line; + return lhs->m_template_id < rhs->m_template_id; + } + + // for sorting tests by suite/file/line + bool suiteOrderComparator(const TestCase* lhs, const TestCase* rhs) { + const int res = std::strcmp(lhs->m_test_suite, rhs->m_test_suite); + if(res != 0) + return res < 0; + return fileOrderComparator(lhs, rhs); + } + + // for sorting tests by name/suite/file/line + bool nameOrderComparator(const TestCase* lhs, const TestCase* rhs) { + const int res = std::strcmp(lhs->m_name, rhs->m_name); + if(res != 0) + return res < 0; + return suiteOrderComparator(lhs, rhs); + } + + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wdeprecated-declarations") + void color_to_stream(std::ostream& s, Color::Enum code) { + static_cast(s); // for DOCTEST_CONFIG_COLORS_NONE or DOCTEST_CONFIG_COLORS_WINDOWS + static_cast(code); // for DOCTEST_CONFIG_COLORS_NONE +#ifdef DOCTEST_CONFIG_COLORS_ANSI + if(g_no_colors || + (isatty(STDOUT_FILENO) == false && getContextOptions()->force_colors == false)) + return; + + auto col = ""; + // clang-format off + switch(code) { //!OCLINT missing break in switch statement / unnecessary default statement in covered switch statement + case Color::Red: col = "[0;31m"; break; + case Color::Green: col = "[0;32m"; break; + case Color::Blue: col = "[0;34m"; break; + case Color::Cyan: col = "[0;36m"; break; + case Color::Yellow: col = "[0;33m"; break; + case Color::Grey: col = "[1;30m"; break; + case Color::LightGrey: col = "[0;37m"; break; + case Color::BrightRed: col = "[1;31m"; break; + case Color::BrightGreen: col = "[1;32m"; break; + case Color::BrightWhite: col = "[1;37m"; break; + case Color::Bright: // invalid + case Color::None: + case Color::White: + default: col = "[0m"; + } + // clang-format on + s << "\033" << col; +#endif // DOCTEST_CONFIG_COLORS_ANSI + +#ifdef DOCTEST_CONFIG_COLORS_WINDOWS + if(g_no_colors || + (_isatty(_fileno(stdout)) == false && getContextOptions()->force_colors == false)) + return; + + static struct ConsoleHelper { + HANDLE stdoutHandle; + WORD origFgAttrs; + WORD origBgAttrs; + + ConsoleHelper() { + stdoutHandle = GetStdHandle(STD_OUTPUT_HANDLE); + CONSOLE_SCREEN_BUFFER_INFO csbiInfo; + GetConsoleScreenBufferInfo(stdoutHandle, &csbiInfo); + origFgAttrs = csbiInfo.wAttributes & ~(BACKGROUND_GREEN | BACKGROUND_RED | + BACKGROUND_BLUE | BACKGROUND_INTENSITY); + origBgAttrs = csbiInfo.wAttributes & ~(FOREGROUND_GREEN | FOREGROUND_RED | + FOREGROUND_BLUE | FOREGROUND_INTENSITY); + } + } ch; + +#define DOCTEST_SET_ATTR(x) SetConsoleTextAttribute(ch.stdoutHandle, x | ch.origBgAttrs) + + // clang-format off + switch (code) { + case Color::White: DOCTEST_SET_ATTR(FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_BLUE); break; + case Color::Red: DOCTEST_SET_ATTR(FOREGROUND_RED); break; + case Color::Green: DOCTEST_SET_ATTR(FOREGROUND_GREEN); break; + case Color::Blue: DOCTEST_SET_ATTR(FOREGROUND_BLUE); break; + case Color::Cyan: DOCTEST_SET_ATTR(FOREGROUND_BLUE | FOREGROUND_GREEN); break; + case Color::Yellow: DOCTEST_SET_ATTR(FOREGROUND_RED | FOREGROUND_GREEN); break; + case Color::Grey: DOCTEST_SET_ATTR(0); break; + case Color::LightGrey: DOCTEST_SET_ATTR(FOREGROUND_INTENSITY); break; + case Color::BrightRed: DOCTEST_SET_ATTR(FOREGROUND_INTENSITY | FOREGROUND_RED); break; + case Color::BrightGreen: DOCTEST_SET_ATTR(FOREGROUND_INTENSITY | FOREGROUND_GREEN); break; + case Color::BrightWhite: DOCTEST_SET_ATTR(FOREGROUND_INTENSITY | FOREGROUND_GREEN | FOREGROUND_RED | FOREGROUND_BLUE); break; + case Color::None: + case Color::Bright: // invalid + default: DOCTEST_SET_ATTR(ch.origFgAttrs); + } + // clang-format on +#endif // DOCTEST_CONFIG_COLORS_WINDOWS + } + DOCTEST_CLANG_SUPPRESS_WARNING_POP + + std::vector& getExceptionTranslators() { + static std::vector data; + return data; + } + + String translateActiveException() { +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + String res; + auto& translators = getExceptionTranslators(); + for(auto& curr : translators) + if(curr->translate(res)) + return res; + // clang-format off + DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wcatch-value") + try { + throw; + } catch(std::exception& ex) { + return ex.what(); + } catch(std::string& msg) { + return msg.c_str(); + } catch(const char* msg) { + return msg; + } catch(...) { + return "unknown exception"; + } + DOCTEST_GCC_SUPPRESS_WARNING_POP +// clang-format on +#else // DOCTEST_CONFIG_NO_EXCEPTIONS + return ""; +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + } +} // namespace + +namespace detail { + // used by the macros for registering tests + int regTest(const TestCase& tc) { + getRegisteredTests().insert(tc); + return 0; + } + + // sets the current test suite + int setTestSuite(const TestSuite& ts) { + doctest_detail_test_suite_ns::getCurrentTestSuite() = ts; + return 0; + } + +#ifdef DOCTEST_IS_DEBUGGER_ACTIVE + bool isDebuggerActive() { return DOCTEST_IS_DEBUGGER_ACTIVE(); } +#else // DOCTEST_IS_DEBUGGER_ACTIVE +#ifdef DOCTEST_PLATFORM_LINUX + class ErrnoGuard { + public: + ErrnoGuard() : m_oldErrno(errno) {} + ~ErrnoGuard() { errno = m_oldErrno; } + private: + int m_oldErrno; + }; + // See the comments in Catch2 for the reasoning behind this implementation: + // https://github.com/catchorg/Catch2/blob/v2.13.1/include/internal/catch_debugger.cpp#L79-L102 + bool isDebuggerActive() { + ErrnoGuard guard; + std::ifstream in("/proc/self/status"); + for(std::string line; std::getline(in, line);) { + static const int PREFIX_LEN = 11; + if(line.compare(0, PREFIX_LEN, "TracerPid:\t") == 0) { + return line.length() > PREFIX_LEN && line[PREFIX_LEN] != '0'; + } + } + return false; + } +#elif defined(DOCTEST_PLATFORM_MAC) + // The following function is taken directly from the following technical note: + // https://developer.apple.com/library/archive/qa/qa1361/_index.html + // Returns true if the current process is being debugged (either + // running under the debugger or has a debugger attached post facto). + bool isDebuggerActive() { + int mib[4]; + kinfo_proc info; + size_t size; + // Initialize the flags so that, if sysctl fails for some bizarre + // reason, we get a predictable result. + info.kp_proc.p_flag = 0; + // Initialize mib, which tells sysctl the info we want, in this case + // we're looking for information about a specific process ID. + mib[0] = CTL_KERN; + mib[1] = KERN_PROC; + mib[2] = KERN_PROC_PID; + mib[3] = getpid(); + // Call sysctl. + size = sizeof(info); + if(sysctl(mib, DOCTEST_COUNTOF(mib), &info, &size, 0, 0) != 0) { + std::cerr << "\nCall to sysctl failed - unable to determine if debugger is active **\n"; + return false; + } + // We're being debugged if the P_TRACED flag is set. + return ((info.kp_proc.p_flag & P_TRACED) != 0); + } +#elif DOCTEST_MSVC || defined(__MINGW32__) || defined(__MINGW64__) + bool isDebuggerActive() { return ::IsDebuggerPresent() != 0; } +#else + bool isDebuggerActive() { return false; } +#endif // Platform +#endif // DOCTEST_IS_DEBUGGER_ACTIVE + + void registerExceptionTranslatorImpl(const IExceptionTranslator* et) { + if(std::find(getExceptionTranslators().begin(), getExceptionTranslators().end(), et) == + getExceptionTranslators().end()) + getExceptionTranslators().push_back(et); + } + + DOCTEST_THREAD_LOCAL std::vector g_infoContexts; // for logging with INFO() + + ContextScopeBase::ContextScopeBase() { + g_infoContexts.push_back(this); + } + + ContextScopeBase::ContextScopeBase(ContextScopeBase&& other) noexcept { + if (other.need_to_destroy) { + other.destroy(); + } + other.need_to_destroy = false; + g_infoContexts.push_back(this); + } + + DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4996) // std::uncaught_exception is deprecated in C++17 + DOCTEST_GCC_SUPPRESS_WARNING_WITH_PUSH("-Wdeprecated-declarations") + DOCTEST_CLANG_SUPPRESS_WARNING_WITH_PUSH("-Wdeprecated-declarations") + + // destroy cannot be inlined into the destructor because that would mean calling stringify after + // ContextScope has been destroyed (base class destructors run after derived class destructors). + // Instead, ContextScope calls this method directly from its destructor. + void ContextScopeBase::destroy() { +#if defined(__cpp_lib_uncaught_exceptions) && __cpp_lib_uncaught_exceptions >= 201411L && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101200) + if(std::uncaught_exceptions() > 0) { +#else + if(std::uncaught_exception()) { +#endif + std::ostringstream s; + this->stringify(&s); + g_cs->stringifiedContexts.push_back(s.str().c_str()); + } + g_infoContexts.pop_back(); + } + + DOCTEST_CLANG_SUPPRESS_WARNING_POP + DOCTEST_GCC_SUPPRESS_WARNING_POP + DOCTEST_MSVC_SUPPRESS_WARNING_POP +} // namespace detail +namespace { + using namespace detail; + +#if !defined(DOCTEST_CONFIG_POSIX_SIGNALS) && !defined(DOCTEST_CONFIG_WINDOWS_SEH) + struct FatalConditionHandler + { + static void reset() {} + static void allocateAltStackMem() {} + static void freeAltStackMem() {} + }; +#else // DOCTEST_CONFIG_POSIX_SIGNALS || DOCTEST_CONFIG_WINDOWS_SEH + + void reportFatal(const std::string&); + +#ifdef DOCTEST_PLATFORM_WINDOWS + + struct SignalDefs + { + DWORD id; + const char* name; + }; + // There is no 1-1 mapping between signals and windows exceptions. + // Windows can easily distinguish between SO and SigSegV, + // but SigInt, SigTerm, etc are handled differently. + SignalDefs signalDefs[] = { + {static_cast(EXCEPTION_ILLEGAL_INSTRUCTION), + "SIGILL - Illegal instruction signal"}, + {static_cast(EXCEPTION_STACK_OVERFLOW), "SIGSEGV - Stack overflow"}, + {static_cast(EXCEPTION_ACCESS_VIOLATION), + "SIGSEGV - Segmentation violation signal"}, + {static_cast(EXCEPTION_INT_DIVIDE_BY_ZERO), "Divide by zero error"}, + }; + + struct FatalConditionHandler + { + static LONG CALLBACK handleException(PEXCEPTION_POINTERS ExceptionInfo) { + // Multiple threads may enter this filter/handler at once. We want the error message to be printed on the + // console just once no matter how many threads have crashed. + DOCTEST_DECLARE_STATIC_MUTEX(mutex) + static bool execute = true; + { + DOCTEST_LOCK_MUTEX(mutex) + if(execute) { + bool reported = false; + for(size_t i = 0; i < DOCTEST_COUNTOF(signalDefs); ++i) { + if(ExceptionInfo->ExceptionRecord->ExceptionCode == signalDefs[i].id) { + reportFatal(signalDefs[i].name); + reported = true; + break; + } + } + if(reported == false) + reportFatal("Unhandled SEH exception caught"); + if(isDebuggerActive() && !g_cs->no_breaks) + DOCTEST_BREAK_INTO_DEBUGGER(); + } + execute = false; + } + std::exit(EXIT_FAILURE); + } + + static void allocateAltStackMem() {} + static void freeAltStackMem() {} + + FatalConditionHandler() { + isSet = true; + // 32k seems enough for doctest to handle stack overflow, + // but the value was found experimentally, so there is no strong guarantee + guaranteeSize = 32 * 1024; + // Register an unhandled exception filter + previousTop = SetUnhandledExceptionFilter(handleException); + // Pass in guarantee size to be filled + SetThreadStackGuarantee(&guaranteeSize); + + // On Windows uncaught exceptions from another thread, exceptions from + // destructors, or calls to std::terminate are not a SEH exception + + // The terminal handler gets called when: + // - std::terminate is called FROM THE TEST RUNNER THREAD + // - an exception is thrown from a destructor FROM THE TEST RUNNER THREAD + original_terminate_handler = std::get_terminate(); + std::set_terminate([]() DOCTEST_NOEXCEPT { + reportFatal("Terminate handler called"); + if(isDebuggerActive() && !g_cs->no_breaks) + DOCTEST_BREAK_INTO_DEBUGGER(); + std::exit(EXIT_FAILURE); // explicitly exit - otherwise the SIGABRT handler may be called as well + }); + + // SIGABRT is raised when: + // - std::terminate is called FROM A DIFFERENT THREAD + // - an exception is thrown from a destructor FROM A DIFFERENT THREAD + // - an uncaught exception is thrown FROM A DIFFERENT THREAD + prev_sigabrt_handler = std::signal(SIGABRT, [](int signal) DOCTEST_NOEXCEPT { + if(signal == SIGABRT) { + reportFatal("SIGABRT - Abort (abnormal termination) signal"); + if(isDebuggerActive() && !g_cs->no_breaks) + DOCTEST_BREAK_INTO_DEBUGGER(); + std::exit(EXIT_FAILURE); + } + }); + + // The following settings are taken from google test, and more + // specifically from UnitTest::Run() inside of gtest.cc + + // the user does not want to see pop-up dialogs about crashes + prev_error_mode_1 = SetErrorMode(SEM_FAILCRITICALERRORS | SEM_NOALIGNMENTFAULTEXCEPT | + SEM_NOGPFAULTERRORBOX | SEM_NOOPENFILEERRORBOX); + // This forces the abort message to go to stderr in all circumstances. + prev_error_mode_2 = _set_error_mode(_OUT_TO_STDERR); + // In the debug version, Visual Studio pops up a separate dialog + // offering a choice to debug the aborted program - we want to disable that. + prev_abort_behavior = _set_abort_behavior(0x0, _WRITE_ABORT_MSG | _CALL_REPORTFAULT); + // In debug mode, the Windows CRT can crash with an assertion over invalid + // input (e.g. passing an invalid file descriptor). The default handling + // for these assertions is to pop up a dialog and wait for user input. + // Instead ask the CRT to dump such assertions to stderr non-interactively. + prev_report_mode = _CrtSetReportMode(_CRT_ASSERT, _CRTDBG_MODE_FILE | _CRTDBG_MODE_DEBUG); + prev_report_file = _CrtSetReportFile(_CRT_ASSERT, _CRTDBG_FILE_STDERR); + } + + static void reset() { + if(isSet) { + // Unregister handler and restore the old guarantee + SetUnhandledExceptionFilter(previousTop); + SetThreadStackGuarantee(&guaranteeSize); + std::set_terminate(original_terminate_handler); + std::signal(SIGABRT, prev_sigabrt_handler); + SetErrorMode(prev_error_mode_1); + _set_error_mode(prev_error_mode_2); + _set_abort_behavior(prev_abort_behavior, _WRITE_ABORT_MSG | _CALL_REPORTFAULT); + static_cast(_CrtSetReportMode(_CRT_ASSERT, prev_report_mode)); + static_cast(_CrtSetReportFile(_CRT_ASSERT, prev_report_file)); + isSet = false; + } + } + + ~FatalConditionHandler() { reset(); } + + private: + static UINT prev_error_mode_1; + static int prev_error_mode_2; + static unsigned int prev_abort_behavior; + static int prev_report_mode; + static _HFILE prev_report_file; + static void (DOCTEST_CDECL *prev_sigabrt_handler)(int); + static std::terminate_handler original_terminate_handler; + static bool isSet; + static ULONG guaranteeSize; + static LPTOP_LEVEL_EXCEPTION_FILTER previousTop; + }; + + UINT FatalConditionHandler::prev_error_mode_1; + int FatalConditionHandler::prev_error_mode_2; + unsigned int FatalConditionHandler::prev_abort_behavior; + int FatalConditionHandler::prev_report_mode; + _HFILE FatalConditionHandler::prev_report_file; + void (DOCTEST_CDECL *FatalConditionHandler::prev_sigabrt_handler)(int); + std::terminate_handler FatalConditionHandler::original_terminate_handler; + bool FatalConditionHandler::isSet = false; + ULONG FatalConditionHandler::guaranteeSize = 0; + LPTOP_LEVEL_EXCEPTION_FILTER FatalConditionHandler::previousTop = nullptr; + +#else // DOCTEST_PLATFORM_WINDOWS + + struct SignalDefs + { + int id; + const char* name; + }; + SignalDefs signalDefs[] = {{SIGINT, "SIGINT - Terminal interrupt signal"}, + {SIGILL, "SIGILL - Illegal instruction signal"}, + {SIGFPE, "SIGFPE - Floating point error signal"}, + {SIGSEGV, "SIGSEGV - Segmentation violation signal"}, + {SIGTERM, "SIGTERM - Termination request signal"}, + {SIGABRT, "SIGABRT - Abort (abnormal termination) signal"}}; + + struct FatalConditionHandler + { + static bool isSet; + static struct sigaction oldSigActions[DOCTEST_COUNTOF(signalDefs)]; + static stack_t oldSigStack; + static size_t altStackSize; + static char* altStackMem; + + static void handleSignal(int sig) { + const char* name = ""; + for(std::size_t i = 0; i < DOCTEST_COUNTOF(signalDefs); ++i) { + SignalDefs& def = signalDefs[i]; + if(sig == def.id) { + name = def.name; + break; + } + } + reset(); + reportFatal(name); + raise(sig); + } + + static void allocateAltStackMem() { + altStackMem = new char[altStackSize]; + } + + static void freeAltStackMem() { + delete[] altStackMem; + } + + FatalConditionHandler() { + isSet = true; + stack_t sigStack; + sigStack.ss_sp = altStackMem; + sigStack.ss_size = altStackSize; + sigStack.ss_flags = 0; + sigaltstack(&sigStack, &oldSigStack); + struct sigaction sa = {}; + sa.sa_handler = handleSignal; + sa.sa_flags = SA_ONSTACK; + for(std::size_t i = 0; i < DOCTEST_COUNTOF(signalDefs); ++i) { + sigaction(signalDefs[i].id, &sa, &oldSigActions[i]); + } + } + + ~FatalConditionHandler() { reset(); } + static void reset() { + if(isSet) { + // Set signals back to previous values -- hopefully nobody overwrote them in the meantime + for(std::size_t i = 0; i < DOCTEST_COUNTOF(signalDefs); ++i) { + sigaction(signalDefs[i].id, &oldSigActions[i], nullptr); + } + // Return the old stack + sigaltstack(&oldSigStack, nullptr); + isSet = false; + } + } + }; + + bool FatalConditionHandler::isSet = false; + struct sigaction FatalConditionHandler::oldSigActions[DOCTEST_COUNTOF(signalDefs)] = {}; + stack_t FatalConditionHandler::oldSigStack = {}; + size_t FatalConditionHandler::altStackSize = 4 * SIGSTKSZ; + char* FatalConditionHandler::altStackMem = nullptr; + +#endif // DOCTEST_PLATFORM_WINDOWS +#endif // DOCTEST_CONFIG_POSIX_SIGNALS || DOCTEST_CONFIG_WINDOWS_SEH + +} // namespace + +namespace { + using namespace detail; + +#ifdef DOCTEST_PLATFORM_WINDOWS +#define DOCTEST_OUTPUT_DEBUG_STRING(text) ::OutputDebugStringA(text) +#else + // TODO: integration with XCode and other IDEs +#define DOCTEST_OUTPUT_DEBUG_STRING(text) +#endif // Platform + + void addAssert(assertType::Enum at) { + if((at & assertType::is_warn) == 0) //!OCLINT bitwise operator in conditional + g_cs->numAssertsCurrentTest_atomic++; + } + + void addFailedAssert(assertType::Enum at) { + if((at & assertType::is_warn) == 0) //!OCLINT bitwise operator in conditional + g_cs->numAssertsFailedCurrentTest_atomic++; + } + +#if defined(DOCTEST_CONFIG_POSIX_SIGNALS) || defined(DOCTEST_CONFIG_WINDOWS_SEH) + void reportFatal(const std::string& message) { + g_cs->failure_flags |= TestCaseFailureReason::Crash; + + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_exception, {message.c_str(), true}); + + while (g_cs->subcaseStack.size()) { + g_cs->subcaseStack.pop_back(); + DOCTEST_ITERATE_THROUGH_REPORTERS(subcase_end, DOCTEST_EMPTY); + } + + g_cs->finalizeTestCaseData(); + + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_end, *g_cs); + + DOCTEST_ITERATE_THROUGH_REPORTERS(test_run_end, *g_cs); + } +#endif // DOCTEST_CONFIG_POSIX_SIGNALS || DOCTEST_CONFIG_WINDOWS_SEH +} // namespace + +AssertData::AssertData(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type, const StringContains& exception_string) + : m_test_case(g_cs->currentTest), m_at(at), m_file(file), m_line(line), m_expr(expr), + m_failed(true), m_threw(false), m_threw_as(false), m_exception_type(exception_type), + m_exception_string(exception_string) { +#if DOCTEST_MSVC + if (m_expr[0] == ' ') // this happens when variadic macros are disabled under MSVC + ++m_expr; +#endif // MSVC +} + +namespace detail { + ResultBuilder::ResultBuilder(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type, const String& exception_string) + : AssertData(at, file, line, expr, exception_type, exception_string) { } + + ResultBuilder::ResultBuilder(assertType::Enum at, const char* file, int line, const char* expr, + const char* exception_type, const Contains& exception_string) + : AssertData(at, file, line, expr, exception_type, exception_string) { } + + void ResultBuilder::setResult(const Result& res) { + m_decomp = res.m_decomp; + m_failed = !res.m_passed; + } + + void ResultBuilder::translateException() { + m_threw = true; + m_exception = translateActiveException(); + } + + bool ResultBuilder::log() { + if(m_at & assertType::is_throws) { //!OCLINT bitwise operator in conditional + m_failed = !m_threw; + } else if((m_at & assertType::is_throws_as) && (m_at & assertType::is_throws_with)) { //!OCLINT + m_failed = !m_threw_as || !m_exception_string.check(m_exception); + } else if(m_at & assertType::is_throws_as) { //!OCLINT bitwise operator in conditional + m_failed = !m_threw_as; + } else if(m_at & assertType::is_throws_with) { //!OCLINT bitwise operator in conditional + m_failed = !m_exception_string.check(m_exception); + } else if(m_at & assertType::is_nothrow) { //!OCLINT bitwise operator in conditional + m_failed = m_threw; + } + + if(m_exception.size()) + m_exception = "\"" + m_exception + "\""; + + if(is_running_in_test) { + addAssert(m_at); + DOCTEST_ITERATE_THROUGH_REPORTERS(log_assert, *this); + + if(m_failed) + addFailedAssert(m_at); + } else if(m_failed) { + failed_out_of_a_testing_context(*this); + } + + return m_failed && isDebuggerActive() && !getContextOptions()->no_breaks && + (g_cs->currentTest == nullptr || !g_cs->currentTest->m_no_breaks); // break into debugger + } + + void ResultBuilder::react() const { + if(m_failed && checkIfShouldThrow(m_at)) + throwException(); + } + + void failed_out_of_a_testing_context(const AssertData& ad) { + if(g_cs->ah) + g_cs->ah(ad); + else + std::abort(); + } + + bool decomp_assert(assertType::Enum at, const char* file, int line, const char* expr, + const Result& result) { + bool failed = !result.m_passed; + + // ################################################################################### + // IF THE DEBUGGER BREAKS HERE - GO 1 LEVEL UP IN THE CALLSTACK FOR THE FAILING ASSERT + // THIS IS THE EFFECT OF HAVING 'DOCTEST_CONFIG_SUPER_FAST_ASSERTS' DEFINED + // ################################################################################### + DOCTEST_ASSERT_OUT_OF_TESTS(result.m_decomp); + DOCTEST_ASSERT_IN_TESTS(result.m_decomp); + return !failed; + } + + MessageBuilder::MessageBuilder(const char* file, int line, assertType::Enum severity) { + m_stream = tlssPush(); + m_file = file; + m_line = line; + m_severity = severity; + } + + MessageBuilder::~MessageBuilder() { + if (!logged) + tlssPop(); + } + + DOCTEST_DEFINE_INTERFACE(IExceptionTranslator) + + bool MessageBuilder::log() { + if (!logged) { + m_string = tlssPop(); + logged = true; + } + + DOCTEST_ITERATE_THROUGH_REPORTERS(log_message, *this); + + const bool isWarn = m_severity & assertType::is_warn; + + // warn is just a message in this context so we don't treat it as an assert + if(!isWarn) { + addAssert(m_severity); + addFailedAssert(m_severity); + } + + return isDebuggerActive() && !getContextOptions()->no_breaks && !isWarn && + (g_cs->currentTest == nullptr || !g_cs->currentTest->m_no_breaks); // break into debugger + } + + void MessageBuilder::react() { + if(m_severity & assertType::is_require) //!OCLINT bitwise operator in conditional + throwException(); + } +} // namespace detail +namespace { + using namespace detail; + + // clang-format off + +// ================================================================================================= +// The following code has been taken verbatim from Catch2/include/internal/catch_xmlwriter.h/cpp +// This is done so cherry-picking bug fixes is trivial - even the style/formatting is untouched. +// ================================================================================================= + + class XmlEncode { + public: + enum ForWhat { ForTextNodes, ForAttributes }; + + XmlEncode( std::string const& str, ForWhat forWhat = ForTextNodes ); + + void encodeTo( std::ostream& os ) const; + + friend std::ostream& operator << ( std::ostream& os, XmlEncode const& xmlEncode ); + + private: + std::string m_str; + ForWhat m_forWhat; + }; + + class XmlWriter { + public: + + class ScopedElement { + public: + ScopedElement( XmlWriter* writer ); + + ScopedElement( ScopedElement&& other ) DOCTEST_NOEXCEPT; + ScopedElement& operator=( ScopedElement&& other ) DOCTEST_NOEXCEPT; + + ~ScopedElement(); + + ScopedElement& writeText( std::string const& text, bool indent = true ); + + template + ScopedElement& writeAttribute( std::string const& name, T const& attribute ) { + m_writer->writeAttribute( name, attribute ); + return *this; + } + + private: + mutable XmlWriter* m_writer = nullptr; + }; + +#ifndef DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + XmlWriter( std::ostream& os = std::cout ); +#else // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + XmlWriter( std::ostream& os ); +#endif // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + ~XmlWriter(); + + XmlWriter( XmlWriter const& ) = delete; + XmlWriter& operator=( XmlWriter const& ) = delete; + + XmlWriter& startElement( std::string const& name ); + + ScopedElement scopedElement( std::string const& name ); + + XmlWriter& endElement(); + + XmlWriter& writeAttribute( std::string const& name, std::string const& attribute ); + + XmlWriter& writeAttribute( std::string const& name, const char* attribute ); + + XmlWriter& writeAttribute( std::string const& name, bool attribute ); + + template + XmlWriter& writeAttribute( std::string const& name, T const& attribute ) { + std::stringstream rss; + rss << attribute; + return writeAttribute( name, rss.str() ); + } + + XmlWriter& writeText( std::string const& text, bool indent = true ); + + //XmlWriter& writeComment( std::string const& text ); + + //void writeStylesheetRef( std::string const& url ); + + //XmlWriter& writeBlankLine(); + + void ensureTagClosed(); + + void writeDeclaration(); + + private: + + void newlineIfNecessary(); + + bool m_tagIsOpen = false; + bool m_needsNewline = false; + std::vector m_tags; + std::string m_indent; + std::ostream& m_os; + }; + +// ================================================================================================= +// The following code has been taken verbatim from Catch2/include/internal/catch_xmlwriter.h/cpp +// This is done so cherry-picking bug fixes is trivial - even the style/formatting is untouched. +// ================================================================================================= + +using uchar = unsigned char; + +namespace { + + size_t trailingBytes(unsigned char c) { + if ((c & 0xE0) == 0xC0) { + return 2; + } + if ((c & 0xF0) == 0xE0) { + return 3; + } + if ((c & 0xF8) == 0xF0) { + return 4; + } + DOCTEST_INTERNAL_ERROR("Invalid multibyte utf-8 start byte encountered"); + } + + uint32_t headerValue(unsigned char c) { + if ((c & 0xE0) == 0xC0) { + return c & 0x1F; + } + if ((c & 0xF0) == 0xE0) { + return c & 0x0F; + } + if ((c & 0xF8) == 0xF0) { + return c & 0x07; + } + DOCTEST_INTERNAL_ERROR("Invalid multibyte utf-8 start byte encountered"); + } + + void hexEscapeChar(std::ostream& os, unsigned char c) { + std::ios_base::fmtflags f(os.flags()); + os << "\\x" + << std::uppercase << std::hex << std::setfill('0') << std::setw(2) + << static_cast(c); + os.flags(f); + } + +} // anonymous namespace + + XmlEncode::XmlEncode( std::string const& str, ForWhat forWhat ) + : m_str( str ), + m_forWhat( forWhat ) + {} + + void XmlEncode::encodeTo( std::ostream& os ) const { + // Apostrophe escaping not necessary if we always use " to write attributes + // (see: https://www.w3.org/TR/xml/#syntax) + + for( std::size_t idx = 0; idx < m_str.size(); ++ idx ) { + uchar c = m_str[idx]; + switch (c) { + case '<': os << "<"; break; + case '&': os << "&"; break; + + case '>': + // See: https://www.w3.org/TR/xml/#syntax + if (idx > 2 && m_str[idx - 1] == ']' && m_str[idx - 2] == ']') + os << ">"; + else + os << c; + break; + + case '\"': + if (m_forWhat == ForAttributes) + os << """; + else + os << c; + break; + + default: + // Check for control characters and invalid utf-8 + + // Escape control characters in standard ascii + // see https://stackoverflow.com/questions/404107/why-are-control-characters-illegal-in-xml-1-0 + if (c < 0x09 || (c > 0x0D && c < 0x20) || c == 0x7F) { + hexEscapeChar(os, c); + break; + } + + // Plain ASCII: Write it to stream + if (c < 0x7F) { + os << c; + break; + } + + // UTF-8 territory + // Check if the encoding is valid and if it is not, hex escape bytes. + // Important: We do not check the exact decoded values for validity, only the encoding format + // First check that this bytes is a valid lead byte: + // This means that it is not encoded as 1111 1XXX + // Or as 10XX XXXX + if (c < 0xC0 || + c >= 0xF8) { + hexEscapeChar(os, c); + break; + } + + auto encBytes = trailingBytes(c); + // Are there enough bytes left to avoid accessing out-of-bounds memory? + if (idx + encBytes - 1 >= m_str.size()) { + hexEscapeChar(os, c); + break; + } + // The header is valid, check data + // The next encBytes bytes must together be a valid utf-8 + // This means: bitpattern 10XX XXXX and the extracted value is sane (ish) + bool valid = true; + uint32_t value = headerValue(c); + for (std::size_t n = 1; n < encBytes; ++n) { + uchar nc = m_str[idx + n]; + valid &= ((nc & 0xC0) == 0x80); + value = (value << 6) | (nc & 0x3F); + } + + if ( + // Wrong bit pattern of following bytes + (!valid) || + // Overlong encodings + (value < 0x80) || + ( value < 0x800 && encBytes > 2) || // removed "0x80 <= value &&" because redundant + (0x800 < value && value < 0x10000 && encBytes > 3) || + // Encoded value out of range + (value >= 0x110000) + ) { + hexEscapeChar(os, c); + break; + } + + // If we got here, this is in fact a valid(ish) utf-8 sequence + for (std::size_t n = 0; n < encBytes; ++n) { + os << m_str[idx + n]; + } + idx += encBytes - 1; + break; + } + } + } + + std::ostream& operator << ( std::ostream& os, XmlEncode const& xmlEncode ) { + xmlEncode.encodeTo( os ); + return os; + } + + XmlWriter::ScopedElement::ScopedElement( XmlWriter* writer ) + : m_writer( writer ) + {} + + XmlWriter::ScopedElement::ScopedElement( ScopedElement&& other ) DOCTEST_NOEXCEPT + : m_writer( other.m_writer ){ + other.m_writer = nullptr; + } + XmlWriter::ScopedElement& XmlWriter::ScopedElement::operator=( ScopedElement&& other ) DOCTEST_NOEXCEPT { + if ( m_writer ) { + m_writer->endElement(); + } + m_writer = other.m_writer; + other.m_writer = nullptr; + return *this; + } + + + XmlWriter::ScopedElement::~ScopedElement() { + if( m_writer ) + m_writer->endElement(); + } + + XmlWriter::ScopedElement& XmlWriter::ScopedElement::writeText( std::string const& text, bool indent ) { + m_writer->writeText( text, indent ); + return *this; + } + + XmlWriter::XmlWriter( std::ostream& os ) : m_os( os ) + { + // writeDeclaration(); // called explicitly by the reporters that use the writer class - see issue #627 + } + + XmlWriter::~XmlWriter() { + while( !m_tags.empty() ) + endElement(); + } + + XmlWriter& XmlWriter::startElement( std::string const& name ) { + ensureTagClosed(); + newlineIfNecessary(); + m_os << m_indent << '<' << name; + m_tags.push_back( name ); + m_indent += " "; + m_tagIsOpen = true; + return *this; + } + + XmlWriter::ScopedElement XmlWriter::scopedElement( std::string const& name ) { + ScopedElement scoped( this ); + startElement( name ); + return scoped; + } + + XmlWriter& XmlWriter::endElement() { + newlineIfNecessary(); + m_indent = m_indent.substr( 0, m_indent.size()-2 ); + if( m_tagIsOpen ) { + m_os << "/>"; + m_tagIsOpen = false; + } + else { + m_os << m_indent << ""; + } + m_os << std::endl; + m_tags.pop_back(); + return *this; + } + + XmlWriter& XmlWriter::writeAttribute( std::string const& name, std::string const& attribute ) { + if( !name.empty() && !attribute.empty() ) + m_os << ' ' << name << "=\"" << XmlEncode( attribute, XmlEncode::ForAttributes ) << '"'; + return *this; + } + + XmlWriter& XmlWriter::writeAttribute( std::string const& name, const char* attribute ) { + if( !name.empty() && attribute && attribute[0] != '\0' ) + m_os << ' ' << name << "=\"" << XmlEncode( attribute, XmlEncode::ForAttributes ) << '"'; + return *this; + } + + XmlWriter& XmlWriter::writeAttribute( std::string const& name, bool attribute ) { + m_os << ' ' << name << "=\"" << ( attribute ? "true" : "false" ) << '"'; + return *this; + } + + XmlWriter& XmlWriter::writeText( std::string const& text, bool indent ) { + if( !text.empty() ){ + bool tagWasOpen = m_tagIsOpen; + ensureTagClosed(); + if( tagWasOpen && indent ) + m_os << m_indent; + m_os << XmlEncode( text ); + m_needsNewline = true; + } + return *this; + } + + //XmlWriter& XmlWriter::writeComment( std::string const& text ) { + // ensureTagClosed(); + // m_os << m_indent << ""; + // m_needsNewline = true; + // return *this; + //} + + //void XmlWriter::writeStylesheetRef( std::string const& url ) { + // m_os << "\n"; + //} + + //XmlWriter& XmlWriter::writeBlankLine() { + // ensureTagClosed(); + // m_os << '\n'; + // return *this; + //} + + void XmlWriter::ensureTagClosed() { + if( m_tagIsOpen ) { + m_os << ">" << std::endl; + m_tagIsOpen = false; + } + } + + void XmlWriter::writeDeclaration() { + m_os << "\n"; + } + + void XmlWriter::newlineIfNecessary() { + if( m_needsNewline ) { + m_os << std::endl; + m_needsNewline = false; + } + } + +// ================================================================================================= +// End of copy-pasted code from Catch +// ================================================================================================= + + // clang-format on + + struct XmlReporter : public IReporter + { + XmlWriter xml; + DOCTEST_DECLARE_MUTEX(mutex) + + // caching pointers/references to objects of these types - safe to do + const ContextOptions& opt; + const TestCaseData* tc = nullptr; + + XmlReporter(const ContextOptions& co) + : xml(*co.cout) + , opt(co) {} + + void log_contexts() { + int num_contexts = get_num_active_contexts(); + if(num_contexts) { + auto contexts = get_active_contexts(); + std::stringstream ss; + for(int i = 0; i < num_contexts; ++i) { + contexts[i]->stringify(&ss); + xml.scopedElement("Info").writeText(ss.str()); + ss.str(""); + } + } + } + + unsigned line(unsigned l) const { return opt.no_line_numbers ? 0 : l; } + + void test_case_start_impl(const TestCaseData& in) { + bool open_ts_tag = false; + if(tc != nullptr) { // we have already opened a test suite + if(std::strcmp(tc->m_test_suite, in.m_test_suite) != 0) { + xml.endElement(); + open_ts_tag = true; + } + } + else { + open_ts_tag = true; // first test case ==> first test suite + } + + if(open_ts_tag) { + xml.startElement("TestSuite"); + xml.writeAttribute("name", in.m_test_suite); + } + + tc = ∈ + xml.startElement("TestCase") + .writeAttribute("name", in.m_name) + .writeAttribute("filename", skipPathFromFilename(in.m_file.c_str())) + .writeAttribute("line", line(in.m_line)) + .writeAttribute("description", in.m_description); + + if(Approx(in.m_timeout) != 0) + xml.writeAttribute("timeout", in.m_timeout); + if(in.m_may_fail) + xml.writeAttribute("may_fail", true); + if(in.m_should_fail) + xml.writeAttribute("should_fail", true); + } + + // ========================================================================================= + // WHAT FOLLOWS ARE OVERRIDES OF THE VIRTUAL METHODS OF THE REPORTER INTERFACE + // ========================================================================================= + + void report_query(const QueryData& in) override { + test_run_start(); + if(opt.list_reporters) { + for(auto& curr : getListeners()) + xml.scopedElement("Listener") + .writeAttribute("priority", curr.first.first) + .writeAttribute("name", curr.first.second); + for(auto& curr : getReporters()) + xml.scopedElement("Reporter") + .writeAttribute("priority", curr.first.first) + .writeAttribute("name", curr.first.second); + } else if(opt.count || opt.list_test_cases) { + for(unsigned i = 0; i < in.num_data; ++i) { + xml.scopedElement("TestCase").writeAttribute("name", in.data[i]->m_name) + .writeAttribute("testsuite", in.data[i]->m_test_suite) + .writeAttribute("filename", skipPathFromFilename(in.data[i]->m_file.c_str())) + .writeAttribute("line", line(in.data[i]->m_line)) + .writeAttribute("skipped", in.data[i]->m_skip); + } + xml.scopedElement("OverallResultsTestCases") + .writeAttribute("unskipped", in.run_stats->numTestCasesPassingFilters); + } else if(opt.list_test_suites) { + for(unsigned i = 0; i < in.num_data; ++i) + xml.scopedElement("TestSuite").writeAttribute("name", in.data[i]->m_test_suite); + xml.scopedElement("OverallResultsTestCases") + .writeAttribute("unskipped", in.run_stats->numTestCasesPassingFilters); + xml.scopedElement("OverallResultsTestSuites") + .writeAttribute("unskipped", in.run_stats->numTestSuitesPassingFilters); + } + xml.endElement(); + } + + void test_run_start() override { + xml.writeDeclaration(); + + // remove .exe extension - mainly to have the same output on UNIX and Windows + std::string binary_name = skipPathFromFilename(opt.binary_name.c_str()); +#ifdef DOCTEST_PLATFORM_WINDOWS + if(binary_name.rfind(".exe") != std::string::npos) + binary_name = binary_name.substr(0, binary_name.length() - 4); +#endif // DOCTEST_PLATFORM_WINDOWS + + xml.startElement("doctest").writeAttribute("binary", binary_name); + if(opt.no_version == false) + xml.writeAttribute("version", DOCTEST_VERSION_STR); + + // only the consequential ones (TODO: filters) + xml.scopedElement("Options") + .writeAttribute("order_by", opt.order_by.c_str()) + .writeAttribute("rand_seed", opt.rand_seed) + .writeAttribute("first", opt.first) + .writeAttribute("last", opt.last) + .writeAttribute("abort_after", opt.abort_after) + .writeAttribute("subcase_filter_levels", opt.subcase_filter_levels) + .writeAttribute("case_sensitive", opt.case_sensitive) + .writeAttribute("no_throw", opt.no_throw) + .writeAttribute("no_skip", opt.no_skip); + } + + void test_run_end(const TestRunStats& p) override { + if(tc) // the TestSuite tag - only if there has been at least 1 test case + xml.endElement(); + + xml.scopedElement("OverallResultsAsserts") + .writeAttribute("successes", p.numAsserts - p.numAssertsFailed) + .writeAttribute("failures", p.numAssertsFailed); + + xml.startElement("OverallResultsTestCases") + .writeAttribute("successes", + p.numTestCasesPassingFilters - p.numTestCasesFailed) + .writeAttribute("failures", p.numTestCasesFailed); + if(opt.no_skipped_summary == false) + xml.writeAttribute("skipped", p.numTestCases - p.numTestCasesPassingFilters); + xml.endElement(); + + xml.endElement(); + } + + void test_case_start(const TestCaseData& in) override { + test_case_start_impl(in); + xml.ensureTagClosed(); + } + + void test_case_reenter(const TestCaseData&) override {} + + void test_case_end(const CurrentTestCaseStats& st) override { + xml.startElement("OverallResultsAsserts") + .writeAttribute("successes", + st.numAssertsCurrentTest - st.numAssertsFailedCurrentTest) + .writeAttribute("failures", st.numAssertsFailedCurrentTest) + .writeAttribute("test_case_success", st.testCaseSuccess); + if(opt.duration) + xml.writeAttribute("duration", st.seconds); + if(tc->m_expected_failures) + xml.writeAttribute("expected_failures", tc->m_expected_failures); + xml.endElement(); + + xml.endElement(); + } + + void test_case_exception(const TestCaseException& e) override { + DOCTEST_LOCK_MUTEX(mutex) + + xml.scopedElement("Exception") + .writeAttribute("crash", e.is_crash) + .writeText(e.error_string.c_str()); + } + + void subcase_start(const SubcaseSignature& in) override { + xml.startElement("SubCase") + .writeAttribute("name", in.m_name) + .writeAttribute("filename", skipPathFromFilename(in.m_file)) + .writeAttribute("line", line(in.m_line)); + xml.ensureTagClosed(); + } + + void subcase_end() override { xml.endElement(); } + + void log_assert(const AssertData& rb) override { + if(!rb.m_failed && !opt.success) + return; + + DOCTEST_LOCK_MUTEX(mutex) + + xml.startElement("Expression") + .writeAttribute("success", !rb.m_failed) + .writeAttribute("type", assertString(rb.m_at)) + .writeAttribute("filename", skipPathFromFilename(rb.m_file)) + .writeAttribute("line", line(rb.m_line)); + + xml.scopedElement("Original").writeText(rb.m_expr); + + if(rb.m_threw) + xml.scopedElement("Exception").writeText(rb.m_exception.c_str()); + + if(rb.m_at & assertType::is_throws_as) + xml.scopedElement("ExpectedException").writeText(rb.m_exception_type); + if(rb.m_at & assertType::is_throws_with) + xml.scopedElement("ExpectedExceptionString").writeText(rb.m_exception_string.c_str()); + if((rb.m_at & assertType::is_normal) && !rb.m_threw) + xml.scopedElement("Expanded").writeText(rb.m_decomp.c_str()); + + log_contexts(); + + xml.endElement(); + } + + void log_message(const MessageData& mb) override { + DOCTEST_LOCK_MUTEX(mutex) + + xml.startElement("Message") + .writeAttribute("type", failureString(mb.m_severity)) + .writeAttribute("filename", skipPathFromFilename(mb.m_file)) + .writeAttribute("line", line(mb.m_line)); + + xml.scopedElement("Text").writeText(mb.m_string.c_str()); + + log_contexts(); + + xml.endElement(); + } + + void test_case_skipped(const TestCaseData& in) override { + if(opt.no_skipped_summary == false) { + test_case_start_impl(in); + xml.writeAttribute("skipped", "true"); + xml.endElement(); + } + } + }; + + DOCTEST_REGISTER_REPORTER("xml", 0, XmlReporter); + + void fulltext_log_assert_to_stream(std::ostream& s, const AssertData& rb) { + if((rb.m_at & (assertType::is_throws_as | assertType::is_throws_with)) == + 0) //!OCLINT bitwise operator in conditional + s << Color::Cyan << assertString(rb.m_at) << "( " << rb.m_expr << " ) " + << Color::None; + + if(rb.m_at & assertType::is_throws) { //!OCLINT bitwise operator in conditional + s << (rb.m_threw ? "threw as expected!" : "did NOT throw at all!") << "\n"; + } else if((rb.m_at & assertType::is_throws_as) && + (rb.m_at & assertType::is_throws_with)) { //!OCLINT + s << Color::Cyan << assertString(rb.m_at) << "( " << rb.m_expr << ", \"" + << rb.m_exception_string.c_str() + << "\", " << rb.m_exception_type << " ) " << Color::None; + if(rb.m_threw) { + if(!rb.m_failed) { + s << "threw as expected!\n"; + } else { + s << "threw a DIFFERENT exception! (contents: " << rb.m_exception << ")\n"; + } + } else { + s << "did NOT throw at all!\n"; + } + } else if(rb.m_at & + assertType::is_throws_as) { //!OCLINT bitwise operator in conditional + s << Color::Cyan << assertString(rb.m_at) << "( " << rb.m_expr << ", " + << rb.m_exception_type << " ) " << Color::None + << (rb.m_threw ? (rb.m_threw_as ? "threw as expected!" : + "threw a DIFFERENT exception: ") : + "did NOT throw at all!") + << Color::Cyan << rb.m_exception << "\n"; + } else if(rb.m_at & + assertType::is_throws_with) { //!OCLINT bitwise operator in conditional + s << Color::Cyan << assertString(rb.m_at) << "( " << rb.m_expr << ", \"" + << rb.m_exception_string.c_str() + << "\" ) " << Color::None + << (rb.m_threw ? (!rb.m_failed ? "threw as expected!" : + "threw a DIFFERENT exception: ") : + "did NOT throw at all!") + << Color::Cyan << rb.m_exception << "\n"; + } else if(rb.m_at & assertType::is_nothrow) { //!OCLINT bitwise operator in conditional + s << (rb.m_threw ? "THREW exception: " : "didn't throw!") << Color::Cyan + << rb.m_exception << "\n"; + } else { + s << (rb.m_threw ? "THREW exception: " : + (!rb.m_failed ? "is correct!\n" : "is NOT correct!\n")); + if(rb.m_threw) + s << rb.m_exception << "\n"; + else + s << " values: " << assertString(rb.m_at) << "( " << rb.m_decomp << " )\n"; + } + } + + // TODO: + // - log_message() + // - respond to queries + // - honor remaining options + // - more attributes in tags + struct JUnitReporter : public IReporter + { + XmlWriter xml; + DOCTEST_DECLARE_MUTEX(mutex) + Timer timer; + std::vector deepestSubcaseStackNames; + + struct JUnitTestCaseData + { + static std::string getCurrentTimestamp() { + // Beware, this is not reentrant because of backward compatibility issues + // Also, UTC only, again because of backward compatibility (%z is C++11) + time_t rawtime; + std::time(&rawtime); + auto const timeStampSize = sizeof("2017-01-16T17:06:45Z"); + + std::tm timeInfo; +#ifdef DOCTEST_PLATFORM_WINDOWS + gmtime_s(&timeInfo, &rawtime); +#else // DOCTEST_PLATFORM_WINDOWS + gmtime_r(&rawtime, &timeInfo); +#endif // DOCTEST_PLATFORM_WINDOWS + + char timeStamp[timeStampSize]; + const char* const fmt = "%Y-%m-%dT%H:%M:%SZ"; + + std::strftime(timeStamp, timeStampSize, fmt, &timeInfo); + return std::string(timeStamp); + } + + struct JUnitTestMessage + { + JUnitTestMessage(const std::string& _message, const std::string& _type, const std::string& _details) + : message(_message), type(_type), details(_details) {} + + JUnitTestMessage(const std::string& _message, const std::string& _details) + : message(_message), type(), details(_details) {} + + std::string message, type, details; + }; + + struct JUnitTestCase + { + JUnitTestCase(const std::string& _classname, const std::string& _name) + : classname(_classname), name(_name), time(0), failures() {} + + std::string classname, name; + double time; + std::vector failures, errors; + }; + + void add(const std::string& classname, const std::string& name) { + testcases.emplace_back(classname, name); + } + + void appendSubcaseNamesToLastTestcase(std::vector nameStack) { + for(auto& curr: nameStack) + if(curr.size()) + testcases.back().name += std::string("/") + curr.c_str(); + } + + void addTime(double time) { + if(time < 1e-4) + time = 0; + testcases.back().time = time; + totalSeconds += time; + } + + void addFailure(const std::string& message, const std::string& type, const std::string& details) { + testcases.back().failures.emplace_back(message, type, details); + ++totalFailures; + } + + void addError(const std::string& message, const std::string& details) { + testcases.back().errors.emplace_back(message, details); + ++totalErrors; + } + + std::vector testcases; + double totalSeconds = 0; + int totalErrors = 0, totalFailures = 0; + }; + + JUnitTestCaseData testCaseData; + + // caching pointers/references to objects of these types - safe to do + const ContextOptions& opt; + const TestCaseData* tc = nullptr; + + JUnitReporter(const ContextOptions& co) + : xml(*co.cout) + , opt(co) {} + + unsigned line(unsigned l) const { return opt.no_line_numbers ? 0 : l; } + + // ========================================================================================= + // WHAT FOLLOWS ARE OVERRIDES OF THE VIRTUAL METHODS OF THE REPORTER INTERFACE + // ========================================================================================= + + void report_query(const QueryData&) override { + xml.writeDeclaration(); + } + + void test_run_start() override { + xml.writeDeclaration(); + } + + void test_run_end(const TestRunStats& p) override { + // remove .exe extension - mainly to have the same output on UNIX and Windows + std::string binary_name = skipPathFromFilename(opt.binary_name.c_str()); +#ifdef DOCTEST_PLATFORM_WINDOWS + if(binary_name.rfind(".exe") != std::string::npos) + binary_name = binary_name.substr(0, binary_name.length() - 4); +#endif // DOCTEST_PLATFORM_WINDOWS + xml.startElement("testsuites"); + xml.startElement("testsuite").writeAttribute("name", binary_name) + .writeAttribute("errors", testCaseData.totalErrors) + .writeAttribute("failures", testCaseData.totalFailures) + .writeAttribute("tests", p.numAsserts); + if(opt.no_time_in_output == false) { + xml.writeAttribute("time", testCaseData.totalSeconds); + xml.writeAttribute("timestamp", JUnitTestCaseData::getCurrentTimestamp()); + } + if(opt.no_version == false) + xml.writeAttribute("doctest_version", DOCTEST_VERSION_STR); + + for(const auto& testCase : testCaseData.testcases) { + xml.startElement("testcase") + .writeAttribute("classname", testCase.classname) + .writeAttribute("name", testCase.name); + if(opt.no_time_in_output == false) + xml.writeAttribute("time", testCase.time); + // This is not ideal, but it should be enough to mimic gtest's junit output. + xml.writeAttribute("status", "run"); + + for(const auto& failure : testCase.failures) { + xml.scopedElement("failure") + .writeAttribute("message", failure.message) + .writeAttribute("type", failure.type) + .writeText(failure.details, false); + } + + for(const auto& error : testCase.errors) { + xml.scopedElement("error") + .writeAttribute("message", error.message) + .writeText(error.details); + } + + xml.endElement(); + } + xml.endElement(); + xml.endElement(); + } + + void test_case_start(const TestCaseData& in) override { + testCaseData.add(skipPathFromFilename(in.m_file.c_str()), in.m_name); + timer.start(); + } + + void test_case_reenter(const TestCaseData& in) override { + testCaseData.addTime(timer.getElapsedSeconds()); + testCaseData.appendSubcaseNamesToLastTestcase(deepestSubcaseStackNames); + deepestSubcaseStackNames.clear(); + + timer.start(); + testCaseData.add(skipPathFromFilename(in.m_file.c_str()), in.m_name); + } + + void test_case_end(const CurrentTestCaseStats&) override { + testCaseData.addTime(timer.getElapsedSeconds()); + testCaseData.appendSubcaseNamesToLastTestcase(deepestSubcaseStackNames); + deepestSubcaseStackNames.clear(); + } + + void test_case_exception(const TestCaseException& e) override { + DOCTEST_LOCK_MUTEX(mutex) + testCaseData.addError("exception", e.error_string.c_str()); + } + + void subcase_start(const SubcaseSignature& in) override { + deepestSubcaseStackNames.push_back(in.m_name); + } + + void subcase_end() override {} + + void log_assert(const AssertData& rb) override { + if(!rb.m_failed) // report only failures & ignore the `success` option + return; + + DOCTEST_LOCK_MUTEX(mutex) + + std::ostringstream os; + os << skipPathFromFilename(rb.m_file) << (opt.gnu_file_line ? ":" : "(") + << line(rb.m_line) << (opt.gnu_file_line ? ":" : "):") << std::endl; + + fulltext_log_assert_to_stream(os, rb); + log_contexts(os); + testCaseData.addFailure(rb.m_decomp.c_str(), assertString(rb.m_at), os.str()); + } + + void log_message(const MessageData& mb) override { + if(mb.m_severity & assertType::is_warn) // report only failures + return; + + DOCTEST_LOCK_MUTEX(mutex) + + std::ostringstream os; + os << skipPathFromFilename(mb.m_file) << (opt.gnu_file_line ? ":" : "(") + << line(mb.m_line) << (opt.gnu_file_line ? ":" : "):") << std::endl; + + os << mb.m_string.c_str() << "\n"; + log_contexts(os); + + testCaseData.addFailure(mb.m_string.c_str(), + mb.m_severity & assertType::is_check ? "FAIL_CHECK" : "FAIL", os.str()); + } + + void test_case_skipped(const TestCaseData&) override {} + + void log_contexts(std::ostringstream& s) { + int num_contexts = get_num_active_contexts(); + if(num_contexts) { + auto contexts = get_active_contexts(); + + s << " logged: "; + for(int i = 0; i < num_contexts; ++i) { + s << (i == 0 ? "" : " "); + contexts[i]->stringify(&s); + s << std::endl; + } + } + } + }; + + DOCTEST_REGISTER_REPORTER("junit", 0, JUnitReporter); + + struct Whitespace + { + int nrSpaces; + explicit Whitespace(int nr) + : nrSpaces(nr) {} + }; + + std::ostream& operator<<(std::ostream& out, const Whitespace& ws) { + if(ws.nrSpaces != 0) + out << std::setw(ws.nrSpaces) << ' '; + return out; + } + + struct ConsoleReporter : public IReporter + { + std::ostream& s; + bool hasLoggedCurrentTestStart; + std::vector subcasesStack; + size_t currentSubcaseLevel; + DOCTEST_DECLARE_MUTEX(mutex) + + // caching pointers/references to objects of these types - safe to do + const ContextOptions& opt; + const TestCaseData* tc; + + ConsoleReporter(const ContextOptions& co) + : s(*co.cout) + , opt(co) {} + + ConsoleReporter(const ContextOptions& co, std::ostream& ostr) + : s(ostr) + , opt(co) {} + + // ========================================================================================= + // WHAT FOLLOWS ARE HELPERS USED BY THE OVERRIDES OF THE VIRTUAL METHODS OF THE INTERFACE + // ========================================================================================= + + void separator_to_stream() { + s << Color::Yellow + << "===============================================================================" + "\n"; + } + + const char* getSuccessOrFailString(bool success, assertType::Enum at, + const char* success_str) { + if(success) + return success_str; + return failureString(at); + } + + Color::Enum getSuccessOrFailColor(bool success, assertType::Enum at) { + return success ? Color::BrightGreen : + (at & assertType::is_warn) ? Color::Yellow : Color::Red; + } + + void successOrFailColoredStringToStream(bool success, assertType::Enum at, + const char* success_str = "SUCCESS") { + s << getSuccessOrFailColor(success, at) + << getSuccessOrFailString(success, at, success_str) << ": "; + } + + void log_contexts() { + int num_contexts = get_num_active_contexts(); + if(num_contexts) { + auto contexts = get_active_contexts(); + + s << Color::None << " logged: "; + for(int i = 0; i < num_contexts; ++i) { + s << (i == 0 ? "" : " "); + contexts[i]->stringify(&s); + s << "\n"; + } + } + + s << "\n"; + } + + // this was requested to be made virtual so users could override it + virtual void file_line_to_stream(const char* file, int line, + const char* tail = "") { + s << Color::LightGrey << skipPathFromFilename(file) << (opt.gnu_file_line ? ":" : "(") + << (opt.no_line_numbers ? 0 : line) // 0 or the real num depending on the option + << (opt.gnu_file_line ? ":" : "):") << tail; + } + + void logTestStart() { + if(hasLoggedCurrentTestStart) + return; + + separator_to_stream(); + file_line_to_stream(tc->m_file.c_str(), tc->m_line, "\n"); + if(tc->m_description) + s << Color::Yellow << "DESCRIPTION: " << Color::None << tc->m_description << "\n"; + if(tc->m_test_suite && tc->m_test_suite[0] != '\0') + s << Color::Yellow << "TEST SUITE: " << Color::None << tc->m_test_suite << "\n"; + if(strncmp(tc->m_name, " Scenario:", 11) != 0) + s << Color::Yellow << "TEST CASE: "; + s << Color::None << tc->m_name << "\n"; + + for(size_t i = 0; i < currentSubcaseLevel; ++i) { + if(subcasesStack[i].m_name[0] != '\0') + s << " " << subcasesStack[i].m_name << "\n"; + } + + if(currentSubcaseLevel != subcasesStack.size()) { + s << Color::Yellow << "\nDEEPEST SUBCASE STACK REACHED (DIFFERENT FROM THE CURRENT ONE):\n" << Color::None; + for(size_t i = 0; i < subcasesStack.size(); ++i) { + if(subcasesStack[i].m_name[0] != '\0') + s << " " << subcasesStack[i].m_name << "\n"; + } + } + + s << "\n"; + + hasLoggedCurrentTestStart = true; + } + + void printVersion() { + if(opt.no_version == false) + s << Color::Cyan << "[doctest] " << Color::None << "doctest version is \"" + << DOCTEST_VERSION_STR << "\"\n"; + } + + void printIntro() { + if(opt.no_intro == false) { + printVersion(); + s << Color::Cyan << "[doctest] " << Color::None + << "run with \"--" DOCTEST_OPTIONS_PREFIX_DISPLAY "help\" for options\n"; + } + } + + void printHelp() { + int sizePrefixDisplay = static_cast(strlen(DOCTEST_OPTIONS_PREFIX_DISPLAY)); + printVersion(); + // clang-format off + s << Color::Cyan << "[doctest]\n" << Color::None; + s << Color::Cyan << "[doctest] " << Color::None; + s << "boolean values: \"1/on/yes/true\" or \"0/off/no/false\"\n"; + s << Color::Cyan << "[doctest] " << Color::None; + s << "filter values: \"str1,str2,str3\" (comma separated strings)\n"; + s << Color::Cyan << "[doctest]\n" << Color::None; + s << Color::Cyan << "[doctest] " << Color::None; + s << "filters use wildcards for matching strings\n"; + s << Color::Cyan << "[doctest] " << Color::None; + s << "something passes a filter if any of the strings in a filter matches\n"; +#ifndef DOCTEST_CONFIG_NO_UNPREFIXED_OPTIONS + s << Color::Cyan << "[doctest]\n" << Color::None; + s << Color::Cyan << "[doctest] " << Color::None; + s << "ALL FLAGS, OPTIONS AND FILTERS ALSO AVAILABLE WITH A \"" DOCTEST_CONFIG_OPTIONS_PREFIX "\" PREFIX!!!\n"; +#endif + s << Color::Cyan << "[doctest]\n" << Color::None; + s << Color::Cyan << "[doctest] " << Color::None; + s << "Query flags - the program quits after them. Available:\n\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "?, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "help, -" DOCTEST_OPTIONS_PREFIX_DISPLAY "h " + << Whitespace(sizePrefixDisplay*0) << "prints this message\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "v, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "version " + << Whitespace(sizePrefixDisplay*1) << "prints the version\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "c, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "count " + << Whitespace(sizePrefixDisplay*1) << "prints the number of matching tests\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ltc, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "list-test-cases " + << Whitespace(sizePrefixDisplay*1) << "lists all matching tests by name\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "lts, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "list-test-suites " + << Whitespace(sizePrefixDisplay*1) << "lists all matching test suites\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "lr, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "list-reporters " + << Whitespace(sizePrefixDisplay*1) << "lists all registered reporters\n\n"; + // ================================================================================== << 79 + s << Color::Cyan << "[doctest] " << Color::None; + s << "The available / options/filters are:\n\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "tc, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "test-case= " + << Whitespace(sizePrefixDisplay*1) << "filters tests by their name\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "tce, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "test-case-exclude= " + << Whitespace(sizePrefixDisplay*1) << "filters OUT tests by their name\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "sf, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "source-file= " + << Whitespace(sizePrefixDisplay*1) << "filters tests by their file\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "sfe, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "source-file-exclude= " + << Whitespace(sizePrefixDisplay*1) << "filters OUT tests by their file\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ts, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "test-suite= " + << Whitespace(sizePrefixDisplay*1) << "filters tests by their test suite\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "tse, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "test-suite-exclude= " + << Whitespace(sizePrefixDisplay*1) << "filters OUT tests by their test suite\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "sc, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "subcase= " + << Whitespace(sizePrefixDisplay*1) << "filters subcases by their name\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "sce, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "subcase-exclude= " + << Whitespace(sizePrefixDisplay*1) << "filters OUT subcases by their name\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "r, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "reporters= " + << Whitespace(sizePrefixDisplay*1) << "reporters to use (console is default)\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "o, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "out= " + << Whitespace(sizePrefixDisplay*1) << "output filename\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ob, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "order-by= " + << Whitespace(sizePrefixDisplay*1) << "how the tests should be ordered\n"; + s << Whitespace(sizePrefixDisplay*3) << " - [file/suite/name/rand/none]\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "rs, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "rand-seed= " + << Whitespace(sizePrefixDisplay*1) << "seed for random ordering\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "f, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "first= " + << Whitespace(sizePrefixDisplay*1) << "the first test passing the filters to\n"; + s << Whitespace(sizePrefixDisplay*3) << " execute - for range-based execution\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "l, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "last= " + << Whitespace(sizePrefixDisplay*1) << "the last test passing the filters to\n"; + s << Whitespace(sizePrefixDisplay*3) << " execute - for range-based execution\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "aa, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "abort-after= " + << Whitespace(sizePrefixDisplay*1) << "stop after failed assertions\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "scfl,--" DOCTEST_OPTIONS_PREFIX_DISPLAY "subcase-filter-levels= " + << Whitespace(sizePrefixDisplay*1) << "apply filters for the first levels\n"; + s << Color::Cyan << "\n[doctest] " << Color::None; + s << "Bool options - can be used like flags and true is assumed. Available:\n\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "s, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "success= " + << Whitespace(sizePrefixDisplay*1) << "include successful assertions in output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "cs, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "case-sensitive= " + << Whitespace(sizePrefixDisplay*1) << "filters being treated as case sensitive\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "e, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "exit= " + << Whitespace(sizePrefixDisplay*1) << "exits after the tests finish\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "d, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "duration= " + << Whitespace(sizePrefixDisplay*1) << "prints the time duration of each test\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "m, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "minimal= " + << Whitespace(sizePrefixDisplay*1) << "minimal console output (only failures)\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "q, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "quiet= " + << Whitespace(sizePrefixDisplay*1) << "no console output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nt, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-throw= " + << Whitespace(sizePrefixDisplay*1) << "skips exceptions-related assert checks\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ne, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-exitcode= " + << Whitespace(sizePrefixDisplay*1) << "returns (or exits) always with success\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nr, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-run= " + << Whitespace(sizePrefixDisplay*1) << "skips all runtime doctest operations\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ni, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-intro= " + << Whitespace(sizePrefixDisplay*1) << "omit the framework intro in the output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nv, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-version= " + << Whitespace(sizePrefixDisplay*1) << "omit the framework version in the output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nc, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-colors= " + << Whitespace(sizePrefixDisplay*1) << "disables colors in output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "fc, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "force-colors= " + << Whitespace(sizePrefixDisplay*1) << "use colors even when not in a tty\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nb, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-breaks= " + << Whitespace(sizePrefixDisplay*1) << "disables breakpoints in debuggers\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "ns, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-skip= " + << Whitespace(sizePrefixDisplay*1) << "don't skip test cases marked as skip\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "gfl, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "gnu-file-line= " + << Whitespace(sizePrefixDisplay*1) << ":n: vs (n): for line numbers in output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "npf, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-path-filenames= " + << Whitespace(sizePrefixDisplay*1) << "only filenames and no paths in output\n"; + s << " -" DOCTEST_OPTIONS_PREFIX_DISPLAY "nln, --" DOCTEST_OPTIONS_PREFIX_DISPLAY "no-line-numbers= " + << Whitespace(sizePrefixDisplay*1) << "0 instead of real line numbers in output\n"; + // ================================================================================== << 79 + // clang-format on + + s << Color::Cyan << "\n[doctest] " << Color::None; + s << "for more information visit the project documentation\n\n"; + } + + void printRegisteredReporters() { + printVersion(); + auto printReporters = [this] (const reporterMap& reporters, const char* type) { + if(reporters.size()) { + s << Color::Cyan << "[doctest] " << Color::None << "listing all registered " << type << "\n"; + for(auto& curr : reporters) + s << "priority: " << std::setw(5) << curr.first.first + << " name: " << curr.first.second << "\n"; + } + }; + printReporters(getListeners(), "listeners"); + printReporters(getReporters(), "reporters"); + } + + // ========================================================================================= + // WHAT FOLLOWS ARE OVERRIDES OF THE VIRTUAL METHODS OF THE REPORTER INTERFACE + // ========================================================================================= + + void report_query(const QueryData& in) override { + if(opt.version) { + printVersion(); + } else if(opt.help) { + printHelp(); + } else if(opt.list_reporters) { + printRegisteredReporters(); + } else if(opt.count || opt.list_test_cases) { + if(opt.list_test_cases) { + s << Color::Cyan << "[doctest] " << Color::None + << "listing all test case names\n"; + separator_to_stream(); + } + + for(unsigned i = 0; i < in.num_data; ++i) + s << Color::None << in.data[i]->m_name << "\n"; + + separator_to_stream(); + + s << Color::Cyan << "[doctest] " << Color::None + << "unskipped test cases passing the current filters: " + << g_cs->numTestCasesPassingFilters << "\n"; + + } else if(opt.list_test_suites) { + s << Color::Cyan << "[doctest] " << Color::None << "listing all test suites\n"; + separator_to_stream(); + + for(unsigned i = 0; i < in.num_data; ++i) + s << Color::None << in.data[i]->m_test_suite << "\n"; + + separator_to_stream(); + + s << Color::Cyan << "[doctest] " << Color::None + << "unskipped test cases passing the current filters: " + << g_cs->numTestCasesPassingFilters << "\n"; + s << Color::Cyan << "[doctest] " << Color::None + << "test suites with unskipped test cases passing the current filters: " + << g_cs->numTestSuitesPassingFilters << "\n"; + } + } + + void test_run_start() override { + if(!opt.minimal) + printIntro(); + } + + void test_run_end(const TestRunStats& p) override { + if(opt.minimal && p.numTestCasesFailed == 0) + return; + + separator_to_stream(); + s << std::dec; + + auto totwidth = int(std::ceil(log10(static_cast(std::max(p.numTestCasesPassingFilters, static_cast(p.numAsserts))) + 1))); + auto passwidth = int(std::ceil(log10(static_cast(std::max(p.numTestCasesPassingFilters - p.numTestCasesFailed, static_cast(p.numAsserts - p.numAssertsFailed))) + 1))); + auto failwidth = int(std::ceil(log10(static_cast(std::max(p.numTestCasesFailed, static_cast(p.numAssertsFailed))) + 1))); + const bool anythingFailed = p.numTestCasesFailed > 0 || p.numAssertsFailed > 0; + s << Color::Cyan << "[doctest] " << Color::None << "test cases: " << std::setw(totwidth) + << p.numTestCasesPassingFilters << " | " + << ((p.numTestCasesPassingFilters == 0 || anythingFailed) ? Color::None : + Color::Green) + << std::setw(passwidth) << p.numTestCasesPassingFilters - p.numTestCasesFailed << " passed" + << Color::None << " | " << (p.numTestCasesFailed > 0 ? Color::Red : Color::None) + << std::setw(failwidth) << p.numTestCasesFailed << " failed" << Color::None << " |"; + if(opt.no_skipped_summary == false) { + const int numSkipped = p.numTestCases - p.numTestCasesPassingFilters; + s << " " << (numSkipped == 0 ? Color::None : Color::Yellow) << numSkipped + << " skipped" << Color::None; + } + s << "\n"; + s << Color::Cyan << "[doctest] " << Color::None << "assertions: " << std::setw(totwidth) + << p.numAsserts << " | " + << ((p.numAsserts == 0 || anythingFailed) ? Color::None : Color::Green) + << std::setw(passwidth) << (p.numAsserts - p.numAssertsFailed) << " passed" << Color::None + << " | " << (p.numAssertsFailed > 0 ? Color::Red : Color::None) << std::setw(failwidth) + << p.numAssertsFailed << " failed" << Color::None << " |\n"; + s << Color::Cyan << "[doctest] " << Color::None + << "Status: " << (p.numTestCasesFailed > 0 ? Color::Red : Color::Green) + << ((p.numTestCasesFailed > 0) ? "FAILURE!" : "SUCCESS!") << Color::None << std::endl; + } + + void test_case_start(const TestCaseData& in) override { + hasLoggedCurrentTestStart = false; + tc = ∈ + subcasesStack.clear(); + currentSubcaseLevel = 0; + } + + void test_case_reenter(const TestCaseData&) override { + subcasesStack.clear(); + } + + void test_case_end(const CurrentTestCaseStats& st) override { + if(tc->m_no_output) + return; + + // log the preamble of the test case only if there is something + // else to print - something other than that an assert has failed + if(opt.duration || + (st.failure_flags && st.failure_flags != static_cast(TestCaseFailureReason::AssertFailure))) + logTestStart(); + + if(opt.duration) + s << Color::None << std::setprecision(6) << std::fixed << st.seconds + << " s: " << tc->m_name << "\n"; + + if(st.failure_flags & TestCaseFailureReason::Timeout) + s << Color::Red << "Test case exceeded time limit of " << std::setprecision(6) + << std::fixed << tc->m_timeout << "!\n"; + + if(st.failure_flags & TestCaseFailureReason::ShouldHaveFailedButDidnt) { + s << Color::Red << "Should have failed but didn't! Marking it as failed!\n"; + } else if(st.failure_flags & TestCaseFailureReason::ShouldHaveFailedAndDid) { + s << Color::Yellow << "Failed as expected so marking it as not failed\n"; + } else if(st.failure_flags & TestCaseFailureReason::CouldHaveFailedAndDid) { + s << Color::Yellow << "Allowed to fail so marking it as not failed\n"; + } else if(st.failure_flags & TestCaseFailureReason::DidntFailExactlyNumTimes) { + s << Color::Red << "Didn't fail exactly " << tc->m_expected_failures + << " times so marking it as failed!\n"; + } else if(st.failure_flags & TestCaseFailureReason::FailedExactlyNumTimes) { + s << Color::Yellow << "Failed exactly " << tc->m_expected_failures + << " times as expected so marking it as not failed!\n"; + } + if(st.failure_flags & TestCaseFailureReason::TooManyFailedAsserts) { + s << Color::Red << "Aborting - too many failed asserts!\n"; + } + s << Color::None; // lgtm [cpp/useless-expression] + } + + void test_case_exception(const TestCaseException& e) override { + DOCTEST_LOCK_MUTEX(mutex) + if(tc->m_no_output) + return; + + logTestStart(); + + file_line_to_stream(tc->m_file.c_str(), tc->m_line, " "); + successOrFailColoredStringToStream(false, e.is_crash ? assertType::is_require : + assertType::is_check); + s << Color::Red << (e.is_crash ? "test case CRASHED: " : "test case THREW exception: ") + << Color::Cyan << e.error_string << "\n"; + + int num_stringified_contexts = get_num_stringified_contexts(); + if(num_stringified_contexts) { + auto stringified_contexts = get_stringified_contexts(); + s << Color::None << " logged: "; + for(int i = num_stringified_contexts; i > 0; --i) { + s << (i == num_stringified_contexts ? "" : " ") + << stringified_contexts[i - 1] << "\n"; + } + } + s << "\n" << Color::None; + } + + void subcase_start(const SubcaseSignature& subc) override { + subcasesStack.push_back(subc); + ++currentSubcaseLevel; + hasLoggedCurrentTestStart = false; + } + + void subcase_end() override { + --currentSubcaseLevel; + hasLoggedCurrentTestStart = false; + } + + void log_assert(const AssertData& rb) override { + if((!rb.m_failed && !opt.success) || tc->m_no_output) + return; + + DOCTEST_LOCK_MUTEX(mutex) + + logTestStart(); + + file_line_to_stream(rb.m_file, rb.m_line, " "); + successOrFailColoredStringToStream(!rb.m_failed, rb.m_at); + + fulltext_log_assert_to_stream(s, rb); + + log_contexts(); + } + + void log_message(const MessageData& mb) override { + if(tc->m_no_output) + return; + + DOCTEST_LOCK_MUTEX(mutex) + + logTestStart(); + + file_line_to_stream(mb.m_file, mb.m_line, " "); + s << getSuccessOrFailColor(false, mb.m_severity) + << getSuccessOrFailString(mb.m_severity & assertType::is_warn, mb.m_severity, + "MESSAGE") << ": "; + s << Color::None << mb.m_string << "\n"; + log_contexts(); + } + + void test_case_skipped(const TestCaseData&) override {} + }; + + DOCTEST_REGISTER_REPORTER("console", 0, ConsoleReporter); + +#ifdef DOCTEST_PLATFORM_WINDOWS + struct DebugOutputWindowReporter : public ConsoleReporter + { + DOCTEST_THREAD_LOCAL static std::ostringstream oss; + + DebugOutputWindowReporter(const ContextOptions& co) + : ConsoleReporter(co, oss) {} + +#define DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(func, type, arg) \ + void func(type arg) override { \ + bool with_col = g_no_colors; \ + g_no_colors = false; \ + ConsoleReporter::func(arg); \ + if(oss.tellp() != std::streampos{}) { \ + DOCTEST_OUTPUT_DEBUG_STRING(oss.str().c_str()); \ + oss.str(""); \ + } \ + g_no_colors = with_col; \ + } + + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_run_start, DOCTEST_EMPTY, DOCTEST_EMPTY) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_run_end, const TestRunStats&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_case_start, const TestCaseData&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_case_reenter, const TestCaseData&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_case_end, const CurrentTestCaseStats&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_case_exception, const TestCaseException&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(subcase_start, const SubcaseSignature&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(subcase_end, DOCTEST_EMPTY, DOCTEST_EMPTY) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(log_assert, const AssertData&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(log_message, const MessageData&, in) + DOCTEST_DEBUG_OUTPUT_REPORTER_OVERRIDE(test_case_skipped, const TestCaseData&, in) + }; + + DOCTEST_THREAD_LOCAL std::ostringstream DebugOutputWindowReporter::oss; +#endif // DOCTEST_PLATFORM_WINDOWS + + // the implementation of parseOption() + bool parseOptionImpl(int argc, const char* const* argv, const char* pattern, String* value) { + // going from the end to the beginning and stopping on the first occurrence from the end + for(int i = argc; i > 0; --i) { + auto index = i - 1; + auto temp = std::strstr(argv[index], pattern); + if(temp && (value || strlen(temp) == strlen(pattern))) { //!OCLINT prefer early exits and continue + // eliminate matches in which the chars before the option are not '-' + bool noBadCharsFound = true; + auto curr = argv[index]; + while(curr != temp) { + if(*curr++ != '-') { + noBadCharsFound = false; + break; + } + } + if(noBadCharsFound && argv[index][0] == '-') { + if(value) { + // parsing the value of an option + temp += strlen(pattern); + const unsigned len = strlen(temp); + if(len) { + *value = temp; + return true; + } + } else { + // just a flag - no value + return true; + } + } + } + } + return false; + } + + // parses an option and returns the string after the '=' character + bool parseOption(int argc, const char* const* argv, const char* pattern, String* value = nullptr, + const String& defaultVal = String()) { + if(value) + *value = defaultVal; +#ifndef DOCTEST_CONFIG_NO_UNPREFIXED_OPTIONS + // offset (normally 3 for "dt-") to skip prefix + if(parseOptionImpl(argc, argv, pattern + strlen(DOCTEST_CONFIG_OPTIONS_PREFIX), value)) + return true; +#endif // DOCTEST_CONFIG_NO_UNPREFIXED_OPTIONS + return parseOptionImpl(argc, argv, pattern, value); + } + + // locates a flag on the command line + bool parseFlag(int argc, const char* const* argv, const char* pattern) { + return parseOption(argc, argv, pattern); + } + + // parses a comma separated list of words after a pattern in one of the arguments in argv + bool parseCommaSepArgs(int argc, const char* const* argv, const char* pattern, + std::vector& res) { + String filtersString; + if(parseOption(argc, argv, pattern, &filtersString)) { + // tokenize with "," as a separator, unless escaped with backslash + std::ostringstream s; + auto flush = [&s, &res]() { + auto string = s.str(); + if(string.size() > 0) { + res.push_back(string.c_str()); + } + s.str(""); + }; + + bool seenBackslash = false; + const char* current = filtersString.c_str(); + const char* end = current + strlen(current); + while(current != end) { + char character = *current++; + if(seenBackslash) { + seenBackslash = false; + if(character == ',' || character == '\\') { + s.put(character); + continue; + } + s.put('\\'); + } + if(character == '\\') { + seenBackslash = true; + } else if(character == ',') { + flush(); + } else { + s.put(character); + } + } + + if(seenBackslash) { + s.put('\\'); + } + flush(); + return true; + } + return false; + } + + enum optionType + { + option_bool, + option_int + }; + + // parses an int/bool option from the command line + bool parseIntOption(int argc, const char* const* argv, const char* pattern, optionType type, + int& res) { + String parsedValue; + if(!parseOption(argc, argv, pattern, &parsedValue)) + return false; + + if(type) { + // integer + // TODO: change this to use std::stoi or something else! currently it uses undefined behavior - assumes '0' on failed parse... + int theInt = std::atoi(parsedValue.c_str()); + if (theInt != 0) { + res = theInt; //!OCLINT parameter reassignment + return true; + } + } else { + // boolean + const char positive[][5] = { "1", "true", "on", "yes" }; // 5 - strlen("true") + 1 + const char negative[][6] = { "0", "false", "off", "no" }; // 6 - strlen("false") + 1 + + // if the value matches any of the positive/negative possibilities + for (unsigned i = 0; i < 4; i++) { + if (parsedValue.compare(positive[i], true) == 0) { + res = 1; //!OCLINT parameter reassignment + return true; + } + if (parsedValue.compare(negative[i], true) == 0) { + res = 0; //!OCLINT parameter reassignment + return true; + } + } + } + return false; + } +} // namespace + +Context::Context(int argc, const char* const* argv) + : p(new detail::ContextState) { + parseArgs(argc, argv, true); + if(argc) + p->binary_name = argv[0]; +} + +Context::~Context() { + if(g_cs == p) + g_cs = nullptr; + delete p; +} + +void Context::applyCommandLine(int argc, const char* const* argv) { + parseArgs(argc, argv); + if(argc) + p->binary_name = argv[0]; +} + +// parses args +void Context::parseArgs(int argc, const char* const* argv, bool withDefaults) { + using namespace detail; + + // clang-format off + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "source-file=", p->filters[0]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "sf=", p->filters[0]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "source-file-exclude=",p->filters[1]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "sfe=", p->filters[1]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "test-suite=", p->filters[2]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "ts=", p->filters[2]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "test-suite-exclude=", p->filters[3]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "tse=", p->filters[3]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "test-case=", p->filters[4]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "tc=", p->filters[4]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "test-case-exclude=", p->filters[5]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "tce=", p->filters[5]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "subcase=", p->filters[6]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "sc=", p->filters[6]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "subcase-exclude=", p->filters[7]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "sce=", p->filters[7]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "reporters=", p->filters[8]); + parseCommaSepArgs(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "r=", p->filters[8]); + // clang-format on + + int intRes = 0; + String strRes; + +#define DOCTEST_PARSE_AS_BOOL_OR_FLAG(name, sname, var, default) \ + if(parseIntOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX name "=", option_bool, intRes) || \ + parseIntOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX sname "=", option_bool, intRes)) \ + p->var = static_cast(intRes); \ + else if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX name) || \ + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX sname)) \ + p->var = true; \ + else if(withDefaults) \ + p->var = default + +#define DOCTEST_PARSE_INT_OPTION(name, sname, var, default) \ + if(parseIntOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX name "=", option_int, intRes) || \ + parseIntOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX sname "=", option_int, intRes)) \ + p->var = intRes; \ + else if(withDefaults) \ + p->var = default + +#define DOCTEST_PARSE_STR_OPTION(name, sname, var, default) \ + if(parseOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX name "=", &strRes, default) || \ + parseOption(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX sname "=", &strRes, default) || \ + withDefaults) \ + p->var = strRes + + // clang-format off + DOCTEST_PARSE_STR_OPTION("out", "o", out, ""); + DOCTEST_PARSE_STR_OPTION("order-by", "ob", order_by, "file"); + DOCTEST_PARSE_INT_OPTION("rand-seed", "rs", rand_seed, 0); + + DOCTEST_PARSE_INT_OPTION("first", "f", first, 0); + DOCTEST_PARSE_INT_OPTION("last", "l", last, UINT_MAX); + + DOCTEST_PARSE_INT_OPTION("abort-after", "aa", abort_after, 0); + DOCTEST_PARSE_INT_OPTION("subcase-filter-levels", "scfl", subcase_filter_levels, INT_MAX); + + DOCTEST_PARSE_AS_BOOL_OR_FLAG("success", "s", success, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("case-sensitive", "cs", case_sensitive, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("exit", "e", exit, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("duration", "d", duration, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("minimal", "m", minimal, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("quiet", "q", quiet, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-throw", "nt", no_throw, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-exitcode", "ne", no_exitcode, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-run", "nr", no_run, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-intro", "ni", no_intro, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-version", "nv", no_version, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-colors", "nc", no_colors, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("force-colors", "fc", force_colors, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-breaks", "nb", no_breaks, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-skip", "ns", no_skip, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("gnu-file-line", "gfl", gnu_file_line, !bool(DOCTEST_MSVC)); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-path-filenames", "npf", no_path_in_filenames, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-line-numbers", "nln", no_line_numbers, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-debug-output", "ndo", no_debug_output, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-skipped-summary", "nss", no_skipped_summary, false); + DOCTEST_PARSE_AS_BOOL_OR_FLAG("no-time-in-output", "ntio", no_time_in_output, false); + // clang-format on + + if(withDefaults) { + p->help = false; + p->version = false; + p->count = false; + p->list_test_cases = false; + p->list_test_suites = false; + p->list_reporters = false; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "help") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "h") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "?")) { + p->help = true; + p->exit = true; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "version") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "v")) { + p->version = true; + p->exit = true; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "count") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "c")) { + p->count = true; + p->exit = true; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "list-test-cases") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "ltc")) { + p->list_test_cases = true; + p->exit = true; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "list-test-suites") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "lts")) { + p->list_test_suites = true; + p->exit = true; + } + if(parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "list-reporters") || + parseFlag(argc, argv, DOCTEST_CONFIG_OPTIONS_PREFIX "lr")) { + p->list_reporters = true; + p->exit = true; + } +} + +// allows the user to add procedurally to the filters from the command line +void Context::addFilter(const char* filter, const char* value) { setOption(filter, value); } + +// allows the user to clear all filters from the command line +void Context::clearFilters() { + for(auto& curr : p->filters) + curr.clear(); +} + +// allows the user to override procedurally the bool options from the command line +void Context::setOption(const char* option, bool value) { + setOption(option, value ? "true" : "false"); +} + +// allows the user to override procedurally the int options from the command line +void Context::setOption(const char* option, int value) { + setOption(option, toString(value).c_str()); +} + +// allows the user to override procedurally the string options from the command line +void Context::setOption(const char* option, const char* value) { + auto argv = String("-") + option + "=" + value; + auto lvalue = argv.c_str(); + parseArgs(1, &lvalue); +} + +// users should query this in their main() and exit the program if true +bool Context::shouldExit() { return p->exit; } + +void Context::setAsDefaultForAssertsOutOfTestCases() { g_cs = p; } + +void Context::setAssertHandler(detail::assert_handler ah) { p->ah = ah; } + +void Context::setCout(std::ostream* out) { p->cout = out; } + +static class DiscardOStream : public std::ostream +{ +private: + class : public std::streambuf + { + private: + // allowing some buffering decreases the amount of calls to overflow + char buf[1024]; + + protected: + std::streamsize xsputn(const char_type*, std::streamsize count) override { return count; } + + int_type overflow(int_type ch) override { + setp(std::begin(buf), std::end(buf)); + return traits_type::not_eof(ch); + } + } discardBuf; + +public: + DiscardOStream() + : std::ostream(&discardBuf) {} +} discardOut; + +// the main function that does all the filtering and test running +int Context::run() { + using namespace detail; + + // save the old context state in case such was setup - for using asserts out of a testing context + auto old_cs = g_cs; + // this is the current contest + g_cs = p; + is_running_in_test = true; + + g_no_colors = p->no_colors; + p->resetRunData(); + + std::fstream fstr; + if(p->cout == nullptr) { + if(p->quiet) { + p->cout = &discardOut; + } else if(p->out.size()) { + // to a file if specified + fstr.open(p->out.c_str(), std::fstream::out); + p->cout = &fstr; + } else { +#ifndef DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + // stdout by default + p->cout = &std::cout; +#else // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + return EXIT_FAILURE; +#endif // DOCTEST_CONFIG_NO_INCLUDE_IOSTREAM + } + } + + FatalConditionHandler::allocateAltStackMem(); + + auto cleanup_and_return = [&]() { + FatalConditionHandler::freeAltStackMem(); + + if(fstr.is_open()) + fstr.close(); + + // restore context + g_cs = old_cs; + is_running_in_test = false; + + // we have to free the reporters which were allocated when the run started + for(auto& curr : p->reporters_currently_used) + delete curr; + p->reporters_currently_used.clear(); + + if(p->numTestCasesFailed && !p->no_exitcode) + return EXIT_FAILURE; + return EXIT_SUCCESS; + }; + + // setup default reporter if none is given through the command line + if(p->filters[8].empty()) + p->filters[8].push_back("console"); + + // check to see if any of the registered reporters has been selected + for(auto& curr : getReporters()) { + if(matchesAny(curr.first.second.c_str(), p->filters[8], false, p->case_sensitive)) + p->reporters_currently_used.push_back(curr.second(*g_cs)); + } + + // TODO: check if there is nothing in reporters_currently_used + + // prepend all listeners + for(auto& curr : getListeners()) + p->reporters_currently_used.insert(p->reporters_currently_used.begin(), curr.second(*g_cs)); + +#ifdef DOCTEST_PLATFORM_WINDOWS + if(isDebuggerActive() && p->no_debug_output == false) + p->reporters_currently_used.push_back(new DebugOutputWindowReporter(*g_cs)); +#endif // DOCTEST_PLATFORM_WINDOWS + + // handle version, help and no_run + if(p->no_run || p->version || p->help || p->list_reporters) { + DOCTEST_ITERATE_THROUGH_REPORTERS(report_query, QueryData()); + + return cleanup_and_return(); + } + + std::vector testArray; + for(auto& curr : getRegisteredTests()) + testArray.push_back(&curr); + p->numTestCases = testArray.size(); + + // sort the collected records + if(!testArray.empty()) { + if(p->order_by.compare("file", true) == 0) { + std::sort(testArray.begin(), testArray.end(), fileOrderComparator); + } else if(p->order_by.compare("suite", true) == 0) { + std::sort(testArray.begin(), testArray.end(), suiteOrderComparator); + } else if(p->order_by.compare("name", true) == 0) { + std::sort(testArray.begin(), testArray.end(), nameOrderComparator); + } else if(p->order_by.compare("rand", true) == 0) { + std::srand(p->rand_seed); + + // random_shuffle implementation + const auto first = &testArray[0]; + for(size_t i = testArray.size() - 1; i > 0; --i) { + int idxToSwap = std::rand() % (i + 1); + + const auto temp = first[i]; + + first[i] = first[idxToSwap]; + first[idxToSwap] = temp; + } + } else if(p->order_by.compare("none", true) == 0) { + // means no sorting - beneficial for death tests which call into the executable + // with a specific test case in mind - we don't want to slow down the startup times + } + } + + std::set testSuitesPassingFilt; + + bool query_mode = p->count || p->list_test_cases || p->list_test_suites; + std::vector queryResults; + + if(!query_mode) + DOCTEST_ITERATE_THROUGH_REPORTERS(test_run_start, DOCTEST_EMPTY); + + // invoke the registered functions if they match the filter criteria (or just count them) + for(auto& curr : testArray) { + const auto& tc = *curr; + + bool skip_me = false; + if(tc.m_skip && !p->no_skip) + skip_me = true; + + if(!matchesAny(tc.m_file.c_str(), p->filters[0], true, p->case_sensitive)) + skip_me = true; + if(matchesAny(tc.m_file.c_str(), p->filters[1], false, p->case_sensitive)) + skip_me = true; + if(!matchesAny(tc.m_test_suite, p->filters[2], true, p->case_sensitive)) + skip_me = true; + if(matchesAny(tc.m_test_suite, p->filters[3], false, p->case_sensitive)) + skip_me = true; + if(!matchesAny(tc.m_name, p->filters[4], true, p->case_sensitive)) + skip_me = true; + if(matchesAny(tc.m_name, p->filters[5], false, p->case_sensitive)) + skip_me = true; + + if(!skip_me) + p->numTestCasesPassingFilters++; + + // skip the test if it is not in the execution range + if((p->last < p->numTestCasesPassingFilters && p->first <= p->last) || + (p->first > p->numTestCasesPassingFilters)) + skip_me = true; + + if(skip_me) { + if(!query_mode) + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_skipped, tc); + continue; + } + + // do not execute the test if we are to only count the number of filter passing tests + if(p->count) + continue; + + // print the name of the test and don't execute it + if(p->list_test_cases) { + queryResults.push_back(&tc); + continue; + } + + // print the name of the test suite if not done already and don't execute it + if(p->list_test_suites) { + if((testSuitesPassingFilt.count(tc.m_test_suite) == 0) && tc.m_test_suite[0] != '\0') { + queryResults.push_back(&tc); + testSuitesPassingFilt.insert(tc.m_test_suite); + p->numTestSuitesPassingFilters++; + } + continue; + } + + // execute the test if it passes all the filtering + { + p->currentTest = &tc; + + p->failure_flags = TestCaseFailureReason::None; + p->seconds = 0; + + // reset atomic counters + p->numAssertsFailedCurrentTest_atomic = 0; + p->numAssertsCurrentTest_atomic = 0; + + p->fullyTraversedSubcases.clear(); + + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_start, tc); + + p->timer.start(); + + bool run_test = true; + + do { + // reset some of the fields for subcases (except for the set of fully passed ones) + p->reachedLeaf = false; + // May not be empty if previous subcase exited via exception. + p->subcaseStack.clear(); + p->currentSubcaseDepth = 0; + + p->shouldLogCurrentException = true; + + // reset stuff for logging with INFO() + p->stringifiedContexts.clear(); + +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + try { +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS +// MSVC 2015 diagnoses fatalConditionHandler as unused (because reset() is a static method) +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4101) // unreferenced local variable + FatalConditionHandler fatalConditionHandler; // Handle signals + // execute the test + tc.m_test(); + fatalConditionHandler.reset(); +DOCTEST_MSVC_SUPPRESS_WARNING_POP +#ifndef DOCTEST_CONFIG_NO_EXCEPTIONS + } catch(const TestFailureException&) { + p->failure_flags |= TestCaseFailureReason::AssertFailure; + } catch(...) { + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_exception, + {translateActiveException(), false}); + p->failure_flags |= TestCaseFailureReason::Exception; + } +#endif // DOCTEST_CONFIG_NO_EXCEPTIONS + + // exit this loop if enough assertions have failed - even if there are more subcases + if(p->abort_after > 0 && + p->numAssertsFailed + p->numAssertsFailedCurrentTest_atomic >= p->abort_after) { + run_test = false; + p->failure_flags |= TestCaseFailureReason::TooManyFailedAsserts; + } + + if(!p->nextSubcaseStack.empty() && run_test) + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_reenter, tc); + if(p->nextSubcaseStack.empty()) + run_test = false; + } while(run_test); + + p->finalizeTestCaseData(); + + DOCTEST_ITERATE_THROUGH_REPORTERS(test_case_end, *g_cs); + + p->currentTest = nullptr; + + // stop executing tests if enough assertions have failed + if(p->abort_after > 0 && p->numAssertsFailed >= p->abort_after) + break; + } + } + + if(!query_mode) { + DOCTEST_ITERATE_THROUGH_REPORTERS(test_run_end, *g_cs); + } else { + QueryData qdata; + qdata.run_stats = g_cs; + qdata.data = queryResults.data(); + qdata.num_data = unsigned(queryResults.size()); + DOCTEST_ITERATE_THROUGH_REPORTERS(report_query, qdata); + } + + return cleanup_and_return(); +} + +DOCTEST_DEFINE_INTERFACE(IReporter) + +int IReporter::get_num_active_contexts() { return detail::g_infoContexts.size(); } +const IContextScope* const* IReporter::get_active_contexts() { + return get_num_active_contexts() ? &detail::g_infoContexts[0] : nullptr; +} + +int IReporter::get_num_stringified_contexts() { return detail::g_cs->stringifiedContexts.size(); } +const String* IReporter::get_stringified_contexts() { + return get_num_stringified_contexts() ? &detail::g_cs->stringifiedContexts[0] : nullptr; +} + +namespace detail { + void registerReporterImpl(const char* name, int priority, reporterCreatorFunc c, bool isReporter) { + if(isReporter) + getReporters().insert(reporterMap::value_type(reporterMap::key_type(priority, name), c)); + else + getListeners().insert(reporterMap::value_type(reporterMap::key_type(priority, name), c)); + } +} // namespace detail + +} // namespace doctest + +#endif // DOCTEST_CONFIG_DISABLE + +#ifdef DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +DOCTEST_MSVC_SUPPRESS_WARNING_WITH_PUSH(4007) // 'function' : must be 'attribute' - see issue #182 +int main(int argc, char** argv) { return doctest::Context(argc, argv).run(); } +DOCTEST_MSVC_SUPPRESS_WARNING_POP +#endif // DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN + +DOCTEST_CLANG_SUPPRESS_WARNING_POP +DOCTEST_MSVC_SUPPRESS_WARNING_POP +DOCTEST_GCC_SUPPRESS_WARNING_POP + +DOCTEST_SUPPRESS_COMMON_WARNINGS_POP + +#endif // DOCTEST_LIBRARY_IMPLEMENTATION +#endif // DOCTEST_CONFIG_IMPLEMENT + +#ifdef DOCTEST_UNDEF_WIN32_LEAN_AND_MEAN +#undef WIN32_LEAN_AND_MEAN +#undef DOCTEST_UNDEF_WIN32_LEAN_AND_MEAN +#endif // DOCTEST_UNDEF_WIN32_LEAN_AND_MEAN + +#ifdef DOCTEST_UNDEF_NOMINMAX +#undef NOMINMAX +#undef DOCTEST_UNDEF_NOMINMAX +#endif // DOCTEST_UNDEF_NOMINMAX diff --git a/CMakeLists.txt b/CMakeLists.txt index c0907910..f3af2554 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,15 @@ add_subdirectory(core) add_subdirectory(calib_core) add_subdirectory(raylib_widgets) +# ============================================================================ +# Unit tests (opt-in: cmake -DBUILD_TESTING=ON) +# ============================================================================ +option(BUILD_TESTING "Build HDMapping unit tests" OFF) +if(BUILD_TESTING) + enable_testing() + add_subdirectory(shared/tests) +endif() + set(CORE_LIBRARIES core) set(GUI_LIBRARIES imgui imguizmo implot) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 796453ac..c6e16b51 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -72,62 +73,6 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) } -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) { @@ -1650,13 +1595,7 @@ int main(int argc, char* argv[]) { 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)) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 8af30cb0..149b5364 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -44,62 +44,6 @@ std::vector decimate(const std::vector& points, double bucke return out; } -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; -} - void limit_covariance(Eigen::Matrix3d& io_cov) { return; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 2f64e95d..ea5b58b1 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -238,9 +239,6 @@ inline VQFParams buildVQFParams(const LidarOdometryParams& p) return vp; } -// this function finds interpolated pose between two poses according to query_time -Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); - // this function reduces number of points by preserving only first point for each bucket {bucket_x, bucket_y, bucket_z} std::vector decimate(const std::vector& points, double bucket_x, double bucket_y, double bucket_z); diff --git a/shared/include/HDMapping/PoseInterpolation.h b/shared/include/HDMapping/PoseInterpolation.h new file mode 100644 index 00000000..5159b204 --- /dev/null +++ b/shared/include/HDMapping/PoseInterpolation.h @@ -0,0 +1,71 @@ +#pragma once +#include +#include +#include + +// Was byte-for-byte duplicated between apps/lidar_odometry_step_1's +// lidar_odometry_utils.h/.cpp (also compiled directly into +// multi_view_tls_registration_step_2 and mandeye_compare_trajectories) and +// apps/camera_lidar_trajectory_viewer's TrajectoryViewer.cpp. Lives under +// shared/ (like HDMapping/Version.hpp) rather than calib_core or Core: it's +// plain Eigen/std with no family-specific dependencies, and shared/include +// is already on every target's include path via the top-level +// CMakeLists.txt's include_directories(shared/include), so no target needs +// a new include dir or library link to use it. + +// Interpolates (SLERP for rotation, linear for translation) the pose at +// query_time from a time(seconds) -> T_world_lidar trajectory map. Returns a +// zero matrix if query_time falls outside the trajectory's covered range. +inline 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; + } + + constexpr double MaxInterpolationS = 0.1; + double t1 = it_lower->first; + double t2 = it_next->first; + double difft1 = t1 - query_time; + + if (t1 == t2 && std::fabs(difft1) < MaxInterpolationS) + { + 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; + } + + { + assert(t2 > t1); + assert(query_time > t1); + assert(query_time < t2); + ret = Eigen::Matrix4d::Identity(); + const double res = (query_time - t1) / (t2 - t1); //residual + const Eigen::Vector3d diff = it_next->second.col(3).head<3>() - it_lower->second.col(3).head<3>(); + ret.col(3).head<3>() = it_lower->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; + } +} diff --git a/shared/tests/CMakeLists.txt b/shared/tests/CMakeLists.txt new file mode 100644 index 00000000..82be362f --- /dev/null +++ b/shared/tests/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(hdmapping_shared_tests) + +# Unit tests for shared/include/HDMapping/*.h -- header-only utilities used +# across multiple apps (see PoseInterpolation.h's own top comment). Uses +# doctest (3rdparty/doctest/doctest.h, vendored single-header, MIT) rather +# than GoogleTest/Catch2: neither is vendored for offline use elsewhere in +# this repo (3rdparty/manif/test/gtest fetches GoogleTest live via +# ExternalProject_Add), and doctest compiles fast enough not to add +# noticeably to build time. +add_executable(hdmapping_shared_tests + test_pose_interpolation.cpp +) + +target_include_directories(hdmapping_shared_tests PRIVATE + ${THIRDPARTY_DIRECTORY}/doctest + ${EIGEN3_INCLUDE_DIR} +) + +if (MSVC) + target_compile_definitions(hdmapping_shared_tests PRIVATE _USE_MATH_DEFINES) +endif() + +include(CTest) +add_test(NAME hdmapping_shared_tests COMMAND hdmapping_shared_tests) diff --git a/shared/tests/test_pose_interpolation.cpp b/shared/tests/test_pose_interpolation.cpp new file mode 100644 index 00000000..021321a3 --- /dev/null +++ b/shared/tests/test_pose_interpolation.cpp @@ -0,0 +1,118 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include + +#include + +namespace +{ +Eigen::Matrix4d makePose(double tx, double ty, double tz, double yawRad = 0.0) +{ + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.topLeftCorner<3, 3>() = Eigen::AngleAxisd(yawRad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + T.col(3).head<3>() = Eigen::Vector3d(tx, ty, tz); + return T; +} + +bool isZeroMatrix(const Eigen::Matrix4d& m) +{ + return m.isZero(0.0); +} +} // namespace + +TEST_CASE("getInterpolatedPose: empty trajectory returns a zero matrix") +{ + std::map traj; + CHECK(isZeroMatrix(getInterpolatedPose(traj, 0.0))); + CHECK(isZeroMatrix(getInterpolatedPose(traj, 123.456))); +} + +TEST_CASE("getInterpolatedPose: query before the first timestamp returns a zero matrix") +{ + std::map traj{ + { 1.0, makePose(0, 0, 0) }, + { 2.0, makePose(10, 0, 0) }, + { 3.0, makePose(20, 0, 0) }, + }; + CHECK(isZeroMatrix(getInterpolatedPose(traj, 0.0))); + CHECK(isZeroMatrix(getInterpolatedPose(traj, -100.0))); +} + +TEST_CASE("getInterpolatedPose: exact match at an interior timestamp returns that exact pose") +{ + // Interior = not the very first key -- see the "first segment" case below + // for why the first key specifically behaves differently. + std::map traj{ + { 1.0, makePose(0, 0, 0) }, + { 2.0, makePose(10, 0, 0) }, + { 3.0, makePose(20, 0, 0) }, + }; + Eigen::Matrix4d result = getInterpolatedPose(traj, 2.0); + CHECK(result.isApprox(makePose(10, 0, 0), 1e-9)); + + result = getInterpolatedPose(traj, 3.0); + CHECK(result.isApprox(makePose(20, 0, 0), 1e-9)); +} + +TEST_CASE("getInterpolatedPose: linearly interpolates translation between two interior poses") +{ + std::map traj{ + { 0.0, makePose(0, 0, 0) }, + { 10.0, makePose(0, 0, 0) }, + { 20.0, makePose(100, 0, 0) }, + }; + // Query inside the *second* segment [10, 20] -- the first segment [0, 10] + // is covered separately below, since (see that test) it behaves + // differently from every later segment. + Eigen::Matrix4d result = getInterpolatedPose(traj, 15.0); + CHECK(result(0, 3) == doctest::Approx(50.0)); + CHECK(result(1, 3) == doctest::Approx(0.0)); + CHECK(result(2, 3) == doctest::Approx(0.0)); + + result = getInterpolatedPose(traj, 12.0); + CHECK(result(0, 3) == doctest::Approx(20.0)); +} + +TEST_CASE("getInterpolatedPose: SLERPs rotation between two interior poses") +{ + const double kHalfPi = M_PI / 2.0; + std::map traj{ + { 0.0, makePose(0, 0, 0, 0.0) }, + { 10.0, makePose(0, 0, 0, 0.0) }, + { 20.0, makePose(0, 0, 0, kHalfPi) }, + }; + Eigen::Matrix4d result = getInterpolatedPose(traj, 15.0); + Eigen::Matrix3d expectedR = Eigen::AngleAxisd(kHalfPi / 2.0, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + CHECK(result.topLeftCorner<3, 3>().isApprox(expectedR, 1e-9)); +} + +TEST_CASE("getInterpolatedPose: exact match at the very first timestamp currently returns a zero matrix") +{ + // Documented current behavior, not necessarily desired: there's no pose + // "before" the first sample to interpolate from, so getInterpolatedPose + // treats query_time <= the first timestamp as out-of-range and signals + // that with a zero matrix, same as querying before the range entirely. + // This test pins that behavior down so a future change to it is a + // deliberate, visible decision rather than an accidental regression. + std::map traj{ + { 1.0, makePose(5, 0, 0) }, + { 2.0, makePose(10, 0, 0) }, + }; + CHECK(isZeroMatrix(getInterpolatedPose(traj, 1.0))); +} + +TEST_CASE("getInterpolatedPose: query strictly inside the first segment currently returns a zero matrix") +{ + // Same current limitation as above, one step further: even a query + // strictly *between* the first and second timestamps returns zero + // rather than interpolating -- only segments from the second one onward + // interpolate normally (see the "linearly interpolates" test above, + // which deliberately queries the *second* segment for that reason). + std::map traj{ + { 0.0, makePose(0, 0, 0) }, + { 10.0, makePose(100, 0, 0) }, + { 20.0, makePose(200, 0, 0) }, + }; + CHECK(isZeroMatrix(getInterpolatedPose(traj, 5.0))); +} From 67adfb9aef437e3d5d7e07b5da94f5b0b3abc674 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 4 Aug 2026 22:12:35 +0200 Subject: [PATCH 12/13] Clang format Signed-off-by: Michal Pelka --- apps/camera_lidar_calibration/App.h | 55 ++- apps/camera_lidar_calibration/Renderer.cpp | 264 ++++++----- apps/camera_lidar_calibration/Renderer.h | 59 +-- .../RendererShaders.h | 28 +- apps/camera_lidar_calibration/UI.cpp | 4 +- apps/camera_lidar_calibration/UI.h | 15 +- apps/camera_lidar_calibration/main.cpp | 53 ++- .../IntrinsicsCalib.cpp | 1 - .../RosExport.cpp | 424 ++++++++++-------- .../RosExport.h | 49 +- .../TrajectoryViewer.cpp | 24 +- .../TrajectoryViewerShaders.h | 9 +- .../multi_view_tls_registration_gui.cpp | 15 +- apps/multi_view_tls_registration/rl_utils.cpp | 9 +- apps/multi_view_tls_registration/rl_utils.h | 6 +- core/src/raylib_render.cpp | 2 +- core/src/raylib_render_shaders.hpp | 29 +- shared/include/HDMapping/PoseInterpolation.h | 2 +- shared/tests/test_pose_interpolation.cpp | 22 +- 19 files changed, 603 insertions(+), 467 deletions(-) diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index 8da95f73..54573bc7 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -1,24 +1,25 @@ #pragma once -#include -#include #include "Renderer.h" #include "UI.h" #include "raylib.h" +#include +#include #include #include #include using namespace calib; -struct AppState { +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; + 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; @@ -35,34 +36,48 @@ struct AppState { // ── sub-systems ─────────────────────────────────────────────────────────── Renderer renderer; - UI ui; + 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 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 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 { +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 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; + AppState state; std::string pendingImage; std::vector pendingClouds; std::string pendingIntrinsics; diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp index 3f794610..57f9834c 100644 --- a/apps/camera_lidar_calibration/Renderer.cpp +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -1,26 +1,22 @@ #include "Renderer.h" -#include "rlgl.h" #include "raymath.h" +#include "rlgl.h" // glad function pointers are compiled into raylib; the header only declares them -#include "external/glad.h" #include "RendererShaders.h" -#include +#include "external/glad.h" #include +#include #include #include // ── Jet colormap ───────────────────────────────────────────────────────────── -Color jetColor(float t) { +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 - }; + 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::toRaylib()/update() now live in raylib_widgets/src/OrbitCamera.cpp @@ -30,93 +26,105 @@ Color jetColor(float t) { // 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) { +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 + 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) { +void Renderer::init(int imgW, int imgH) +{ if (imageTexValid) UnloadRenderTexture(imageTex); texW = imgW; texH = imgH; - imageTex = LoadRenderTexture(imgW, imgH); + imageTex = LoadRenderTexture(imgW, imgH); imageTexValid = true; } -void Renderer::shutdown() { - if (imageTexValid) { +void Renderer::shutdown() +{ + if (imageTexValid) + { UnloadRenderTexture(imageTex); imageTexValid = false; } unloadCloudGPU(); - if (shaderValid) { + if (shaderValid) + { UnloadShader(pointShader); shaderValid = false; } } -using renderer_shaders::kPointVS; using renderer_shaders::kPointFS; -using renderer_shaders::kProjVS; +using renderer_shaders::kPointVS; using renderer_shaders::kProjFS; +using renderer_shaders::kProjVS; -void Renderer::initPointShader() { +void Renderer::initPointShader() +{ pointShader = LoadShaderFromMemory(kPointVS, kPointFS.c_str()); shaderValid = pointShader.id > 0; - if (!shaderValid) { + 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"); + } + 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"); + 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.c_str()); projShaderValid = projShader.id > 0; - if (!projShaderValid) { + 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"); + } + 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"); + 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) { +void Renderer::uploadCloud(const PointCloud& cloud) +{ unloadCloudGPU(); - if (cloud.empty() || !shaderValid) return; + 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) { + 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); @@ -126,9 +134,7 @@ void Renderer::uploadCloud(const PointCloud& cloud) { cloudVAO = rlLoadVertexArray(); rlEnableVertexArray(cloudVAO); - cloudVBO = rlLoadVertexBuffer(data.data(), - static_cast(data.size() * sizeof(float)), - false); + 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); @@ -140,54 +146,68 @@ void Renderer::uploadCloud(const PointCloud& cloud) { cloudCount = static_cast(cloud.points.size()); } -void Renderer::unloadCloudGPU() { - if (cloudVAO) { rlUnloadVertexArray(cloudVAO); cloudVAO = 0; } - if (cloudVBO) { rlUnloadVertexBuffer(cloudVBO); cloudVBO = 0; } +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; +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); + 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 + 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 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}; + 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); + 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); @@ -198,11 +218,18 @@ void Renderer::renderImageOverlay(const Texture2D& img, int imgW, int imgH, 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; +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(); @@ -213,31 +240,32 @@ void Renderer::draw3DCloud(const PointCloud& cloud, const VisualizationParams& v 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); + 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}; + 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 + 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)}; + 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); + 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(locCamK, k, RL_SHADER_UNIFORM_VEC4, 1); rlSetUniform(locCamImgSize, imgSize, RL_SHADER_UNIFORM_VEC2, 1); - if (colorMode == 3) { + if (colorMode == 3) + { rlActiveTextureSlot(0); rlEnableTexture(image.id); int slot = 0; @@ -250,27 +278,28 @@ void Renderer::draw3DCloud(const PointCloud& cloud, const VisualizationParams& v rlDisableShader(); } -void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, - int imgW, int imgH, float scale) { +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 + 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}, + { (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 { + 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()}; + return { pl.x(), pl.z(), -pl.y() }; }; Vector3 w[4]; for (int i = 0; i < 4; i++) @@ -287,8 +316,9 @@ void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, 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) +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 index e6f3a215..e48d0a54 100644 --- a/apps/camera_lidar_calibration/Renderer.h +++ b/apps/camera_lidar_calibration/Renderer.h @@ -1,26 +1,28 @@ #pragma once #include "raylib.h" -#include #include #include +#include #include using namespace calib; using raylib_widgets::OrbitCamera; -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 +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] +Color jetColor(float t); // t in [0,1] -class Renderer { +class Renderer +{ public: - RenderTexture2D imageTex = {}; // image + 2D projection overlay + RenderTexture2D imageTex = {}; // image + 2D projection overlay bool imageTexValid = false; void init(int imgW, int imgH); @@ -36,23 +38,30 @@ class Renderer { // 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); + 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); + 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); + 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); @@ -61,11 +70,11 @@ class Renderer { int texW = 0, texH = 0; // GPU point cloud (VAO shared by both shaders via fixed attrib locations) - Shader pointShader = {}; - bool shaderValid = false; + Shader pointShader = {}; + bool shaderValid = false; unsigned int cloudVAO = 0; unsigned int cloudVBO = 0; - int cloudCount = 0; + int cloudCount = 0; // 3D view shader uniforms int locMVP = -1, locColorMode = -1, locHeightRange = -1; int locMaxDist = -1, locOpacity = -1, locPointSize = -1; @@ -73,7 +82,7 @@ class Renderer { // 2D image-projection shader Shader projShader = {}; - bool projShaderValid = false; + bool projShaderValid = false; int locPrjXform = -1, locPrjK = -1, locPrjImgSize = -1; int locPrjRad1 = -1, locPrjRad2 = -1, locPrjTan = -1; int locPrjDepthRange = -1, locPrjOpacity = -1; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index 175bc2e4..41998bbf 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -9,10 +9,10 @@ namespace renderer_shaders { -// ── GPU point cloud shaders ────────────────────────────────────────────────── -// Explicit attribute locations so one VAO works with both programs: -// location 0 = position (raylib coords), location 1 = intensity. -inline constexpr const char* kPointVS = R"( + // ── GPU point cloud shaders ────────────────────────────────────────────────── + // Explicit attribute locations so one VAO works with both programs: + // location 0 = position (raylib coords), location 1 = intensity. + inline constexpr const char* kPointVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; layout(location = 1) in float vertexIntensity; @@ -40,7 +40,7 @@ void main() { } )"; -inline const std::string kPointFS = std::string(R"( + inline const std::string kPointFS = std::string(R"( #version 330 in vec3 fragPos; in float fragIntensity; @@ -52,7 +52,8 @@ uniform float maxDist; uniform float opacity; uniform sampler2D imageTex; out vec4 finalColor; -)") + raylib_widgets::kJetColormapGLSL + R"( +)") + raylib_widgets::kJetColormapGLSL + + R"( void main() { if (colorMode == 3) { bool seen = fragCamDepth > 0.0 @@ -74,11 +75,11 @@ void main() { } )"; -// 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). -inline constexpr const char* kProjVS = R"( + // 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). + inline constexpr const char* kProjVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; layout(location = 1) in float vertexIntensity; @@ -116,7 +117,7 @@ void main() { } )"; -inline const std::string kProjFS = std::string(R"( + inline const std::string kProjFS = std::string(R"( #version 330 in float fragDepth; in float fragIntensity; @@ -124,7 +125,8 @@ uniform vec2 depthRange; uniform float opacity; uniform int colorMode; out vec4 finalColor; -)") + raylib_widgets::kJetColormapGLSL + R"( +)") + raylib_widgets::kJetColormapGLSL + + R"( void main() { if (fragDepth < depthRange.x || fragDepth > depthRange.y) discard; float t = (colorMode == 1) diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 0895b2d6..ab4820f9 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -63,8 +63,8 @@ void UI::draw(AppState& state) // Alt/Cmd = toggle Camera RGB ↔ Intensity (works anywhere in the window). // Cmd (Super) alongside Alt for macOS, where Option is awkward to use as // a modifier (it composes special characters). - if (ImGui::IsKeyPressed(ImGuiKey_LeftAlt) || ImGui::IsKeyPressed(ImGuiKey_RightAlt) || - ImGui::IsKeyPressed(ImGuiKey_LeftSuper) || ImGui::IsKeyPressed(ImGuiKey_RightSuper)) + if (ImGui::IsKeyPressed(ImGuiKey_LeftAlt) || ImGui::IsKeyPressed(ImGuiKey_RightAlt) || ImGui::IsKeyPressed(ImGuiKey_LeftSuper) || + ImGui::IsKeyPressed(ImGuiKey_RightSuper)) { auto& cm = state.vizParams.colorMode; if (cm == 3) diff --git a/apps/camera_lidar_calibration/UI.h b/apps/camera_lidar_calibration/UI.h index 146cbad4..deed3e8f 100644 --- a/apps/camera_lidar_calibration/UI.h +++ b/apps/camera_lidar_calibration/UI.h @@ -1,13 +1,14 @@ #pragma once -#include #include "Renderer.h" +#include #include -#include #include +#include struct AppState; -class UI { +class UI +{ public: // Called once per frame inside rlImGuiBegin()/rlImGuiEnd() void draw(AppState& state); @@ -15,13 +16,13 @@ class UI { private: char imagePathBuf[512] = {}; char cloudPathBuf[512] = {}; - char intrPathBuf[512] = {}; - char savePath[512] = "calibration.json"; + char intrPathBuf[512] = {}; + char savePath[512] = "calibration.json"; // 2D image view pan/zoom state - float zoom2D = 1.f; // 1 = fit to window + 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; + int viewImgW = 0, viewImgH = 0; void drawImageView(AppState& state); void panelMenuBar(AppState& state); diff --git a/apps/camera_lidar_calibration/main.cpp b/apps/camera_lidar_calibration/main.cpp index 0d4acd90..18148fb8 100644 --- a/apps/camera_lidar_calibration/main.cpp +++ b/apps/camera_lidar_calibration/main.cpp @@ -1,42 +1,53 @@ #include "App.h" #include +#include #include #include #include -#include using namespace calib; namespace fs = std::filesystem; -static std::string ext(const std::string& path) { +static std::string ext(const std::string& path) +{ auto pos = path.rfind('.'); - if (pos == std::string::npos) return ""; + 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) { +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) { +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()); + 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[]) { +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) { + 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) { + if (!args.valid) + { std::fprintf(stderr, "%s\n\n", args.error.c_str()); printUsage("CalibrationApp", "Camera/LiDAR calibration tool", usage, /*toStderr=*/true); return 1; @@ -49,19 +60,25 @@ int main(int argc, char* argv[]) { app.preloadCloud(laz.c_str()); // --calib: calibration json (intrinsic + extrinsic). - if (args.has("calib")) app.preloadCalibration(args.get("calib").c_str()); + 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")) { + if (args.has("camera_dir")) + { fs::path dir(args.get("camera_dir")); - if (fs::is_directory(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)) { + if (!imgs.empty()) + app.preloadImage(imgs.front().string().c_str()); + } + else if (fs::is_regular_file(dir)) + { app.preloadImage(dir.string().c_str()); } } diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index 9bab769d..299701e2 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -37,7 +37,6 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) buf[bufSize - 1] = '\0'; } - // ── per-image state ─────────────────────────────────────────────────────────── struct CalibImage { diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 9335fcd4..53669d08 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -2,33 +2,34 @@ #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) { +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 -#include -#include +#include +#include #include #include -#include #include "PointCloud.h" @@ -37,125 +38,138 @@ bool exportRos2Bag(const RosExportInput&, const RosExportOptions&, std::string& #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; -} +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; -} + 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); + // 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; } - 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; }; + struct RawPt + { + int64_t ts; + float x, y, z, intensity; + }; } // namespace -bool exportRos2Bag(const RosExportInput& in, - const RosExportOptions& opt, - std::string& status) { +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 + if (wantRaw && !haveTraj) + wantRaw = false; // need poses to undo motion rosbag2_storage::StorageOptions so; - so.uri = opt.outUri; + so.uri = opt.outUri; so.storage_id = opt.storageId; rosbag2_cpp::ConverterOptions co; - co.input_serialization_format = "cdr"; + co.input_serialization_format = "cdr"; co.output_serialization_format = "cdr"; rosbag2_cpp::Writer writer; - try { + try + { writer.open(so, co); - } catch (const std::exception& e) { + } 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; + 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()); + std::fprintf(stderr, "[RosExport] writing bag '%s' (%s)\n", opt.outUri.c_str(), opt.storageId.c_str()); - try { + try + { // ── /tf_static : lidar -> camera (from extrinsics) ──────────────────── - if (opt.exportTf && in.calibLoaded) { + 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.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.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.stamp = toRosTime(startTs); ts.header.frame_id = in.lidarFrame; - ts.child_frame_id = in.cameraFrame; - ts.transform = toTransform(T_lc); + ts.child_frame_id = in.cameraFrame; + ts.transform = toTransform(T_lc); tf2_msgs::msg::TFMessage m; m.transforms.push_back(ts); @@ -163,13 +177,15 @@ bool exportRos2Bag(const RosExportInput& in, } // ── /tf : map -> lidar, one message per trajectory pose ─────────────── - if (opt.exportTf && haveTraj) { - for (const auto& p : in.traj.poses) { + 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.stamp = toRosTime(p.ts_ns); ts.header.frame_id = in.mapFrame; - ts.child_frame_id = in.lidarFrame; - ts.transform = toTransform(p.T); + ts.child_frame_id = in.lidarFrame; + ts.transform = toTransform(p.T); tf2_msgs::msg::TFMessage m; m.transforms.push_back(ts); @@ -181,103 +197,116 @@ bool exportRos2Bag(const RosExportInput& in, std::fprintf(stderr, "[RosExport] tf: %zu transforms\n", nTf); // ── camera images (+ camera_info) ───────────────────────────────────── - if (opt.exportCamera && !in.imageFiles.empty()) { + 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 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 + for (const auto& [ts, path] : in.imageFiles) + { + std::vector outBytes; // jpeg, when compressed + cv::Mat outImg; // bgr8, when raw - if (copyJpegBytes) { + 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 { + 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); + 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) { + camW = bgr.cols; + camH = bgr.rows; + if (opt.compressCamera) + { cv::imencode(".jpg", bgr, outBytes); - } else { - if (!bgr.isContinuous()) bgr = bgr.clone(); + } + else + { + if (!bgr.isContinuous()) + bgr = bgr.clone(); outImg = bgr; } } - if (opt.compressCamera) { + if (opt.compressCamera) + { sensor_msgs::msg::CompressedImage img; - img.header.stamp = toRosTime(ts); + img.header.stamp = toRosTime(ts); img.header.frame_id = in.cameraFrame; - img.format = "jpeg"; - img.data = std::move(outBytes); + img.format = "jpeg"; + img.data = std::move(outBytes); writer.write(img, kTopicImgCompressed, rclcpp::Time(ts)); - } else { + } + else + { sensor_msgs::msg::Image img; - img.header.stamp = toRosTime(ts); + 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.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 + 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 (!probe.empty()) + { + camW = probe.cols; + camH = probe.rows; + } } - if (camW > 0) { + if (camW > 0) + { sensor_msgs::msg::CameraInfo ci; - ci.header.stamp = toRosTime(ts); + ci.header.stamp = toRosTime(ts); ci.header.frame_id = in.cameraFrame; - ci.height = static_cast(camH); - ci.width = static_cast(camW); + 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}; + 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}; + 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)); } } @@ -287,80 +316,105 @@ bool exportRos2Bag(const RosExportInput& in, 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()) { + if ((opt.exportLidarUndistorted || wantRaw) && !in.lidarChunks.empty()) + { std::vector pts; - for (const auto& ch : in.lidarChunks) { + 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) { + 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 (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; }); + 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); + 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(); + Eigen::Affine3f lastInv = Eigen::Affine3f::Identity(); size_t i = 0; - while (i < pts.size()) { - const int64_t w = (pts[i].ts - t0) / aggNs; + 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; + while (j < pts.size() && (pts[j].ts - t0) / aggNs == w) + ++j; - if (opt.exportLidarUndistorted) { + 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); + 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)); + writer.write(makeCloud(in.mapFrame, winStamp, buf), kTopicLidarUndist, rclcpp::Time(winStamp)); ++nCloud; } - if (wantRaw) { + if (wantRaw) + { std::vector buf; buf.reserve((j - i) * 4); - for (size_t k = i; k < j; ++k) { + 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(); } + 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); + 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)); + writer.write(makeCloud(in.lidarFrame, winStamp, buf), kTopicLidarRaw, rclcpp::Time(winStamp)); ++nCloud; } i = j; } } } - } catch (const std::exception& e) { + } 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]" : ""); + 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 +#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 index 82518bb1..04be7875 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.h +++ b/apps/camera_lidar_trajectory_viewer/RosExport.h @@ -13,16 +13,17 @@ #include #include -#include // Intrinsics, Extrinsics -#include // Trajectory, TrajPose +#include // Intrinsics, Extrinsics +#include // Trajectory, TrajPose using namespace calib; // Everything the exporter needs, gathered by the viewer. Plain data only. -struct RosExportInput { +struct RosExportInput +{ // Frame names used in the bag. - std::string mapFrame = "map"; - std::string lidarFrame = "lidar"; + std::string mapFrame = "map"; + std::string lidarFrame = "lidar"; std::string cameraFrame = "camera"; // Trajectory of T_map_lidar poses (timestamps in nanoseconds, shared clock). @@ -31,31 +32,33 @@ struct RosExportInput { // 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; + 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; + 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" +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) + 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; + bool undistortCamera = true; // LiDAR can be exported in two flavours, independently: // - undistorted: points as registered by LIO, in the map frame (already @@ -63,16 +66,14 @@ struct RosExportOptions { // - 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; + 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) + 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 +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 index c6e16b51..12069d5b 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -1,4 +1,5 @@ #include "RosExport.h" +#include "TrajectoryViewerShaders.h" #include "external/glad.h" #include "imgui.h" #include "raylib.h" @@ -7,6 +8,8 @@ #include "rlgl.h" #include #include +#include +#include #include #include #include @@ -15,9 +18,6 @@ #include #include #include -#include -#include -#include "TrajectoryViewerShaders.h" #include #include #include @@ -72,7 +72,6 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) buf[bufSize - 1] = '\0'; } - // Build a time(seconds) -> T_world_lidar map suitable for getInterpolatedPose(). static std::map buildTrajMap(const Trajectory& traj) { @@ -93,8 +92,8 @@ static bool interpPose(const std::map& trajMap, int64_t return true; } -using trajectory_viewer_shaders::kVS; using trajectory_viewer_shaders::kFS; +using trajectory_viewer_shaders::kVS; struct GpuCloud { @@ -1392,12 +1391,12 @@ int main(int argc, char* argv[]) EndMode3D(); if (s.showCompassRuler) - { - Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); - Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); - Vector3 up = Vector3CrossProduct(right, fwd); - raylib_widgets::drawCompassRuler(right, up, s.orbit.distance, LIGHTGRAY); - } + { + Vector3 fwd = Vector3Normalize(Vector3Subtract(cam.target, cam.position)); + Vector3 right = Vector3Normalize(Vector3CrossProduct(fwd, cam.up)); + Vector3 up = Vector3CrossProduct(right, fwd); + raylib_widgets::drawCompassRuler(right, up, s.orbit.distance, LIGHTGRAY); + } // ── upload image viewer texture if worker produced one ──────────────── { @@ -1519,8 +1518,7 @@ int main(int argc, char* argv[]) // only tracks its immediate-mode batch renderer, not custom // glDrawArrays calls like ScanRenderer's), so these are scan_renderer's // own per-frame counts of the calls/points it issued in draw(). - ImGui::Text( - "(%d FPS)", GetFPS()); + ImGui::Text("(%d FPS)", GetFPS()); ImGui::EndMainMenuBar(); } diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h index 95c9c950..bd24a1c8 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h @@ -9,8 +9,8 @@ namespace trajectory_viewer_shaders { -// colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI -inline constexpr const char* kVS = R"( + // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI + inline constexpr const char* kVS = R"( #version 330 layout(location = 0) in vec3 pos; layout(location = 1) in float colorPacked; @@ -43,7 +43,7 @@ void main() { } )"; -inline const std::string kFS = std::string(R"( + inline const std::string kFS = std::string(R"( #version 330 in float fragIntensity; in vec4 vertColor; @@ -52,7 +52,8 @@ flat in float fragInRoi; uniform int colorMode; uniform int selectedCamera; // -1 = show all, else keep only points from this image out vec4 finalColor; -)") + raylib_widgets::kJetColormapGLSL + R"( +)") + raylib_widgets::kJetColormapGLSL + + R"( 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); diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 761fb09d..d1cd852a 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -3046,7 +3046,11 @@ void display() // window, the rest showing just the clear color. rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - ClearBackground(ColorFromNormalized(Vector4{ app_state.bg_color.x * app_state.bg_color.w, app_state.bg_color.y * app_state.bg_color.w, app_state.bg_color.z * app_state.bg_color.w, app_state.bg_color.w })); + ClearBackground(ColorFromNormalized( + Vector4{ app_state.bg_color.x * app_state.bg_color.w, + app_state.bg_color.y * app_state.bg_color.w, + app_state.bg_color.z * app_state.bg_color.w, + app_state.bg_color.w })); rlEnableDepthTest(); rlMatrixMode(RL_PROJECTION); @@ -3185,9 +3189,12 @@ void display() { session.control_points.index_picked_point = -1; // reset picked point when pose changes - app_state.new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); - app_state.new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); - app_state.new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); + app_state.new_rotation_center.x() = + session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); + app_state.new_rotation_center.y() = + session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); + app_state.new_rotation_center.z() = + session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); app_state.new_rotate_x = app_state.rotate_x; app_state.new_rotate_y = app_state.rotate_y; diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index bd36ff33..e0a062e0 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -828,7 +828,8 @@ void drawMiniCompassWithRuler() const Eigen::Matrix3f& R = app_state.viewLocal.rotation(); Vector3 right = { R(0, 0), R(0, 1), R(0, 2) }; Vector3 up = { R(1, 0), R(1, 1), R(1, 2) }; - Color rulerColor = ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); + Color rulerColor = + ColorFromNormalized(Vector4{ 1.0f - app_state.bg_color.x, 1.0f - app_state.bg_color.y, 1.0f - app_state.bg_color.z, 1.0f }); raylib_widgets::drawCompassRuler( right, up, app_state.translate_z, rulerColor, raylib_widgets::CompassAxisLabels{ "X (long.)", "Y (lat.)", "Z (vert.)" }); } @@ -1018,8 +1019,10 @@ void updateOrthoView() std::copy(&proj[0][0], &proj[3][3], app_state.m_ortho_projection); - Eigen::Vector3d v_eye_t(-app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h); + Eigen::Vector3d v_eye_t( + -app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t( + -app_state.camera_ortho_xy_view_shift_x, app_state.camera_ortho_xy_view_shift_y, app_state.camera_mode_ortho_z_center_h); Eigen::Vector3d v(0, 1, 0); TaitBryanPose pose_tb; diff --git a/apps/multi_view_tls_registration/rl_utils.h b/apps/multi_view_tls_registration/rl_utils.h index ca03ccda..aed861bc 100644 --- a/apps/multi_view_tls_registration/rl_utils.h +++ b/apps/multi_view_tls_registration/rl_utils.h @@ -121,10 +121,8 @@ struct AppStateBase // rlGetMatrixProjection() live at that point would still see the previous // frame's post-end3DMatrixStack() state (identity modelview, 2D ortho // projection), not the 3D camera, producing a meaningless pick ray. - Matrix frame_view_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; - Matrix frame_proj_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + Matrix frame_view_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; + Matrix frame_proj_3d = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; // Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width // support is uniform enough here not to need a runtime check -- always true. diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 75a4e3f0..c4e6759d 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -28,8 +28,8 @@ namespace // ============================================================================ namespace { - using raylib_render_shaders::kPointVS; using raylib_render_shaders::kPointFS; + using raylib_render_shaders::kPointVS; // Bytes per vertex in the uploaded buffer (xyz position + intensity). constexpr int kVertexStride = 4 * sizeof(float); diff --git a/core/src/raylib_render_shaders.hpp b/core/src/raylib_render_shaders.hpp index 60701dda..e2eafcc4 100644 --- a/core/src/raylib_render_shaders.hpp +++ b/core/src/raylib_render_shaders.hpp @@ -8,15 +8,15 @@ namespace raylib_render_shaders { -// vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per -// scan at upload time (see ScanRenderer::rebuild). vertexPosition is -// already world-space (rebuild() applies pc.m_pose before uploading), so -// it doubles as the Elevation/Distance color modes' input with no extra -// per-vertex data needed. colorMode selects between the flat per-scan -// pointColor (0, the original app's only mode) and the ScanColorMode -// gradients (1/2/3), matching the enum's Intensity/Elevation/Distance -// ordering exactly (see ScanRenderer::draw()'s static_cast below). -inline constexpr const char* kPointVS = R"( + // vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per + // scan at upload time (see ScanRenderer::rebuild). vertexPosition is + // already world-space (rebuild() applies pc.m_pose before uploading), so + // it doubles as the Elevation/Distance color modes' input with no extra + // per-vertex data needed. colorMode selects between the flat per-scan + // pointColor (0, the original app's only mode) and the ScanColorMode + // gradients (1/2/3), matching the enum's Intensity/Elevation/Distance + // ordering exactly (see ScanRenderer::draw()'s static_cast below). + inline constexpr const char* kPointVS = R"( #version 330 in vec3 vertexPosition; in float vertexIntensity; @@ -33,10 +33,10 @@ void main() } )"; -// jet() is shared (raylib_widgets/Shaders.h) with camera_lidar_calibration's -// and camera_lidar_trajectory_viewer's point shaders -- was byte-for-byte -// duplicated here. -inline const std::string kPointFS = std::string(R"( + // jet() is shared (raylib_widgets/Shaders.h) with camera_lidar_calibration's + // and camera_lidar_trajectory_viewer's point shaders -- was byte-for-byte + // duplicated here. + inline const std::string kPointFS = std::string(R"( #version 330 uniform vec4 pointColor; uniform int colorMode; @@ -47,7 +47,8 @@ uniform float distMax; in float fragIntensity; in vec3 fragWorldPos; out vec4 finalColor; -)") + raylib_widgets::kJetColormapGLSL + R"( +)") + raylib_widgets::kJetColormapGLSL + + R"( void main() { if (colorMode == 1) diff --git a/shared/include/HDMapping/PoseInterpolation.h b/shared/include/HDMapping/PoseInterpolation.h index 5159b204..019068a3 100644 --- a/shared/include/HDMapping/PoseInterpolation.h +++ b/shared/include/HDMapping/PoseInterpolation.h @@ -57,7 +57,7 @@ inline Eigen::Matrix4d getInterpolatedPose(const std::map t1); assert(query_time < t2); ret = Eigen::Matrix4d::Identity(); - const double res = (query_time - t1) / (t2 - t1); //residual + const double res = (query_time - t1) / (t2 - t1); // residual const Eigen::Vector3d diff = it_next->second.col(3).head<3>() - it_lower->second.col(3).head<3>(); ret.col(3).head<3>() = it_lower->second.col(3).head<3>() + diff * res; Eigen::Matrix3d r1 = it_lower->second.topLeftCorner(3, 3).matrix(); diff --git a/shared/tests/test_pose_interpolation.cpp b/shared/tests/test_pose_interpolation.cpp index 021321a3..ac357c44 100644 --- a/shared/tests/test_pose_interpolation.cpp +++ b/shared/tests/test_pose_interpolation.cpp @@ -7,18 +7,18 @@ namespace { -Eigen::Matrix4d makePose(double tx, double ty, double tz, double yawRad = 0.0) -{ - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - T.topLeftCorner<3, 3>() = Eigen::AngleAxisd(yawRad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - T.col(3).head<3>() = Eigen::Vector3d(tx, ty, tz); - return T; -} + Eigen::Matrix4d makePose(double tx, double ty, double tz, double yawRad = 0.0) + { + Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); + T.topLeftCorner<3, 3>() = Eigen::AngleAxisd(yawRad, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + T.col(3).head<3>() = Eigen::Vector3d(tx, ty, tz); + return T; + } -bool isZeroMatrix(const Eigen::Matrix4d& m) -{ - return m.isZero(0.0); -} + bool isZeroMatrix(const Eigen::Matrix4d& m) + { + return m.isZero(0.0); + } } // namespace TEST_CASE("getInterpolatedPose: empty trajectory returns a zero matrix") From 9b25d5108d92675390bde08bd26d607d4b2c3264 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 4 Aug 2026 22:17:22 +0200 Subject: [PATCH 13/13] update readme Signed-off-by: Michal Pelka --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b7763249..25eda7a9 100644 --- a/README.md +++ b/README.md @@ -134,7 +134,7 @@ More information can be found here: # Camera-LiDAR calibration tools -Three tools (`apps/camera_lidar_*`) supporting the spherical-camera-to-3D-LiDAR calibration/coloring workflow described in Będkowski et al., "Method for spherical camera to 3D LiDAR calibration and synchronization with example on Insta360 X4 and LiVOX MID 360" (2025, EuroCOW, [[PDF]](https://isprs-archives.copernicus.org/articles/XLVIII-1-W4-2025/13/2025/isprs-archives-XLVIII-1-W4-2025-13-2025.pdf)): +Three tools (`apps/camera_lidar_*`) supports camera calibration with LIDAR and perofming applying colors to data from session. - **camera_lidar_intrinsics_calib** -- checkerboard-based camera intrinsic calibration (OpenCV rational distortion model). - **camera_lidar_calibration** -- interactive LiDAR-camera extrinsic calibration: aligns a LAZ/LAS point cloud to a camera image with live GPU-shader reprojection feedback.