From 05ccecc69feab139d9b47b6c749609454669ab89 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 11 Sep 2026 15:14:28 +0200 Subject: [PATCH 01/22] Add calibration tooling to manual_color: intensity view, extrinsics/intensity windows, 2D overlay, CLI, error display - Fix broken intensity grayscale (was unnormalized glColor3f) and add a calibration window (min/max/gamma, auto-range) for it. - Add an extrinsics calibration window with live-preview sliders and precise +/- angle nudge buttons for the camera-to-LiDAR pose. - Add an optional 2D projection overlay (intensity or depth, jet colormap, alpha, decimation) clipped to the image viewport. - Add --photo/--laz CLI flags to load files on startup. - Show per-pair reprojection error and RMS in the point-pair lists, with a "remove pair" action that keeps the 2D/3D correspondence lists in sync. - Add Space as a shortcut to toggle RGB <-> intensity in the 3D view. Co-Authored-By: Claude Sonnet 5 --- apps/manual_color/manual_color.cpp | 507 +++++++++++++++++++++++++++-- 1 file changed, 478 insertions(+), 29 deletions(-) diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 2c187642..3291f1ae 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -8,10 +8,13 @@ #define STB_IMAGE_IMPLEMENTATION #include "stb_image.h" +#include #include +#include #include #include #include +#include #include #include #include @@ -112,7 +115,13 @@ void display(); void reshape(int w, int h); void mouse(int glut_button, int state, int x, int y); void motion(int x, int y); +void keyboard(unsigned char key, int x, int y); bool initGL(int* argc, char** argv); +void loadPhotoFile(const std::string& path); +void loadLazFile(const std::string& path); +void recolorPointsFromImage(); +double reprojectionErrorPx(size_t i); +void removeCorrespondence(size_t i); float imgui_co_size{ 1000.0f }; bool imgui_draw_co{ true }; @@ -134,10 +143,61 @@ namespace SystemData Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); int point_size = 1; + + // ── manual intensity-view calibration ─────────────────────────────────── + // The intensity view maps raw p.intensity to grayscale as: + // t = clamp((intensity - intensityMin) / (intensityMax - intensityMin), 0, 1) ^ intensityGamma + // Defaults cover typical 8-bit LAS intensity; "Auto range" fits them to + // the loaded cloud since raw ranges vary a lot by sensor/scale. + bool showIntensityCalibWindow = false; + float intensityMin = 0.f; + float intensityMax = 255.f; + float intensityGamma = 1.f; + + // ── manual extrinsics calibration ──────────────────────────────────────── + // Lets the user nudge camera_pose directly with sliders, as an alternative + // to (or a starting point / fine-tune step for) the point-pair Optimize(). + bool showExtrinsicsCalibWindow = false; + float angleStepDeg = 1.f; // nudge size for the extrinsics window's -/+ angle buttons + + // ── optional 2D projection overlay (intensity / depth) ────────────────── + // Reprojects the point cloud onto the displayed image with the current + // camera_pose, colored by intensity or by range from the camera -- a + // quick visual check of extrinsics alignment against the photo. + bool showProjectionOverlay = false; + int overlayColorMode = 0; // 0 = intensity, 1 = depth + int overlayDecim = 5; // draw every Nth point (reprojection is not free) + float overlayPointRadius = 1.5f; + float overlayDepthMin = 0.f; + float overlayDepthMax = 20.f; + float overlayAlpha = 0.8f; } // namespace SystemData int main(int argc, char* argv[]) { + std::string photoPath, lazPath; + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + auto nextArg = [&]() -> std::string + { + return (i + 1 < argc) ? argv[++i] : std::string{}; + }; + if (arg == "--photo" || arg == "--image") + photoPath = nextArg(); + else if (arg == "--laz" || arg == "--pointcloud") + lazPath = nextArg(); + else if (arg == "-h" || arg == "--help") + { + std::cout << "Usage: mandeye_with_360_camera_manual_coloring [--photo ] [--laz ]\n" + << " --photo, --image equirectangular image to color the point cloud with\n" + << " --laz, --pointcloud LAZ point cloud to load\n"; + return 0; + } + else + std::cerr << "Unknown argument: " << arg << " (see --help)\n"; + } + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); // pose.om = M_PI * 0.5; // pose.fi = 0; @@ -155,12 +215,102 @@ int main(int argc, char* argv[]) SystemData::camera_pose = affine_matrix_from_pose_tait_bryan(pose); initGL(&argc, argv); + + if (!photoPath.empty()) + loadPhotoFile(photoPath); + if (!lazPath.empty()) + loadLazFile(lazPath); + if (!photoPath.empty() || !lazPath.empty()) + recolorPointsFromImage(); + glutDisplayFunc(display); glutMouseFunc(mouse); glutMotionFunc(motion); + glutKeyboardFunc(keyboard); glutMainLoop(); } +ImU32 jetColor(float t, float alpha = 1.f) +{ + t = std::clamp(t, 0.f, 1.f); + float r = std::clamp(1.5f - std::fabs(4.f * t - 3.f), 0.f, 1.f); + float g = std::clamp(1.5f - std::fabs(4.f * t - 2.f), 0.f, 1.f); + float b = std::clamp(1.5f - std::fabs(4.f * t - 1.f), 0.f, 1.f); + return IM_COL32( + static_cast(r * 255.f), + static_cast(g * 255.f), + static_cast(b * 255.f), + static_cast(std::clamp(alpha, 0.f, 1.f) * 255.f)); +} + +// Reprojects SystemData::points onto the image displayed at [img_start, +// img_start + (my_tex_w, my_tex_h)] using the current camera_pose, colored +// by intensity or by range from the camera. img_start/my_tex_w/my_tex_h use +// the same normalized-to-displayed-image mapping as the point_picked overlay +// drawn right after this in imagePicker(). clip_min/clip_max restrict drawing +// to the image's visible (scrolled) viewport rect, so the overlay doesn't +// spill onto the rest of the UI when scrolled or zoomed. +void drawProjectionOverlay(const ImVec2& img_start, float my_tex_w, float my_tex_h, const ImVec2& clip_min, const ImVec2& clip_max) +{ + namespace SD = SystemData; + if (!SD::showProjectionOverlay || SD::imageWidth <= 0 || SD::imageHeight <= 0) + return; + + const TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SD::camera_pose); + const Eigen::Vector3d camPos = SD::camera_pose.translation(); + auto* drawList = ImGui::GetForegroundDrawList(); + const int step = std::max(1, SD::overlayDecim); + const float intensityRange = std::max(SD::intensityMax - SD::intensityMin, 1e-6f); + const float depthRange = std::max(SD::overlayDepthMax - SD::overlayDepthMin, 1e-6f); + + drawList->PushClipRect(clip_min, clip_max, true); + for (size_t i = 0; i < SD::points.size(); i += step) + { + const auto& p = SD::points[i]; + double du, dv; + equrectangular_camera_colinearity_tait_bryan_wc( + du, + dv, + SD::imageHeight, + SD::imageWidth, + M_PI, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + p.point.x(), + p.point.y(), + p.point.z()); + + if (du < 0 || dv < 0 || du >= SD::imageWidth || dv >= SD::imageHeight) + continue; + + const float u = static_cast(du / SD::imageWidth); + const float v = static_cast(dv / SD::imageHeight); + const ImVec2 center{ img_start.x + u * my_tex_w, img_start.y + v * my_tex_h }; + + if (center.x < clip_min.x || center.x > clip_max.x || center.y < clip_min.y || center.y > clip_max.y) + continue; + + float t; + if (SD::overlayColorMode == 1) // depth + { + const float depth = static_cast((p.point - camPos).norm()); + t = std::clamp((depth - SD::overlayDepthMin) / depthRange, 0.f, 1.f); + } + else // intensity + { + t = std::clamp((p.intensity - SD::intensityMin) / intensityRange, 0.f, 1.f); + t = std::pow(t, SD::intensityGamma); + } + + drawList->AddCircleFilled(center, SD::overlayPointRadius, jetColor(t, SD::overlayAlpha)); + } + drawList->PopClipRect(); +} + void imagePicker( const std::string& name, ImTextureID tex1, std::vector& point_picked, const std::vector& point_pickedInPointcloud) { @@ -204,6 +354,58 @@ void imagePicker( const ImVec2 child_size{ ImGui::GetWindowWidth() * 1.0f, ImGui::GetWindowHeight() * 0.5f }; ImGui::Checkbox("color", &color); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Space also toggles this (3D view: RGB <-> intensity)"); + ImGui::SameLine(); + if (ImGui::Button("Intensity calibration...")) + { + SystemData::showIntensityCalibWindow = true; + } + + ImGui::Checkbox("2D overlay", &SystemData::showProjectionOverlay); + if (SystemData::showProjectionOverlay) + { + ImGui::SameLine(); + ImGui::SetNextItemWidth(110.f); + const char* overlayModes[] = { "Intensity", "Depth" }; + ImGui::Combo("##overlayMode", &SystemData::overlayColorMode, overlayModes, 2); + ImGui::SameLine(); + ImGui::SetNextItemWidth(90.f); + ImGui::DragInt("decim##overlay", &SystemData::overlayDecim, 1, 1, 500); + ImGui::SetNextItemWidth(150.f); + ImGui::SliderFloat("Alpha##overlay", &SystemData::overlayAlpha, 0.f, 1.f, "%.2f"); + if (SystemData::overlayColorMode == 1) + { + ImGui::SameLine(); + ImGui::SetNextItemWidth(80.f); + ImGui::DragFloat("min##depth", &SystemData::overlayDepthMin, 0.1f, 0.f, SystemData::overlayDepthMax); + ImGui::SameLine(); + ImGui::SetNextItemWidth(80.f); + ImGui::DragFloat("max##depth", &SystemData::overlayDepthMax, 0.1f, SystemData::overlayDepthMin, 1000.f); + ImGui::SameLine(); + if (ImGui::SmallButton("Auto##depth")) + { + const Eigen::Vector3d camPos = SystemData::camera_pose.translation(); + float lo = std::numeric_limits::max(); + float hi = -std::numeric_limits::max(); + for (const auto& p : SystemData::points) + { + const float depth = static_cast((p.point - camPos).norm()); + lo = std::min(lo, depth); + hi = std::max(hi, depth); + } + if (lo <= hi) + { + SystemData::overlayDepthMin = lo; + SystemData::overlayDepthMax = hi; + } + } + } + if (SystemData::overlayDepthMax < SystemData::overlayDepthMin) + { + SystemData::overlayDepthMax = SystemData::overlayDepthMin; + } + } struct point_pair { @@ -270,6 +472,7 @@ void imagePicker( const ImVec2 view_port_start = ImGui::GetWindowPos(); const ImVec2 view_port_end{ view_port_start.x + ImGui::GetWindowWidth(), view_port_start.y + ImGui::GetWindowHeight() }; ImVec2 img_start = ImGui::GetItemRectMin(); + drawProjectionOverlay(img_start, my_tex_w, my_tex_h, view_port_start, view_port_end); for (int i = 0; i < point_picked.size(); i++) { const auto& p = point_picked[i]; @@ -646,6 +849,73 @@ void TimeStampCount() } } +void loadPhotoFile(const std::string& path) +{ + tex1 = make_tex(path); + SystemData::imageData = stbi_load(path.c_str(), &SystemData::imageWidth, &SystemData::imageHeight, &SystemData::imageNrChannels, 0); +} + +void loadLazFile(const std::string& path) +{ + auto points = mandeye::load(path); + SystemData::points.resize(points.size()); + std::transform( + points.begin(), + points.end(), + SystemData::points.begin(), + [&](const mandeye::Point& p) + { + return p; + }); +} + +void recolorPointsFromImage() +{ + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); +} + +// Reprojection error, in image pixels, for the i-th picked correspondence +// (pointPickedImage[i] <-> pointPickedPointCloud[i]) under the current +// camera_pose. Returns -1 when the pair doesn't exist (indices out of range +// or no image loaded yet). +double reprojectionErrorPx(size_t i) +{ + namespace SD = SystemData; + if (i >= SD::pointPickedImage.size() || i >= SD::pointPickedPointCloud.size() || SD::imageWidth <= 0 || SD::imageHeight <= 0) + return -1.0; + + const TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SD::camera_pose); + const auto& P = SD::pointPickedPointCloud[i]; + double du, dv; + equrectangular_camera_colinearity_tait_bryan_wc( + du, dv, SD::imageHeight, SD::imageWidth, M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, P.x(), P.y(), P.z()); + + const double u_kp = SD::pointPickedImage[i].x * SD::imageWidth; + const double v_kp = SD::pointPickedImage[i].y * SD::imageHeight; + const double dx = du - u_kp; + const double dy = dv - v_kp; + return std::sqrt(dx * dx + dy * dy); +} + +// Removes the i-th correspondence from both sides at once, keeping +// pointPickedImage[k] <-> pointPickedPointCloud[k] aligned by index (the two +// lists' own per-side "-" buttons only erase from one side, which desyncs +// every later pair -- use this instead when removing a whole pair). +void removeCorrespondence(size_t i) +{ + namespace SD = SystemData; + if (i < SD::pointPickedImage.size()) + SD::pointPickedImage.erase(SD::pointPickedImage.begin() + i); + if (i < SD::pointPickedPointCloud.size()) + SD::pointPickedPointCloud.erase(SD::pointPickedPointCloud.begin() + i); +} + void ImGuiLoadSaveButtons() { namespace SD = SystemData; @@ -654,17 +924,9 @@ void ImGuiLoadSaveButtons() const auto input_file_names = mandeye::fd::OpenFileDialog("Choose Image", mandeye::fd::ImageFilter, false); if (input_file_names.size()) { - tex1 = make_tex(input_file_names.front()); - SD::imageData = stbi_load(input_file_names.front().c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); + loadPhotoFile(input_file_names.front()); } - - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); + recolorPointsFromImage(); } ImGui::SameLine(); if (ImGui::Button("Load Poincloud")) @@ -672,24 +934,9 @@ void ImGuiLoadSaveButtons() const auto input_file_names = mandeye::fd::OpenFileDialog("Choose Pointcloud", mandeye::fd::LazFilter, false); if (!input_file_names.empty()) { - auto points = mandeye::load(input_file_names.front()); - SystemData::points.resize(points.size()); - std::transform( - points.begin(), - points.end(), - SystemData::points.begin(), - [&](const mandeye::Point& p) - { - return p; - }); + loadLazFile(input_file_names.front()); } - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); + recolorPointsFromImage(); } ImGui::SameLine(); if (ImGui::Button("Save Pointcloud")) @@ -1014,8 +1261,10 @@ void display() } else { - glColor3f(p.intensity - 100, p.intensity - 100, p.intensity - 100); - // p.intensity + const float range = std::max(SystemData::intensityMax - SystemData::intensityMin, 1e-6f); + float t = std::clamp((p.intensity - SystemData::intensityMin) / range, 0.f, 1.f); + t = std::pow(t, SystemData::intensityGamma); + glColor3f(t, t, t); } glVertex3dv(p.point.data()); @@ -1130,6 +1379,11 @@ void display() SystemData::imageNrChannels, SystemData::camera_pose); } + ImGui::SameLine(); + if (ImGui::Button("Extrinsics calibration...")) + { + SystemData::showExtrinsicsCalibWindow = true; + } ImGui::InputInt("point_size", &SystemData::point_size); if (SystemData::point_size < 1) @@ -1257,6 +1511,22 @@ void display() ImGui::Text("page down: zoom out"); ImGui::Text("arrows: move image"); + // Reprojection error of the picked pairs under the current camera_pose -- + // a quick sanity check of calibration quality before/instead of Optimize. + { + const size_t nPairs = std::min(SystemData::pointPickedImage.size(), SystemData::pointPickedPointCloud.size()); + if (nPairs > 0) + { + double sumSq = 0.0; + for (size_t i = 0; i < nPairs; ++i) + { + const double e = reprojectionErrorPx(i); + sumSq += e * e; + } + ImGui::Text("Reprojection RMS error: %.2f px over %zu pair(s)", std::sqrt(sumSq / nPairs), nPairs); + } + } + // 2D Points Picked ImGui::BeginChild("2D", ImVec2(300, 0), true); ImGui::Text("2D:"); @@ -1265,6 +1535,22 @@ void display() auto index = std::distance(SystemData::pointPickedImage.begin(), it); const auto& p = *it; ImGui::Text("%d : %.1f,%.1f", index, p.x, p.y); + bool pairRemoved = false; + if (static_cast(index) < SystemData::pointPickedPointCloud.size()) + { + const double err = reprojectionErrorPx(static_cast(index)); + ImGui::SameLine(); + ImGui::TextColored(err > 20.0 ? ImVec4(1.f, 0.35f, 0.35f, 1.f) : ImVec4(0.6f, 0.6f, 0.6f, 1.f), "err %.1fpx", err); + ImGui::SameLine(); + const auto pairLabel = std::string("remove pair##2s") + std::to_string(index); + if (ImGui::SmallButton(pairLabel.c_str())) + { + removeCorrespondence(static_cast(index)); + pairRemoved = true; + } + } + if (pairRemoved) + break; ImGui::SameLine(); const auto label = std::string("-##2s") + std::to_string(index); if (ImGui::Button(label.c_str())) @@ -1316,11 +1602,161 @@ void display() } ImGui::SameLine(); ImGui::Text("%ld: %.1f,%.1f,%.1f", index, p.x(), p.y(), p.z()); + if (static_cast(index) < SystemData::pointPickedImage.size()) + { + const double err = reprojectionErrorPx(static_cast(index)); + ImGui::SameLine(); + ImGui::TextColored(err > 20.0 ? ImVec4(1.f, 0.35f, 0.35f, 1.f) : ImVec4(0.6f, 0.6f, 0.6f, 1.f), "err %.1fpx", err); + ImGui::SameLine(); + const auto pairLabel = std::string("remove pair##3s") + std::to_string(index); + if (ImGui::SmallButton(pairLabel.c_str())) + { + removeCorrespondence(static_cast(index)); + break; + } + } } ImGui::EndChild(); ImGui::End(); + // ── intensity view calibration window ─────────────────────────────────── + if (SystemData::showIntensityCalibWindow) + { + ImGui::SetNextWindowSize(ImVec2(320, 0), ImGuiCond_FirstUseEver); + if (ImGui::Begin("Intensity calibration", &SystemData::showIntensityCalibWindow)) + { + ImGui::TextDisabled("Grayscale remap for the intensity point-cloud view."); + ImGui::SetNextItemWidth(-1); + ImGui::DragFloat("Min", &SystemData::intensityMin, 1.f, -1e6f, SystemData::intensityMax); + ImGui::SetNextItemWidth(-1); + ImGui::DragFloat("Max", &SystemData::intensityMax, 1.f, SystemData::intensityMin, 1e6f); + ImGui::SetNextItemWidth(-1); + ImGui::SliderFloat("Gamma", &SystemData::intensityGamma, 0.1f, 5.f, "%.2f"); + if (SystemData::intensityMax < SystemData::intensityMin) + { + SystemData::intensityMax = SystemData::intensityMin; + } + ImGui::Separator(); + if (ImGui::Button("Auto range")) + { + float lo = std::numeric_limits::max(); + float hi = -std::numeric_limits::max(); + for (const auto& p : SystemData::points) + { + lo = std::min(lo, p.intensity); + hi = std::max(hi, p.intensity); + } + if (lo <= hi) + { + SystemData::intensityMin = lo; + SystemData::intensityMax = hi; + } + } + ImGui::SameLine(); + if (ImGui::Button("Reset")) + { + SystemData::intensityMin = 0.f; + SystemData::intensityMax = 255.f; + SystemData::intensityGamma = 1.f; + } + if (color) + { + ImGui::TextDisabled("(uncheck 'color' to preview the intensity view)"); + } + } + ImGui::End(); + } + + // ── extrinsics (camera-to-lidar pose) calibration window ──────────────── + if (SystemData::showExtrinsicsCalibWindow) + { + ImGui::SetNextWindowSize(ImVec2(340, 0), ImGuiCond_FirstUseEver); + if (ImGui::Begin("Extrinsics calibration", &SystemData::showExtrinsicsCalibWindow)) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); + float px = static_cast(pose.px); + float py = static_cast(pose.py); + float pz = static_cast(pose.pz); + float om = static_cast(pose.om); + float fi = static_cast(pose.fi); + float ka = static_cast(pose.ka); + + ImGui::TextDisabled("Camera-to-LiDAR pose (live preview, recolors on change)."); + ImGui::Text("Translation [m]"); + bool changed = false; + ImGui::SetNextItemWidth(-1); + changed |= ImGui::DragFloat("px", &px, 0.001f, -10.f, 10.f, "%.4f"); + ImGui::SetNextItemWidth(-1); + changed |= ImGui::DragFloat("py", &py, 0.001f, -10.f, 10.f, "%.4f"); + ImGui::SetNextItemWidth(-1); + changed |= ImGui::DragFloat("pz", &pz, 0.001f, -10.f, 10.f, "%.4f"); + ImGui::Separator(); + ImGui::Text("Rotation (Tait-Bryan)"); + ImGui::SetNextItemWidth(90.f); + ImGui::DragFloat("Nudge step (deg)", &SystemData::angleStepDeg, 0.05f, 0.01f, 45.f, "%.2f"); + SystemData::angleStepDeg = std::clamp(SystemData::angleStepDeg, 0.01f, 45.f); + const float stepRad = SystemData::angleStepDeg * static_cast(M_PI) / 180.f; + + auto angleRow = [&](const char* label, float& angle, const char* idSuffix) -> bool + { + ImGui::PushID(idSuffix); + bool rowChanged = false; + ImGui::SetNextItemWidth(150.f); + rowChanged |= ImGui::SliderAngle(label, &angle, -180.f, 180.f); + ImGui::SameLine(); + if (ImGui::Button("-")) + { + angle -= stepRad; + rowChanged = true; + } + ImGui::SameLine(); + if (ImGui::Button("+")) + { + angle += stepRad; + rowChanged = true; + } + ImGui::PopID(); + return rowChanged; + }; + + changed |= angleRow("omega (X)", om, "om"); + changed |= angleRow("phi (Y)", fi, "fi"); + changed |= angleRow("kappa (Z)", ka, "ka"); + + if (changed) + { + pose.px = px; + pose.py = py; + pose.pz = pz; + pose.om = om; + pose.fi = fi; + pose.ka = ka; + SystemData::camera_pose = affine_matrix_from_pose_tait_bryan(pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); + } + + ImGui::Separator(); + if (ImGui::Button("Print pose to console")) + { + std::cout << "pose" << std::endl; + std::cout << "px " << pose.px << std::endl; + std::cout << "py " << pose.py << std::endl; + std::cout << "pz " << pose.pz << std::endl; + std::cout << "om " << pose.om << std::endl; + std::cout << "fi " << pose.fi << std::endl; + std::cout << "ka " << pose.ka << std::endl; + } + } + ImGui::End(); + } + ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); glutSwapBuffers(); @@ -1431,6 +1867,19 @@ void motion(int x, int y) glutPostRedisplay(); } +void keyboard(unsigned char key, int x, int y) +{ + ImGui_ImplGLUT_KeyboardFunc(key, x, y); + ImGuiIO& io = ImGui::GetIO(); + + // Space toggles the 3D view between camera RGB and intensity grayscale, + // unless the key is meant for an ImGui text field (e.g. an InputFloat). + if (key == ' ' && !io.WantCaptureKeyboard) + { + color = !color; + } +} + void reshape(int w, int h) { glViewport(0, 0, (GLsizei)w, (GLsizei)h); From 37820711cc256275a0bb01de2b023deccf4572a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Fri, 11 Sep 2026 17:08:59 +0200 Subject: [PATCH 02/22] Add an equirectangular camera model to calib_core and the trajectory viewer calib::Intrinsics gains a CameraModel tag (Pinhole | Equirectangular) plus the image dimensions, which are what an equirectangular camera projects with in place of fx/fy/cx/cy, and projectPoint branches on it. The change is purely additive: projectPoint's signature is unchanged and the model defaults to Pinhole, so camera_lidar_calibration keeps building and behaving identically. It does not yet read or write the "model" key -- the comment on CameraModel records that gap, the other pinhole-only spots in that app, and how the vendored equirectangular observation equations drop into the solver when it is picked up. camera_lidar_trajectory_viewer wires the model up end to end: - loadCalib reads a "model" key, at the top level or under "intrinsics". - The image scanner accepts equirectangular_.jpg and bare .jpg alongside cam0_.jpg, and infers the model from the prefix when the calibration doesn't name one. Both loaders share one parse now instead of two copies, and the model is resolved in a single place because the calibration and the images arrive in either order. - colorize() calls calib::projectPoint instead of duplicating the distortion math inline, over intrinsics scaled by a new image-scale control -- a chunk of 360 frames is ~2.2 GB at full size and multi-image coloring holds a whole chunk resident. - Image dimensions come from the first scanned frame rather than a hardcoded 4656x3496 that matched neither camera; they drive the ROI default, the frustums and COLMAP's cameras.txt. - Frustums become a position marker and axis triad for a camera with no frustum; COLMAP export refuses, since its text model has no equirectangular type; the ROS CameraInfo reports "equirectangular" with no K rather than a pinhole that would mislead consumers, and rectification is skipped. New calib_core/tests (doctest, following shared/tests) cover the seam wrap, the poles, a bearing round trip, behind-the-camera points and the extrinsics, and pin the pinhole path against the refactor with hand-computed values. Also fixes a pre-existing include in RosExport.cpp that fails to compile whenever CALIB_ENABLE_ROS_EXPORT is ON. Co-Authored-By: Claude Opus 5 --- CMakeLists.txt | 1 + .../RosExport.cpp | 34 +- .../TrajectoryViewer.cpp | 304 ++++++++++++++---- calib_core/include/CalibCore/Camera.h | 51 ++- calib_core/src/Camera.cpp | 36 +++ calib_core/tests/CMakeLists.txt | 28 ++ calib_core/tests/test_camera.cpp | 283 ++++++++++++++++ 7 files changed, 658 insertions(+), 79 deletions(-) create mode 100644 calib_core/tests/CMakeLists.txt create mode 100644 calib_core/tests/test_camera.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b88a85ff..de357c09 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,6 +112,7 @@ option(BUILD_TESTING "Build HDMapping unit tests" OFF) if(BUILD_TESTING) enable_testing() add_subdirectory(shared/tests) + add_subdirectory(calib_core/tests) add_subdirectory(apps/lidar_odometry_step_1/tests) add_subdirectory(rosbags/tests) endif() diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 584be0e7..ff6cdffd 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -207,7 +207,10 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s cv::Mat map1, map2; bool mapsReady = false; int camW = 0, camH = 0; - const bool rectify = opt.undistortCamera && in.calibLoaded; + // initUndistortRectifyMap is pinhole-only: there is nothing to + // rectify on a 360 panorama, and Km/Dm describe a camera it isn't. + const bool equirect = in.K.model == CameraModel::Equirectangular; + const bool rectify = opt.undistortCamera && in.calibLoaded && !equirect; // 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; @@ -299,14 +302,29 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s 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 }; + if (equirect) + { + // No ROS distortion model describes a 360 panorama, + // and there is no K to report -- width/height are + // the whole projection. Leave k/p zeroed rather than + // publish a pinhole that would mislead consumers. + ci.distortion_model = "equirectangular"; + ci.d = {}; + ci.k = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + ci.p = { 0, 0, 0, 0, 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.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)); } } diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 6b9fe3ba..526cb43d 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -219,16 +220,30 @@ struct AppState { Trajectory traj; std::vector imageTsNs; - Intrinsics K; + Intrinsics K; // K.model selects pinhole vs equirectangular (see CalibCore/Camera.h) + // How K.model was decided. The calibration file's "model" key wins; absent + // one, the image filenames are the fallback. Both inputs are kept as state + // rather than applied on the spot because they arrive in either order -- + // loadSession() (and with it loadImages()) runs before loadCalib() at + // startup, but the user can load either on its own afterwards -- so + // resolveCameraModel() below recomputes K.model from scratch each time one + // of them changes. + CameraModel fileModel = CameraModel::Pinhole; + bool modelExplicit = false; // the calibration file named a model + bool namesLookEquirect = false; // the frames carry the equirectangular_ prefix Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame Roi roi; bool calibLoaded = false; - int imgW = 4656, imgH = 3496; + int imgW = 4656, imgH = 3496; // overwritten from the first scanned image by loadImages() // loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - const float imgScale = 1.0f; + // Downscale applied to every image used for coloring. Equirectangular + // frames are large (3840x1920x3 ≈ 22 MB) and multiImgColoring holds a whole + // chunk's worth in RAM at once, so this is what keeps that bounded. The + // intrinsics are scaled to match via calib::scaleIntrinsics. + float imgScale = 1.0f; GpuCloud cloud; Shader shader = {}; bool shaderOk = false; @@ -386,41 +401,104 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) return true; } -// Load all cam0_*.jpg from CAMERA_0 (sibling of session dir) into s.images, resized by s.imgScale. -static void loadImages(AppState& s) +// Prefix marking a frame as a 360 panorama rather than a normal camera image. +static constexpr const char* kEquirectPrefix = "equirectangular_"; + +// Timestamp encoded in a camera frame's filename, or -1 when the file isn't +// one. Three layouts are accepted: Mandeye's own "cam0_.jpg", the +// 360 rig's "equirectangular_.jpg", and a bare +// ".jpg". `equirect`, when given, reports whether the panorama +// prefix was the one found. The all-digits check matters for the bare form -- +// without it every unrelated .jpg in the directory would reach std::stoll. +static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) { - s.imagesFilenamesInTime.clear(); - fs::path camDir; - if (s.cameraBuf[0]) + if (equirect) + *equirect = false; + if (p.extension() != ".jpg") + return -1; + std::string stem = p.stem().string(); + if (stem.rfind(kEquirectPrefix, 0) == 0) { - camDir = fs::path(s.cameraBuf); + stem = stem.substr(std::strlen(kEquirectPrefix)); + if (equirect) + *equirect = true; } - else + else if (stem.rfind("cam0_", 0) == 0) { - camDir = fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; + stem = stem.substr(5); } + if (stem.empty() || stem.find_first_not_of("0123456789") != std::string::npos) + return -1; + try + { + return std::stoll(stem); + } catch (...) + { + return -1; + } +} + +// Directory holding the camera frames: whatever the user picked, else the +// CAMERA_0 sibling of the session dir. +static fs::path cameraDir(const AppState& s) +{ + return s.cameraBuf[0] ? fs::path(s.cameraBuf) : fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; +} + +// Settles K.model from the two inputs that can select it, in precedence order. +// Call after either of them changes; see AppState::fileModel for why this isn't +// done inline in the loaders. +static void resolveCameraModel(AppState& s) +{ + if (s.modelExplicit) + s.K.model = s.fileModel; + else + s.K.model = s.namesLookEquirect ? CameraModel::Equirectangular : CameraModel::Pinhole; +} + +// Index every camera frame in the camera directory by timestamp. Also picks up +// the image dimensions -- which the equirectangular model projects with, and +// which the ROI default, the frustums and the COLMAP cameras.txt line read -- +// and, absent an explicit "model" in the calibration, infers the camera model +// from the filenames. +static void loadImages(AppState& s) +{ + s.imagesFilenamesInTime.clear(); + fs::path camDir = cameraDir(s); if (!fs::is_directory(camDir)) { - s.status = "No CAMERA_0 dir found"; + s.status = "No camera image dir found: " + camDir.string(); return; } int loaded = 0; + int equirectNames = 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") + bool equirect = false; + int64_t ts = parseImageTsNs(e.path(), &equirect); + if (ts < 0) 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.imagesFilenamesInTime[ts] = e.path().string(); + equirectNames += equirect ? 1 : 0; + ++loaded; + } + if (!s.imagesFilenamesInTime.empty()) + { + cv::Mat probe = cv::imread(s.imagesFilenamesInTime.begin()->second, cv::IMREAD_COLOR); + if (!probe.empty()) { + s.imgW = probe.cols; + s.imgH = probe.rows; } } + // The "model" key wins whenever the calibration file carried one; the + // filenames are only a fallback. Either way the resolved model is shown in + // the Calibration panel, so an inferred one is never invisible. + s.namesLookEquirect = equirectNames > 0; + resolveCameraModel(s); + s.K.width = s.imgW; + s.K.height = s.imgH; s.status = "Images loaded: " + std::to_string(loaded) + " from " + camDir.string(); } @@ -501,22 +579,14 @@ static void loadSession(AppState& s) s.poseAngSpeedMax = s.poseAngSpeedDeg.empty() ? 0.f : *std::max_element(s.poseAngSpeedDeg.begin(), s.poseAngSpeedDeg.end()); // camera image timestamps - fs::path camDir = s.cameraBuf[0] ? fs::path(s.cameraBuf) : d.parent_path() / "CAMERA_0"; + fs::path camDir = cameraDir(s); 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 (...) - { - } - } + int64_t ts = parseImageTsNs(e.path()); + if (ts >= 0) + s.imageTsNs.push_back(ts); } std::sort(s.imageTsNs.begin(), s.imageTsNs.end()); } @@ -589,21 +659,26 @@ static void loadCloud(AppState& s) bool canColor = s.calibLoaded && !s.imagesFilenamesInTime.empty(); Eigen::Matrix3f R_wc = canColor ? s.R_wc : Eigen::Matrix3f::Identity(); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); - float K_fx = s.K.fx * s.imgScale, K_fy = s.K.fy * s.imgScale; - float K_cx = s.K.cx * s.imgScale, K_cy = s.K.cy * s.imgScale; - // 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; + // Images are read at s.imgScale, so the intrinsics have to match: this + // scales fx/fy/cx/cy for the pinhole model and width/height for the + // equirectangular one. calib::projectPoint then applies whichever model the + // calibration selected -- for pinhole that is the OpenCV rational + + // tangential distortion, so colours are sampled from the raw (distorted) + // images at the right pixel; with all-zero coefficients it reduces exactly + // to the ideal pinhole. + const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + // Every image of a chunk is held in memory at once (multiImgColoring), so + // for large frames the scale is what keeps that bounded. + auto readImage = [&](const std::string& path) + { + cv::Mat img = cv::imread(path); + if (!img.empty() && s.imgScale != 1.0f) + { + cv::Mat small; + cv::resize(img, small, cv::Size(), s.imgScale, s.imgScale, cv::INTER_AREA); + img = std::move(small); + } + return img; }; // Off-axis cutoff for the model above -- see maxValidRadiusSq(). const float rMaxSq = maxValidRadiusSq(d_k1, d_k2, d_k3, d_k4, d_k5, d_k6); @@ -692,7 +767,7 @@ static void loadCloud(AppState& s) Eigen::Affine3f pose; if (!interpPose(trajMap, imgTs, pose)) continue; - cv::Mat img = cv::imread(fnIt->second); + cv::Mat img = readImage(fnIt->second); if (img.empty()) continue; int gidx = (int)(it - s.imageTsNs.begin()); @@ -720,7 +795,7 @@ static void loadCloud(AppState& s) ++angFilteredImgs; if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) { - cv::Mat img = cv::imread(fnIt->second); + cv::Mat img = readImage(fnIt->second); int gidx = (int)(it - s.imageTsNs.begin()); if (!img.empty()) chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); @@ -792,6 +867,13 @@ static void loadCloud(AppState& s) float inRoiF = -1.f; // 1 inside ROI, 0 outside, -1 not in frustum int globalIdx = -1; }; + // Note for the equirectangular model: a 360 camera has no + // frustum, so every point projects into every image. The + // temporal strategy's outward search therefore always succeeds + // at w == 0, leaving maxTemporalDist as the only real gate, and + // the geometry strategy compares ranges across all of the + // chunk's images rather than only the ones containing the point + // -- still correct, just no longer short-circuiting. auto probe = [&](int idx) -> Hit { Hit h; @@ -799,18 +881,20 @@ static void loadCloud(AppState& s) return h; auto& e = chunkImgs[idx]; Eigen::Vector3f pl = e.pose.inverse() * pw; - Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); - if (pc_.z() <= 0.05f) + float u, v, depth; + if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) return h; - float xn = pc_.x() / pc_.z(), yn = pc_.y() / pc_.z(); - // Outside the cone the lens model is valid over: distorting this would - // fold it back into the frame. See maxValidRadiusSq(). - if (xn * xn + yn * yn > rMaxSq) + if (Ks.model == CameraModel::Pinhole && depth <= 0.05f) return h; - float xd, yd; - distort(xn, yn, xd, yd); - int iu = (int)std::round(K_fx * xd + K_cx); - int iv = (int)std::round(K_fy * yd + K_cy); + int iu = (int)std::round(u); + int iv = (int)std::round(v); + if (Ks.model == CameraModel::Equirectangular) + { + // u is wrapped into [0, cols) but rounding can still + // land on cols at the seam; v spans [0, rows] inclusive. + iu = (iu % e.img.cols + e.img.cols) % e.img.cols; + iv = std::clamp(iv, 0, e.img.rows - 1); + } if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) return h; // point projects into this image — record ROI membership so @@ -826,7 +910,7 @@ static void loadCloud(AppState& s) uint32_t p = (uint32_t(bgr[2]) << 16) | (uint32_t(bgr[1]) << 8) | uint32_t(bgr[0]); std::memcpy(&h.colorF, &p, 4); h.globalIdx = e.globalIdx; - h.depth = pc_.z(); + h.depth = depth; h.ok = true; return h; }; @@ -976,6 +1060,31 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; + // Camera model: "equirectangular"/"equirect" for a 360 panorama, anything + // else for the pinhole model this app started with. Accepted both at the + // top level and inside "intrinsics". Assigned unconditionally so loading a + // pinhole calibration after an equirectangular one clears the flag rather + // than inheriting it. + { + const bool topLevel = j.contains("model"); + const bool nested = j.contains("intrinsics") && j["intrinsics"].contains("model"); + s.modelExplicit = topLevel || nested; + std::string model; + if (topLevel) + model = j.value("model", std::string{}); + else if (nested) + model = j["intrinsics"].value("model", std::string{}); + std::transform( + model.begin(), + model.end(), + model.begin(), + [](unsigned char c) + { + return (char)std::tolower(c); + }); + s.fileModel = (model == "equirectangular" || model == "equirect") ? CameraModel::Equirectangular : CameraModel::Pinhole; + resolveCameraModel(s); + } if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; @@ -1361,6 +1470,13 @@ static void exportColmap(AppState& s) s.status = "COLMAP: no images"; return; } + if (s.K.model == CameraModel::Equirectangular) + { + // COLMAP's text model has no equirectangular camera type, so the + // FULL_OPENCV line below would misdescribe the images. + s.status = "COLMAP: equirectangular camera model is not supported by COLMAP"; + return; + } fs::path out(s.colmapBuf); fs::path sparse = out / "sparse"; @@ -1573,6 +1689,25 @@ static void drawScene(AppState& s) Vector3 origin = toVec3(pose->T * C); + bool hl = (ts == hlTs); + Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; + float sc = hl ? fs * 1.05f : fs; + + if (s.K.model == CameraModel::Equirectangular) + { + // A 360 camera sees the whole sphere, so there is no frustum to + // draw -- show where it was and which way its axes point + // instead. The triad is the usual X=red, Y=green, Z=blue. + DrawSphere(origin, fs * (hl ? 0.08f : 0.05f), fc); + const Color axisColors[3] = { RED, GREEN, BLUE }; + for (int k = 0; k < 3; k++) + { + Eigen::Vector3f tip = R_wc.col(k) * (sc * 0.5f) + C; + DrawLine3D(origin, toVec3(pose->T * tip), hl ? fc : axisColors[k]); + } + continue; + } + Vector3 w[4]; for (int k = 0; k < 4; k++) { @@ -1580,10 +1715,6 @@ static void drawScene(AppState& s) w[k] = toVec3(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 @@ -2202,8 +2333,33 @@ int main(int argc, char* argv[]) 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); + if (s.K.model == CameraModel::Equirectangular) + { + ImGui::Text("Model: equirectangular"); + if (!s.modelExplicit && ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Inferred from the \"%s\" image filenames.\nAdd \"model\" to the calibration JSON to set it explicitly.", + kEquirectPrefix); + ImGui::Text("%dx%d", s.imgW, s.imgH); + } + else + { + 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::Separator(); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + // Bounds every image the colorizer holds in memory: a whole + // chunk's worth is resident at once when multi-image coloring + // is on, which 360 frames make expensive. + ImGui::SliderFloat("Image scale", &s.imgScale, 0.125f, 1.0f, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Downscale applied to images before coloring.\nLower = less RAM and faster, at coarser color detail."); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); ImGui::Separator(); if (ImGui::Checkbox("Region of interest", &s.roi.enabled)) @@ -2315,9 +2471,14 @@ int main(int argc, char* argv[]) ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); if (ImGui::IsItemHovered()) ImGui::SetTooltip("ON: CompressedImage (jpeg)\nOFF: raw Image bgr8"); + const bool noRectify = s.K.model == CameraModel::Equirectangular; + ImGui::BeginDisabled(noRectify); 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::EndDisabled(); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip( + noRectify ? "Not applicable to an equirectangular camera." + : "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); @@ -2366,8 +2527,13 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); s.colmapPtDecim = std::max(1, s.colmapPtDecim); + const bool colmapUnsupported = s.K.model == CameraModel::Equirectangular; + ImGui::BeginDisabled(colmapUnsupported); if (ImGui::Button("Export COLMAP model", ImVec2(-1, 0))) exportColmap(s); + ImGui::EndDisabled(); + if (colmapUnsupported && ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip("COLMAP has no equirectangular camera model."); ImGui::TextDisabled("Writes sparse/{cameras,images,points3D}.txt"); if (ImGui::IsItemHovered()) ImGui::SetTooltip( diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 65d3de88..f90eda44 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -6,8 +6,35 @@ namespace calib { + // Which projection projectPoint() applies. Selected by a "model" key in the + // calibration JSON. + // + // Not every consumer honours this yet. apps/camera_lidar_calibration neither + // writes the key (saveCalibration) nor reads it (loadCalibration / + // loadIntrinsics -- and its OpenCV YAML input cannot express one at all), so + // it always operates as Pinhole: opening an Equirectangular calibration there + // would silently mis-project it, and rebuildImageTexture() would additionally + // run initUndistortRectifyMap over a panorama. Its GLSL projection + // (RendererShaders.h) and Renderer::drawCameraFrustum are pinhole-only too. + // Likewise solveExtrinsicsFromCorrespondences, whose only caller is that app. + // + // The solver drop-in is ready when that app is picked up: the vendored + // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc[_jacobian] + // take the same (tx,ty,tz,om,fi,ka,px,py,pz) order and 9-column layout as the + // perspective ones, so the kCameraLidarAxisOffset pre-rotation, the + // fixTranslation column slicing and the LM loop all carry over unchanged. + // Only three things differ: (fx,fy,cx,cy) becomes (rows,cols,pi), the + // jacobian is Eigen::Matrix rather than + // column-major, and it takes two extra trailing u_kp, v_kp arguments. + enum class CameraModel + { + Pinhole, // fx/fy/cx/cy + the rational distortion coefficients below + Equirectangular // 360 panorama; width/height are the intrinsics, k*/p* unused + }; + struct Intrinsics { + CameraModel model = CameraModel::Pinhole; float fx = 800.f, fy = 800.f; float cx = 640.f, cy = 360.f; // OpenCV rational distortion model: @@ -16,6 +43,11 @@ namespace calib float k4 = 0.f, k5 = 0.f, k6 = 0.f; // tangential float p1 = 0.f, p2 = 0.f; + // Image dimensions in pixels. Read only by CameraModel::Equirectangular, + // where they play the role fx/fy/cx/cy play for a pinhole camera and so + // *must* be set -- from the calibration file or from the loaded image -- + // before projectPoint() is called. + int width = 0, height = 0; }; // Minimum distance (degrees) fi is kept away from the om/fi/ka @@ -95,10 +127,25 @@ namespace calib // -I/O boundary either way. Result is passed through avoidGimbalLock. void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg); + // Intrinsics describing the same camera after its images are resampled by + // `s` (e.g. 0.5 for half-size images): scales fx/fy/cx/cy for Pinhole and + // width/height for Equirectangular, so a downscaled image projects with the + // same geometry. Everything else (distortion, model) is carried over. + Intrinsics scaleIntrinsics(const Intrinsics& K, float s); + // 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). + // + // Pinhole: depth = z component in camera frame (positive = in front), and + // the function returns false for points behind the camera. + // Equirectangular: depth = range from the camera, and only a point + // essentially at the camera itself fails -- a full-sphere camera has no + // frustum and no "behind". u comes back wrapped into [0, width); v spans + // [0, height] *inclusive*, the south pole landing exactly on height. + // + // In both cases the caller owns rounding to integer pixels (which can itself + // land on width at the equirectangular seam), bounds checking and any ROI + // test. bool projectPoint( float px, float py, diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index c2fbabd0..abe3b040 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,5 +1,7 @@ #include +#include + // Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- // header-only, pulls in nothing but Eigen/std (see structures.h), so this // doesn't violate calib_core's no-raylib/imgui/OpenCV design (see @@ -27,6 +29,17 @@ void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, floa ka_deg = static_cast(rad2deg(pose.ka)); } +Intrinsics scaleIntrinsics(const Intrinsics& K, float s) { + Intrinsics out = K; + out.fx *= s; + out.fy *= s; + out.cx *= s; + out.cy *= s; + out.width = static_cast(std::lround(K.width * s)); + out.height = static_cast(std::lround(K.height * s)); + return out; +} + bool projectPoint(float px, float py, float pz, const Intrinsics& K, const Eigen::Matrix3f& R_wc, @@ -35,6 +48,29 @@ bool projectPoint(float px, float py, float pz, // p_cam = R_wc^T * (p_lidar - C) Eigen::Vector3f pc = R_wc.transpose() * (Eigen::Vector3f(px, py, pz) - t); + if (K.model == CameraModel::Equirectangular) { + // Longitude from atan2(x, z) across the full width, latitude from + // asin(y/|p|) across the height -- camera X = right, Y = down, + // Z = forward, i.e. kCameraLidarAxisOffset's convention, so v grows + // downward like image rows. Same model apps/manual_color colors with; + // that app reaches it through the vendored equirectangular_camera_ + // colinearity_tait_bryan_wc_jacobian.h, not used here because it + // re-derives the rotation from a Tait-Bryan pose per point while + // R_wc/t are already in hand. + depth = pc.norm(); + if (depth < 1e-4f) return false; // point sits on the camera itself + + const float pi = static_cast(M_PI); + const float w = static_cast(K.width); + const float h = static_cast(K.height); + + u = w * (0.5f + std::atan2(pc.x(), pc.z()) / (2.f*pi)); + // atan2 returns exactly +pi on the seam, which maps to u == w + u = std::fmod(u + w, w); + v = h * (0.5f + std::asin(std::clamp(pc.y() / depth, -1.f, 1.f)) / pi); + return true; + } + depth = pc.z(); if (depth <= 1e-4f) return false; diff --git a/calib_core/tests/CMakeLists.txt b/calib_core/tests/CMakeLists.txt new file mode 100644 index 00000000..e385345f --- /dev/null +++ b/calib_core/tests/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(calib_core_tests) + +# Unit tests for calib_core's camera model. calib_core deliberately depends on +# nothing but Eigen/LASzip/std (see calib_core/CMakeLists.txt), so its +# projection math is directly testable without a GL context -- which is the +# whole reason the equirectangular model lives there rather than in an app. +# Uses doctest (3rdparty/doctest/doctest.h) for the same reason shared/tests +# does; see that directory's CMakeLists.txt. +add_executable(calib_core_tests + test_camera.cpp +) + +target_link_libraries(calib_core_tests PRIVATE calib_core) + +# calib_core's Eigen include is PRIVATE, so it isn't inherited by linking. +target_include_directories(calib_core_tests PRIVATE + ${THIRDPARTY_DIRECTORY}/doctest + ${EIGEN3_INCLUDE_DIR} +) + +if (MSVC) + target_compile_definitions(calib_core_tests PRIVATE _USE_MATH_DEFINES) +endif() + +include(CTest) +add_test(NAME calib_core_tests COMMAND calib_core_tests) \ No newline at end of file diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp new file mode 100644 index 00000000..179a7d3a --- /dev/null +++ b/calib_core/tests/test_camera.cpp @@ -0,0 +1,283 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include + +#include + +using namespace calib; + +namespace +{ + constexpr int kW = 3840; // the 360 rig's equirect frame size + constexpr int kH = 1920; + + Intrinsics equirect() + { + Intrinsics K; + K.model = CameraModel::Equirectangular; + K.width = kW; + K.height = kH; + return K; + } + + // Identity pose: p_cam == p_lidar, so test points can be written directly + // in camera axes (X = right, Y = down, Z = forward). + const Eigen::Matrix3f kIdentity = Eigen::Matrix3f::Identity(); + const Eigen::Vector3f kOrigin = Eigen::Vector3f::Zero(); + + // Convenience wrapper: projects and returns the pixel, CHECKing success. + struct Px + { + float u, v, depth; + }; + + Px project(const Intrinsics& K, const Eigen::Vector3f& p, const Eigen::Matrix3f& R_wc = kIdentity, const Eigen::Vector3f& t = kOrigin) + { + Px r{ 0, 0, 0 }; + REQUIRE(projectPoint(p.x(), p.y(), p.z(), K, R_wc, t, r.u, r.v, r.depth)); + return r; + } +} // namespace + +// ── Equirectangular ─────────────────────────────────────────────────────────── + +TEST_CASE("equirectangular: cardinal bearings land on the expected pixels") +{ + const Intrinsics K = equirect(); + + SUBCASE("forward is the image centre") + { + Px r = project(K, { 0, 0, 10 }); + CHECK(r.u == doctest::Approx(kW * 0.5)); + CHECK(r.v == doctest::Approx(kH * 0.5)); + CHECK(r.depth == doctest::Approx(10.0)); + } + SUBCASE("right is three quarters across") + { + Px r = project(K, { 5, 0, 0 }); + CHECK(r.u == doctest::Approx(kW * 0.75)); + CHECK(r.v == doctest::Approx(kH * 0.5)); + } + SUBCASE("left is one quarter across") + { + Px r = project(K, { -5, 0, 0 }); + CHECK(r.u == doctest::Approx(kW * 0.25)); + CHECK(r.v == doctest::Approx(kH * 0.5)); + } + SUBCASE("straight down is the bottom edge, inclusive") + { + Px r = project(K, { 0, 3, 0 }); + CHECK(r.v == doctest::Approx(kH)); // documented inclusive upper bound + } + SUBCASE("straight up is the top edge") + { + Px r = project(K, { 0, -3, 0 }); + CHECK(r.v == doctest::Approx(0.0)); + } +} + +TEST_CASE("equirectangular: depth is range, not z") +{ + const Intrinsics K = equirect(); + Px r = project(K, { 3, 0, 4 }); + CHECK(r.depth == doctest::Approx(5.0)); // a pinhole camera would report 4 +} + +TEST_CASE("equirectangular: points behind the camera still project") +{ + const Intrinsics K = equirect(); + + // Directly behind: atan2(0, -1) == +pi maps to u == width, which wraps to 0. + Px back = project(K, { 0, 0, -10 }); + CHECK(back.u == doctest::Approx(0.0)); + CHECK(back.v == doctest::Approx(kH * 0.5)); + + // The same point is rejected outright by the pinhole model. + Intrinsics P; // defaults to Pinhole + float u, v, depth; + CHECK_FALSE(projectPoint(0, 0, -10, P, kIdentity, kOrigin, u, v, depth)); +} + +TEST_CASE("equirectangular: u stays inside [0, width) either side of the seam") +{ + const Intrinsics K = equirect(); + + // Just past the seam on each side -- the wrap must not push u to width. + for (float dx : { -1e-3f, 1e-3f }) + { + Px r = project(K, { dx, 0, -10 }); + CHECK(r.u >= 0.f); + CHECK(r.u < static_cast(kW)); + } +} + +TEST_CASE("equirectangular: poles produce no NaN") +{ + const Intrinsics K = equirect(); + + // asin's argument is y/|p|, which rounds to slightly outside [-1, 1] for a + // point exactly on the axis unless it is clamped. + for (float sign : { -1.f, 1.f }) + { + Px r = project(K, { 0, sign * 7.f, 0 }); + CHECK_FALSE(std::isnan(r.u)); + CHECK_FALSE(std::isnan(r.v)); + } +} + +TEST_CASE("equirectangular: bearing -> pixel -> bearing round trip") +{ + const Intrinsics K = equirect(); + const float pi = static_cast(M_PI); + + const Eigen::Vector3f bearings[] = { + Eigen::Vector3f(0.3f, -0.2f, 0.9f).normalized(), + Eigen::Vector3f(-0.7f, 0.5f, -0.4f).normalized(), + Eigen::Vector3f(0.1f, 0.95f, 0.05f).normalized(), + Eigen::Vector3f(-0.6f, -0.1f, -0.8f).normalized(), + }; + + for (const auto& b : bearings) + { + Px r = project(K, b * 12.f); + + const float az = (r.u / kW - 0.5f) * 2.f * pi; + const float el = (r.v / kH - 0.5f) * pi; + Eigen::Vector3f back(std::cos(el) * std::sin(az), std::sin(el), std::cos(el) * std::cos(az)); + + CHECK(back.x() == doctest::Approx(b.x()).epsilon(1e-4)); + CHECK(back.y() == doctest::Approx(b.y()).epsilon(1e-4)); + CHECK(back.z() == doctest::Approx(b.z()).epsilon(1e-4)); + } +} + +TEST_CASE("equirectangular: respects the extrinsics") +{ + const Intrinsics K = equirect(); + + // om=fi=ka=0 is the nominal camera-vs-LiDAR alignment, so LiDAR forward + // (+X) should come out as camera forward, i.e. the image centre. + const Eigen::Matrix3f R_wc = kCameraLidarAxisOffset; + + SUBCASE("LiDAR forward is the image centre") + { + Px r = project(K, { 10, 0, 0 }, R_wc); + CHECK(r.u == doctest::Approx(kW * 0.5)); + CHECK(r.v == doctest::Approx(kH * 0.5)); + } + SUBCASE("LiDAR left is one quarter across") + { + Px r = project(K, { 0, 10, 0 }, R_wc); + CHECK(r.u == doctest::Approx(kW * 0.25)); + } + SUBCASE("LiDAR up is the top edge") + { + Px r = project(K, { 0, 0, 10 }, R_wc); + CHECK(r.v == doctest::Approx(0.0)); + } + SUBCASE("the camera position is subtracted") + { + // Point at the camera itself: too close to give a bearing. + const Eigen::Vector3f C(1.f, 2.f, 3.f); + float u, v, depth; + CHECK_FALSE(projectPoint(C.x(), C.y(), C.z(), K, R_wc, C, u, v, depth)); + + // One metre in front of the camera, not of the origin. + Px r = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); + CHECK(r.depth == doctest::Approx(1.0)); + CHECK(r.u == doctest::Approx(kW * 0.5)); + } +} + +// ── Pinhole (regression: this path must not change) ─────────────────────────── + +TEST_CASE("pinhole is the default model") +{ + CHECK(Intrinsics{}.model == CameraModel::Pinhole); +} + +TEST_CASE("pinhole: projection matches hand-computed values") +{ + Intrinsics K; // fx = fy = 800, cx = 640, cy = 360 + + SUBCASE("undistorted") + { + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(840.0)); + CHECK(r.v == doctest::Approx(760.0)); + CHECK(r.depth == doctest::Approx(4.0)); + } + SUBCASE("radial numerator") + { + K.k1 = 0.1f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(846.25)); + CHECK(r.v == doctest::Approx(772.5)); + } + SUBCASE("rational denominator") + { + K.k1 = 0.1f; + K.k4 = 0.2f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(834.117647)); + CHECK(r.v == doctest::Approx(748.235294)); + } + SUBCASE("tangential") + { + K.p1 = 0.01f; + K.p2 = 0.02f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(849.0)); + CHECK(r.v == doctest::Approx(770.5)); + } +} + +TEST_CASE("pinhole: rejects points at or behind the camera plane") +{ + Intrinsics K; + float u, v, depth; + CHECK_FALSE(projectPoint(1, 2, -4, K, kIdentity, kOrigin, u, v, depth)); + CHECK_FALSE(projectPoint(1, 2, 0, K, kIdentity, kOrigin, u, v, depth)); +} + +// ── scaleIntrinsics ─────────────────────────────────────────────────────────── + +TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") +{ + SUBCASE("pinhole") + { + Intrinsics K; + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Pinhole); + CHECK(H.fx == doctest::Approx(400.0)); + CHECK(H.cx == doctest::Approx(320.0)); + + Px full = project(K, { 1, 2, 4 }); + Px half = project(H, { 1, 2, 4 }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } + SUBCASE("equirectangular") + { + Intrinsics K = equirect(); + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Equirectangular); + CHECK(H.width == kW / 2); + CHECK(H.height == kH / 2); + + Px full = project(K, { 3, -1, 4 }); + Px half = project(H, { 3, -1, 4 }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } + SUBCASE("distortion and model are carried over unchanged") + { + Intrinsics K; + K.k1 = 0.1f; + K.p2 = 0.02f; + Intrinsics H = scaleIntrinsics(K, 0.25f); + CHECK(H.k1 == doctest::Approx(0.1)); + CHECK(H.p2 == doctest::Approx(0.02)); + } +} \ No newline at end of file From b620a1653df4f620fa0540051a4a21be28c922ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 14 Sep 2026 15:19:48 +0200 Subject: [PATCH 03/22] Add the Mei/unified-sphere camera model to calib_core and camera_lidar_calibration calib::CameraModel gains a Mei enumerator for the Insta360 rig's per-lens fisheye (distortion_model insta360_mei_v2). projectPoint does not re-derive the unified-sphere math -- it wraps the existing MeiCamera (MeiCamera.h/.cpp), which until now was an orphan: absent from every CMakeLists source list, and its own quoted "MeiCamera.h" include didn't match this repo's include/CalibCore layout, so it had never actually been compiled here. Intrinsics gains `xi`; k1/k2/k3 and p1/p2 are reused as Mei's own (non-rational) polynomial, k4/k5/k6 unused. calib_core therefore now compiles MeiCamera.cpp and picks up OpenCV's header-only Point2d/Point3d and yaml-cpp (camera_info.yaml loading) for it. camera_lidar_calibration is wired up end to end: - loadIntrinsics detects a Mei camera_info.yaml by its distortion_model key and loads it via LoadMeiCamera; the OpenCV-YAML path now resets model/xi explicitly so a Mei calibration can't linger behind a pinhole one. - load/saveCalibration round-trip "model" and "xi". - Intrinsics naming a different resolution than the loaded image are auto-scaled (calib::scaleIntrinsics) rather than only warned about, in whichever order the image and the calibration arrive; saveCalibration records the resolution they apply to (width/height) so the next load can do the same. - rebuildImageTexture skips initUndistortRectifyMap for non-pinhole models: it assumes OpenCV's rational pinhole model and would mis-warp a fisheye rather than rectify it, so a Mei image is always shown raw. - Both GLSL shaders gain a Mei branch mirroring MeiCamera::Project, so the projection overlay and Camera-RGB coloring work against that raw image. The clip weight is Xs.z+xi, the model's own "in front of the camera" test (it reduces exactly to the pinhole z_cam when xi==0); using the range there instead let points behind the camera through the hardware clip. - projectPoint applies that same Xs.z+xi>0 guard on the CPU side. MeiCamera:: Project has no domain guard of its own and the projection isn't injective past its valid dome, so a point behind the camera could otherwise be scored as visible at a plausible-looking pixel. - drawCameraFrustum becomes a position marker plus axis triad for non-pinhole models -- a rectangular pyramid misrepresents a fisheye's field of view -- matching what camera_lidar_trajectory_viewer already does for equirectangular. - The Intrinsics panel gains a model combo and an xi drag, and hides k4/k5/k6 for Mei rather than showing dead controls. New solveExtrinsicsMeiCeres: a Ceres-based extrinsics solver for Mei, since no vendored analytic Jacobian exists for this model as it does for Pinhole. It is gated behind -DCALIB_ENABLE_CERES, OFF by default because the README advertises depending on nothing but Eigen for optimization; without it the function compiles to a stub reporting why it is unavailable, so callers need no #ifdef of their own. The Pinhole solver is untouched and stays analytic. Tests check the Mei projection against MeiCamera::Project directly (so the wrapper is verified as delegation, not as a re-implementation), the behind-the-camera rejection for xi<1 alongside full-sphere coverage at xi>=1, scaleIntrinsics for Mei, and the no-Ceres stub. Co-Authored-By: Claude Opus 5 (1M context) --- apps/camera_lidar_calibration/App.cpp | 182 ++++++++++++++-- apps/camera_lidar_calibration/App.h | 17 ++ apps/camera_lidar_calibration/Renderer.cpp | 51 +++++ apps/camera_lidar_calibration/Renderer.h | 3 + .../RendererShaders.h | 89 ++++++-- apps/camera_lidar_calibration/UI.cpp | 56 ++++- calib_core/CMakeLists.txt | 72 ++++++- calib_core/include/CalibCore/Camera.h | 17 +- .../CalibCore/CameraCalibrationSolver.h | 45 +++- calib_core/include/CalibCore/MeiCamera.h | 51 +++++ calib_core/src/Camera.cpp | 33 +++ calib_core/src/CameraCalibrationSolverMei.cpp | 204 ++++++++++++++++++ calib_core/src/MeiCamera.cpp | 100 +++++++++ calib_core/tests/CMakeLists.txt | 16 +- calib_core/tests/test_camera.cpp | 146 +++++++++++++ calib_core/tests/test_solver.cpp | 136 ++++++++++++ 16 files changed, 1159 insertions(+), 59 deletions(-) create mode 100644 calib_core/include/CalibCore/MeiCamera.h create mode 100644 calib_core/src/CameraCalibrationSolverMei.cpp create mode 100644 calib_core/src/MeiCamera.cpp create mode 100644 calib_core/tests/test_solver.cpp diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 4ad06ec0..cd33ebdd 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -3,6 +3,7 @@ #include "raymath.h" #include "rlImGui.h" #include +#include #include #include #include @@ -18,6 +19,30 @@ #include #include +// ── model <-> string, for the calibration JSON's "model" key ───────────────── +// No `default:` case on purpose: -Wswitch (this target builds with -Wall +// -Wextra) then flags a future CameraModel enumerator added here without a +// matching string, instead of it silently falling through to "pinhole". +static const char* modelToString(CameraModel m) +{ + switch (m) + { + case CameraModel::Pinhole: return "pinhole"; + case CameraModel::Equirectangular: return "equirectangular"; + case CameraModel::Mei: return "mei"; + } + return "pinhole"; +} + +static CameraModel modelFromString(const std::string& s) +{ + if (s == "equirectangular") + return CameraModel::Equirectangular; + if (s == "mei") + return CameraModel::Mei; + return CameraModel::Pinhole; +} + // ── AppState::rebuildImageTexture ───────────────────────────────────────────── void AppState::rebuildImageTexture() { @@ -27,7 +52,14 @@ void AppState::rebuildImageTexture() cv::Mat display = originalImage; imageRectified = false; - if (intrinsicsLoaded) + // initUndistortRectifyMap assumes OpenCV's rational pinhole model -- + // running it for Mei (or Equirectangular) would silently mis-warp the + // image instead of undistorting it. Mei has no "undistort to pinhole" + // step here (that would need resampling through MeiCamera::Unproject + // into a virtual pinhole, not implemented), so its image is always + // shown raw; the projection overlay/GPU shaders apply its distortion + // directly to the raw image instead (see Renderer.cpp/RendererShaders.h). + if (intrinsicsLoaded && intrinsics.model == CameraModel::Pinhole) { 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) @@ -61,6 +93,33 @@ void AppState::rebuildImageTexture() imageLoaded = true; } +// ── AppState::autoScaleIntrinsicsToImage ────────────────────────────────────── +std::string AppState::autoScaleIntrinsicsToImage() +{ + if (!intrinsicsLoaded || intrinsicsW <= 0 || imageW <= 0) + return ""; + if (intrinsicsW == imageW && intrinsicsH == imageH) + return ""; + + // Width ratio is the scale factor -- calib::scaleIntrinsics only takes + // one, so a genuine aspect-ratio change (as opposed to a uniform + // resize) can't be fully corrected; sy is only computed to detect and + // warn about that case. + float sx = static_cast(imageW) / static_cast(intrinsicsW); + float sy = static_cast(imageH) / static_cast(intrinsicsH); + intrinsics = calib::scaleIntrinsics(intrinsics, sx); + intrinsicsW = imageW; + intrinsicsH = imageH; + + char buf[192]; + std::snprintf( + buf, sizeof(buf), "intrinsics auto-scaled %.4fx to match the %dx%d image", static_cast(sx), imageW, imageH); + std::string note = buf; + if (std::fabs(sx - sy) > 0.01f * sx) + note += " (WARNING: aspect ratio differs from the calibration -- scaled by width only, results may be off)"; + return note; +} + // ── AppState correspondence picking ─────────────────────────────────────────── void AppState::setPendingImagePoint(float u, float v) { @@ -139,10 +198,17 @@ bool AppState::solvePairs() } double rms = -1.0; - bool ok = calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); + std::string solveErr; + // Pinhole's reused observation equations are a pure rectilinear + // projection with no unified-sphere term, so they can't be used for + // Mei -- solveExtrinsicsMeiCeres (Ceres autodiff, optional at build + // time) is its counterpart instead. See CameraCalibrationSolver.h. + bool ok = (intrinsics.model == CameraModel::Mei) + ? calib::solveExtrinsicsMeiCeres(corr, intrinsics, extrinsics, solveErr, &rms, lockTranslation) + : calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); if (!ok) { - statusMsg = "Solve failed (degenerate correspondences)"; + statusMsg = !solveErr.empty() ? ("Solve failed: " + solveErr) : "Solve failed (degenerate correspondences)"; return false; } @@ -168,9 +234,14 @@ void AppState::loadImage(const char* path) imageW = originalImage.cols; imageH = originalImage.rows; imagePath = path; + // Intrinsics may already be loaded for a different resolution (e.g. a + // calibration taken at full res, then a downscaled image loaded here). + std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); renderer.init(imageW, imageH); statusMsg = imageRectified ? "Image loaded and rectified" : "Image loaded (raw)"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; } // ── AppState::loadCloud ─────────────────────────────────────────────────────── @@ -368,6 +439,28 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img return true; } +// A camera_info.yaml (MeiCamera's format) is a flat top-level mapping with a +// `distortion_model:` key, unlike OpenCV's `camera_matrix:`/`distortion_ +// coefficients:` YAML -- peeked at as plain text (not parsed) so a normal +// OpenCV pinhole YAML never round-trips through LoadMeiCamera and hits its +// "missing an expected field" warnings for fields it was never going to have. +static bool yamlLooksLikeMei(const char* path) +{ + std::ifstream f(path); + std::string line; + while (std::getline(f, line)) + { + auto pos = line.find("distortion_model:"); + if (pos == std::string::npos) + continue; + std::string value = line.substr(pos + std::string("distortion_model:").size()); + for (auto& c : value) + c = static_cast(tolower(static_cast(c))); + return value.find("mei") != std::string::npos; + } + return false; +} + // ── AppState::loadIntrinsics ────────────────────────────────────────────────── void AppState::loadIntrinsics(const char* path) { @@ -377,6 +470,39 @@ void AppState::loadIntrinsics(const char* path) for (auto& c : ext) c = static_cast(tolower(c)); + if ((ext == "yml" || ext == "yaml") && yamlLooksLikeMei(path)) + { + MeiCamera cam = LoadMeiCamera(path); + if (!cam.loaded) + { + statusMsg = std::string("Mei intrinsics failed to load (see console): ") + path; + return; + } + intrinsics.model = CameraModel::Mei; + intrinsics.fx = static_cast(cam.fx); + intrinsics.fy = static_cast(cam.fy); + intrinsics.cx = static_cast(cam.cx); + intrinsics.cy = static_cast(cam.cy); + intrinsics.xi = static_cast(cam.xi); + intrinsics.k1 = static_cast(cam.k1); + intrinsics.k2 = static_cast(cam.k2); + intrinsics.k3 = static_cast(cam.k3); + intrinsics.k4 = intrinsics.k5 = intrinsics.k6 = 0.f; // unused by Mei + intrinsics.p1 = static_cast(cam.p1); + intrinsics.p2 = static_cast(cam.p2); + intrinsicsW = cam.width; + intrinsicsH = cam.height; + intrinsicsLoaded = true; + std::string scaleNote = autoScaleIntrinsicsToImage(); + rebuildImageTexture(); // no-op undistortion for Mei, but refreshes the texture + statusMsg = "Mei intrinsics loaded"; + if (cam.width > 0) + statusMsg += " (calibration " + std::to_string(cam.width) + "x" + std::to_string(cam.height) + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; + return; + } + if (ext == "yml" || ext == "yaml") { int imgW = 0, imgH = 0; @@ -386,17 +512,20 @@ void AppState::loadIntrinsics(const char* path) statusMsg = std::string("YAML error: ") + err + " (" + path + ")"; return; } + intrinsics.model = CameraModel::Pinhole; // this YAML format cannot express any other model + intrinsics.xi = 0.f; + intrinsicsW = imgW; + intrinsicsH = imgH; intrinsicsLoaded = true; - rebuildImageTexture(); // re-rectify with the new coefficients + std::string scaleNote = autoScaleIntrinsicsToImage(); + rebuildImageTexture(); // re-rectify with the new (possibly auto-scaled) coefficients statusMsg = "Intrinsics loaded"; + if (imgW > 0) + statusMsg += " (calibration " + std::to_string(imgW) + "x" + std::to_string(imgH) + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; 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; } @@ -408,10 +537,12 @@ void AppState::loadIntrinsics(const char* path) } nlohmann::json j; f >> j; + intrinsics.model = modelFromString(j.value("model", std::string("pinhole"))); 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.xi = j.value("xi", 0.f); intrinsics.k1 = j.value("k1", 0.f); intrinsics.k2 = j.value("k2", 0.f); intrinsics.k3 = j.value("k3", 0.f); @@ -420,9 +551,17 @@ void AppState::loadIntrinsics(const char* path) intrinsics.k6 = j.value("k6", 0.f); intrinsics.p1 = j.value("p1", 0.f); intrinsics.p2 = j.value("p2", 0.f); + // No "width"/"height" in the file -- assume it matches whatever image is + // already loaded (this format historically had no resolution field at + // all, so anything already loaded is the best guess available). + intrinsicsW = j.value("width", imageLoaded ? imageW : 0); + intrinsicsH = j.value("height", imageLoaded ? imageH : 0); intrinsicsLoaded = true; + std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); statusMsg = "Intrinsics loaded."; + if (!scaleNote.empty()) + statusMsg += " " + scaleNote; } // ── AppState::loadCalibration ───────────────────────────────────────────────── @@ -449,10 +588,12 @@ void AppState::loadCalibration(const char* path) if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; + intrinsics.model = modelFromString(ji.value("model", std::string("pinhole"))); 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.xi = ji.value("xi", 0.f); intrinsics.k1 = ji.value("k1", 0.f); intrinsics.k2 = ji.value("k2", 0.f); intrinsics.k3 = ji.value("k3", 0.f); @@ -461,6 +602,8 @@ void AppState::loadCalibration(const char* path) intrinsics.k6 = ji.value("k6", 0.f); intrinsics.p1 = ji.value("p1", 0.f); intrinsics.p2 = ji.value("p2", 0.f); + intrinsicsW = ji.value("width", imageLoaded ? imageW : 0); + intrinsicsH = ji.value("height", imageLoaded ? imageH : 0); intrinsicsLoaded = true; gotIntrinsics = true; } @@ -498,8 +641,12 @@ void AppState::loadCalibration(const char* path) return; } + std::string scaleNote; if (gotIntrinsics) + { + scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); + } statusMsg = "Loaded"; if (gotIntrinsics) @@ -509,6 +656,8 @@ void AppState::loadCalibration(const char* path) if (gotExtrinsics) statusMsg += " extrinsics"; statusMsg += std::string(" from ") + path; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; } // ── AppState::saveCalibration ───────────────────────────────────────────────── @@ -521,9 +670,16 @@ void AppState::saveCalibration(const char* path) 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 } }; + // width/height record the resolution these intrinsics are valid for + // (intrinsicsW/H, not necessarily the original calibration file's own + // resolution -- see App.h) so a later load against a different-size + // image can auto-scale (AppState::autoScaleIntrinsicsToImage) instead + // of just warning about the mismatch. 0 means unknown. + j["intrinsics"] = { { "model", modelToString(intrinsics.model) }, { "fx", intrinsics.fx }, { "fy", intrinsics.fy }, + { "cx", intrinsics.cx }, { "cy", intrinsics.cy }, { "xi", intrinsics.xi }, { "k1", intrinsics.k1 }, + { "k2", intrinsics.k2 }, { "k3", intrinsics.k3 }, { "k4", intrinsics.k4 }, { "k5", intrinsics.k5 }, + { "k6", intrinsics.k6 }, { "p1", intrinsics.p1 }, { "p2", intrinsics.p2 }, + { "width", intrinsicsW }, { "height", intrinsicsH } }; // Rotation is stored as a matrix only -- convention-independent (no // Euler/Tait-Bryan angle order or units to document/misread) and // directly portable to any external tool. camera_rotation_matrix_in_world diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index aa81695b..eaaa6b8a 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -40,6 +40,14 @@ struct AppState // ── calibration params ─────────────────────────────────────────────────── Intrinsics intrinsics; Extrinsics extrinsics; + // Resolution `intrinsics` are currently valid for -- from the + // calibration file's own width/height when it states one, or (if it + // didn't) whatever image was already loaded at the time. 0 = unknown, + // meaning autoScaleIntrinsicsToImage() has nothing to scale from. + // Kept in sync by that function, so it always names the size the + // *current* (possibly already auto-scaled) intrinsics apply to, not + // necessarily the original calibration file's resolution. + int intrinsicsW = 0, intrinsicsH = 0; // ── visualization ───────────────────────────────────────────────────────── VisualizationParams vizParams; @@ -92,6 +100,15 @@ struct AppState // (Re)build the displayed texture: undistorts with current intrinsics // when they were loaded from a file, otherwise shows the raw image. void rebuildImageTexture(); + // If `intrinsicsW/H` names a resolution other than the current + // imageW/imageH, rescales `intrinsics` (calib::scaleIntrinsics) to + // match and updates intrinsicsW/H to the new size -- called after + // whichever of an image load or an intrinsics load comes second, so a + // calibration and an image of different resolutions just work instead + // of silently mis-projecting or only warning about it. No-op (returns + // "") if either resolution is unknown (0) or they already match. + // Callers still own calling rebuildImageTexture() afterward. + std::string autoScaleIntrinsicsToImage(); }; class App diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp index 1d16e3d3..77c356da 100644 --- a/apps/camera_lidar_calibration/Renderer.cpp +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -89,6 +89,10 @@ void Renderer::initPointShader() locCamK = rlGetLocationUniform(pointShader.id, "K"); locCamImgSize = rlGetLocationUniform(pointShader.id, "imgSize"); locCamTex = rlGetLocationUniform(pointShader.id, "imageTex"); + locCamModel = rlGetLocationUniform(pointShader.id, "model"); + locCamXi = rlGetLocationUniform(pointShader.id, "xi"); + locCamRad1 = rlGetLocationUniform(pointShader.id, "kRad1"); + locCamTan = rlGetLocationUniform(pointShader.id, "pTan"); } projShader = LoadShaderFromMemory(kProjVS, kProjFS.c_str()); @@ -105,6 +109,8 @@ void Renderer::initPointShader() locPrjRad1 = rlGetLocationUniform(projShader.id, "kRad1"); locPrjRad2 = rlGetLocationUniform(projShader.id, "kRad2"); locPrjTan = rlGetLocationUniform(projShader.id, "pTan"); + locPrjModel = rlGetLocationUniform(projShader.id, "model"); + locPrjXi = rlGetLocationUniform(projShader.id, "xi"); locPrjDepthRange = rlGetLocationUniform(projShader.id, "depthRange"); locPrjOpacity = rlGetLocationUniform(projShader.id, "opacity"); locPrjPointSize = rlGetLocationUniform(projShader.id, "pointSize"); @@ -193,6 +199,13 @@ void Renderer::renderImageOverlay( 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 }; + // model/xi only take effect when applyDistortion is set too, same as + // rad1/rad2/tan2 below -- applyDistortion==false means "treat as + // already rectified" regardless of model (kept exactly as before + // for Pinhole; Mei in practice always has applyDistortion==true, + // since AppState::rebuildImageTexture never rectifies it). + int model = 0; + float xiVal = 0.f; if (applyDistortion) { rad1[0] = K.k1; @@ -203,6 +216,11 @@ void Renderer::renderImageOverlay( rad2[2] = K.k6; tan2[0] = K.p1; tan2[1] = K.p2; + if (K.model == CameraModel::Mei) + { + model = 2; + xiVal = K.xi; + } } float depthRange[2] = { vp.depthMin, vp.depthMax }; @@ -213,6 +231,8 @@ void Renderer::renderImageOverlay( 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(locPrjModel, &model, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locPrjXi, &xiVal, RL_SHADER_UNIFORM_FLOAT, 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); @@ -262,6 +282,13 @@ void Renderer::draw3DCloud( 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) }; + // Camera RGB sampling always applies Mei's own distortion (unlike the + // Pinhole path, this displayed image is never rectified -- see + // AppState::rebuildImageTexture and kPointVS's Mei branch). + int model = (K.model == CameraModel::Mei) ? 2 : 0; + float xi = K.xi; + float rad1[3] = { K.k1, K.k2, K.k3 }; + float tan2[2] = { K.p1, K.p2 }; rlEnableShader(pointShader.id); rlSetUniformMatrix(locMVP, mvp); @@ -273,6 +300,10 @@ void Renderer::draw3DCloud( rlSetUniform(locOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniformMatrix(locCamXform, camXform); rlSetUniform(locCamK, k, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locCamModel, &model, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locCamXi, &xi, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(locCamRad1, rad1, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locCamTan, tan2, RL_SHADER_UNIFORM_VEC2, 1); rlSetUniform(locCamImgSize, imgSize, RL_SHADER_UNIFORM_VEC2, 1); if (colorMode == 3) @@ -297,6 +328,26 @@ void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, int i // Camera position in LiDAR frame is directly (E.tx, E.ty, E.tz) Vector3 origin = { E.tx, E.tz, -E.ty }; // LiDAR→raylib + if (K.model != CameraModel::Pinhole) + { + // A rectangular pyramid built from fx/fy/cx/cy/imgW/imgH (below) + // assumes a narrow rectilinear FOV, which misrepresents a Mei + // fisheye's much wider one (and Equirectangular's full sphere, were + // it ever wired into this app) -- draw a position marker + camera + // forward/right/up axis triad instead, same fallback + // camera_lidar_trajectory_viewer uses for CameraModel::Equirectangular. + auto toWorld = [&](const Eigen::Vector3f& axis_c) -> Vector3 + { + Eigen::Vector3f pl = R * (axis_c * scale * 0.5f) + Eigen::Vector3f(E.tx, E.ty, E.tz); + return { pl.x(), pl.z(), -pl.y() }; + }; + DrawSphereWires(origin, scale * 0.08f, 8, 8, YELLOW); + DrawLine3D(origin, toWorld(Eigen::Vector3f(0.f, 0.f, 1.f)), BLUE); // camera forward (Z) + DrawLine3D(origin, toWorld(Eigen::Vector3f(1.f, 0.f, 0.f)), RED); // camera right (X) + DrawLine3D(origin, toWorld(Eigen::Vector3f(0.f, -1.f, 0.f)), GREEN); // camera up (-Y: camera Y is down) + return; + } + // 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 }, diff --git a/apps/camera_lidar_calibration/Renderer.h b/apps/camera_lidar_calibration/Renderer.h index 36990ced..021da274 100644 --- a/apps/camera_lidar_calibration/Renderer.h +++ b/apps/camera_lidar_calibration/Renderer.h @@ -85,12 +85,15 @@ class Renderer int locMVP = -1, locColorMode = -1, locHeightRange = -1; int locMaxDist = -1, locOpacity = -1, locPointSize = -1, locDecim = -1; int locCamXform = -1, locCamK = -1, locCamImgSize = -1, locCamTex = -1; + // CameraModel::Mei only -- see kPointVS's Mei branch (RendererShaders.h) + int locCamModel = -1, locCamXi = -1, locCamRad1 = -1, locCamTan = -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 locPrjModel = -1, locPrjXi = -1; // CameraModel::Mei only int locPrjDepthRange = -1, locPrjOpacity = -1; int locPrjPointSize = -1, locPrjColorMode = -1, locPrjDecim = -1; }; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index 3f803aea..bd3e2df7 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -22,6 +22,10 @@ uniform int drawDecim; // draw only every Nth point; 1 = draw all uniform mat4 lidarToCam; // extrinsics (for RGB mode) uniform vec4 K; // fx, fy, cx, cy uniform vec2 imgSize; +uniform int model; // calib::CameraModel ordinal actually handled here: 0 = Pinhole, 2 = Mei +uniform float xi; // CameraModel::Mei only +uniform vec3 kRad1; // k1 k2 k3, CameraModel::Mei only +uniform vec2 pTan; // p1 p2, CameraModel::Mei only out vec3 fragPos; out float fragIntensity; out vec2 fragUV; @@ -37,12 +41,37 @@ void main() { 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; + + if (model == 2) { + // Mei -- unlike Pinhole (below), AppState::rebuildImageTexture never + // undistorts the displayed image for this model, so sampling it + // needs the actual Mei distortion applied here too. Mirrors + // MeiCamera::Project (CalibCore/MeiCamera.h) and kProjVS's own Mei + // branch below. + float n = length(pc); + vec3 Xs = pc / max(n, 1e-6); + float denom = Xs.z + xi; + // denom>0 is this model's actual "in front of the camera" test -- + // it reduces exactly to Pinhole's pc.z>0 when xi==0 (Xs.z and pc.z + // then share a sign, n>0). fragCamDepth only needs to carry that + // sign here (kPointFS only tests fragCamDepth > 0.0), not a real + // depth -- unlike kProjVS below, this shader has no depthRange + // slider to feed a physical distance to. + fragCamDepth = denom; + vec2 xy = Xs.xy / denom; + float r2 = dot(xy, xy); + float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; + vec2 d = xy*radial + vec2(2.0*pTan.x*xy.x*xy.y + pTan.y*(r2 + 2.0*xy.x*xy.x), + pTan.x*(r2 + 2.0*xy.y*xy.y) + 2.0*pTan.y*xy.x*xy.y); + fragUV = (K.xy * d + K.zw) / imgSize; + } else { + // Project into the camera image for RGB sampling (rectified → pinhole) + fragCamDepth = pc.z; + vec2 uv = (K.xy * (pc.xy / max(pc.z, 1e-6)) + K.zw) / imgSize; + fragUV = uv; + } } )"; @@ -82,9 +111,13 @@ 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). + // in raylib coords, converted back to lidar frame here. Pinhole (model==0): + // rational+tangential distortion (zeros when rectified), w = z_cam so the + // hardware clip rejects points behind the camera. Mei (model==2): unified- + // sphere + polynomial distortion (mirrors MeiCamera::Project), w = Xs.z+xi + // (the model's own "in front of the camera" test -- reduces exactly to + // Pinhole's z_cam when xi==0), so the hardware clip rejects points outside + // its valid dome the same way Pinhole rejects points behind it. inline constexpr const char* kProjVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; @@ -93,8 +126,10 @@ 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 vec3 kRad2; // k4 k5 k6, Pinhole (model==0) only -- Mei has no rational denominator uniform vec2 pTan; // p1 p2 +uniform int model; // calib::CameraModel ordinal actually handled here: 0 = Pinhole, 2 = Mei +uniform float xi; // CameraModel::Mei only uniform float pointSize; uniform int drawDecim; // draw only every Nth point; 1 = draw all out float fragDepth; @@ -108,23 +143,39 @@ 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 d; + float w; + if (model == 2) { + float n = length(pc); + fragDepth = n; // range -- physical distance, for depthRange/jet coloring + vec3 Xs = pc / max(n, 1e-6); + float denom = Xs.z + xi; + vec2 xy = Xs.xy / denom; + float r2 = dot(xy, xy); + float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; + d = xy*radial + vec2(2.0*pTan.x*xy.x*xy.y + pTan.y*(r2 + 2.0*xy.x*xy.x), + pTan.x*(r2 + 2.0*xy.y*xy.y) + 2.0*pTan.y*xy.x*xy.y); + w = denom; // NOT n -- see the block comment above kProjVS + } else { + fragDepth = pc.z; + 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); + 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); + w = pc.z; + } 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, + gl_Position = vec4((2.0*uv.x/imgSize.x - 1.0) * w, + -(2.0*uv.y/imgSize.y - 1.0) * w, 0.0, - pc.z); + w); gl_PointSize = pointSize; } )"; diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 4517e7e2..9b74967a 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -447,22 +447,56 @@ void UI::panelIntrinsics(AppState& state) }; ImGui::PushItemWidth(-80.f); + + // Equirectangular isn't wired into this app yet (see CameraModel's own + // comment in Camera.h) -- offering it here would silently mis-project, + // so the combo only offers the two models this app actually supports. + static const char* kModelNames[] = { "Pinhole", "Mei" }; + int modelIdx = (K.model == CameraModel::Mei) ? 1 : 0; + if (ImGui::Combo("Model", &modelIdx, kModelNames, IM_ARRAYSIZE(kModelNames))) + { + K.model = (modelIdx == 1) ? CameraModel::Mei : CameraModel::Pinhole; + edited = true; + } + ImGui::Separator(); + 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."); + + if (K.model == CameraModel::Mei) + { + // Unified-sphere fisheye (MeiCamera.h): xi + a plain k1/k2/k3 + + // p1/p2 polynomial, no rational denominator -- k4/k5/k6 don't apply + // here, so they're hidden instead of shown as dead controls. + drag("xi", &K.xi, 0.001f, 0.f, 3.f, "%.4f"); + ImGui::Text("Radial (Mei polynomial):"); + 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"); + 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.\nUnlike Pinhole, the displayed image is never undistorted for " + "Mei -- the projection overlay and Camera RGB coloring apply this distortion to the raw image directly."); + } + else + { + 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) diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 99397ebc..ddd999fb 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -6,8 +6,10 @@ project(calib_core) # (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 +# parsing. Deliberately depends on nothing but Eigen/LASzip/std plus, for the +# Mei camera model only (MeiCamera.h/.cpp), OpenCV's header-only Point2d/ +# Point3d and yaml-cpp for camera_info.yaml loading -- no raylib/imgui, and +# no other OpenCV usage, 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. @@ -18,14 +20,53 @@ project(calib_core) # calib_core and core/core_raylib in the same binary. add_library(calib_core STATIC src/Camera.cpp + src/MeiCamera.cpp src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp src/CameraCalibrationSolver.cpp + src/CameraCalibrationSolverMei.cpp ) +# yaml-cpp is a system package everywhere else in this repo already assumes +# is available for it (MeiCamera.cpp's #include predates +# this CMakeLists wiring) -- same "find it on the system" treatment as +# OpenCV gets in 3rdpartyBinary/OpenCV/CMakeLists.txt, just scoped to this +# target since calib_core is its only consumer. +find_package(yaml-cpp REQUIRED) + +# ── Optional Ceres-based Mei extrinsics solver ──────────────────────────────── +# OFF by default: HDMapping's own README advertises depending on nothing but +# Eigen for its optimization (no Ceres/g2o/GTSAM/manif/Sophus), unlike +# yaml-cpp/OpenCV above this is a real optional feature, not something every +# build needs -- CameraCalibrationSolverMei.cpp's #ifdef falls back to a stub +# that explains the gap instead of solving, so callers +# (apps/camera_lidar_calibration's AppState::solvePairs) don't need their own +# #ifdef, just to check solveExtrinsicsMeiCeres()'s return value. Enable with +# cmake -DCALIB_ENABLE_CERES=ON (needs libceres-dev or equivalent). +option(CALIB_ENABLE_CERES "Enable the Ceres-based extrinsics solver for the Mei camera model" OFF) +if(CALIB_ENABLE_CERES) + find_package(Ceres REQUIRED) + # PUBLIC (not PRIVATE, unlike Ceres::ceres's own link below): lets + # calib_core_tests -- calib_core's only other consumer that cares -- + # #ifdef on this to build the real-solve test only when it can actually + # run, without duplicating this option's value into a second place. + target_compile_definitions(calib_core PUBLIC CALIB_ENABLE_CERES) + message(STATUS "calib_core Mei Ceres solver: ENABLED") +else() + message(STATUS "calib_core Mei Ceres solver: disabled (set -DCALIB_ENABLE_CERES=ON to enable)") +endif() + target_include_directories(calib_core PUBLIC include + # MeiCamera.h (a public calib_core header) includes + # directly for cv::Point2d/Point3d, so anything that includes it needs + # OpenCV's headers on its include path too. OpenCV_INCLUDE_DIRS/ + # OpenCV_LIBS themselves come from find_package(OpenCV) in + # cmake/dependencies.cmake, included by the top-level CMakeLists.txt + # before add_subdirectory(calib_core) -- no find_package(OpenCV) needed + # here. + ${OpenCV_INCLUDE_DIRS} ) target_include_directories(calib_core PRIVATE @@ -47,12 +88,33 @@ target_include_directories(calib_core PRIVATE # affine_matrix_from_pose_tait_bryan (core/include/Core/transformations.h) # for the om/fi/ka<->matrix conversion instead of duplicating that math. # Header-only and pulls in nothing but Eigen/std (see structures.h) -- - # doesn't violate calib_core's no-raylib/imgui/OpenCV rule above, and - # nothing here links the core/core_math library, just includes headers. + # doesn't violate calib_core's no-raylib/imgui rule above, and nothing + # here links the core/core_math library, just includes headers. ${REPOSITORY_DIRECTORY}/core/include ) -target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) +target_link_libraries(calib_core PUBLIC + ${PLATFORM_LASZIP_LIB} + # For MeiCamera.cpp/.h -- see the target_include_directories comment + # above. PUBLIC (like PLATFORM_LASZIP_LIB) because calib_core is a + # STATIC library: CMake does not propagate a static library's own link + # dependencies to its consumers unless they're PUBLIC/INTERFACE, so a + # PRIVATE keyword here would leave executables linking calib_core with + # unresolved yaml-cpp symbols. + ${OpenCV_LIBS} + yaml-cpp::yaml-cpp +) + +if(CALIB_ENABLE_CERES) + # PRIVATE, unlike the PUBLIC block above -- CameraCalibrationSolver.h + # never exposes a Ceres type, so no consumer's own compilation needs + # Ceres' include dirs, just the symbols to link (CMake still records a + # STATIC library's PRIVATE link libraries as $ for + # whoever finally links an executable against calib_core, so this alone + # is enough for that final link to resolve solveExtrinsicsMeiCeres's + # ceres:: calls -- verified against this exact target during development). + target_link_libraries(calib_core PRIVATE Ceres::ceres) +endif() if(MSVC) target_compile_options(calib_core PRIVATE /W4) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index f90eda44..9b4e27c2 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -29,7 +29,8 @@ namespace calib enum class CameraModel { Pinhole, // fx/fy/cx/cy + the rational distortion coefficients below - Equirectangular // 360 panorama; width/height are the intrinsics, k*/p* unused + Equirectangular, // 360 panorama; width/height are the intrinsics, k*/p* unused + Mei // Insta 360 }; struct Intrinsics @@ -37,12 +38,19 @@ namespace calib CameraModel model = CameraModel::Pinhole; float fx = 800.f, fy = 800.f; float cx = 640.f, cy = 360.f; - // OpenCV rational distortion model: + // OpenCV rational distortion model (CameraModel::Pinhole): // radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + // + // CameraModel::Mei reuses k1/k2/k3 and p1/p2 below for its own + // (non-rational) radial/tangential polynomial -- see MeiCamera.h -- + // and leaves k4/k5/k6 at 0, unused. 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; + // Unified-sphere mirror parameter, CameraModel::Mei only (see + // MeiCamera.h for the model itself). Unused (0) by every other model. + float xi = 0.f; // Image dimensions in pixels. Read only by CameraModel::Equirectangular, // where they play the role fx/fy/cx/cy play for a pinhole camera and so // *must* be set -- from the calibration file or from the loaded image -- @@ -142,8 +150,11 @@ namespace calib // essentially at the camera itself fails -- a full-sphere camera has no // frustum and no "behind". u comes back wrapped into [0, width); v spans // [0, height] *inclusive*, the south pole landing exactly on height. + // Mei: depth = range from the camera, same "only the camera itself + // fails" rule as Equirectangular; (u, v) come straight out of + // MeiCamera::Project, unclamped and unwrapped. // - // In both cases the caller owns rounding to integer pixels (which can itself + // In all cases the caller owns rounding to integer pixels (which can itself // land on width at the equirectangular seam), bounds checking and any ROI // test. bool projectPoint( diff --git a/calib_core/include/CalibCore/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h index d476dad7..f18d39cc 100644 --- a/calib_core/include/CalibCore/CameraCalibrationSolver.h +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -1,6 +1,7 @@ #pragma once #include "Camera.h" #include +#include #include namespace calib @@ -8,9 +9,13 @@ namespace calib // A single manually-picked correspondence: a 3D point in the LiDAR/world // frame paired with the pixel it should project to in the camera image. - // Pixel coordinates are expected in the *undistorted* (ideal pinhole) - // frame -- i.e. picked from the rectified image display, see - // solveExtrinsicsFromCorrespondences() below. + // Pixel coordinates are expected in whatever frame the displayed image + // itself is in -- the undistorted/ideal-pinhole frame for + // CameraModel::Pinhole (picked from the rectified image display), or + // the raw (distorted) frame for CameraModel::Mei, whose image is never + // rectified (see AppState::rebuildImageTexture in + // apps/camera_lidar_calibration) -- matching whichever solver below is + // used for that model. struct PointPixelCorrespondence { Eigen::Vector3d p; @@ -27,6 +32,12 @@ namespace calib // i.e. picked from the rectified image display (calib::Intrinsics's // distortion terms are ignored here). // + // Pinhole only -- the reused observation equations are a pure + // rectilinear perspective projection with no distortion and no unified- + // sphere term, so this cannot be used for CameraModel::Mei (or + // CameraModel::Equirectangular, not wired into any app yet). See + // solveExtrinsicsMeiCeres() below for Mei. + // // fixTranslation=true blocks tx/ty/tz from being solved for -- they // stay pinned at extrinsicsInOut's initial values and only orientation // (3-DOF) is optimized. Useful when the camera position relative to the @@ -44,4 +55,32 @@ namespace calib double* outRmsPixels = nullptr, bool fixTranslation = false); + // CameraModel::Mei counterpart to solveExtrinsicsFromCorrespondences() + // above: no vendored analytic Jacobian exists for the unified-sphere + // model (unlike Pinhole's), so this minimizes reprojection error with + // Ceres' automatic differentiation instead of a hand-derived one, + // reusing K's fx/fy/cx/cy/xi/k1/k2/k3/p1/p2 fixed and solving the same + // (tx,ty,tz,om,fi,ka) Extrinsics this file's Pinhole solver does, with + // the same fixTranslation meaning. + // + // `errorMessage` is set on failure (degenerate input, Ceres failing to + // converge, or this build not having Ceres at all -- see below) and + // left untouched on success. It is a required, non-defaulted parameter + // -- hence its position ahead of the optional ones -- so that a failure + // reason is never silently dropped. + // + // Only available when calib_core is built with -DCALIB_ENABLE_CERES=ON + // (see calib_core/CMakeLists.txt) -- OFF by default, since HDMapping + // otherwise depends on nothing but Eigen for its own optimization (see + // the project README). Built without it, this always returns false and + // sets `errorMessage` to say so, rather than requiring callers to + // `#ifdef` around calling it at all. + bool solveExtrinsicsMeiCeres( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + std::string& errorMessage, + double* outRmsPixels = nullptr, + bool fixTranslation = false); + } // namespace calib diff --git a/calib_core/include/CalibCore/MeiCamera.h b/calib_core/include/CalibCore/MeiCamera.h new file mode 100644 index 00000000..777cdeb9 --- /dev/null +++ b/calib_core/include/CalibCore/MeiCamera.h @@ -0,0 +1,51 @@ +#pragma once + +#include + +#include + +// Camera intrinsics for the Mei/unified-sphere fisheye model used by the +// Insta360 rig this data comes from (distortion_model: insta360_mei_v2 in +// camera_info.yaml). Field meanings and the forward projection formula below +// are taken from /home/michal/code/insta3360-to-images +// (include/insta360/calibration.hpp, src/equirect.cpp), which cross-checked +// them against an independent reverse-engineering effort and validated them +// visually against Insta360's own equirect export — not re-derived from +// scratch here. +// +// IMPORTANT: camera_info.yaml's `distortion` array is ordered +// (k1, k2, k3, p1, p2) — NOT OpenCV's usual pinhole order (k1, k2, p1, p2, +// k3). The two conventions are trivially easy to mix up (both are just five +// numbers in a row) and doing so produces a plausible-looking but badly wrong +// reprojection with no crash — see calibration.hpp's own comment on this. +struct MeiCamera { + std::string frameId; + std::string distortionModel; + int width = 0, height = 0; + + double fx = 0, fy = 0, cx = 0, cy = 0; + double xi = 0; + double k1 = 0, k2 = 0, k3 = 0, p1 = 0, p2 = 0; + + bool loaded = false; + + // Forward: camera-frame 3D point (any positive scale, need not be unit + // length) -> pixel coordinates. + cv::Point2d Project(cv::Point3d P) const; + + // Inverse: pixel -> unit-length ray direction in camera frame. Closed + // form for the unit-sphere/xi step, iterative (Newton) for the + // radial/tangential part, matching equirect.cpp's forward formula. + // If thetaDeg is given, receives the angle from the optical axis (0 = + // dead center, useful as a "how far into the fisheye edge is this point" + // sanity check). + cv::Point3d Unproject(cv::Point2d uv, double* thetaDeg = nullptr) const; +}; + +// Loads intrinsics from a camera_info.yaml written in this rig's format (see +// data/camera_info.yaml for a sample). On any failure (missing file, missing +// field, distortion array with an unexpected element count) prints a message +// to stderr and returns a default-constructed MeiCamera with loaded=false — +// a missing/malformed file should degrade the app to "no reprojection +// available", not crash it. +MeiCamera LoadMeiCamera(const std::string& path); diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index abe3b040..f3aa73d8 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,5 +1,7 @@ #include +#include + #include // Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- @@ -71,6 +73,37 @@ bool projectPoint(float px, float py, float pz, return true; } + if (K.model == CameraModel::Mei) { + // Delegates to the tested/certified MeiCamera::Project (MeiCamera.h) + // instead of re-deriving the unified-sphere + radial/tangential + // formula here -- only the R_wc/t transform into camera frame, the + // "point sits on the camera itself" guard (same idiom as + // Equirectangular above), and the "in front of the camera" guard + // just below belong to this wrapper. + depth = pc.norm(); + if (depth < 1e-4f) return false; + + // Xs.z + xi > 0 is this model's own validity domain (MeiCamera:: + // Project divides by exactly this with no guard of its own) -- it + // reduces exactly to the familiar Pinhole "pc.z > 0" test when + // xi == 0 (Xs.z and pc.z then share a sign, depth > 0). Without + // this, a point behind the camera can still land inside the image + // bounds (the projection isn't injective outside its valid domain) + // and get silently treated as visible. + if (pc.z() / depth + K.xi <= 0.f) return false; + + MeiCamera cam; + cam.fx = K.fx; cam.fy = K.fy; cam.cx = K.cx; cam.cy = K.cy; + cam.xi = K.xi; + cam.k1 = K.k1; cam.k2 = K.k2; cam.k3 = K.k3; + cam.p1 = K.p1; cam.p2 = K.p2; + + const cv::Point2d px = cam.Project(cv::Point3d(pc.x(), pc.y(), pc.z())); + u = static_cast(px.x); + v = static_cast(px.y); + return true; + } + depth = pc.z(); if (depth <= 1e-4f) return false; diff --git a/calib_core/src/CameraCalibrationSolverMei.cpp b/calib_core/src/CameraCalibrationSolverMei.cpp new file mode 100644 index 00000000..be7a41d2 --- /dev/null +++ b/calib_core/src/CameraCalibrationSolverMei.cpp @@ -0,0 +1,204 @@ +#include + +// Always compiled (see calib_core/CMakeLists.txt); the #ifdef below picks +// between a real Ceres-based implementation and a stub that just explains +// why it isn't available, so callers (apps/camera_lidar_calibration's +// AppState::solvePairs) never need an #ifdef of their own around calling +// solveExtrinsicsMeiCeres -- only its return value. +#ifdef CALIB_ENABLE_CERES + +#include + +#include + +namespace calib +{ + namespace + { + // Templated (Ceres::Jet-compatible) equivalent of Camera.cpp's + // omFiKaToMat3: R = kCameraLidarAxisOffset * Rx(om)*Ry(fi)*Rz(ka), + // om/fi/ka in RADIANS (Extrinsics' own fields are degrees -- solve() + // below converts at the boundary). The Rx*Ry*Rz part mirrors + // Core/transformations.h's affine_matrix_from_pose_tait_bryan + // row-for-row rather than re-deriving it; kCameraLidarAxisOffset + // (Camera.h) is applied by permuting/negating Rdelta's rows + // directly instead of a general 3x3*3x3 product, since its own + // entries are just {0, +-1}: offset = [[0,0,1],[-1,0,0],[0,-1,0]], + // so row 0 of R is row 2 of Rdelta, row 1 is -(row 0), row 2 is + // -(row 1). + template + void rotationMatrix(const T& om, const T& fi, const T& ka, T R[3][3]) + { + const T sx = sin(om), cx = cos(om); + const T sy = sin(fi), cy = cos(fi); + const T sz = sin(ka), cz = cos(ka); + + T Rdelta[3][3]; + Rdelta[0][0] = cy * cz; + Rdelta[1][0] = cz * sx * sy + cx * sz; + Rdelta[2][0] = -cx * cz * sy + sx * sz; + Rdelta[0][1] = -cy * sz; + Rdelta[1][1] = cx * cz - sx * sy * sz; + Rdelta[2][1] = cz * sx + cx * sy * sz; + Rdelta[0][2] = sy; + Rdelta[1][2] = -cy * sx; + Rdelta[2][2] = cx * cy; + + for (int c = 0; c < 3; ++c) + { + R[0][c] = Rdelta[2][c]; + R[1][c] = -Rdelta[0][c]; + R[2][c] = -Rdelta[1][c]; + } + } + + // Templated equivalent of MeiCamera::Project (CalibCore/ + // MeiCamera.h) for Ceres autodiff -- same formula, not re-derived; + // intrinsics are plain doubles (fixed, not solved for), only pc is + // the Jet-typed autodiff variable. + template + void projectMei( + const T pc[3], + double fx, + double fy, + double cx, + double cy, + double xi, + double k1, + double k2, + double k3, + double p1, + double p2, + T& u, + T& v) + { + const T n = sqrt(pc[0] * pc[0] + pc[1] * pc[1] + pc[2] * pc[2]); + const T Xx = pc[0] / n, Xy = pc[1] / n, Xz = pc[2] / n; + const T denom = Xz + T(xi); + const T x = Xx / denom, y = Xy / denom; + const T r2 = x * x + y * y; + const T radial = T(1.0) + T(k1) * r2 + T(k2) * r2 * r2 + T(k3) * r2 * r2 * r2; + const T xd = x * radial + T(2.0 * p1) * x * y + T(p2) * (r2 + T(2.0) * x * x); + const T yd = y * radial + T(p1) * (r2 + T(2.0) * y * y) + T(2.0 * p2) * x * y; + u = T(fx) * xd + T(cx); + v = T(fy) * yd + T(cy); + } + + // Reprojection residual for one correspondence: predicted (u, v) + // minus the picked pixel, exactly like the Pinhole solver's reused + // observation_equation_perspective_camera_tait_bryan_wc, just + // autodiff'd instead of symbolically pre-differentiated (no + // vendored Mei Jacobian exists to reuse -- see CameraCalibrationSolver.h). + struct MeiReprojectionResidual + { + MeiReprojectionResidual(const Eigen::Vector3d& p, double u_kp, double v_kp, const Intrinsics& K) + : p_(p), u_kp_(u_kp), v_kp_(v_kp), K_(K) + { + } + + template + bool operator()(const T* const tx_ty_tz, const T* const om_fi_ka, T* residual) const + { + T R[3][3]; + rotationMatrix(om_fi_ka[0], om_fi_ka[1], om_fi_ka[2], R); + + const T d[3] = { T(p_.x()) - tx_ty_tz[0], T(p_.y()) - tx_ty_tz[1], T(p_.z()) - tx_ty_tz[2] }; + // p_cam = R_wc^T * (p_world - C) + const T pc[3] = { + R[0][0] * d[0] + R[1][0] * d[1] + R[2][0] * d[2], + R[0][1] * d[0] + R[1][1] * d[1] + R[2][1] * d[2], + R[0][2] * d[0] + R[1][2] * d[1] + R[2][2] * d[2], + }; + + T u, v; + projectMei(pc, K_.fx, K_.fy, K_.cx, K_.cy, K_.xi, K_.k1, K_.k2, K_.k3, K_.p1, K_.p2, u, v); + residual[0] = u - T(u_kp_); + residual[1] = v - T(v_kp_); + return true; + } + + const Eigen::Vector3d p_; + const double u_kp_, v_kp_; + const Intrinsics K_; + }; + } // namespace + + bool solveExtrinsicsMeiCeres( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + std::string& errorMessage, + double* outRmsPixels, + bool fixTranslation) + { + const int nParams = fixTranslation ? 3 : 6; + if (static_cast(correspondences.size()) < 3 || static_cast(correspondences.size()) * 2 < nParams) + { + errorMessage = "Need at least 3 correspondences"; + return false; + } + + const double d2r = M_PI / 180.0; + double txyz[3] = { extrinsicsInOut.tx, extrinsicsInOut.ty, extrinsicsInOut.tz }; + double omfika[3] = { extrinsicsInOut.om * d2r, extrinsicsInOut.fi * d2r, extrinsicsInOut.ka * d2r }; + + ceres::Problem problem; + for (const auto& c : correspondences) + { + auto* cost = + new ceres::AutoDiffCostFunction(new MeiReprojectionResidual(c.p, c.u, c.v, K)); + problem.AddResidualBlock(cost, nullptr, txyz, omfika); + } + if (fixTranslation) + problem.SetParameterBlockConstant(txyz); + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_QR; + options.max_num_iterations = 100; + options.logging_type = ceres::SILENT; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + if (!summary.IsSolutionUsable()) + { + errorMessage = "Ceres solve failed: " + summary.BriefReport(); + return false; + } + + extrinsicsInOut.tx = static_cast(txyz[0]); + extrinsicsInOut.ty = static_cast(txyz[1]); + extrinsicsInOut.tz = static_cast(txyz[2]); + extrinsicsInOut.om = static_cast(omfika[0] / d2r); + extrinsicsInOut.fi = static_cast(omfika[1] / d2r); + extrinsicsInOut.ka = static_cast(omfika[2] / d2r); + + // Ceres' final_cost is 0.5*sum(residual_i^2) over every SCALAR + // residual (2 per correspondence: du, dv), so sum(du^2+dv^2) = + // 2*final_cost -- matching the Pinhole solver's own rms formula + // (sqrt(sum(du^2+dv^2) / (2*N))) then simplifies to sqrt(final_cost/N). + if (outRmsPixels) + *outRmsPixels = std::sqrt(summary.final_cost / static_cast(correspondences.size())); + + return true; + } +} // namespace calib + +#else // !CALIB_ENABLE_CERES + +namespace calib +{ + bool solveExtrinsicsMeiCeres( + const std::vector&, + const Intrinsics&, + Extrinsics&, + std::string& errorMessage, + double*, + bool) + { + errorMessage = "Mei extrinsics solving needs calib_core built with -DCALIB_ENABLE_CERES=ON (see calib_core/CMakeLists.txt)"; + return false; + } +} // namespace calib + +#endif \ No newline at end of file diff --git a/calib_core/src/MeiCamera.cpp b/calib_core/src/MeiCamera.cpp new file mode 100644 index 00000000..1e93febb --- /dev/null +++ b/calib_core/src/MeiCamera.cpp @@ -0,0 +1,100 @@ +#include + +#include + +#include +#include +#include + +cv::Point2d MeiCamera::Project(cv::Point3d P) const { + const double n = cv::norm(P); + const cv::Point3d Xs(P.x / n, P.y / n, P.z / n); // onto the unit sphere + + const double denom = Xs.z + xi; + const double x = Xs.x / denom, y = Xs.y / denom; + const double r2 = x * x + y * y; + const double radial = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const double xd = x * radial + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + const double yd = y * radial + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; + return {fx * xd + cx, fy * yd + cy}; +} + +cv::Point3d MeiCamera::Unproject(cv::Point2d uv, double* thetaDeg) const { + const double xd = (uv.x - cx) / fx, yd = (uv.y - cy) / fy; + + // Invert the radial/tangential distortion by fixed-point (Newton-style) + // iteration, same scheme cv::undistortPoints uses for the pinhole model. + double x = xd, y = yd; + for (int it = 0; it < 30; ++it) { + const double r2 = x * x + y * y; + const double radial = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const double dx = 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + const double dy = p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; + x = (xd - dx) / radial; + y = (yd - dy) / radial; + } + + // Closed-form inverse of the unit-sphere/xi projection: solve + // (rho2+1)*s^2 - 2*xi*s + (xi^2-1) = 0 for s = Xs.z + xi, where + // Xs.x = x*s, Xs.y = y*s, Xs.z = s - xi, subject to |Xs| = 1. + const double rho2 = x * x + y * y; + const double s = (xi + std::sqrt(std::max(0.0, 1.0 + rho2 * (1.0 - xi * xi)))) / (rho2 + 1.0); + const cv::Point3d Xs(x * s, y * s, s - xi); + + if (thetaDeg) *thetaDeg = std::acos(std::clamp(Xs.z, -1.0, 1.0)) * 180.0 / CV_PI; + return Xs; +} + +MeiCamera LoadMeiCamera(const std::string& path) { + MeiCamera cam; + YAML::Node node; + try { + node = YAML::LoadFile(path); + } catch (const std::exception& e) { + std::fprintf(stderr, "calib_app: failed to load '%s': %s\n", path.c_str(), e.what()); + return cam; + } + + try { + cam.frameId = node["frame_id"] ? node["frame_id"].as() : ""; + cam.distortionModel = node["distortion_model"] ? node["distortion_model"].as() : ""; + cam.width = node["width"].as(); + cam.height = node["height"].as(); + cam.fx = node["fx"].as(); + cam.fy = node["fy"].as(); + cam.cx = node["cx"].as(); + cam.cy = node["cy"].as(); + cam.xi = node["xi"].as(); + + // distortion is (k1, k2, k3, p1, p2) for insta360_mei_v2 — see + // MeiCamera.h. Read defensively: warn (don't silently drop data) if + // the array isn't exactly the 5 elements this order assumes. + YAML::Node d = node["distortion"]; + const size_t n = d.size(); + if (n != 5) { + std::fprintf(stderr, + "calib_app: WARNING '%s' distortion has %zu elements, expected 5 " + "(k1,k2,k3,p1,p2 for %s) — missing ones default to 0, extras are ignored\n", + path.c_str(), n, cam.distortionModel.c_str()); + } + auto at = [&](size_t i) { return i < n ? d[i].as() : 0.0; }; + cam.k1 = at(0); cam.k2 = at(1); cam.k3 = at(2); cam.p1 = at(3); cam.p2 = at(4); + } catch (const std::exception& e) { + std::fprintf(stderr, "calib_app: '%s' is missing an expected field: %s\n", path.c_str(), e.what()); + return cam; + } + + if (cam.distortionModel != "insta360_mei_v2") { + std::fprintf(stderr, + "calib_app: WARNING '%s' has distortion_model='%s', this app only knows how to " + "reproject insta360_mei_v2 (results will be wrong if the model differs)\n", + path.c_str(), cam.distortionModel.c_str()); + } + + cam.loaded = true; + std::printf("calib_app: loaded intrinsics from %s (frame '%s', %dx%d, fx=%.3f fy=%.3f cx=%.3f cy=%.3f " + "xi=%.4f k1=%.6g k2=%.6g k3=%.6g p1=%.6g p2=%.6g)\n", + path.c_str(), cam.frameId.c_str(), cam.width, cam.height, cam.fx, cam.fy, cam.cx, cam.cy, + cam.xi, cam.k1, cam.k2, cam.k3, cam.p1, cam.p2); + return cam; +} diff --git a/calib_core/tests/CMakeLists.txt b/calib_core/tests/CMakeLists.txt index e385345f..9ca32ebe 100644 --- a/calib_core/tests/CMakeLists.txt +++ b/calib_core/tests/CMakeLists.txt @@ -2,14 +2,20 @@ cmake_minimum_required(VERSION 4.0.0) project(calib_core_tests) -# Unit tests for calib_core's camera model. calib_core deliberately depends on -# nothing but Eigen/LASzip/std (see calib_core/CMakeLists.txt), so its -# projection math is directly testable without a GL context -- which is the -# whole reason the equirectangular model lives there rather than in an app. +# Unit tests for calib_core's camera model and solvers. calib_core +# deliberately depends on nothing but Eigen/LASzip/std, plus OpenCV/yaml-cpp +# for the Mei model and (optionally, see CALIB_ENABLE_CERES) Ceres for its +# extrinsics solver (see calib_core/CMakeLists.txt) -- no raylib/imgui/GL -- +# so its projection math is directly testable without a GL context, which is +# the whole reason the equirectangular and Mei models live there rather than +# in an app. # Uses doctest (3rdparty/doctest/doctest.h) for the same reason shared/tests -# does; see that directory's CMakeLists.txt. +# does; see that directory's CMakeLists.txt. test_camera.cpp owns doctest's +# main() (DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN); test_solver.cpp just adds more +# TEST_CASEs to the same registry, same multi-TU doctest setup as shared/tests. add_executable(calib_core_tests test_camera.cpp + test_solver.cpp ) target_link_libraries(calib_core_tests PRIVATE calib_core) diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 179a7d3a..3708ffc2 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -2,6 +2,7 @@ #include #include +#include #include @@ -21,6 +22,36 @@ namespace return K; } + // A representative Mei/unified-sphere fisheye, values in the shape + // insta360_mei_v2 calibrations take (see MeiCamera.h) rather than a + // real calibrated camera. + Intrinsics mei() + { + Intrinsics K; + K.model = CameraModel::Mei; + K.fx = 300.f; K.fy = 300.f; + K.cx = 320.f; K.cy = 240.f; + K.xi = 1.2f; + K.k1 = -0.15f; K.k2 = 0.02f; K.k3 = -0.001f; + K.p1 = 0.001f; K.p2 = -0.0005f; + K.width = 640; K.height = 480; + return K; + } + + // The same camera as mei(), built directly as a MeiCamera -- used to + // check projectPoint()'s Mei branch against the type it wraps, not + // against a re-derivation of the formula. + MeiCamera meiCamera() + { + const Intrinsics K = mei(); + MeiCamera cam; + cam.fx = K.fx; cam.fy = K.fy; cam.cx = K.cx; cam.cy = K.cy; + cam.xi = K.xi; + cam.k1 = K.k1; cam.k2 = K.k2; cam.k3 = K.k3; + cam.p1 = K.p1; cam.p2 = K.p2; + return cam; + } + // Identity pose: p_cam == p_lidar, so test points can be written directly // in camera axes (X = right, Y = down, Z = forward). const Eigen::Matrix3f kIdentity = Eigen::Matrix3f::Identity(); @@ -190,6 +221,104 @@ TEST_CASE("equirectangular: respects the extrinsics") } } +// ── Mei ───────────────────────────────────────────────────────────────────── + +TEST_CASE("mei: forward is the image centre, depth is range") +{ + const Intrinsics K = mei(); + + Px r = project(K, { 0, 0, 10 }); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + CHECK(r.depth == doctest::Approx(10.0)); // range, not z -- see below + + Px oblique = project(K, { 3, 0, 4 }); + CHECK(oblique.depth == doctest::Approx(5.0)); // a pinhole camera would report 4 +} + +TEST_CASE("mei: projectPoint wraps MeiCamera::Project rather than re-deriving it") +{ + const Intrinsics K = mei(); + const MeiCamera cam = meiCamera(); + + const Eigen::Vector3f points[] = { + { 0.3f, -0.2f, 0.9f }, + { -1.5f, 0.8f, 2.0f }, + { 0.05f, 0.02f, 1.0f }, + { -0.6f, -1.1f, 0.8f }, + }; + + for (const auto& p : points) + { + Px r = project(K, p); + const cv::Point2d expected = cam.Project(cv::Point3d(p.x(), p.y(), p.z())); + CHECK(r.u == doctest::Approx(expected.x)); + CHECK(r.v == doctest::Approx(expected.y)); + } +} + +TEST_CASE("mei: a point on the camera itself is rejected") +{ + const Intrinsics K = mei(); + float u, v, depth; + CHECK_FALSE(projectPoint(0, 0, 0, K, kIdentity, kOrigin, u, v, depth)); +} + +TEST_CASE("mei: a point behind the camera is rejected, not silently mis-projected") +{ + // Regression: MeiCamera::Project has no domain guard of its own (it + // divides by Xs.z+xi unconditionally), so a point behind the camera + // does NOT reliably land outside the image -- the projection isn't + // injective past the model's valid dome. projectPoint() must reject it + // itself rather than return a plausible-looking wrong pixel. + float u, v, depth; + + SUBCASE("xi >= 1 covers the full sphere -- straight behind still succeeds") + { + // mei()'s xi = 1.2: Xs.z + xi ranges over [xi-1, xi+1] = [0.2, 2.2] + // for any direction (Xs.z in [-1, 1]), always positive, so no + // direction is ever excluded at this xi. + const Intrinsics K = mei(); + CHECK(projectPoint(0, 0, -10, K, kIdentity, kOrigin, u, v, depth)); + } + + SUBCASE("xi < 1 excludes a cone behind the camera") + { + Intrinsics K = mei(); + K.xi = 0.5f; // Xs.z <= -0.5 is now out of domain + + // Straight behind: Xs.z = -1, so Xs.z + xi = -0.5 <= 0. + CHECK_FALSE(projectPoint(0, 0, -10, K, kIdentity, kOrigin, u, v, depth)); + // Straight ahead is unaffected. + CHECK(projectPoint(0, 0, 10, K, kIdentity, kOrigin, u, v, depth)); + } +} + +TEST_CASE("mei: respects the extrinsics") +{ + const Intrinsics K = mei(); + const MeiCamera cam = meiCamera(); + + // om=fi=ka=0 is the nominal camera-vs-LiDAR alignment, so LiDAR forward + // (+X) should come out as camera forward, i.e. the image centre. + const Eigen::Matrix3f R_wc = kCameraLidarAxisOffset; + + Px r = project(K, { 10, 0, 0 }, R_wc); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + + // The camera position is subtracted: one metre in front of an offset + // camera reprojects the same as one metre in front of the origin. + const Eigen::Vector3f C(1.f, 2.f, 3.f); + Px offset = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); + // p_lidar - C = LiDAR +X, which R_wc's transpose turns into camera +Z + // (camera-forward) -- same axis remap as the centre check above. + const cv::Point2d expected = cam.Project(cv::Point3d(0.0, 0.0, 1.0)); + CHECK(offset.u == doctest::Approx(expected.x)); + CHECK(offset.v == doctest::Approx(expected.y)); + CHECK(offset.depth == doctest::Approx(1.0)); +} + // ── Pinhole (regression: this path must not change) ─────────────────────────── TEST_CASE("pinhole is the default model") @@ -280,4 +409,21 @@ TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") CHECK(H.k1 == doctest::Approx(0.1)); CHECK(H.p2 == doctest::Approx(0.02)); } + SUBCASE("mei") + { + Intrinsics K = mei(); + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Mei); + CHECK(H.fx == doctest::Approx(K.fx * 0.5)); + CHECK(H.cx == doctest::Approx(K.cx * 0.5)); + // xi and the k*/p* polynomial are dimensionless, carried over as-is. + CHECK(H.xi == doctest::Approx(K.xi)); + CHECK(H.k1 == doctest::Approx(K.k1)); + CHECK(H.p2 == doctest::Approx(K.p2)); + + Px full = project(K, { 0.3f, -0.2f, 0.9f }); + Px half = project(H, { 0.3f, -0.2f, 0.9f }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } } \ No newline at end of file diff --git a/calib_core/tests/test_solver.cpp b/calib_core/tests/test_solver.cpp new file mode 100644 index 00000000..f2d09fce --- /dev/null +++ b/calib_core/tests/test_solver.cpp @@ -0,0 +1,136 @@ +// Solver tests, split out of test_camera.cpp (which owns doctest's +// DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN / main()) -- this file just registers +// more TEST_CASEs into the same executable/registry, same multi-TU doctest +// setup shared/tests uses. +#include + +#include + +#include + +using namespace calib; + +namespace +{ + Intrinsics meiIntrinsics() + { + Intrinsics K; + K.model = CameraModel::Mei; + K.fx = 300.f; K.fy = 300.f; + K.cx = 320.f; K.cy = 240.f; + K.xi = 1.2f; + K.k1 = -0.15f; K.k2 = 0.02f; K.k3 = -0.001f; + K.p1 = 0.001f; K.p2 = -0.0005f; + return K; + } +} // namespace + +#ifndef CALIB_ENABLE_CERES + +TEST_CASE("solveExtrinsicsMeiCeres: stub explains the build flag when Ceres is disabled") +{ + std::vector corr(3); // content doesn't matter -- fails before using it + Extrinsics E; + std::string err; + CHECK_FALSE(solveExtrinsicsMeiCeres(corr, meiIntrinsics(), E, err)); + CHECK(err.find("CALIB_ENABLE_CERES") != std::string::npos); +} + +#else // CALIB_ENABLE_CERES + +namespace +{ + // Ground truth this test solves for, expressed the same way + // AppState::saveCalibration/loadCalibration do: camera position + a + // small om/fi/ka deviation from kCameraLidarAxisOffset. + Extrinsics groundTruthExtrinsics() + { + Extrinsics E; + E.tx = 1.5f; E.ty = -0.3f; E.tz = 0.8f; + E.om = 4.f; E.fi = -6.f; E.ka = 2.f; + return E; + } + + // A handful of LiDAR-frame points spread across the field of view, + // roughly in front of groundTruthExtrinsics()'s camera. + const Eigen::Vector3f kLidarPoints[] = { + { 3.f, 0.f, 0.f }, { 4.f, 1.5f, 0.5f }, { 5.f, -1.f, -0.5f }, { 3.5f, 0.8f, -0.8f }, + { 6.f, -1.8f, 1.f }, { 4.5f, 0.3f, 1.2f }, { 3.f, -0.6f, 0.4f }, + }; +} // namespace + +TEST_CASE("solveExtrinsicsMeiCeres: recovers known extrinsics from synthetic correspondences") +{ + const Intrinsics K = meiIntrinsics(); + const Extrinsics truth = groundTruthExtrinsics(); + const Eigen::Matrix3f R_wc = omFiKaToMat3(truth.om, truth.fi, truth.ka); + const Eigen::Vector3f C(truth.tx, truth.ty, truth.tz); + + std::vector corr; + for (const auto& p : kLidarPoints) + { + float u, v, depth; + REQUIRE(projectPoint(p.x(), p.y(), p.z(), K, R_wc, C, u, v, depth)); + PointPixelCorrespondence c; + c.p = p.cast(); + c.u = u; + c.v = v; + corr.push_back(c); + } + + // Perturbed initial guess -- a solver that just echoed its input back + // unchanged (e.g. Ceres silently failing to run and IsSolutionUsable() + // being CHECK_FALSE'd elsewhere) would not pass this. + Extrinsics guess = truth; + guess.tx += 0.3f; guess.ty -= 0.2f; guess.tz += 0.15f; + guess.om += 2.f; guess.fi -= 1.5f; guess.ka += 1.f; + + double rms = -1.0; + std::string err; + REQUIRE(solveExtrinsicsMeiCeres(corr, K, guess, err, &rms, false)); + + CHECK(rms < 0.5); // px -- points are noise-free, should fit almost exactly + CHECK(guess.tx == doctest::Approx(truth.tx).epsilon(1e-3)); + CHECK(guess.ty == doctest::Approx(truth.ty).epsilon(1e-3)); + CHECK(guess.tz == doctest::Approx(truth.tz).epsilon(1e-3)); + CHECK(guess.om == doctest::Approx(truth.om).epsilon(1e-2)); + CHECK(guess.fi == doctest::Approx(truth.fi).epsilon(1e-2)); + CHECK(guess.ka == doctest::Approx(truth.ka).epsilon(1e-2)); +} + +TEST_CASE("solveExtrinsicsMeiCeres: fixTranslation leaves tx/ty/tz untouched") +{ + const Intrinsics K = meiIntrinsics(); + const Extrinsics truth = groundTruthExtrinsics(); + const Eigen::Matrix3f R_wc = omFiKaToMat3(truth.om, truth.fi, truth.ka); + const Eigen::Vector3f C(truth.tx, truth.ty, truth.tz); + + std::vector corr; + for (const auto& p : kLidarPoints) + { + float u, v, depth; + REQUIRE(projectPoint(p.x(), p.y(), p.z(), K, R_wc, C, u, v, depth)); + PointPixelCorrespondence c; + c.p = p.cast(); + c.u = u; + c.v = v; + corr.push_back(c); + } + + Extrinsics guess = truth; + guess.om += 3.f; guess.fi -= 2.f; guess.ka += 1.5f; + const float lockedTx = guess.tx, lockedTy = guess.ty, lockedTz = guess.tz; + + double rms = -1.0; + std::string err; + REQUIRE(solveExtrinsicsMeiCeres(corr, K, guess, err, &rms, /*fixTranslation=*/true)); + + CHECK(guess.tx == doctest::Approx(lockedTx)); + CHECK(guess.ty == doctest::Approx(lockedTy)); + CHECK(guess.tz == doctest::Approx(lockedTz)); + CHECK(guess.om == doctest::Approx(truth.om).epsilon(1e-2)); + CHECK(guess.fi == doctest::Approx(truth.fi).epsilon(1e-2)); + CHECK(guess.ka == doctest::Approx(truth.ka).epsilon(1e-2)); +} + +#endif // CALIB_ENABLE_CERES \ No newline at end of file From 01cb28b8f21a4a5bd12e200bc13b96cdb4a14c06 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Sat, 12 Sep 2026 05:39:11 +0200 Subject: [PATCH 04/22] Draw intensity (cherry picked from commit f78fe804 on fork/mp/work) Adapted during the cherry-pick: the source branch carries the fast-rotation image filter (#526), which this branch does not, and f78fe804 touches the same lines. The filter's machinery -- poseAngSpeedDeg, poseAngSpeedMax, filterFastImages, maxImageAngSpeedDeg, angFilteredImgs and the angularSpeedDegAt()/computePoseAngularSpeedDeg() helpers -- was dropped rather than carried along: the calls came across in the conflicted hunks but their definitions did not, so keeping them would not have compiled. Bringing the filter here is a matter for cherry-picking #526 on its own. What the commit actually contributes is kept whole: the intensity drawing, the Camera.cpp fold-back guard ported from #527, and the imageTimeOffsetMs camera/LiDAR clock offset with its imageOffsetNs() plumbing. Note that imageTimeOffsetMs has no writer -- no widget, CLI flag or calibration-file key sets it, here or on the source branch -- so it stays 0 and the offset plumbing is inert until something wires it up. Co-Authored-By: Claude Opus 5 (1M context) --- .../TrajectoryViewer.cpp | 249 +++++++++++++++++- apps/manual_color/manual_color.cpp | 111 ++++++++ calib_core/src/Camera.cpp | 56 ++++ 3 files changed, 403 insertions(+), 13 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 526cb43d..24af27e9 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -290,6 +290,14 @@ struct AppState float maxImageAngSpeedDeg = 60.f; // deg/s threshold int angFilteredImgs = 0; // images skipped by the filter in the last colorize pass + + // Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed + // trigger/USB latency the camera's own timestamps don't account for). + // Applied wherever an image timestamp is matched against the LiDAR/pose + // timeline (loadCloud's chunk selection + point matching, exportColmap's + // per-image pose lookup) -- never to the raw timestamps used for + // filename lookup or image-list indexing (s.imageTsNs/imagesFilenamesInTime). + float imageTimeOffsetMs = 0.f; 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 @@ -347,8 +355,29 @@ struct AppState cv::Mat imgViewPending; bool imgViewHasNew = false; std::thread imgViewThread; + + // ── synthetic intensity-projection image (drawn next to the photo) ───── + // Reprojects the (already colorized) exportCloud through the same + // calibration/projectPoint() as loadCloud()'s colorize pass, painted with + // a jet colormap over each point's normalized intensity -- a reference + // image to visually check the calibration/coloring against the photo. + bool showIntensityProjection = false; + bool intensityProjNeedsUpdate = false; // set on toggle/refresh/image change + Texture2D intensityProjTex = {}; + bool intensityProjTexValid = false; + int intensityProjDecim = 1; // use every Nth point of exportCloud (perf) + float intensityProjPointRadius = 1.5f; // splat radius, in output-image pixels + bool intensityProjOverlay = false; // true: alpha-blend on top of the photo instead of side-by-side + float intensityProjAlpha = 0.6f; // blend strength when intensityProjOverlay is on }; +// s.imageTimeOffsetMs, in nanoseconds -- added to a raw image timestamp +// before comparing it against the LiDAR/pose timeline. +static int64_t imageOffsetNs(const AppState& s) +{ + return static_cast(std::llround(static_cast(s.imageTimeOffsetMs) * 1e6)); +} + // ── helpers ─────────────────────────────────────────────────────────────────── // Plain Eigen::Vector3f -> raylib Vector3 conversion. Used to be an axis // remap (x, z, -y) that made this app's native Z-up LiDAR data render @@ -702,6 +731,7 @@ static void loadCloud(AppState& s) // time(s) -> T_world_lidar, for interpolating the pose at each image time. std::map trajMap = buildTrajMap(s.traj); + const int64_t offNs = imageOffsetNs(s); std::vector gpuData; float mx = 0.f; @@ -750,12 +780,15 @@ static void loadCloud(AppState& s) { 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); + // new: every image whose timestamp falls inside the chunk range. + // Search bounds are shifted by -offNs since s.imageTsNs holds raw + // (unshifted) camera timestamps: imgTs+offNs in [chunkFirst, + // chunkLast] <=> imgTs in [chunkFirst-offNs, chunkLast-offNs]. + auto it0 = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkFirst - offNs); + auto it1 = std::upper_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkLast - offNs); for (auto it = it0; it != it1; ++it) { - int64_t imgTs = *it; + int64_t imgTs = *it; // raw camera-clock timestamp; keyed as-is into imagesFilenamesInTime auto fnIt = s.imagesFilenamesInTime.find(imgTs); if (fnIt == s.imagesFilenamesInTime.end()) continue; @@ -765,19 +798,22 @@ static void loadCloud(AppState& s) continue; } Eigen::Affine3f pose; - if (!interpPose(trajMap, imgTs, pose)) + if (!interpPose(trajMap, imgTs + offNs, pose)) continue; cv::Mat img = readImage(fnIt->second); if (img.empty()) continue; int gidx = (int)(it - s.imageTsNs.begin()); - chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + // ImgEntry.ts is stored already shifted into the LiDAR clock, + // since it's compared against pt.ts_ns further below. + chunkImgs.push_back({ imgTs + offNs, pose, std::move(img), gidx }); } } else { - // legacy: single image nearest to chunk midpoint - int64_t mid = chunkFirst; + // legacy: single image nearest to chunk midpoint (see note above + // on why the search target is shifted by -offNs) + int64_t mid = chunkFirst - offNs; auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), mid); if (it == s.imageTsNs.end()) --it; @@ -793,12 +829,12 @@ static void loadCloud(AppState& s) const bool tooFast = dropFastImgs && angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; if (tooFast) ++angFilteredImgs; - if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) + if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs + offNs, pose)) { cv::Mat img = readImage(fnIt->second); int gidx = (int)(it - s.imageTsNs.begin()); if (!img.empty()) - chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + chunkImgs.push_back({ imgTs + offNs, pose, std::move(img), gidx }); } } } @@ -1050,6 +1086,98 @@ static void loadCloud(AppState& s) s.status += " | Fast-img filtered: " + std::to_string(angFilteredImgs); } +// Small CPU jet colormap approximation, matching the GLSL one used by the +// GPU point renderer's Intensity color mode (raylib_widgets::kJetColormapGLSL) +// closely enough for a visual reference image. Returns BGR (OpenCV order). +static cv::Vec3b jetColorBGR(float t) +{ + t = std::clamp(t, 0.f, 1.f); + float r = std::clamp(1.5f - std::fabs(4.f * t - 3.f), 0.f, 1.f); + float g = std::clamp(1.5f - std::fabs(4.f * t - 2.f), 0.f, 1.f); + float b = std::clamp(1.5f - std::fabs(4.f * t - 1.f), 0.f, 1.f); + return cv::Vec3b((uchar)(b * 255.f), (uchar)(g * 255.f), (uchar)(r * 255.f)); +} + +// Rasterizes a synthetic "intensity image" for the camera pose at imgTsAdj +// (already shifted by the photo time offset), by reprojecting s.exportCloud +// through the same fixed camera-to-LiDAR extrinsics (R_wc/C) and +// calib::projectPoint() as loadCloud()'s colorize pass, painted with a jet +// colormap over each point's normalized [0,1] intensity and a simple +// per-pixel depth test (nearest point wins) so occluded points don't bleed +// through. Points farther than s.maxTemporalDist (or a 1s fallback) in time +// from imgTsAdj are skipped -- otherwise the whole session's merged cloud +// would be tested against every single preview, which is the same temporal +// gate loadCloud()'s "Temporal" coloring strategy already applies per point. +static cv::Mat renderIntensityProjection(const AppState& s, int64_t imgTsAdj) +{ + const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + cv::Mat out(std::max(1, Ks.height), std::max(1, Ks.width), CV_8UC3, cv::Scalar(25, 25, 25)); + if (s.exportCloud.empty() || Ks.width <= 0 || Ks.height <= 0) + return out; + + auto trajMap = buildTrajMap(s.traj); + Eigen::Affine3f pose; + if (!interpPose(trajMap, imgTsAdj, pose)) + return out; + const Eigen::Affine3f poseInv = pose.inverse(); + const Eigen::Matrix3f& R_wc = s.R_wc; + const Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); + + const int64_t windowNs = (int64_t)((s.maxTemporalDist > 0.f ? s.maxTemporalDist : 1.0f) * 1e9); + const int step = std::max(1, s.intensityProjDecim); + const int radius = std::max(1, (int)std::lround(s.intensityProjPointRadius)); + + cv::Mat depthBuf(out.rows, out.cols, CV_32F, cv::Scalar(std::numeric_limits::max())); + for (size_t i = 0; i < s.exportCloud.size(); i += step) + { + const auto& p = s.exportCloud[i]; + if (std::abs(p.ts_ns - imgTsAdj) > windowNs) + continue; + Eigen::Vector3f pl = poseInv * Eigen::Vector3f(p.x, p.y, p.z); + float u, v, depth; + if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) + continue; + if (Ks.model == CameraModel::Pinhole && depth <= 0.05f) + continue; + // Points near-grazing the camera plane (small but positive depth, + // e.g. off to the side) get blown up to huge u/v by the perspective + // divide -- unlike colorize()'s tight per-point temporal matching, + // this function pulls in every point within a whole time window, so + // it hits that edge case far more often. (int)std::round() on such a + // value is undefined behavior, which is what produced the + // "wrapping"/bowtie look; reject before the cast instead. + if (!std::isfinite(u) || !std::isfinite(v) || std::fabs(u) > 1e6f || std::fabs(v) > 1e6f) + continue; + int iu = (int)std::round(u); + int iv = (int)std::round(v); + const cv::Vec3b col = jetColorBGR(p.intensity); + + for (int dy = -radius; dy <= radius; ++dy) + { + int yy = iv + dy; + if (yy < 0 || yy >= out.rows) + continue; + for (int dx = -radius; dx <= radius; ++dx) + { + if (dx * dx + dy * dy > radius * radius) + continue; + int xx = iu + dx; + if (Ks.model == CameraModel::Equirectangular) + xx = (xx % out.cols + out.cols) % out.cols; + else if (xx < 0 || xx >= out.cols) + continue; + float& zb = depthBuf.at(yy, xx); + if (depth < zb) + { + zb = depth; + out.at(yy, xx) = col; + } + } + } + } + return out; +} + static void loadCalib(AppState& s) { std::ifstream f(s.calibBuf); @@ -1513,11 +1641,12 @@ static void exportColmap(AppState& s) "# 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); + const int64_t offNs = imageOffsetNs(s); int id = 1; for (auto& [ts, path] : s.imagesFilenamesInTime) { Eigen::Affine3f pose; - if (!interpPose(trajMap, ts, pose)) + if (!interpPose(trajMap, ts + offNs, pose)) continue; Eigen::Affine3f T_wc = pose * T_lc; // camera in world Eigen::Affine3f T_cw = T_wc.inverse(); // world -> camera @@ -2018,11 +2147,13 @@ int main(int argc, char* argv[]) { s.imgViewIdx = std::max(s.imgViewIdx - 1, 0); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } if (IsKeyPressed(KEY_RIGHT)) { s.imgViewIdx = std::min(s.imgViewIdx + 1, (int)s.imageTsNs.size()); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } } @@ -2112,6 +2243,23 @@ int main(int argc, char* argv[]) } } + // ── (re)build the intensity-projection texture on demand ──────────────── + // Rasterization is cheap enough (already-decimated, in-memory + // exportCloud) to do synchronously on toggle/refresh/image-change, + // unlike the photo loader above which reads a file off disk. + if (s.showIntensityProjection && s.intensityProjNeedsUpdate && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) + { + s.intensityProjNeedsUpdate = false; + int64_t imgTsAdj = s.imageTsNs[s.imgViewIdx] + imageOffsetNs(s); + cv::Mat proj = renderIntensityProjection(s, imgTsAdj); + cv::cvtColor(proj, proj, cv::COLOR_BGR2RGB); + if (s.intensityProjTexValid) + UnloadTexture(s.intensityProjTex); + Image ri = { proj.data, proj.cols, proj.rows, 1, PIXELFORMAT_UNCOMPRESSED_R8G8B8 }; + s.intensityProjTex = LoadTextureFromImage(ri); + s.intensityProjTexValid = s.intensityProjTex.id > 0; + } + // ── ImGui panel ─────────────────────────────────────────────────────── rlImGuiBegin(); @@ -2408,8 +2556,15 @@ int main(int argc, char* argv[]) { s.imgViewIdx = std::clamp(s.imgViewIdx, 0, nImgs - 1); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } ImGui::TextDisabled("ts: %lld", (long long)s.imageTsNs[s.imgViewIdx]); + if (s.imageTimeOffsetMs != 0.f) + { + ImGui::SameLine(); + ImGui::TextDisabled( + "(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageOffsetNs(s))); + } { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); bool fast = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f && as > s.maxImageAngSpeedDeg; @@ -2426,6 +2581,37 @@ int main(int argc, char* argv[]) 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::Separator(); + if (ImGui::Checkbox("Show intensity projection", &s.showIntensityProjection)) + { + if (s.showIntensityProjection) + s.intensityProjNeedsUpdate = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Draws a synthetic intensity image next to the photo, by\n" + "reprojecting the colorized cloud through the current\n" + "calibration -- a reference to check it against the photo.\n" + "Requires 'Load cloud' to have run first."); + if (s.showIntensityProjection) + { + ImGui::PushItemWidth(-140.f); + if (ImGui::InputInt("Point decimation##proj", &s.intensityProjDecim)) + s.intensityProjDecim = std::max(1, s.intensityProjDecim); + if (ImGui::InputFloat("Point radius (px)##proj", &s.intensityProjPointRadius, 0.5f, 1.f, "%.1f")) + s.intensityProjPointRadius = std::max(1.f, s.intensityProjPointRadius); + ImGui::Checkbox("Overlay on photo", &s.intensityProjOverlay); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("ON: alpha-blended on top of the photo\nOFF: shown side-by-side with it"); + if (s.intensityProjOverlay) + ImGui::SliderFloat("Overlay alpha", &s.intensityProjAlpha, 0.f, 1.f, "%.2f"); + ImGui::PopItemWidth(); + if (ImGui::Button("Refresh projection", ImVec2(-1, 0))) + s.intensityProjNeedsUpdate = true; + if (s.exportCloud.empty()) + ImGui::TextColored(ImVec4(1, 0.6f, 0, 1), "No colorized cloud yet -- run 'Load cloud'."); + } } } @@ -2571,9 +2757,13 @@ int main(int argc, char* argv[]) ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_Once); ImGui::Begin("Image##viewer", nullptr, ImGuiWindowFlags_NoScrollbar); ImVec2 avail = ImGui::GetContentRegionAvail(); + const bool showProj = s.showIntensityProjection && s.intensityProjTexValid; + const bool overlayMode = showProj && s.intensityProjOverlay; + const float colW = (showProj && !overlayMode) ? (avail.x - 4.f) * 0.5f : avail.x; + float aspect = (float)s.imgViewTex.height / (float)s.imgViewTex.width; - int dispW = (int)avail.x; - int dispH = (int)(avail.x * aspect); + int dispW = (int)colW; + int dispH = (int)(colW * aspect); if (dispH > (int)avail.y) { dispH = (int)avail.y; @@ -2581,6 +2771,23 @@ int main(int argc, char* argv[]) } ImVec2 imgPos = ImGui::GetCursorScreenPos(); rlImGuiImageSize(&s.imgViewTex, dispW, dispH); + + if (overlayMode) + { + // Redraw the projection texture at the same screen rect, tinted + // with a reduced alpha -- ImGui's renderer alpha-blends draw + // commands, so this composites over the photo just drawn above. + ImGui::SetCursorScreenPos(imgPos); + ImVec4 tint(1.f, 1.f, 1.f, std::clamp(s.intensityProjAlpha, 0.f, 1.f)); + ImGui::ImageWithBg( + ImTextureID(s.intensityProjTex.id), + ImVec2((float)dispW, (float)dispH), + ImVec2(0.f, 0.f), + ImVec2(1.f, 1.f), + ImVec4(0.f, 0.f, 0.f, 0.f), + tint); + } + // overlay the ROI, mapping full-res image pixels to the displayed rect if (s.roi.enabled && s.imgViewTex.width > 0 && s.imgViewTex.height > 0) { @@ -2595,6 +2802,20 @@ int main(int argc, char* argv[]) /*rounding=*/0.f, /*thickness=*/2.f); } + + if (showProj && !overlayMode) + { + ImGui::SameLine(); + float pAspect = (float)s.intensityProjTex.height / (float)s.intensityProjTex.width; + int pDispW = (int)colW; + int pDispH = (int)(colW * pAspect); + if (pDispH > (int)avail.y) + { + pDispH = (int)avail.y; + pDispW = (int)(avail.y / pAspect); + } + rlImGuiImageSize(&s.intensityProjTex, pDispW, pDispH); + } ImGui::End(); } @@ -2608,6 +2829,8 @@ int main(int argc, char* argv[]) s.rosThread.join(); if (s.imgViewTexValid) UnloadTexture(s.imgViewTex); + if (s.intensityProjTexValid) + UnloadTexture(s.intensityProjTex); s.cloud.unload(); if (s.shaderOk) diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 3291f1ae..8777a968 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -122,6 +122,8 @@ void loadLazFile(const std::string& path); void recolorPointsFromImage(); double reprojectionErrorPx(size_t i); void removeCorrespondence(size_t i); +bool loadCalibrationJson(const std::string& path); +bool saveCalibrationJson(const std::string& path); float imgui_co_size{ 1000.0f }; bool imgui_draw_co{ true }; @@ -916,6 +918,94 @@ void removeCorrespondence(size_t i) SD::pointPickedPointCloud.erase(SD::pointPickedPointCloud.begin() + i); } +// JSON calibration using the same schema as camera_lidar_trajectory_viewer's +// loadCalib()/saveCalib() (apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp): +// { "model", "intrinsics": {fx,fy,cx,cy,k1..k6,p1,p2,width,height}, +// "extrinsics": {camera_position_in_world_xyz, camera_rotation_matrix_in_world}, "roi" }. +// Only extrinsics (+ intrinsics width/height) round-trip here -- this app is +// fixed to the equirectangular model. +// +// NOTE the two functions are NOT each other's inverse: loadCalibrationJson +// reads "extrinsics" straight into camera_pose (camera-to-LiDAR, matching +// this app's own "load camera to lidar relative pose (*.reg)" button and +// camera_lidar_trajectory_viewer's convention), while saveCalibrationJson +// below writes camera_pose.inverse() (LiDAR-to-camera) per request. Loading a +// file this app just saved will therefore NOT reproduce the same camera_pose +// -- flag this if that round-trip turns out to matter. +bool loadCalibrationJson(const std::string& path) +{ + std::ifstream f(path); + if (!f) + return false; + nlohmann::json j; + try + { + f >> j; + } catch (const std::exception&) + { + return false; + } + + if (!j.contains("extrinsics")) + return false; + auto& je = j["extrinsics"]; + if (!je.contains("camera_position_in_world_xyz") || !je.contains("camera_rotation_matrix_in_world")) + return false; + + auto& t = je["camera_position_in_world_xyz"]; + auto& m = je["camera_rotation_matrix_in_world"]; + if (t.size() < 3 || m.size() < 3) + return false; + + Eigen::Matrix3d R; + for (int r = 0; r < 3; ++r) + for (int c = 0; c < 3; ++c) + R(r, c) = m[r][c].get(); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + pose.linear() = R; + pose.translation() = Eigen::Vector3d(t[0].get(), t[1].get(), t[2].get()); + SystemData::camera_pose = pose; + return true; +} + +bool saveCalibrationJson(const std::string& path) +{ + nlohmann::json j; + j["model"] = "equirectangular"; + + nlohmann::json ji; + ji["width"] = SystemData::imageWidth; + ji["height"] = SystemData::imageHeight; + j["intrinsics"] = ji; + + // Exported inverted: camera_pose is this app's camera-to-LiDAR transform + // (see the "save camera to lidar relative pose (*.reg)" button above, + // which dumps it un-inverted), so its inverse is the LiDAR-to-camera + // transform -- write that under the same field names. + const Eigen::Affine3d inv = SystemData::camera_pose.inverse(); + const Eigen::Vector3d t = inv.translation(); + const Eigen::Matrix3d R = inv.linear(); + nlohmann::json je; + je["camera_position_in_world_xyz"] = { t.x(), t.y(), t.z() }; + nlohmann::json rows = nlohmann::json::array(); + for (int r = 0; r < 3; ++r) + { + nlohmann::json row = nlohmann::json::array(); + for (int c = 0; c < 3; ++c) + row.push_back(R(r, c)); + rows.push_back(row); + } + je["camera_rotation_matrix_in_world"] = rows; + j["extrinsics"] = je; + + std::ofstream f(path); + if (!f) + return false; + f << j.dump(2); + return true; +} + void ImGuiLoadSaveButtons() { namespace SD = SystemData; @@ -1501,6 +1591,27 @@ void display() SystemData::imageNrChannels, SystemData::camera_pose); } + ImGui::Separator(); + if (ImGui::Button("Load calibration (JSON)...")) + { + const std::string path = mandeye::fd::OpenFileDialogOneFile("Load calibration", mandeye::fd::json_filter); + if (!path.empty()) + { + if (loadCalibrationJson(path)) + recolorPointsFromImage(); + else + std::cerr << "Cannot load calibration: " << path << std::endl; + } + } + ImGui::SameLine(); + if (ImGui::Button("Save calibration (JSON)...")) + { + const std::string path = mandeye::fd::SaveFileDialog("Save calibration", mandeye::fd::json_filter, ".json", "calibration.json"); + if (!path.empty() && !saveCalibrationJson(path)) + std::cerr << "Cannot save calibration: " << path << std::endl; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Same calibration JSON schema as camera_lidar_trajectory_viewer\n(model/intrinsics/extrinsics) -- interchangeable with it."); imagePicker("ImagePicker", (ImTextureID)tex1, SystemData::pointPickedImage, picked3DPoints); diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index f3aa73d8..94aae7be 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -31,6 +31,56 @@ void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, floa ka_deg = static_cast(rad2deg(pose.ka)); } +// Radius (in normalized camera coords, squared) past which the rational distortion model +// stops being usable. r -> r*radial(r) is only injective up to its turning point; beyond it +// the model folds, so directions far outside the lens' actual field of view map back onto +// valid pixel coordinates -- painting whatever is at the centre of the frame onto geometry +// the camera never saw. The projection alone cannot tell such a fold-back from a genuine +// hit, so find the turning point once and reject everything past it. Scanned numerically -- +// the turning point of a 6th-order rational function has no useful closed form. It always +// lies outside the image itself (otherwise the calibration could not reach its own corners), +// so no legitimate pixel is lost. Ported from the equivalent fix applied directly in +// TrajectoryViewer.cpp's (now-removed) inline distortion code -- see upstream commit +// "Fix colorization for calibration for invalid points" (#527) -- but placed here so every +// caller of projectPoint() gets it, not just that one call site. +static float maxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) { + auto g = [&](float r) { + float r2 = r * r; + float den = 1.f + (k4 + (k5 + k6 * r2) * r2) * r2; + if (std::fabs(den) < 1e-9f) + return -1.f; // pole -- certainly past the turning point + return r * (1.f + (k1 + (k2 + k3 * r2) * r2) * r2) / den; + }; + // 8.0 == tan(83 deg), wider than any lens this app sees. A distortion-free model is + // monotonic everywhere and so keeps the whole range, i.e. no behaviour change. + const float kLimit = 8.f, kStep = 0.005f; + float prev = 0.f; + for (float r = kStep; r <= kLimit; r += kStep) { + float cur = g(r); + if (cur <= prev) + return (r - kStep) * (r - kStep); + prev = cur; + } + return kLimit * kLimit; +} + +// projectPoint() is called per-point -- potentially millions of times per colorize pass -- +// with the SAME Intrinsics each time, so re-running the numeric scan above on every call +// would be a severe perf regression. Memoize on the six coefficients actually scanned; exact +// float equality is fine here since it's detecting "same Intrinsics as last call", not +// comparing independently-derived values. +static float cachedMaxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) { + thread_local float lastK[6] = { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }; + thread_local float lastResult = -1.f; + if (lastResult >= 0.f && lastK[0] == k1 && lastK[1] == k2 && lastK[2] == k3 && + lastK[3] == k4 && lastK[4] == k5 && lastK[5] == k6) { + return lastResult; + } + lastResult = maxValidRadiusSq(k1, k2, k3, k4, k5, k6); + lastK[0] = k1; lastK[1] = k2; lastK[2] = k3; lastK[3] = k4; lastK[4] = k5; lastK[5] = k6; + return lastResult; +} + Intrinsics scaleIntrinsics(const Intrinsics& K, float s) { Intrinsics out = K; out.fx *= s; @@ -110,7 +160,13 @@ bool projectPoint(float px, float py, float pz, float xn = pc.x() / depth; float yn = pc.y() / depth; + // Off-axis cutoff: beyond the rational distortion model's turning point, the projection + // folds back and would paint frame-centre content onto geometry the camera never saw. + // See maxValidRadiusSq() above. float r2 = xn*xn + yn*yn; + if (r2 > cachedMaxValidRadiusSq(K.k1, K.k2, K.k3, K.k4, K.k5, K.k6)) + return false; + float r4 = r2 * r2; float r6 = r4 * r2; float radial = (1.f + K.k1*r2 + K.k2*r4 + K.k3*r6) From d55f33d835f0d100ea40af9dfc578a236e8fee91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 14 Sep 2026 18:15:29 +0200 Subject: [PATCH 05/22] Add Mei camera support to the trajectory viewer and its exports The calibration loader reads "mei"/"insta360_mei_v2" and xi, the 5 cm near clip now applies to Mei as well as pinhole (its depth is a range rather than a z, but 5 cm means the same thing physically), and every non-pinhole model draws an axis triad instead of a frustum -- a fisheye sees far more than the pyramid fx/fy/cx/cy imply. Rectification and the COLMAP export are gated on Pinhole: initUndistortRectifyMap would mis-warp a fisheye rather than rectify it, and no COLMAP camera type carries an xi. The ROS 2 export reports the rig's own insta360_mei_v2 distortion tag rather than claiming plumb_bob, with xi appended to d since CameraInfo has nowhere else to put it. Also: image filenames parse as "_.jpg", so the 360 rig's per-lens frames work without the parser knowing the list of rigs, and the camera/LiDAR clock offset becomes timeOffsetSec (seconds, double), applied to the stamps the ROS 2 bag is written with as well. Co-Authored-By: Claude Opus 5 (1M context) --- .../RosExport.cpp | 30 +++- .../TrajectoryViewer.cpp | 154 ++++++++++++------ 2 files changed, 131 insertions(+), 53 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index ff6cdffd..2b6a41bc 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -208,9 +208,13 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s bool mapsReady = false; int camW = 0, camH = 0; // initUndistortRectifyMap is pinhole-only: there is nothing to - // rectify on a 360 panorama, and Km/Dm describe a camera it isn't. + // rectify on a 360 panorama, and Km/Dm describe a camera neither + // it nor a Mei fisheye is (Mei's k1/k2/k3/p1/p2 are its own + // polynomial, applied after a unit-sphere step Km/Dm can't + // express), so both keep their raw frames. const bool equirect = in.K.model == CameraModel::Equirectangular; - const bool rectify = opt.undistortCamera && in.calibLoaded && !equirect; + const bool mei = in.K.model == CameraModel::Mei; + const bool rectify = opt.undistortCamera && in.calibLoaded && in.K.model == CameraModel::Pinhole; // 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; @@ -314,6 +318,28 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; ci.p = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; } + else if (mei) + { + // No standard ROS distortion model is a unified + // sphere either, so this reports the rig's own tag + // (the same string its camera_info.yaml carries, + // see CalibCore/MeiCamera.h) rather than claiming + // to be plumb_bob/rational_polynomial, which a + // consumer would undistort with badly wrong math. + // + // d is the yaml's own (k1, k2, k3, p1, p2) order -- + // NOT OpenCV's (k1, k2, p1, p2, k3) -- with xi + // appended, since CameraInfo has nowhere else to + // put it and the model is unusable without it. + // K/P stay populated: fx/fy/cx/cy do mean the + // usual thing here, they are just applied after + // the unit-sphere step. + ci.distortion_model = "insta360_mei_v2"; + ci.d = { in.K.k1, in.K.k2, in.K.k3, in.K.p1, in.K.p2, in.K.xi }; + 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 }; + } else { ci.distortion_model = "rational_polynomial"; diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 24af27e9..d37dac78 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -220,7 +220,7 @@ struct AppState { Trajectory traj; std::vector imageTsNs; - Intrinsics K; // K.model selects pinhole vs equirectangular (see CalibCore/Camera.h) + Intrinsics K; // K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) // How K.model was decided. The calibration file's "model" key wins; absent // one, the image filenames are the fallback. Both inputs are kept as state // rather than applied on the spot because they arrive in either order -- @@ -244,6 +244,14 @@ struct AppState // chunk's worth in RAM at once, so this is what keeps that bounded. The // intrinsics are scaled to match via calib::scaleIntrinsics. float imgScale = 1.0f; + // Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed + // trigger/USB latency the camera's own timestamps don't account for): + // t_traj = t_image + timeOffsetSec. Applied wherever an image timestamp is + // matched against the LiDAR/pose timeline (loadCloud's chunk selection + + // point matching, exportColmap's per-image pose lookup) -- never to the raw + // timestamps used for filename lookup or image-list indexing + // (s.imageTsNs/imagesFilenamesInTime). + double timeOffsetSec = 0.0; GpuCloud cloud; Shader shader = {}; bool shaderOk = false; @@ -371,13 +379,6 @@ struct AppState float intensityProjAlpha = 0.6f; // blend strength when intensityProjOverlay is on }; -// s.imageTimeOffsetMs, in nanoseconds -- added to a raw image timestamp -// before comparing it against the LiDAR/pose timeline. -static int64_t imageOffsetNs(const AppState& s) -{ - return static_cast(std::llround(static_cast(s.imageTimeOffsetMs) * 1e6)); -} - // ── helpers ─────────────────────────────────────────────────────────────────── // Plain Eigen::Vector3f -> raylib Vector3 conversion. Used to be an axis // remap (x, z, -y) that made this app's native Z-up LiDAR data render @@ -434,11 +435,14 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) static constexpr const char* kEquirectPrefix = "equirectangular_"; // Timestamp encoded in a camera frame's filename, or -1 when the file isn't -// one. Three layouts are accepted: Mandeye's own "cam0_.jpg", the -// 360 rig's "equirectangular_.jpg", and a bare -// ".jpg". `equirect`, when given, reports whether the panorama -// prefix was the one found. The all-digits check matters for the bare form -- -// without it every unrelated .jpg in the directory would reach std::stoll. +// one. The layout is "_.jpg" or a bare +// ".jpg": everything up to and including the last '_' is +// ignored, so Mandeye's "cam0_", the 360 rig's "equirectangular_" and +// its per-lens "back_"/"front_" frames all parse without this needing +// to know the list of rigs. `equirect`, when given, reports whether the +// panorama prefix was the one found -- that one prefix still carries meaning +// (it selects the camera model, see resolveCameraModel). The all-digits check +// is what rejects unrelated .jpgs, which would otherwise reach std::stoll. static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) { if (equirect) @@ -446,16 +450,10 @@ static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) if (p.extension() != ".jpg") return -1; std::string stem = p.stem().string(); - if (stem.rfind(kEquirectPrefix, 0) == 0) - { - stem = stem.substr(std::strlen(kEquirectPrefix)); - if (equirect) - *equirect = true; - } - else if (stem.rfind("cam0_", 0) == 0) - { - stem = stem.substr(5); - } + if (equirect) + *equirect = stem.rfind(kEquirectPrefix, 0) == 0; + if (auto us = stem.rfind('_'); us != std::string::npos) + stem = stem.substr(us + 1); if (stem.empty() || stem.find_first_not_of("0123456789") != std::string::npos) return -1; try @@ -474,9 +472,20 @@ static fs::path cameraDir(const AppState& s) return s.cameraBuf[0] ? fs::path(s.cameraBuf) : fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; } +// AppState::timeOffsetSec in nanoseconds, to match the timestamps. +static int64_t imageTimeOffsetNs(const AppState& s) +{ + return (int64_t)std::llround(s.timeOffsetSec * 1e9); +} + // Settles K.model from the two inputs that can select it, in precedence order. // Call after either of them changes; see AppState::fileModel for why this isn't // done inline in the loaders. +// +// Only Pinhole and Equirectangular are ever inferred: the filename fallback +// can distinguish those two because the 360 rig marks its frames with +// kEquirectPrefix, but nothing in a frame's name identifies a Mei fisheye, so +// CameraModel::Mei is reachable only through an explicit "model" key. static void resolveCameraModel(AppState& s) { if (s.modelExplicit) @@ -696,6 +705,7 @@ static void loadCloud(AppState& s) // images at the right pixel; with all-zero coefficients it reduces exactly // to the ideal pinhole. const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + const int64_t offNs = imageTimeOffsetNs(s); // Every image of a chunk is held in memory at once (multiImgColoring), so // for large frames the scale is what keeps that bounded. auto readImage = [&](const std::string& path) @@ -731,7 +741,6 @@ static void loadCloud(AppState& s) // time(s) -> T_world_lidar, for interpolating the pose at each image time. std::map trajMap = buildTrajMap(s.traj); - const int64_t offNs = imageOffsetNs(s); std::vector gpuData; float mx = 0.f; @@ -920,7 +929,13 @@ static void loadCloud(AppState& s) float u, v, depth; if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) return h; - if (Ks.model == CameraModel::Pinhole && depth <= 0.05f) + // Too close to the lens to be a real observation. Applies + // to Mei as well as Pinhole (its depth is a range rather + // than a z, but 5 cm means the same thing physically); + // projectPoint's own Mei guard only rejects a point + // essentially AT the camera. Equirectangular keeps its + // long-standing "no near clip" behaviour. + if ((Ks.model == CameraModel::Pinhole || Ks.model == CameraModel::Mei) && depth <= 0.05f) return h; int iu = (int)std::round(u); int iv = (int)std::round(v); @@ -1188,10 +1203,11 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; - // Camera model: "equirectangular"/"equirect" for a 360 panorama, anything - // else for the pinhole model this app started with. Accepted both at the - // top level and inside "intrinsics". Assigned unconditionally so loading a - // pinhole calibration after an equirectangular one clears the flag rather + // Camera model: "equirectangular"/"equirect" for a 360 panorama, "mei" (or + // the rig's own "insta360_mei_v2" tag) for a unified-sphere fisheye, + // anything else for the pinhole model this app started with. Accepted both + // at the top level and inside "intrinsics". Assigned unconditionally so + // loading a pinhole calibration after another model clears the flag rather // than inheriting it. { const bool topLevel = j.contains("model"); @@ -1210,7 +1226,12 @@ static void loadCalib(AppState& s) { return (char)std::tolower(c); }); - s.fileModel = (model == "equirectangular" || model == "equirect") ? CameraModel::Equirectangular : CameraModel::Pinhole; + if (model == "equirectangular" || model == "equirect") + s.fileModel = CameraModel::Equirectangular; + else if (model == "mei" || model == "insta360_mei_v2") + s.fileModel = CameraModel::Mei; + else + s.fileModel = CameraModel::Pinhole; resolveCameraModel(s); } if (j.contains("intrinsics")) @@ -1220,7 +1241,10 @@ static void loadCalib(AppState& s) 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) + // Pinhole: the rational distortion model (also what the ROS export + // rectifies with). Mei reuses k1/k2/k3 and p1/p2 as its own plain + // polynomial and adds xi, leaving k4/k5/k6 unused -- see + // CalibCore/Camera.h. 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); @@ -1229,6 +1253,7 @@ static void loadCalib(AppState& s) 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.xi = ji.value("xi", s.K.xi); } if (j.contains("extrinsics")) { @@ -1598,11 +1623,14 @@ static void exportColmap(AppState& s) s.status = "COLMAP: no images"; return; } - if (s.K.model == CameraModel::Equirectangular) - { - // COLMAP's text model has no equirectangular camera type, so the - // FULL_OPENCV line below would misdescribe the images. - s.status = "COLMAP: equirectangular camera model is not supported by COLMAP"; + if (s.K.model != CameraModel::Pinhole) + { + // COLMAP's text model has no equirectangular camera type, and none of + // its fisheye types is the unified-sphere (Mei) model -- none carries + // an xi -- so the FULL_OPENCV line below would misdescribe the images. + s.status = s.K.model == CameraModel::Equirectangular + ? "COLMAP: equirectangular camera model is not supported by COLMAP" + : "COLMAP: Mei camera model is not supported by COLMAP"; return; } @@ -1641,7 +1669,7 @@ static void exportColmap(AppState& s) "# 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); - const int64_t offNs = imageOffsetNs(s); + const int64_t offNs = imageTimeOffsetNs(s); int id = 1; for (auto& [ts, path] : s.imagesFilenamesInTime) { @@ -1714,7 +1742,10 @@ static void exportColmap(AppState& s) static void buildRosInput(AppState& s, RosExportInput& in) { in.traj = s.traj; - in.imageFiles = s.imagesFilenamesInTime; + // Stamps go into the bag on the trajectory clock, like every other topic. + in.imageFiles.clear(); + for (const auto& [ts, path] : s.imagesFilenamesInTime) + in.imageFiles[ts + imageTimeOffsetNs(s)] = path; in.calibLoaded = s.calibLoaded; in.K = s.K; in.E = s.E; @@ -1812,7 +1843,7 @@ static void drawScene(AppState& s) for (int64_t ts : s.imageTsNs) { - const TrajPose* pose = s.traj.nearest(ts); + const TrajPose* pose = s.traj.nearest(ts + imageTimeOffsetNs(s)); if (!pose) continue; @@ -1822,11 +1853,13 @@ static void drawScene(AppState& s) Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; float sc = hl ? fs * 1.05f : fs; - if (s.K.model == CameraModel::Equirectangular) + if (s.K.model != CameraModel::Pinhole) { - // A 360 camera sees the whole sphere, so there is no frustum to - // draw -- show where it was and which way its axes point - // instead. The triad is the usual X=red, Y=green, Z=blue. + // A 360 camera sees the whole sphere and a Mei fisheye sees far + // more than the rectangular pyramid fx/fy/cx/cy imply, so + // there is no frustum worth drawing -- show where the camera + // was and which way its axes point instead. The triad is the + // usual X=red, Y=green, Z=blue. DrawSphere(origin, fs * (hl ? 0.08f : 0.05f), fc); const Color axisColors[3] = { RED, GREEN, BLUE }; for (int k = 0; k < 3; k++) @@ -2250,7 +2283,7 @@ int main(int argc, char* argv[]) if (s.showIntensityProjection && s.intensityProjNeedsUpdate && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) { s.intensityProjNeedsUpdate = false; - int64_t imgTsAdj = s.imageTsNs[s.imgViewIdx] + imageOffsetNs(s); + int64_t imgTsAdj = s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s); cv::Mat proj = renderIntensityProjection(s, imgTsAdj); cv::cvtColor(proj, proj, cv::COLOR_BGR2RGB); if (s.intensityProjTexValid) @@ -2490,6 +2523,14 @@ int main(int argc, char* argv[]) kEquirectPrefix); ImGui::Text("%dx%d", s.imgW, s.imgH); } + else if (s.K.model == CameraModel::Mei) + { + // Mei is never inferred (see resolveCameraModel), so it is + // always an explicit "model" key -- no tooltip needed. + ImGui::Text("Model: mei (xi=%.4f)", s.K.xi); + ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); + ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + } else { ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); @@ -2506,6 +2547,12 @@ int main(int argc, char* argv[]) if (ImGui::IsItemHovered()) ImGui::SetTooltip( "Downscale applied to images before coloring.\nLower = less RAM and faster, at coarser color detail."); + ImGui::InputDouble("Time offset (s)", &s.timeOffsetSec, 0.001, 0.01, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Camera clock minus trajectory clock: t_traj = t_image + offset.\n" + "Fixes colors smeared along the direction of travel.\n" + "Re-run Colorize to apply."); ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); @@ -2559,11 +2606,11 @@ int main(int argc, char* argv[]) s.intensityProjNeedsUpdate = true; } ImGui::TextDisabled("ts: %lld", (long long)s.imageTsNs[s.imgViewIdx]); - if (s.imageTimeOffsetMs != 0.f) + if (s.timeOffsetSec != 0.0) { ImGui::SameLine(); ImGui::TextDisabled( - "(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageOffsetNs(s))); + "(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s))); } { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); @@ -2657,14 +2704,17 @@ int main(int argc, char* argv[]) ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); if (ImGui::IsItemHovered()) ImGui::SetTooltip("ON: CompressedImage (jpeg)\nOFF: raw Image bgr8"); - const bool noRectify = s.K.model == CameraModel::Equirectangular; + // Rectification is OpenCV's pinhole initUndistortRectifyMap; + // it would mis-warp a panorama or a fisheye, not rectify it. + const bool noRectify = s.K.model != CameraModel::Pinhole; ImGui::BeginDisabled(noRectify); ImGui::Checkbox("Undistort (rectify)", &s.ros.undistortCamera); ImGui::EndDisabled(); if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) ImGui::SetTooltip( - noRectify ? "Not applicable to an equirectangular camera." - : "Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)."); + !noRectify ? "Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)." + : s.K.model == CameraModel::Equirectangular ? "Not applicable to an equirectangular camera." + : "Not applicable to a Mei (fisheye) camera."); ImGui::Unindent(); } ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); @@ -2713,13 +2763,15 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); s.colmapPtDecim = std::max(1, s.colmapPtDecim); - const bool colmapUnsupported = s.K.model == CameraModel::Equirectangular; + const bool colmapUnsupported = s.K.model != CameraModel::Pinhole; ImGui::BeginDisabled(colmapUnsupported); if (ImGui::Button("Export COLMAP model", ImVec2(-1, 0))) exportColmap(s); ImGui::EndDisabled(); if (colmapUnsupported && ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) - ImGui::SetTooltip("COLMAP has no equirectangular camera model."); + ImGui::SetTooltip( + s.K.model == CameraModel::Equirectangular ? "COLMAP has no equirectangular camera model." + : "COLMAP has no unified-sphere (Mei) camera model."); ImGui::TextDisabled("Writes sparse/{cameras,images,points3D}.txt"); if (ImGui::IsItemHovered()) ImGui::SetTooltip( From 3723a664859e22a5097e77bed7b12466bb38d468 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 14 Sep 2026 18:15:57 +0200 Subject: [PATCH 06/22] Mask images in the trajectory viewer's coloring, and scale the ROI The mask is the free-form counterpart of the ROI: a point projecting onto a masked-out pixel stays uncolored, which is what it takes to drop the operator and the rig itself out of a 360 frame -- no rectangle can cut those out without cutting out the scene with them. Loaded from any image OpenCV reads (File > Open Image Mask..., the Calibration panel, or drag & drop) and thresholded to a strict 0/255, so a hand-painted PNG and a jpeg with compression noise behave the same. It is resampled to whatever size the frames are actually read at, so its own resolution doesn't have to match: one drawn over a downscaled copy of a frame works as well as a full-res one. Rejected pixels are tinted red over the image preview, and the "In ROI" point color mode becomes "In ROI / mask", painting what either filter rejects. The ROI had the scale mismatch the mask sidesteps: it is specified in full-resolution pixels (calib::Roi) but was compared against pixels of the downscaled images the colorizer reads, stretching it by 1/imgScale at any Image scale below 1. calib::scaleRoi now scales it alongside the intrinsics. It rounds both edges and subtracts, rather than scaling the width on its own, so abutting rectangles cannot come back overlapping, and it never lets a non-empty rectangle collapse to w/h == 0 -- the sentinel every caller reads as "no ROI set", i.e. accept everything. Co-Authored-By: Claude Opus 5 (1M context) --- .../TrajectoryViewer.cpp | 215 +++++++++++++++++- .../TrajectoryViewerShaders.h | 9 +- calib_core/include/CalibCore/Camera.h | 8 + calib_core/src/Camera.cpp | 21 ++ calib_core/tests/test_camera.cpp | 44 ++++ 5 files changed, 283 insertions(+), 14 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index d37dac78..88511202 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -234,6 +234,24 @@ struct AppState Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame Roi roi; + // Free-form counterpart of `roi`: a per-pixel mask image whose rejected + // pixels are excluded from coloring. This is what it takes to drop the + // operator/backpack a 360 rig has in frame permanently -- no rectangle can + // cut that out without cutting out the scene with it. Kept at whatever + // resolution the file had, strictly 0/255 (see loadMask), and resampled to + // the working image size where it is used: images are read at s.imgScale, + // so there is no one size to pre-fit it to. + // + // Coloring only -- the images written by the ROS 2 and COLMAP exports are + // not masked. + cv::Mat mask; // empty = none loaded + bool maskEnabled = false; // acted on only while `mask` is non-empty + bool maskInvert = false; // UI state; loadMask and the toggle flip `mask` itself + char maskBuf[512] = {}; + float maskRejectFrac = 0.f; // share of pixels the mask drops, for the UI + bool showMaskOverlay = true; // tint the rejected area over the image preview + Texture2D maskTex = {}; // that tint, RGBA, built by refreshMaskDerived + bool maskTexValid = false; bool calibLoaded = false; int imgW = 4656, imgH = 3496; // overwritten from the first scanned image by loadImages() @@ -705,7 +723,18 @@ static void loadCloud(AppState& s) // images at the right pixel; with all-zero coefficients it reduces exactly // to the ideal pinhole. const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + // The ROI is given in full-resolution image pixels (see calib::Roi), but + // the iu/iv probe() tests it against below are pixels of the images as + // they are actually read, i.e. at s.imgScale -- so the rectangle is scaled + // exactly as the intrinsics above are. + const Roi roiS = scaleRoi(s.roi, s.imgScale); const int64_t offNs = imageTimeOffsetNs(s); + // The mask arrives at the resolution of whatever file was loaded while the + // images are read at s.imgScale, so it is resampled to the size the frames + // actually have -- filled lazily below, on the first image probed, since + // that size isn't known until one has been read. + const bool haveMask = !s.mask.empty(); + cv::Mat maskFit; // Every image of a chunk is held in memory at once (multiImgColoring), so // for large frames the scale is what keeps that bounded. auto readImage = [&](const std::string& path) @@ -948,14 +977,27 @@ static void loadCloud(AppState& s) } if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) return h; - // point projects into this image — record ROI membership so - // the "In ROI" render mode can show it, independent of whether - // the ROI filter is currently enabled. - bool haveRoi = s.roi.w > 0 && s.roi.h > 0; - bool insideRoi = !haveRoi || (iu >= s.roi.x && iu < s.roi.x + s.roi.w && iv >= s.roi.y && iv < s.roi.y + s.roi.h); - h.inRoiF = insideRoi ? 1.f : 0.f; - // outside the region of interest? leave the point uncolored - if (s.roi.enabled && !insideRoi) + // point projects into this image — record ROI/mask membership + // so the "In ROI / mask" render mode can show it, independent + // of whether either filter is currently enabled. + bool haveRoi = roiS.w > 0 && roiS.h > 0; + bool insideRoi = !haveRoi || (iu >= roiS.x && iu < roiS.x + roiS.w && iv >= roiS.y && iv < roiS.y + roiS.h); + bool insideMask = true; + if (haveMask) + { + // INTER_NEAREST, so the mask stays strictly 0/255: a + // bilinear resize would invent half-masked pixels along + // every edge, which the test below would then silently + // round one way. Every frame of a session is the same + // size, so this resizes once. + if (maskFit.cols != e.img.cols || maskFit.rows != e.img.rows) + cv::resize(s.mask, maskFit, e.img.size(), 0, 0, cv::INTER_NEAREST); + insideMask = maskFit.at(iv, iu) != 0; + } + h.inRoiF = (insideRoi && insideMask) ? 1.f : 0.f; + // outside the region of interest, or masked out? leave the + // point uncolored + if ((s.roi.enabled && !insideRoi) || (s.maskEnabled && !insideMask)) 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]); @@ -1292,6 +1334,89 @@ static void loadCalib(AppState& s) s.status = "Calibration loaded"; } +// Rebuilds what is derived from s.mask: the rejected-pixel share the UI +// reports, and the translucent red overlay drawn over the image preview. Call +// after anything that changes the mask. Main thread only -- it creates a GL +// texture. +static void refreshMaskDerived(AppState& s) +{ + if (s.maskTexValid) + { + UnloadTexture(s.maskTex); + s.maskTexValid = false; + } + if (s.mask.empty()) + { + s.maskRejectFrac = 0.f; + return; + } + const int total = s.mask.rows * s.mask.cols; + const int kept = cv::countNonZero(s.mask); + s.maskRejectFrac = total ? (float)(total - kept) / (float)total : 0.f; + + // The overlay only has to read correctly in a preview pane, so it is capped + // well below the frame size a 360 rig produces rather than uploading a + // 22 MP texture to show a hand-painted blob. + cv::Mat m = s.mask; + const int kMaxSide = 1024; + const int longSide = std::max(m.cols, m.rows); + if (longSide > kMaxSide) + cv::resize(s.mask, m, cv::Size(), (double)kMaxSide / longSide, (double)kMaxSide / longSide, cv::INTER_NEAREST); + cv::Mat rgba(m.rows, m.cols, CV_8UC4); + for (int y = 0; y < m.rows; ++y) + { + const uint8_t* srcRow = m.ptr(y); + cv::Vec4b* dstRow = rgba.ptr(y); + for (int x = 0; x < m.cols; ++x) + dstRow[x] = srcRow[x] ? cv::Vec4b(0, 0, 0, 0) : cv::Vec4b(255, 40, 40, 110); + } + Image ri = { rgba.data, rgba.cols, rgba.rows, 1, PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 }; + s.maskTex = LoadTextureFromImage(ri); + s.maskTexValid = s.maskTex.id > 0; +} + +// Loads the mask image named by s.maskBuf. Any format OpenCV reads is accepted +// and reduced to one 8-bit channel thresholded at 128, so a hand-painted +// black/white PNG, a grayscale one and an RGB one all behave identically: a +// pixel is either kept or dropped, never partly -- and a jpeg mask's +// compression noise can't leak in as almost-black. White keeps the pixel, +// black drops it, unless "Invert mask" is on. +// +// No particular resolution is required: the mask is resampled to whatever the +// frames turn out to be (loadCloud), so one drawn over a downscaled copy of a +// frame works as well as a full-resolution one. +static void loadMask(AppState& s) +{ + if (!s.maskBuf[0]) + { + s.status = "No mask file selected"; + return; + } + cv::Mat img = cv::imread(s.maskBuf, cv::IMREAD_GRAYSCALE); + if (img.empty()) + { + s.status = std::string("Failed to read mask: ") + s.maskBuf; + return; + } + cv::threshold(img, s.mask, 128, 255, s.maskInvert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY); + s.maskEnabled = true; + refreshMaskDerived(s); + char msg[160]; + std::snprintf(msg, sizeof(msg), "Mask loaded: %dx%d, %.1f%% masked out", s.mask.cols, s.mask.rows, s.maskRejectFrac * 100.f); + s.status = msg; +} + +// Drops the mask entirely, as opposed to unticking "Image mask", which keeps +// it loaded and ready to re-enable. +static void clearMask(AppState& s) +{ + s.mask.release(); + s.maskEnabled = false; + s.maskBuf[0] = '\0'; + refreshMaskDerived(s); + s.status = "Mask cleared"; +} + static void exportLAZ(AppState& s) { if (s.exportCloud.empty()) @@ -1525,6 +1650,21 @@ static bool isCameraDir(const fs::path& dir) // swapped, so the trajectory and the loaded cloud survive); any other directory is this // app's session (LIO result dir). A dropped *.json is treated as a calibration file. Used by // the drag & drop handler in main()'s loop below. +static void actionOpenMask(AppState& s) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select image mask", mandeye::fd::ImageFilter); + if (!path.empty()) + { + setBuf(s.maskBuf, sizeof(s.maskBuf), path); + loadMask(s); + } +} + +// Drag & drop equivalent of actionSelectLioResultDir()/actionOpenCalibration(): a dropped +// directory is this app's session (LIO result dir), and unlike the menu action it loads +// immediately instead of waiting for the "Load session" button, since a drop is already an +// explicit "load this" gesture. A dropped *.json is treated as a calibration file. Used by the +// drag & drop handler in main()'s loop below. static void handleDroppedPath(AppState& s, const std::string& path) { if (fs::is_directory(path)) @@ -1560,6 +1700,14 @@ static void handleDroppedPath(AppState& s, const std::string& path) setBuf(s.calibBuf, sizeof(s.calibBuf), path); loadCalib(s); } + else if (ext == ".png" || ext == ".bmp" || ext == ".jpg" || ext == ".jpeg") + { + // The only single image this app takes as input is a mask -- camera + // frames arrive as the session's whole CAMERA_0 directory, never one + // file at a time. + setBuf(s.maskBuf, sizeof(s.maskBuf), path); + loadMask(s); + } else { s.status = "Unsupported dropped file: " + path; @@ -2307,6 +2455,8 @@ int main(int argc, char* argv[]) ImGui::Separator(); if (ImGui::MenuItem("Open Calibration...", "Ctrl+Shift+C")) actionOpenCalibration(s); + if (ImGui::MenuItem("Open Image Mask...")) + actionOpenMask(s); ImGui::Separator(); if (ImGui::MenuItem("Export Colored Point Cloud (LAS/LAZ)...", "Ctrl+S")) actionExportColoredLAZ(s); @@ -2373,7 +2523,7 @@ int main(int argc, char* argv[]) s.colorMode = 1; if (ImGui::MenuItem("Camera ID", nullptr, s.colorMode == 2)) s.colorMode = 2; - if (ImGui::MenuItem("In ROI", nullptr, s.colorMode == 3)) + if (ImGui::MenuItem("In ROI / mask", nullptr, s.colorMode == 3)) s.colorMode = 3; } ImGui::EndMenu(); @@ -2569,7 +2719,10 @@ int main(int argc, char* argv[]) } } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Only points projecting inside the ROI get colored.\nDrawn on the image preview."); + ImGui::SetTooltip( + "Only points projecting inside the ROI get colored.\n" + "Full-resolution image pixels, scaled along with Image scale.\n" + "Drawn on the image preview."); if (s.roi.enabled) { ImGui::PopItemWidth(); @@ -2581,6 +2734,37 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); } + + ImGui::Separator(); + ImGui::BeginDisabled(s.mask.empty()); + ImGui::Checkbox("Image mask", &s.maskEnabled); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip( + "Points projecting onto a masked-out (black) pixel stay uncolored.\n" + "Free-form counterpart of the ROI -- for the operator, the rig itself,\n" + "the sky. Coloring only: exported images are never masked.\n" + "Re-run Load cloud to apply."); + ImGui::Text("Mask image:"); + ImGui::InputText("##mask", s.maskBuf, sizeof(s.maskBuf)); + if (ImGui::Button("Load mask", ImVec2(-1, 0))) + loadMask(s); + if (!s.mask.empty()) + { + ImGui::TextDisabled("%dx%d, %.1f%% masked out", s.mask.cols, s.mask.rows, s.maskRejectFrac * 100.f); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Resampled to the image size in use; any resolution with the same framing works."); + if (ImGui::Checkbox("Invert mask", &s.maskInvert)) + { + // The mask is strictly 0/255, so flipping it in place is + // exact and its own inverse -- no need to re-read the file. + cv::bitwise_not(s.mask, s.mask); + refreshMaskDerived(s); + } + ImGui::Checkbox("Show mask on preview", &s.showMaskOverlay); + if (ImGui::Button("Clear mask", ImVec2(-1, 0))) + clearMask(s); + } } ImGui::PopItemWidth(); } @@ -2840,6 +3024,15 @@ int main(int argc, char* argv[]) tint); } + // masked-out pixels, tinted red over the same rect as the photo (the + // mask is resampled wherever it is used, so a mask of a different + // resolution is expected and stretches to fit here too) + if (s.maskEnabled && s.showMaskOverlay && s.maskTexValid) + { + ImGui::SetCursorScreenPos(imgPos); + rlImGuiImageSize(&s.maskTex, 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) { @@ -2883,6 +3076,8 @@ int main(int argc, char* argv[]) UnloadTexture(s.imgViewTex); if (s.intensityProjTexValid) UnloadTexture(s.intensityProjTex); + if (s.maskTexValid) + UnloadTexture(s.maskTex); s.cloud.unload(); if (s.shaderOk) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h index bd24a1c8..a8907f43 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h @@ -9,14 +9,14 @@ namespace trajectory_viewer_shaders { - // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI + // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI/mask 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 +layout(location = 4) in float inRoi; // 1=kept by ROI+mask, 0=rejected by either, -1=projects into no image uniform mat4 mvp; uniform float pointSize; uniform int drawDecim; @@ -86,8 +86,9 @@ void main() { } 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). + // ROI/mask membership: green = a pixel the ROI and the image mask both + // keep, red = projects into an image but is rejected by one of them, + // dim gray = projects into no image (spatial context). if (fragInRoi < 0.0) finalColor = vec4(0.28, 0.28, 0.28, 1.0); else diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 9b4e27c2..2af992e1 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -141,6 +141,14 @@ namespace calib // same geometry. Everything else (distortion, model) is carried over. Intrinsics scaleIntrinsics(const Intrinsics& K, float s); + // The same rectangle on an image resampled by `s`, so a ROI -- which is + // given in full-resolution pixels, see Roi above -- can be tested against + // the pixels of a downscaled copy. Both edges are scaled rather than the + // width alone, so abutting rectangles stay abutting. An empty (w/h == 0) + // ROI comes back unchanged, and a non-empty one never scales down to + // empty, which every caller would read as "no ROI set". + Roi scaleRoi(const Roi& r, float s); + // Project a point from LiDAR frame to image pixel (u, v). // R_wc = camera orientation in world, t = camera position in world. // diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 94aae7be..213edb66 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -92,6 +92,27 @@ Intrinsics scaleIntrinsics(const Intrinsics& K, float s) { return out; } +Roi scaleRoi(const Roi& r, float s) { + Roi out = r; + if (r.w <= 0 || r.h <= 0) { + return out; // w/h == 0 is the "no ROI set" sentinel; leave it alone + } + const int x0 = static_cast(std::lround(r.x * s)); + const int y0 = static_cast(std::lround(r.y * s)); + const int x1 = static_cast(std::lround((r.x + r.w) * s)); + const int y1 = static_cast(std::lround((r.y + r.h) * s)); + out.x = x0; + out.y = y0; + // Both edges are rounded and then subtracted, rather than the width being + // scaled on its own, so two abutting rectangles cannot come back + // overlapping. The clamp keeps a rectangle too small to survive the scale + // at one pixel: collapsing it to w/h == 0 would read as "no ROI" and + // silently pass everything the ROI was there to reject. + out.w = std::max(1, x1 - x0); + out.h = std::max(1, y1 - y0); + return out; +} + bool projectPoint(float px, float py, float pz, const Intrinsics& K, const Eigen::Matrix3f& R_wc, diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 3708ffc2..c237d7dc 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -370,6 +370,50 @@ TEST_CASE("pinhole: rejects points at or behind the camera plane") CHECK_FALSE(projectPoint(1, 2, 0, K, kIdentity, kOrigin, u, v, depth)); } +// ── scaleRoi ────────────────────────────────────────────────────────────────── + +TEST_CASE("scaleRoi: a half-size image halves the rectangle") +{ + Roi r{ true, 100, 200, 40, 60 }; + Roi h = scaleRoi(r, 0.5f); + CHECK(h.enabled); + CHECK(h.x == 50); + CHECK(h.y == 100); + CHECK(h.w == 20); + CHECK(h.h == 30); +} + +TEST_CASE("scaleRoi: abutting rectangles stay abutting") +{ + // Scaling the width on its own would give both of these w == 2 and make + // them overlap at x == 2; rounding the two edges and subtracting cannot. + Roi a{ true, 1, 1, 3, 3 }; + Roi b{ true, 4, 4, 3, 3 }; + Roi as = scaleRoi(a, 0.5f); + Roi bs = scaleRoi(b, 0.5f); + CHECK(as.x + as.w == bs.x); + CHECK(as.y + as.h == bs.y); +} + +TEST_CASE("scaleRoi: a non-empty rectangle never scales down to empty") +{ + // w/h == 0 reads as "no ROI set", i.e. accept everything -- the exact + // opposite of what a ROI this small is asking for. + Roi tiny{ true, 10, 10, 2, 2 }; + Roi s = scaleRoi(tiny, 0.1f); + CHECK(s.w >= 1); + CHECK(s.h >= 1); +} + +TEST_CASE("scaleRoi: an unset rectangle is left alone") +{ + Roi none; + Roi s = scaleRoi(none, 0.5f); + CHECK_FALSE(s.enabled); + CHECK(s.w == 0); + CHECK(s.h == 0); +} + // ── scaleIntrinsics ─────────────────────────────────────────────────────────── TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") From 9145f7cdcd69e86de6dcb7bd483358c1febf6240 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 14 Sep 2026 18:37:06 +0200 Subject: [PATCH 07/22] Reject Mei directions past the fold-back angle, not just past the denominator The Mei domain guard was "Xs.z + xi > 0", which is the right limit only for xi <= 1. For xi > 1 -- both rigs in use are calibrated at xi = 2.0 -- that expression never goes non-positive (Xs.z + 2 stays in [1, 3] for every direction on the sphere), so the guard never fired and every direction, including straight backwards, produced a finite pixel. It is worse than an unfiltered projection: r(theta) = sin/(cos + xi) is only injective up to its turning point at cos(theta) = -1/xi, and past that the radius shrinks again, folding far-off-axis directions back onto real pixels rather than pushing them out of frame. At theta = 180 deg the radius is exactly 0, so a point directly behind the camera lands on (cx, cy) -- dead centre. That is the "masked points still get colours" symptom: geometry behind the rig painting into the middle of the image. The limit both regimes share: xi <= 1: Xs.z > -xi (blow-up; reduces to Pinhole's pc.z > 0 at xi = 0) xi > 1: Xs.z > -1/xi (fold-back) applied in calib::projectPoint and in both of camera_lidar_calibration's GLSL shaders -- kProjVS's clip weight becomes the distance inside that dome rather than the denominator, and kPointVS's fragCamDepth carries the same sign for the Camera-RGB "seen" test. For a real lens the calibrated image circle is tighter still (the 3840x3840 xi = 2.0 front lens reaches its edge at theta = 101.2 deg, i.e. ~202 deg FOV, while the fold is at 120 deg), so the bounds check remains the binding constraint in normal use; this only removes the directions that were bypassing it entirely. Tests replace a case that asserted the old behaviour outright ("xi >= 1 covers the full sphere -- straight behind still succeeds") with the cutoff at acos(-1/xi), acos(-xi) and the xi = 0 pinhole half-space. The helper building those directions needs an explicit -> Eigen::Vector3f: with auto it deduces an expression template holding a reference to the temporary, which dangles and made the tests report nonsense. Co-Authored-By: Claude Opus 5 (1M context) --- .../RendererShaders.h | 23 ++++----- calib_core/src/Camera.cpp | 18 ++++--- calib_core/tests/test_camera.cpp | 51 ++++++++++++------- 3 files changed, 55 insertions(+), 37 deletions(-) diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index bd3e2df7..21953183 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -53,13 +53,11 @@ void main() { float n = length(pc); vec3 Xs = pc / max(n, 1e-6); float denom = Xs.z + xi; - // denom>0 is this model's actual "in front of the camera" test -- - // it reduces exactly to Pinhole's pc.z>0 when xi==0 (Xs.z and pc.z - // then share a sign, n>0). fragCamDepth only needs to carry that - // sign here (kPointFS only tests fragCamDepth > 0.0), not a real - // depth -- unlike kProjVS below, this shader has no depthRange - // slider to feed a physical distance to. - fragCamDepth = denom; + // Validity domain, same rule as calib::projectPoint: the projection + // folds back past cos(theta) = -1/xi for xi > 1, and blows up past + // -xi otherwise. fragCamDepth only carries this sign (kPointFS tests + // fragCamDepth > 0.0), not a real depth. + fragCamDepth = Xs.z - ((xi > 1.0) ? -1.0 / xi : -xi); vec2 xy = Xs.xy / denom; float r2 = dot(xy, xy); float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; @@ -114,10 +112,10 @@ void main() { // in raylib coords, converted back to lidar frame here. Pinhole (model==0): // rational+tangential distortion (zeros when rectified), w = z_cam so the // hardware clip rejects points behind the camera. Mei (model==2): unified- - // sphere + polynomial distortion (mirrors MeiCamera::Project), w = Xs.z+xi - // (the model's own "in front of the camera" test -- reduces exactly to - // Pinhole's z_cam when xi==0), so the hardware clip rejects points outside - // its valid dome the same way Pinhole rejects points behind it. + // sphere + polynomial distortion (mirrors MeiCamera::Project), with w the + // distance inside the model's valid dome -- Xs.z + min(xi, 1/xi) -- so the + // hardware clip drops both the blow-up (xi <= 1) and the fold-back + // (xi > 1, where far-off-axis directions otherwise re-enter the image). inline constexpr const char* kProjVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; @@ -157,7 +155,8 @@ void main() { float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; d = xy*radial + vec2(2.0*pTan.x*xy.x*xy.y + pTan.y*(r2 + 2.0*xy.x*xy.x), pTan.x*(r2 + 2.0*xy.y*xy.y) + 2.0*pTan.y*xy.x*xy.y); - w = denom; // NOT n -- see the block comment above kProjVS + // >0 exactly inside the valid dome -- see the block comment above kProjVS + w = Xs.z - ((xi > 1.0) ? -1.0 / xi : -xi); } else { fragDepth = pc.z; vec2 n = pc.xy / max(pc.z, 1e-6); diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 213edb66..09ec8e4d 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -154,14 +154,16 @@ bool projectPoint(float px, float py, float pz, depth = pc.norm(); if (depth < 1e-4f) return false; - // Xs.z + xi > 0 is this model's own validity domain (MeiCamera:: - // Project divides by exactly this with no guard of its own) -- it - // reduces exactly to the familiar Pinhole "pc.z > 0" test when - // xi == 0 (Xs.z and pc.z then share a sign, depth > 0). Without - // this, a point behind the camera can still land inside the image - // bounds (the projection isn't injective outside its valid domain) - // and get silently treated as visible. - if (pc.z() / depth + K.xi <= 0.f) return false; + // Validity domain. r(theta) = sin/(cos+xi) is only injective up to + // its turning point at cos(theta) = -1/xi; past it the radius shrinks + // again and far-off-axis directions FOLD BACK onto valid pixels -- + // at theta = 180 deg exactly onto (cx, cy). For xi <= 1 the + // denominator blows up first, so "Xs.z + xi > 0" is the limit there. + // xi <= 1: Xs.z > -xi (reduces to Pinhole's pc.z > 0 at xi = 0) + // xi > 1: Xs.z > -1/xi + // MeiCamera::Project has no guard of its own, so it belongs here. + const float zMin = (K.xi > 1.f) ? -1.f / K.xi : -K.xi; + if (pc.z() / depth <= zMin) return false; MeiCamera cam; cam.fx = K.fx; cam.fy = K.fy; cam.cx = K.cx; cam.cy = K.cy; diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index c237d7dc..705bd758 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -266,31 +266,48 @@ TEST_CASE("mei: a point on the camera itself is rejected") TEST_CASE("mei: a point behind the camera is rejected, not silently mis-projected") { - // Regression: MeiCamera::Project has no domain guard of its own (it - // divides by Xs.z+xi unconditionally), so a point behind the camera - // does NOT reliably land outside the image -- the projection isn't - // injective past the model's valid dome. projectPoint() must reject it - // itself rather than return a plausible-looking wrong pixel. + // MeiCamera::Project has no domain guard of its own, and past the valid + // dome the projection is not injective -- it folds far-off-axis + // directions back onto real pixels instead of pushing them out of frame. float u, v, depth; - SUBCASE("xi >= 1 covers the full sphere -- straight behind still succeeds") + // Direction at `deg` from the optical axis, in the plane y = 0. The + // explicit return type matters: `auto` would deduce an Eigen expression + // template holding a reference to the temporary, and dangle. + auto at = [](float deg) -> Eigen::Vector3f { - // mei()'s xi = 1.2: Xs.z + xi ranges over [xi-1, xi+1] = [0.2, 2.2] - // for any direction (Xs.z in [-1, 1]), always positive, so no - // direction is ever excluded at this xi. - const Intrinsics K = mei(); - CHECK(projectPoint(0, 0, -10, K, kIdentity, kOrigin, u, v, depth)); + const float r = deg * float(M_PI) / 180.f; + return Eigen::Vector3f(std::sin(r), 0.f, std::cos(r)) * 10.f; + }; + auto projects = [&](const Intrinsics& K, const Eigen::Vector3f& p) + { return projectPoint(p.x(), p.y(), p.z(), K, kIdentity, kOrigin, u, v, depth); }; + + SUBCASE("xi > 1: the limit is the fold-back angle, acos(-1/xi)") + { + const Intrinsics K = mei(); // xi = 1.2 -> 146.44 deg + CHECK(projects(K, at(0.f))); + CHECK(projects(K, at(145.f))); + CHECK_FALSE(projects(K, at(148.f))); + // Straight behind used to land on (cx, cy) -- the whole point of the guard. + CHECK_FALSE(projects(K, at(180.f))); } - SUBCASE("xi < 1 excludes a cone behind the camera") + SUBCASE("xi <= 1: the limit is where the denominator blows up, acos(-xi)") { Intrinsics K = mei(); - K.xi = 0.5f; // Xs.z <= -0.5 is now out of domain + K.xi = 0.5f; // -> 120 deg + CHECK(projects(K, at(0.f))); + CHECK(projects(K, at(119.f))); + CHECK_FALSE(projects(K, at(121.f))); + CHECK_FALSE(projects(K, at(180.f))); + } - // Straight behind: Xs.z = -1, so Xs.z + xi = -0.5 <= 0. - CHECK_FALSE(projectPoint(0, 0, -10, K, kIdentity, kOrigin, u, v, depth)); - // Straight ahead is unaffected. - CHECK(projectPoint(0, 0, 10, K, kIdentity, kOrigin, u, v, depth)); + SUBCASE("xi = 0 reduces to the pinhole half-space") + { + Intrinsics K = mei(); + K.xi = 0.f; + CHECK(projects(K, at(89.f))); + CHECK_FALSE(projects(K, at(91.f))); } } From f8fd5d8af2b743d19a45f444d9b3ea08632fc6bc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 14 Sep 2026 18:45:33 +0200 Subject: [PATCH 08/22] Drop the inline off-axis cutoff the rebase reinstated in the trajectory viewer Upstream's fix for colorizing invalid points (#527) applies the rational model's fold-back cutoff inline in TrajectoryViewer.cpp, against local d_k1..d_k6 distortion variables. This branch had already moved that same logic into calib_core when coloring switched to calib::projectPoint, deleting those locals with the rest of the inline distortion math. Both sides merged without a textual conflict, leaving a call to maxValidRadiusSq() with arguments that no longer exist -- so the app did not compile after the rebase. Removes the reinstated copy of the function and its call site. The cutoff itself is unchanged in behaviour: calib::projectPoint runs it internally (Camera.cpp's cachedMaxValidRadiusSq), which is where the equirectangular commit deliberately put it so every caller is covered rather than this one call site. rMaxSq had no other reader. Co-Authored-By: Claude Opus 5 (1M context) --- .../TrajectoryViewer.cpp | 40 ++----------------- 1 file changed, 3 insertions(+), 37 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 88511202..0a0e1b07 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -651,40 +651,9 @@ static void loadSession(AppState& s) (mrp.empty() ? " (no MRP)" : " +MRP") + " — press Load cloud"; } -// Radius (in normalized camera coords, squared) past which the rational distortion model -// stops being usable. r -> r*radial(r) is only injective up to its turning point; beyond it -// the model folds, so directions far outside the lens' actual field of view map back onto -// valid pixel coordinates. With a strongly-fitted model that is not a corner case: for the -// intrinsics this app is used with, a direction 56 deg off the optical axis lands mid-image -// and one at 60 deg lands exactly on the principal point, painting whatever is at the centre -// of the frame onto geometry the camera never saw. The projection alone cannot tell such a -// fold-back from a genuine hit, so find the turning point once and reject everything past -// it. Scanned numerically -- the turning point of a 6th-order rational function has no -// useful closed form. It always lies outside the image itself (otherwise the calibration -// could not reach its own corners), so no legitimate pixel is lost. -static float maxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) -{ - auto g = [&](float r) - { - float r2 = r * r; - float den = 1.f + (k4 + (k5 + k6 * r2) * r2) * r2; - if (std::fabs(den) < 1e-9f) - return -1.f; // pole -- certainly past the turning point - return r * (1.f + (k1 + (k2 + k3 * r2) * r2) * r2) / den; - }; - // 8.0 == tan(83 deg), wider than any lens this app sees. A distortion-free model is - // monotonic everywhere and so keeps the whole range, i.e. no behaviour change. - const float kLimit = 8.f, kStep = 0.005f; - float prev = 0.f; - for (float r = kStep; r <= kLimit; r += kStep) - { - float cur = g(r); - if (cur <= prev) - return (r - kStep) * (r - kStep); - prev = cur; - } - return kLimit * kLimit; -} +// The off-axis fold-back cutoff that used to live here now lives in +// calib_core (Camera.cpp's maxValidRadiusSq), applied inside +// calib::projectPoint so every caller gets it -- not just this one. static void loadCloud(AppState& s) { @@ -748,9 +717,6 @@ static void loadCloud(AppState& s) } return img; }; - // Off-axis cutoff for the model above -- see maxValidRadiusSq(). - const float rMaxSq = maxValidRadiusSq(d_k1, d_k2, d_k3, d_k4, d_k5, d_k6); - auto packGray = [](float intensity) -> float { uint8_t g = (uint8_t)(std::min(1.f, std::max(0.f, intensity)) * 255.f); From 4933058dee1254f8f1fe8725418345d5c03be30b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:12:30 +0200 Subject: [PATCH 09/22] Ignore local build directories and rosbags.zip build-ceres/ and rosbags.zip sat untracked and unignored, so `git add -A` would sweep 424 MB of build output into a commit. /build-* follows the anchored style of the existing /build2 and /build3 entries and covers future variants; /rosbags.zip is anchored to the root so the tracked rosbags/ source directory is unaffected. Co-Authored-By: Claude Opus 5 (1M context) --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index 4b81de55..105973fd 100644 --- a/.gitignore +++ b/.gitignore @@ -72,6 +72,10 @@ imgui.ini /.gitmodules /build2 /build3 +/build-* + +# local test data +/rosbags.zip # deploy_mandeye.bat output /deploy From 5a9e17416ee19d807de7956a2de252cd1a7aae6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:12:37 +0200 Subject: [PATCH 10/22] Invert the extrinsics manual_color reads from a calibration JSON saveCalibrationJson writes camera_pose.inverse(), which is the R_wc/C the shared schema calls for, but loadCalibrationJson read those fields straight back into camera_pose without inverting. The two are not the same thing: the *.reg file stores camera_pose verbatim (the LiDAR-to-camera transform, p_cam = M*p), while the JSON's camera_rotation_matrix_in_world and camera_position_in_world_xyz are R_wc and C (p_cam = R_wc^T * (p - C)). Reading them the same way left the pose inverted, which mis-projects plausibly rather than failing, and meant loading a file this app had just saved did not reproduce the pose it saved. Load now converts, so the pair round-trips exactly. Co-Authored-By: Claude Opus 5 (1M context) --- apps/manual_color/manual_color.cpp | 33 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 8777a968..df0d98d6 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -925,13 +925,14 @@ void removeCorrespondence(size_t i) // Only extrinsics (+ intrinsics width/height) round-trip here -- this app is // fixed to the equirectangular model. // -// NOTE the two functions are NOT each other's inverse: loadCalibrationJson -// reads "extrinsics" straight into camera_pose (camera-to-LiDAR, matching -// this app's own "load camera to lidar relative pose (*.reg)" button and -// camera_lidar_trajectory_viewer's convention), while saveCalibrationJson -// below writes camera_pose.inverse() (LiDAR-to-camera) per request. Loading a -// file this app just saved will therefore NOT reproduce the same camera_pose -// -- flag this if that round-trip turns out to matter. +// Note the JSON and the *.reg file use opposite conventions, so they are read +// differently. The JSON's camera_rotation_matrix_in_world/ +// camera_position_in_world_xyz are R_wc and C (camera orientation and position +// in the LiDAR frame, giving p_cam = R_wc^T * (p - C)), while this app's +// camera_pose -- and the *.reg file, which stores it verbatim -- is the +// LiDAR-to-camera transform itself (p_cam = M*p). The two are inverses, so +// both functions below convert; reading the JSON straight into camera_pose +// would give a plausible-looking but wrong projection rather than a failure. bool loadCalibrationJson(const std::string& path) { std::ifstream f(path); @@ -962,10 +963,11 @@ bool loadCalibrationJson(const std::string& path) for (int c = 0; c < 3; ++c) R(r, c) = m[r][c].get(); - Eigen::Affine3d pose = Eigen::Affine3d::Identity(); - pose.linear() = R; - pose.translation() = Eigen::Vector3d(t[0].get(), t[1].get(), t[2].get()); - SystemData::camera_pose = pose; + // (R, t) are R_wc and C; camera_pose is their inverse -- see above. + Eigen::Affine3d wc = Eigen::Affine3d::Identity(); + wc.linear() = R; + wc.translation() = Eigen::Vector3d(t[0].get(), t[1].get(), t[2].get()); + SystemData::camera_pose = wc.inverse(); return true; } @@ -979,10 +981,8 @@ bool saveCalibrationJson(const std::string& path) ji["height"] = SystemData::imageHeight; j["intrinsics"] = ji; - // Exported inverted: camera_pose is this app's camera-to-LiDAR transform - // (see the "save camera to lidar relative pose (*.reg)" button above, - // which dumps it un-inverted), so its inverse is the LiDAR-to-camera - // transform -- write that under the same field names. + // camera_pose is the LiDAR-to-camera transform (the *.reg button above + // dumps it un-inverted); its inverse is the R_wc/C the schema wants. const Eigen::Affine3d inv = SystemData::camera_pose.inverse(); const Eigen::Vector3d t = inv.translation(); const Eigen::Matrix3d R = inv.linear(); @@ -1611,7 +1611,8 @@ void display() std::cerr << "Cannot save calibration: " << path << std::endl; } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Same calibration JSON schema as camera_lidar_trajectory_viewer\n(model/intrinsics/extrinsics) -- interchangeable with it."); + ImGui::SetTooltip( + "Same calibration JSON schema as camera_lidar_trajectory_viewer\n(model/intrinsics/extrinsics) -- interchangeable with it."); imagePicker("ImagePicker", (ImTextureID)tex1, SystemData::pointPickedImage, picked3DPoints); From 842e9dcd335763e474151fc7b7bc01ff744525c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:12:51 +0200 Subject: [PATCH 11/22] Drop calib_core's yaml-cpp and OpenCV dependencies, and tidy the contribution calib_core had picked up find_package(yaml-cpp REQUIRED), but nothing else in the repo uses yaml-cpp, no CI workflow installs it, and Windows/macOS take their dependencies from pre-downloaded binaries with no equivalent path for it -- so the build configured only where it happened to be installed already. cv::FileStorage cannot substitute (it rejects plain YAML without a %YAML:1.0 header), so LoadMeiCamera now parses this rig's flat camera_info.yaml directly, covered by tests. MeiCamera::Project takes Eigen rather than cv::Point, which drops OpenCV from calib_core's public interface and restores its "nothing but Eigen/LASzip/std" rule. Also: - modelToString/modelFromString were declared static in Camera.h. At namespace scope that is internal linkage, so every includer warned about an unused function and the definitions in Camera.cpp were never exported -- App.cpp's call sites would not have linked. Their -Wswitch rationale moved across with them. - Removed MeiCamera::Unproject and AppState::imageTimeOffsetMs, neither of which had any caller; imageTimeOffsetMs also duplicated timeOffsetSec and documented behavior it did not have. - Camera.h's CameraModel comment claimed camera_lidar_calibration neither reads nor writes the "model" key, which all three of loadIntrinsics, loadCalibration and saveCalibration now do. - Fixed 55 clang-format 21 violations across App.cpp, TrajectoryViewer.cpp and manual_color.cpp; apps/ is clean again under the version CI runs. - Camera.h's documentation is now doxygen, matching the //! and @param style the rest of the codebase uses. - Trimmed comments that restated the code or speculated about future work, and dropped a personal absolute path and a pointer to a file that does not exist. Co-Authored-By: Claude Opus 5 (1M context) --- apps/camera_lidar_calibration/App.cpp | 89 +++---- apps/camera_lidar_calibration/App.h | 24 +- .../RosExport.cpp | 36 ++- .../TrajectoryViewer.cpp | 129 ++++------ calib_core/CMakeLists.txt | 62 +---- calib_core/include/CalibCore/Camera.h | 226 +++++++++--------- .../CalibCore/CameraCalibrationSolver.h | 46 ++-- calib_core/include/CalibCore/MeiCamera.h | 44 ++-- calib_core/src/Camera.cpp | 50 ++-- calib_core/src/CameraCalibrationSolverMei.cpp | 48 ++-- calib_core/src/MeiCamera.cpp | 197 +++++++++------ calib_core/tests/CMakeLists.txt | 19 +- calib_core/tests/test_camera.cpp | 95 +++++++- 13 files changed, 534 insertions(+), 531 deletions(-) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index cd33ebdd..d5875a51 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -19,30 +19,6 @@ #include #include -// ── model <-> string, for the calibration JSON's "model" key ───────────────── -// No `default:` case on purpose: -Wswitch (this target builds with -Wall -// -Wextra) then flags a future CameraModel enumerator added here without a -// matching string, instead of it silently falling through to "pinhole". -static const char* modelToString(CameraModel m) -{ - switch (m) - { - case CameraModel::Pinhole: return "pinhole"; - case CameraModel::Equirectangular: return "equirectangular"; - case CameraModel::Mei: return "mei"; - } - return "pinhole"; -} - -static CameraModel modelFromString(const std::string& s) -{ - if (s == "equirectangular") - return CameraModel::Equirectangular; - if (s == "mei") - return CameraModel::Mei; - return CameraModel::Pinhole; -} - // ── AppState::rebuildImageTexture ───────────────────────────────────────────── void AppState::rebuildImageTexture() { @@ -54,11 +30,9 @@ void AppState::rebuildImageTexture() // initUndistortRectifyMap assumes OpenCV's rational pinhole model -- // running it for Mei (or Equirectangular) would silently mis-warp the - // image instead of undistorting it. Mei has no "undistort to pinhole" - // step here (that would need resampling through MeiCamera::Unproject - // into a virtual pinhole, not implemented), so its image is always - // shown raw; the projection overlay/GPU shaders apply its distortion - // directly to the raw image instead (see Renderer.cpp/RendererShaders.h). + // image rather than undistort it. Those models are shown raw instead, + // with the projection overlay and GPU shaders applying their distortion + // directly to the raw image (see Renderer.cpp/RendererShaders.h). if (intrinsicsLoaded && intrinsics.model == CameraModel::Pinhole) { cv::Mat K = (cv::Mat_(3, 3) << intrinsics.fx, 0, intrinsics.cx, 0, intrinsics.fy, intrinsics.cy, 0, 0, 1); @@ -101,10 +75,8 @@ std::string AppState::autoScaleIntrinsicsToImage() if (intrinsicsW == imageW && intrinsicsH == imageH) return ""; - // Width ratio is the scale factor -- calib::scaleIntrinsics only takes - // one, so a genuine aspect-ratio change (as opposed to a uniform - // resize) can't be fully corrected; sy is only computed to detect and - // warn about that case. + // calib::scaleIntrinsics takes a single factor, so the width ratio is it; + // sy exists only to detect and warn about a real aspect-ratio change. float sx = static_cast(imageW) / static_cast(intrinsicsW); float sy = static_cast(imageH) / static_cast(intrinsicsH); intrinsics = calib::scaleIntrinsics(intrinsics, sx); @@ -112,8 +84,7 @@ std::string AppState::autoScaleIntrinsicsToImage() intrinsicsH = imageH; char buf[192]; - std::snprintf( - buf, sizeof(buf), "intrinsics auto-scaled %.4fx to match the %dx%d image", static_cast(sx), imageW, imageH); + std::snprintf(buf, sizeof(buf), "intrinsics auto-scaled %.4fx to match the %dx%d image", static_cast(sx), imageW, imageH); std::string note = buf; if (std::fabs(sx - sy) > 0.01f * sx) note += " (WARNING: aspect ratio differs from the calibration -- scaled by width only, results may be off)"; @@ -199,13 +170,11 @@ bool AppState::solvePairs() double rms = -1.0; std::string solveErr; - // Pinhole's reused observation equations are a pure rectilinear - // projection with no unified-sphere term, so they can't be used for - // Mei -- solveExtrinsicsMeiCeres (Ceres autodiff, optional at build - // time) is its counterpart instead. See CameraCalibrationSolver.h. + // Pinhole's observation equations have no unified-sphere term, so Mei + // uses solveExtrinsicsMeiCeres instead. See CameraCalibrationSolver.h. bool ok = (intrinsics.model == CameraModel::Mei) - ? calib::solveExtrinsicsMeiCeres(corr, intrinsics, extrinsics, solveErr, &rms, lockTranslation) - : calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); + ? calib::solveExtrinsicsMeiCeres(corr, intrinsics, extrinsics, solveErr, &rms, lockTranslation) + : calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); if (!ok) { statusMsg = !solveErr.empty() ? ("Solve failed: " + solveErr) : "Solve failed (degenerate correspondences)"; @@ -439,11 +408,10 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img return true; } -// A camera_info.yaml (MeiCamera's format) is a flat top-level mapping with a -// `distortion_model:` key, unlike OpenCV's `camera_matrix:`/`distortion_ -// coefficients:` YAML -- peeked at as plain text (not parsed) so a normal -// OpenCV pinhole YAML never round-trips through LoadMeiCamera and hits its -// "missing an expected field" warnings for fields it was never going to have. +// MeiCamera's camera_info.yaml is a flat mapping with a `distortion_model:` +// key, unlike OpenCV's `camera_matrix:`/`distortion_coefficients:` YAML. +// Peeked at as text so an OpenCV pinhole YAML never reaches LoadMeiCamera and +// warns about fields it was never going to have. static bool yamlLooksLikeMei(const char* path) { std::ifstream f(path); @@ -670,16 +638,25 @@ void AppState::saveCalibration(const char* path) Eigen::Vector3f ti = -(R.transpose() * C); // translation of T_lidar_to_camera nlohmann::json j; - // width/height record the resolution these intrinsics are valid for - // (intrinsicsW/H, not necessarily the original calibration file's own - // resolution -- see App.h) so a later load against a different-size - // image can auto-scale (AppState::autoScaleIntrinsicsToImage) instead - // of just warning about the mismatch. 0 means unknown. - j["intrinsics"] = { { "model", modelToString(intrinsics.model) }, { "fx", intrinsics.fx }, { "fy", intrinsics.fy }, - { "cx", intrinsics.cx }, { "cy", intrinsics.cy }, { "xi", intrinsics.xi }, { "k1", intrinsics.k1 }, - { "k2", intrinsics.k2 }, { "k3", intrinsics.k3 }, { "k4", intrinsics.k4 }, { "k5", intrinsics.k5 }, - { "k6", intrinsics.k6 }, { "p1", intrinsics.p1 }, { "p2", intrinsics.p2 }, - { "width", intrinsicsW }, { "height", intrinsicsH } }; + // width/height record the resolution these intrinsics are valid for (see + // App.h) so a later load against a different-size image can auto-scale + // rather than just warn. 0 means unknown. + j["intrinsics"] = { { "model", modelToString(intrinsics.model) }, + { "fx", intrinsics.fx }, + { "fy", intrinsics.fy }, + { "cx", intrinsics.cx }, + { "cy", intrinsics.cy }, + { "xi", intrinsics.xi }, + { "k1", intrinsics.k1 }, + { "k2", intrinsics.k2 }, + { "k3", intrinsics.k3 }, + { "k4", intrinsics.k4 }, + { "k5", intrinsics.k5 }, + { "k6", intrinsics.k6 }, + { "p1", intrinsics.p1 }, + { "p2", intrinsics.p2 }, + { "width", intrinsicsW }, + { "height", intrinsicsH } }; // Rotation is stored as a matrix only -- convention-independent (no // Euler/Tait-Bryan angle order or units to document/misread) and // directly portable to any external tool. camera_rotation_matrix_in_world diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index eaaa6b8a..82801899 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -40,13 +40,10 @@ struct AppState // ── calibration params ─────────────────────────────────────────────────── Intrinsics intrinsics; Extrinsics extrinsics; - // Resolution `intrinsics` are currently valid for -- from the - // calibration file's own width/height when it states one, or (if it - // didn't) whatever image was already loaded at the time. 0 = unknown, - // meaning autoScaleIntrinsicsToImage() has nothing to scale from. - // Kept in sync by that function, so it always names the size the - // *current* (possibly already auto-scaled) intrinsics apply to, not - // necessarily the original calibration file's resolution. + // Resolution `intrinsics` are currently valid for: the calibration file's + // own width/height, else whatever image was loaded at the time. 0 = + // unknown. autoScaleIntrinsicsToImage() keeps this in sync, so it names + // the size the *current* intrinsics apply to, not the file's original. int intrinsicsW = 0, intrinsicsH = 0; // ── visualization ───────────────────────────────────────────────────────── @@ -100,14 +97,11 @@ struct AppState // (Re)build the displayed texture: undistorts with current intrinsics // when they were loaded from a file, otherwise shows the raw image. void rebuildImageTexture(); - // If `intrinsicsW/H` names a resolution other than the current - // imageW/imageH, rescales `intrinsics` (calib::scaleIntrinsics) to - // match and updates intrinsicsW/H to the new size -- called after - // whichever of an image load or an intrinsics load comes second, so a - // calibration and an image of different resolutions just work instead - // of silently mis-projecting or only warning about it. No-op (returns - // "") if either resolution is unknown (0) or they already match. - // Callers still own calling rebuildImageTexture() afterward. + // Rescales `intrinsics` to the current imageW/imageH when intrinsicsW/H + // names a different resolution, so a calibration and an image of + // different sizes just work instead of silently mis-projecting. Called + // after whichever of the two loads comes second. No-op (returns "") if + // either size is unknown or they match. Caller owns rebuildImageTexture(). std::string autoScaleIntrinsicsToImage(); }; diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 2b6a41bc..94e99fb6 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -207,11 +207,10 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s cv::Mat map1, map2; bool mapsReady = false; int camW = 0, camH = 0; - // initUndistortRectifyMap is pinhole-only: there is nothing to - // rectify on a 360 panorama, and Km/Dm describe a camera neither - // it nor a Mei fisheye is (Mei's k1/k2/k3/p1/p2 are its own - // polynomial, applied after a unit-sphere step Km/Dm can't - // express), so both keep their raw frames. + // initUndistortRectifyMap is pinhole-only: a 360 panorama has + // nothing to rectify, and Mei's k1/k2/k3/p1/p2 are its own + // polynomial applied after a unit-sphere step Km/Dm cannot + // express -- so both models keep their raw frames. const bool equirect = in.K.model == CameraModel::Equirectangular; const bool mei = in.K.model == CameraModel::Mei; const bool rectify = opt.undistortCamera && in.calibLoaded && in.K.model == CameraModel::Pinhole; @@ -308,10 +307,10 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s ci.width = static_cast(camW); if (equirect) { - // No ROS distortion model describes a 360 panorama, + // No ROS distortion model describes a 360 panorama // and there is no K to report -- width/height are - // the whole projection. Leave k/p zeroed rather than - // publish a pinhole that would mislead consumers. + // the whole projection. Zeroed rather than + // publishing a pinhole that would mislead consumers. ci.distortion_model = "equirectangular"; ci.d = {}; ci.k = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -320,20 +319,17 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s } else if (mei) { - // No standard ROS distortion model is a unified - // sphere either, so this reports the rig's own tag - // (the same string its camera_info.yaml carries, - // see CalibCore/MeiCamera.h) rather than claiming - // to be plumb_bob/rational_polynomial, which a + // No standard ROS model is a unified sphere, so + // this reports the rig's own tag rather than + // claiming plumb_bob/rational_polynomial, which a // consumer would undistort with badly wrong math. // - // d is the yaml's own (k1, k2, k3, p1, p2) order -- - // NOT OpenCV's (k1, k2, p1, p2, k3) -- with xi - // appended, since CameraInfo has nowhere else to - // put it and the model is unusable without it. - // K/P stay populated: fx/fy/cx/cy do mean the - // usual thing here, they are just applied after - // the unit-sphere step. + // d is the yaml's (k1, k2, k3, p1, p2) order -- NOT + // OpenCV's (k1, k2, p1, p2, k3) -- with xi appended, + // since CameraInfo has nowhere else to put it and + // the model is unusable without it. K/P stay + // populated: fx/fy/cx/cy mean the usual thing, just + // applied after the unit-sphere step. ci.distortion_model = "insta360_mei_v2"; ci.d = { in.K.k1, in.K.k2, in.K.k3, in.K.p1, in.K.p2, in.K.xi }; ci.k = { in.K.fx, 0.f, in.K.cx, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 1.f }; diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 0a0e1b07..c25f2140 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -221,29 +221,22 @@ struct AppState Trajectory traj; std::vector imageTsNs; Intrinsics K; // K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) - // How K.model was decided. The calibration file's "model" key wins; absent - // one, the image filenames are the fallback. Both inputs are kept as state - // rather than applied on the spot because they arrive in either order -- - // loadSession() (and with it loadImages()) runs before loadCalib() at - // startup, but the user can load either on its own afterwards -- so - // resolveCameraModel() below recomputes K.model from scratch each time one - // of them changes. + // How K.model was decided: the calibration file's "model" key wins, the + // image filenames are the fallback. Both are kept as state rather than + // applied on the spot because they arrive in either order, so + // resolveCameraModel() recomputes K.model whenever one changes. CameraModel fileModel = CameraModel::Pinhole; bool modelExplicit = false; // the calibration file named a model bool namesLookEquirect = false; // the frames carry the equirectangular_ prefix Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame Roi roi; - // Free-form counterpart of `roi`: a per-pixel mask image whose rejected - // pixels are excluded from coloring. This is what it takes to drop the - // operator/backpack a 360 rig has in frame permanently -- no rectangle can - // cut that out without cutting out the scene with it. Kept at whatever - // resolution the file had, strictly 0/255 (see loadMask), and resampled to - // the working image size where it is used: images are read at s.imgScale, - // so there is no one size to pre-fit it to. - // - // Coloring only -- the images written by the ROS 2 and COLMAP exports are - // not masked. + // Free-form counterpart of `roi`: a per-pixel mask whose rejected pixels + // are excluded from coloring. Needed to drop the operator/backpack a 360 + // rig has permanently in frame, which no rectangle can cut out without + // taking the scene with it. Kept at the file's own resolution, strictly + // 0/255 (see loadMask), and resampled where used since images are read at + // s.imgScale. Coloring only -- the ROS 2 and COLMAP exports are not masked. cv::Mat mask; // empty = none loaded bool maskEnabled = false; // acted on only while `mask` is non-empty bool maskInvert = false; // UI state; loadMask and the toggle flip `mask` itself @@ -257,10 +250,9 @@ struct AppState // loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - // Downscale applied to every image used for coloring. Equirectangular - // frames are large (3840x1920x3 ≈ 22 MB) and multiImgColoring holds a whole - // chunk's worth in RAM at once, so this is what keeps that bounded. The - // intrinsics are scaled to match via calib::scaleIntrinsics. + // Downscale applied to every image used for coloring: equirectangular + // frames are large (3840x1920x3 ~ 22 MB) and multiImgColoring holds a + // chunk's worth at once. Intrinsics are scaled to match. float imgScale = 1.0f; // Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed // trigger/USB latency the camera's own timestamps don't account for): @@ -316,14 +308,6 @@ struct AppState float maxImageAngSpeedDeg = 60.f; // deg/s threshold int angFilteredImgs = 0; // images skipped by the filter in the last colorize pass - - // Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed - // trigger/USB latency the camera's own timestamps don't account for). - // Applied wherever an image timestamp is matched against the LiDAR/pose - // timeline (loadCloud's chunk selection + point matching, exportColmap's - // per-image pose lookup) -- never to the raw timestamps used for - // filename lookup or image-list indexing (s.imageTsNs/imagesFilenamesInTime). - float imageTimeOffsetMs = 0.f; 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 @@ -383,10 +367,9 @@ struct AppState std::thread imgViewThread; // ── synthetic intensity-projection image (drawn next to the photo) ───── - // Reprojects the (already colorized) exportCloud through the same - // calibration/projectPoint() as loadCloud()'s colorize pass, painted with - // a jet colormap over each point's normalized intensity -- a reference - // image to visually check the calibration/coloring against the photo. + // Reprojects exportCloud through the same calibration as the colorize + // pass, jet-colormapped over intensity -- a reference image to check the + // calibration against the photo by eye. bool showIntensityProjection = false; bool intensityProjNeedsUpdate = false; // set on toggle/refresh/image change Texture2D intensityProjTex = {}; @@ -453,14 +436,12 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) static constexpr const char* kEquirectPrefix = "equirectangular_"; // Timestamp encoded in a camera frame's filename, or -1 when the file isn't -// one. The layout is "_.jpg" or a bare -// ".jpg": everything up to and including the last '_' is -// ignored, so Mandeye's "cam0_", the 360 rig's "equirectangular_" and -// its per-lens "back_"/"front_" frames all parse without this needing -// to know the list of rigs. `equirect`, when given, reports whether the -// panorama prefix was the one found -- that one prefix still carries meaning -// (it selects the camera model, see resolveCameraModel). The all-digits check -// is what rejects unrelated .jpgs, which would otherwise reach std::stoll. +// one. Layout is "_.jpg" or a bare +// ".jpg" -- everything up to the last '_' is ignored, so +// Mandeye's "cam0_" and the 360 rig's "equirectangular_" both parse +// without a list of rigs here. `equirect` reports whether the panorama prefix +// was the one found, since that one selects the camera model. The all-digits +// check rejects unrelated .jpgs, which would otherwise reach std::stoll. static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) { if (equirect) @@ -497,13 +478,11 @@ static int64_t imageTimeOffsetNs(const AppState& s) } // Settles K.model from the two inputs that can select it, in precedence order. -// Call after either of them changes; see AppState::fileModel for why this isn't -// done inline in the loaders. +// Call after either changes; see AppState::fileModel for why. // -// Only Pinhole and Equirectangular are ever inferred: the filename fallback -// can distinguish those two because the 360 rig marks its frames with -// kEquirectPrefix, but nothing in a frame's name identifies a Mei fisheye, so -// CameraModel::Mei is reachable only through an explicit "model" key. +// Only Pinhole and Equirectangular are inferred: the 360 rig marks its frames +// with kEquirectPrefix, but nothing in a filename identifies a Mei fisheye, so +// Mei is reachable only through an explicit "model" key. static void resolveCameraModel(AppState& s) { if (s.modelExplicit) @@ -513,10 +492,9 @@ static void resolveCameraModel(AppState& s) } // Index every camera frame in the camera directory by timestamp. Also picks up -// the image dimensions -- which the equirectangular model projects with, and -// which the ROI default, the frustums and the COLMAP cameras.txt line read -- -// and, absent an explicit "model" in the calibration, infers the camera model -// from the filenames. +// the image dimensions -- read by the equirectangular projection, the ROI +// default, the frustums and COLMAP's cameras.txt -- and, absent an explicit +// "model" in the calibration, infers the camera model from the filenames. static void loadImages(AppState& s) { s.imagesFilenamesInTime.clear(); @@ -684,24 +662,17 @@ static void loadCloud(AppState& s) bool canColor = s.calibLoaded && !s.imagesFilenamesInTime.empty(); Eigen::Matrix3f R_wc = canColor ? s.R_wc : Eigen::Matrix3f::Identity(); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); - // Images are read at s.imgScale, so the intrinsics have to match: this - // scales fx/fy/cx/cy for the pinhole model and width/height for the - // equirectangular one. calib::projectPoint then applies whichever model the - // calibration selected -- for pinhole that is the OpenCV rational + - // tangential distortion, so colours are sampled from the raw (distorted) - // images at the right pixel; with all-zero coefficients it reduces exactly - // to the ideal pinhole. + // Images are read at s.imgScale, so the intrinsics must match. For pinhole + // calib::projectPoint applies the rational + tangential distortion, so + // colours are sampled from the raw (distorted) images at the right pixel. const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); - // The ROI is given in full-resolution image pixels (see calib::Roi), but - // the iu/iv probe() tests it against below are pixels of the images as - // they are actually read, i.e. at s.imgScale -- so the rectangle is scaled - // exactly as the intrinsics above are. + // The ROI is in full-resolution pixels (see calib::Roi) but probe() tests + // it against pixels read at s.imgScale, so it scales like the intrinsics. const Roi roiS = scaleRoi(s.roi, s.imgScale); const int64_t offNs = imageTimeOffsetNs(s); - // The mask arrives at the resolution of whatever file was loaded while the - // images are read at s.imgScale, so it is resampled to the size the frames - // actually have -- filled lazily below, on the first image probed, since - // that size isn't known until one has been read. + // The mask is at its file's resolution while images are read at s.imgScale, + // so it is resampled -- lazily, on the first image probed, since the frame + // size isn't known until one has been read. const bool haveMask = !s.mask.empty(); cv::Mat maskFit; // Every image of a chunk is held in memory at once (multiImgColoring), so @@ -833,7 +804,7 @@ static void loadCloud(AppState& s) const bool tooFast = dropFastImgs && angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; if (tooFast) ++angFilteredImgs; - if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs + offNs, pose)) + if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs + offNs, pose)) { cv::Mat img = readImage(fnIt->second); int gidx = (int)(it - s.imageTsNs.begin()); @@ -907,13 +878,11 @@ static void loadCloud(AppState& s) float inRoiF = -1.f; // 1 inside ROI, 0 outside, -1 not in frustum int globalIdx = -1; }; - // Note for the equirectangular model: a 360 camera has no - // frustum, so every point projects into every image. The - // temporal strategy's outward search therefore always succeeds - // at w == 0, leaving maxTemporalDist as the only real gate, and - // the geometry strategy compares ranges across all of the - // chunk's images rather than only the ones containing the point - // -- still correct, just no longer short-circuiting. + // Equirectangular: a 360 camera has no frustum, so every point + // projects into every image. The temporal search therefore + // always succeeds at w == 0, leaving maxTemporalDist the only + // real gate, and the geometry strategy compares ranges across + // every image of the chunk -- correct, just not short-circuiting. auto probe = [&](int idx) -> Hit { Hit h; @@ -1742,9 +1711,8 @@ static void exportColmap(AppState& s) // COLMAP's text model has no equirectangular camera type, and none of // its fisheye types is the unified-sphere (Mei) model -- none carries // an xi -- so the FULL_OPENCV line below would misdescribe the images. - s.status = s.K.model == CameraModel::Equirectangular - ? "COLMAP: equirectangular camera model is not supported by COLMAP" - : "COLMAP: Mei camera model is not supported by COLMAP"; + s.status = s.K.model == CameraModel::Equirectangular ? "COLMAP: equirectangular camera model is not supported by COLMAP" + : "COLMAP: Mei camera model is not supported by COLMAP"; return; } @@ -2759,8 +2727,7 @@ int main(int argc, char* argv[]) if (s.timeOffsetSec != 0.0) { ImGui::SameLine(); - ImGui::TextDisabled( - "(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s))); + ImGui::TextDisabled("(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s))); } { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); @@ -2863,8 +2830,8 @@ int main(int argc, char* argv[]) if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) ImGui::SetTooltip( !noRectify ? "Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)." - : s.K.model == CameraModel::Equirectangular ? "Not applicable to an equirectangular camera." - : "Not applicable to a Mei (fisheye) camera."); + : s.K.model == CameraModel::Equirectangular ? "Not applicable to an equirectangular camera." + : "Not applicable to a Mei (fisheye) camera."); ImGui::Unindent(); } ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index ddd999fb..0f9b7a3d 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -6,10 +6,8 @@ project(calib_core) # (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 plus, for the -# Mei camera model only (MeiCamera.h/.cpp), OpenCV's header-only Point2d/ -# Point3d and yaml-cpp for camera_info.yaml loading -- no raylib/imgui, and -# no other OpenCV usage, here -- so it stays reusable and cheap to build for +# 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. @@ -28,29 +26,16 @@ add_library(calib_core STATIC src/CameraCalibrationSolverMei.cpp ) -# yaml-cpp is a system package everywhere else in this repo already assumes -# is available for it (MeiCamera.cpp's #include predates -# this CMakeLists wiring) -- same "find it on the system" treatment as -# OpenCV gets in 3rdpartyBinary/OpenCV/CMakeLists.txt, just scoped to this -# target since calib_core is its only consumer. -find_package(yaml-cpp REQUIRED) - # ── Optional Ceres-based Mei extrinsics solver ──────────────────────────────── -# OFF by default: HDMapping's own README advertises depending on nothing but -# Eigen for its optimization (no Ceres/g2o/GTSAM/manif/Sophus), unlike -# yaml-cpp/OpenCV above this is a real optional feature, not something every -# build needs -- CameraCalibrationSolverMei.cpp's #ifdef falls back to a stub -# that explains the gap instead of solving, so callers -# (apps/camera_lidar_calibration's AppState::solvePairs) don't need their own -# #ifdef, just to check solveExtrinsicsMeiCeres()'s return value. Enable with +# OFF by default: HDMapping otherwise depends on nothing but Eigen for its +# optimization. Built without it, solveExtrinsicsMeiCeres() returns false and +# explains why, so callers need no #ifdef of their own. Enable with # cmake -DCALIB_ENABLE_CERES=ON (needs libceres-dev or equivalent). option(CALIB_ENABLE_CERES "Enable the Ceres-based extrinsics solver for the Mei camera model" OFF) if(CALIB_ENABLE_CERES) find_package(Ceres REQUIRED) - # PUBLIC (not PRIVATE, unlike Ceres::ceres's own link below): lets - # calib_core_tests -- calib_core's only other consumer that cares -- - # #ifdef on this to build the real-solve test only when it can actually - # run, without duplicating this option's value into a second place. + # PUBLIC so calib_core_tests can #ifdef on it to build the real-solve + # test only when it can actually run. target_compile_definitions(calib_core PUBLIC CALIB_ENABLE_CERES) message(STATUS "calib_core Mei Ceres solver: ENABLED") else() @@ -59,14 +44,6 @@ endif() target_include_directories(calib_core PUBLIC include - # MeiCamera.h (a public calib_core header) includes - # directly for cv::Point2d/Point3d, so anything that includes it needs - # OpenCV's headers on its include path too. OpenCV_INCLUDE_DIRS/ - # OpenCV_LIBS themselves come from find_package(OpenCV) in - # cmake/dependencies.cmake, included by the top-level CMakeLists.txt - # before add_subdirectory(calib_core) -- no find_package(OpenCV) needed - # here. - ${OpenCV_INCLUDE_DIRS} ) target_include_directories(calib_core PRIVATE @@ -88,31 +65,16 @@ target_include_directories(calib_core PRIVATE # affine_matrix_from_pose_tait_bryan (core/include/Core/transformations.h) # for the om/fi/ka<->matrix conversion instead of duplicating that math. # Header-only and pulls in nothing but Eigen/std (see structures.h) -- - # doesn't violate calib_core's no-raylib/imgui rule above, and nothing - # here links the core/core_math library, just includes headers. + # doesn't violate calib_core's no-raylib/imgui/OpenCV rule above, and + # nothing here links the core/core_math library, just includes headers. ${REPOSITORY_DIRECTORY}/core/include ) -target_link_libraries(calib_core PUBLIC - ${PLATFORM_LASZIP_LIB} - # For MeiCamera.cpp/.h -- see the target_include_directories comment - # above. PUBLIC (like PLATFORM_LASZIP_LIB) because calib_core is a - # STATIC library: CMake does not propagate a static library's own link - # dependencies to its consumers unless they're PUBLIC/INTERFACE, so a - # PRIVATE keyword here would leave executables linking calib_core with - # unresolved yaml-cpp symbols. - ${OpenCV_LIBS} - yaml-cpp::yaml-cpp -) +target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) if(CALIB_ENABLE_CERES) - # PRIVATE, unlike the PUBLIC block above -- CameraCalibrationSolver.h - # never exposes a Ceres type, so no consumer's own compilation needs - # Ceres' include dirs, just the symbols to link (CMake still records a - # STATIC library's PRIVATE link libraries as $ for - # whoever finally links an executable against calib_core, so this alone - # is enough for that final link to resolve solveExtrinsicsMeiCeres's - # ceres:: calls -- verified against this exact target during development). + # PRIVATE: CameraCalibrationSolver.h never exposes a Ceres type, so + # consumers need the symbols at link time but not Ceres' include dirs. target_link_libraries(calib_core PRIVATE Ceres::ceres) endif() diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 2af992e1..77eb7412 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -2,30 +2,17 @@ #include #include #include +#include namespace calib { - // Which projection projectPoint() applies. Selected by a "model" key in the - // calibration JSON. - // - // Not every consumer honours this yet. apps/camera_lidar_calibration neither - // writes the key (saveCalibration) nor reads it (loadCalibration / - // loadIntrinsics -- and its OpenCV YAML input cannot express one at all), so - // it always operates as Pinhole: opening an Equirectangular calibration there - // would silently mis-project it, and rebuildImageTexture() would additionally - // run initUndistortRectifyMap over a panorama. Its GLSL projection - // (RendererShaders.h) and Renderer::drawCameraFrustum are pinhole-only too. - // Likewise solveExtrinsicsFromCorrespondences, whose only caller is that app. - // - // The solver drop-in is ready when that app is picked up: the vendored - // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc[_jacobian] - // take the same (tx,ty,tz,om,fi,ka,px,py,pz) order and 9-column layout as the - // perspective ones, so the kCameraLidarAxisOffset pre-rotation, the - // fixTranslation column slicing and the LM loop all carry over unchanged. - // Only three things differ: (fx,fy,cx,cy) becomes (rows,cols,pi), the - // jacobian is Eigen::Matrix rather than - // column-major, and it takes two extra trailing u_kp, v_kp arguments. + //! Which projection @ref projectPoint applies. Selected by a "model" key + //! in the calibration JSON; absent, it is Pinhole. + //! @note apps/camera_lidar_calibration supports Pinhole and Mei only -- it + //! has no Equirectangular solver, and its GLSL projection + //! (RendererShaders.h) and Renderer::drawCameraFrustum assume a + //! frustum a 360 panorama doesn't have. enum class CameraModel { Pinhole, // fx/fy/cx/cy + the rational distortion coefficients below @@ -38,133 +25,146 @@ namespace calib CameraModel model = CameraModel::Pinhole; float fx = 800.f, fy = 800.f; float cx = 640.f, cy = 360.f; - // OpenCV rational distortion model (CameraModel::Pinhole): - // radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) - // - // CameraModel::Mei reuses k1/k2/k3 and p1/p2 below for its own - // (non-rational) radial/tangential polynomial -- see MeiCamera.h -- - // and leaves k4/k5/k6 at 0, unused. + //! OpenCV rational distortion model (CameraModel::Pinhole): + //! radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + //! @note CameraModel::Mei reuses k1/k2/k3 and p1/p2 for its own + //! (non-rational) polynomial -- see @ref MeiCamera -- and leaves + //! k4/k5/k6 unused. float k1 = 0.f, k2 = 0.f, k3 = 0.f; float k4 = 0.f, k5 = 0.f, k6 = 0.f; - // tangential + //! Tangential distortion. float p1 = 0.f, p2 = 0.f; - // Unified-sphere mirror parameter, CameraModel::Mei only (see - // MeiCamera.h for the model itself). Unused (0) by every other model. + //! Unified-sphere mirror parameter, CameraModel::Mei only. + //! @see MeiCamera float xi = 0.f; - // Image dimensions in pixels. Read only by CameraModel::Equirectangular, - // where they play the role fx/fy/cx/cy play for a pinhole camera and so - // *must* be set -- from the calibration file or from the loaded image -- - // before projectPoint() is called. + //! Read only by CameraModel::Equirectangular, where they play the role + //! fx/fy/cx/cy play for a pinhole camera and so *must* be set before + //! @ref projectPoint is called. int width = 0, height = 0; }; - // Minimum distance (degrees) fi is kept away from the om/fi/ka - // parameterization's gimbal-lock points (fi = +/-90 deg), where om and - // ka become individually non-unique (only om+ka, or om-ka, is - // determined) and CameraCalibrationSolver's normal equations go - // rank-deficient in that 2x2 block. Used by UI code that edits fi - // interactively (see apps/camera_lidar_calibration/UI.cpp's - // avoidGimbalLock) so a manual drag can't land exactly on the - // singularity. Extrinsics' own default (below) no longer needs this -- - // see kCameraLidarAxisOffset -- but it's kept as a cheap safety net for - // whatever fi a user or a loaded file lands on. + //! Name of a camera model, as written to the calibration JSON's "model" key. + //! @param m model to name + //! @return one of "pinhole", "equirectangular", "mei" + const char* modelToString(CameraModel m); + + //! Camera model named by a calibration JSON's "model" key. + //! @param s model name, as written by @ref modelToString + //! @return the named model, or CameraModel::Pinhole for anything + //! unrecognized (including an absent key) + CameraModel modelFromString(const std::string& s); + + //! Minimum distance (degrees) fi is kept away from the om/fi/ka + //! parameterization's gimbal-lock points (fi = +/-90 deg), where om and ka + //! become individually non-unique (only om+ka, or om-ka, is determined) + //! and CameraCalibrationSolver's normal equations go rank-deficient in + //! that 2x2 block. Used by UI code that edits fi interactively so a manual + //! drag can't land exactly on the singularity. + //! @note @ref Extrinsics' own default no longer needs this -- see + //! kCameraLidarAxisOffset -- but it is kept as a cheap safety net + //! for whatever fi a user or a loaded file lands on. constexpr float kGimbalLockEpsilonDeg = 0.1f; - // Nudges fi_deg off the nearest gimbal-lock point (+/-90 deg) if it's - // within kGimbalLockEpsilonDeg of one, in place. A no-op otherwise. - // Safe to call unconditionally every frame after any edit to fi (manual - // slider drag, typed value, or loaded from a file) -- idempotent. + //! Nudges fi_deg off the nearest gimbal-lock point (+/-90 deg) if it is + //! within @ref kGimbalLockEpsilonDeg of one. A no-op otherwise. + //! @param fi_deg angle to adjust, in place + //! @note Idempotent, so it is safe to call unconditionally every frame + //! after any edit to fi (slider drag, typed value, or file load). void avoidGimbalLock(float& fi_deg); - // Fixed rotation baked into Extrinsics' om/fi/ka (see below): the - // "camera axes vs LiDAR axes" alignment -- camera X=right, Y=down, - // Z=forward matched to LiDAR X=forward, Y=left, Z=up. This is a - // constant coordinate-convention twist that has nothing to do with the - // actual calibration being solved for, so it's factored out as a fixed - // offset rather than folded into om/fi/ka: om=fi=ka=0 is then already - // the correct nominal alignment (Extrinsics' literal default), and - // om/fi/ka become exactly "how far off nominal the real mount is" -- - // normally a few degrees at most, so nowhere near the om/fi/ka - // parameterization's gimbal-lock points (fi=+/-90 deg) in practice, - // unlike the old scheme where fi had to carry this entire 90-degree - // twist directly and sat right on top of the singularity by default. + //! Fixed rotation baked into @ref Extrinsics' om/fi/ka: the "camera axes + //! vs LiDAR axes" alignment -- camera X=right, Y=down, Z=forward matched + //! to LiDAR X=forward, Y=left, Z=up. + //! @note This constant coordinate-convention twist has nothing to do with + //! the calibration being solved for, so it is factored out rather + //! than folded into om/fi/ka. om=fi=ka=0 is then already the correct + //! nominal alignment, and om/fi/ka become exactly "how far off + //! nominal the real mount is" -- a few degrees at most, so nowhere + //! near fi=+/-90 deg in practice, unlike the old scheme where fi + //! carried the whole 90-degree twist and sat on the singularity. inline const Eigen::Matrix3f kCameraLidarAxisOffset = (Eigen::Matrix3f() << 0.f, 0.f, 1.f, -1.f, 0.f, 0.f, 0.f, -1.f, 0.f).finished(); struct Extrinsics { - // Camera position in LiDAR/world frame + //! Camera position in the LiDAR/world frame. float tx = 0.f, ty = 0.f, tz = 0.f; - // Camera orientation in LiDAR/world frame, as a SMALL deviation from - // the fixed kCameraLidarAxisOffset alignment: R_wc = - // kCameraLidarAxisOffset * Rx(om) * Ry(fi) * Rz(ka). om/fi/ka are - // degrees, Tait-Bryan, matching CameraCalibrationSolver's own - // parameterization (om/fi/ka feed the vendored observation - // equations directly there too -- see CameraCalibrationSolver.cpp - // for how the offset is threaded through the solve without - // modifying those equations). - // Default: om=fi=ka=0, i.e. exactly the nominal alignment -- a - // real calibration only needs to move these by however far the - // actual camera mount deviates from nominal, typically a few - // degrees, so "0,0,0" is already a good initial guess, not just a - // mathematically convenient one. + //! Camera orientation in the LiDAR/world frame, as a SMALL deviation + //! from the fixed kCameraLidarAxisOffset alignment: + //! R_wc = kCameraLidarAxisOffset * Rx(om) * Ry(fi) * Rz(ka). Degrees, + //! Tait-Bryan, matching CameraCalibrationSolver's parameterization. + //! @note Default om=fi=ka=0 is exactly the nominal alignment, so a real + //! calibration only moves these by however far the mount deviates + //! from nominal -- typically a few degrees. "0,0,0" is therefore + //! a good initial guess, not just a convenient one. float om = 0.f, fi = 0.f, ka = 0.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. + //! 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 = kCameraLidarAxisOffset * Rx * Ry * Rz (Tait-Bryan om/fi/ka, - // degrees → rotation matrix). Matches Extrinsics' own om/fi/ka - // convention above -- om=fi=ka=0 returns kCameraLidarAxisOffset exactly. + //! R = kCameraLidarAxisOffset * Rx * Ry * Rz (Tait-Bryan om/fi/ka). + //! Matches @ref Extrinsics' own om/fi/ka convention. + //! @param om_deg,fi_deg,ka_deg Tait-Bryan angles in degrees + //! @return the rotation matrix; om=fi=ka=0 returns kCameraLidarAxisOffset + //! exactly Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg); - // Inverse of omFiKaToMat3: decomposes kCameraLidarAxisOffset^T * R - // assuming that equals Rx(om)*Ry(fi)*Rz(ka), for reading a rotation - // matrix (e.g. from a saved calibration file) back into Extrinsics' - // om/fi/ka fields. Calibration files store the rotation as a plain - // matrix (convention-independent, portable to any external tool, and - // knows nothing about kCameraLidarAxisOffset), while the app's own - // UI/solver work in om/fi/ka, so this conversion is needed at the file - // -I/O boundary either way. Result is passed through avoidGimbalLock. + //! Inverse of @ref omFiKaToMat3: decomposes kCameraLidarAxisOffset^T * R + //! assuming that equals Rx(om)*Ry(fi)*Rz(ka), for reading a rotation + //! matrix (e.g. from a saved calibration file) back into @ref Extrinsics' + //! om/fi/ka fields. + //! @param R rotation matrix to decompose + //! @param om_deg,fi_deg,ka_deg receive the Tait-Bryan angles, in degrees, + //! passed through @ref avoidGimbalLock + //! @note Calibration files store the rotation as a plain matrix + //! (convention-independent, portable, and knowing nothing about + //! kCameraLidarAxisOffset) while the UI and solver work in om/fi/ka, + //! so this conversion is needed at the file-I/O boundary either way. void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg); - // Intrinsics describing the same camera after its images are resampled by - // `s` (e.g. 0.5 for half-size images): scales fx/fy/cx/cy for Pinhole and - // width/height for Equirectangular, so a downscaled image projects with the - // same geometry. Everything else (distortion, model) is carried over. + //! The same camera after its images are resampled, so a downscaled image + //! projects with the same geometry. Distortion terms are dimensionless and + //! carry over unchanged. + //! @param K intrinsics at the original resolution + //! @param s resample factor (0.5 = half size) + //! @return intrinsics valid for the resampled image Intrinsics scaleIntrinsics(const Intrinsics& K, float s); - // The same rectangle on an image resampled by `s`, so a ROI -- which is - // given in full-resolution pixels, see Roi above -- can be tested against - // the pixels of a downscaled copy. Both edges are scaled rather than the - // width alone, so abutting rectangles stay abutting. An empty (w/h == 0) - // ROI comes back unchanged, and a non-empty one never scales down to - // empty, which every caller would read as "no ROI set". + //! The same rectangle on a resampled image, so a ROI -- given in + //! full-resolution pixels, see @ref Roi -- can be tested against a + //! downscaled copy. + //! @param r rectangle in full-resolution pixels + //! @param s resample factor (0.5 = half size) + //! @return the scaled rectangle + //! @note An unset (w/h == 0) ROI comes back unchanged, and a set one never + //! collapses to empty, which callers would read as "no ROI". Roi scaleRoi(const Roi& r, float s); - // Project a point from LiDAR frame to image pixel (u, v). - // R_wc = camera orientation in world, t = camera position in world. - // - // Pinhole: depth = z component in camera frame (positive = in front), and - // the function returns false for points behind the camera. - // Equirectangular: depth = range from the camera, and only a point - // essentially at the camera itself fails -- a full-sphere camera has no - // frustum and no "behind". u comes back wrapped into [0, width); v spans - // [0, height] *inclusive*, the south pole landing exactly on height. - // Mei: depth = range from the camera, same "only the camera itself - // fails" rule as Equirectangular; (u, v) come straight out of - // MeiCamera::Project, unclamped and unwrapped. - // - // In all cases the caller owns rounding to integer pixels (which can itself - // land on width at the equirectangular seam), bounds checking and any ROI - // test. + //! Project a point from the LiDAR frame to an image pixel, applying + //! whichever model K.model selects. + //! @param px,py,pz point in the LiDAR frame + //! @param K camera intrinsics; K.model picks the projection + //! @param R_wc camera orientation in world + //! @param t camera position in world + //! @param u,v receive the image pixel + //! @param depth receives the camera-frame z for Pinhole, range from the + //! camera for Equirectangular and Mei + //! @return false when the point does not project: behind the camera for + //! Pinhole, at the camera itself for Equirectangular, and either + //! of those or past the fold-back angle (where the projection + //! stops being injective) for Mei + //! @note Equirectangular wraps u into [0, width); v spans [0, height] + //! *inclusive*, the south pole landing exactly on height. + //! @note The caller owns rounding to integer pixels (which can land on + //! width at the equirectangular seam), bounds checking and any ROI + //! test. bool projectPoint( float px, float py, diff --git a/calib_core/include/CalibCore/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h index f18d39cc..ddeda2fd 100644 --- a/calib_core/include/CalibCore/CameraCalibrationSolver.h +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -9,13 +9,10 @@ namespace calib // A single manually-picked correspondence: a 3D point in the LiDAR/world // frame paired with the pixel it should project to in the camera image. - // Pixel coordinates are expected in whatever frame the displayed image - // itself is in -- the undistorted/ideal-pinhole frame for - // CameraModel::Pinhole (picked from the rectified image display), or - // the raw (distorted) frame for CameraModel::Mei, whose image is never - // rectified (see AppState::rebuildImageTexture in - // apps/camera_lidar_calibration) -- matching whichever solver below is - // used for that model. + // Pixel coordinates are expected in whatever frame the displayed image is + // in: undistorted/ideal-pinhole for CameraModel::Pinhole (picked from the + // rectified display), raw/distorted for CameraModel::Mei, whose image is + // never rectified. struct PointPixelCorrespondence { Eigen::Vector3d p; @@ -32,11 +29,9 @@ namespace calib // i.e. picked from the rectified image display (calib::Intrinsics's // distortion terms are ignored here). // - // Pinhole only -- the reused observation equations are a pure - // rectilinear perspective projection with no distortion and no unified- - // sphere term, so this cannot be used for CameraModel::Mei (or - // CameraModel::Equirectangular, not wired into any app yet). See - // solveExtrinsicsMeiCeres() below for Mei. + // Pinhole only: the reused observation equations are a pure rectilinear + // perspective projection, with no distortion and no unified-sphere term. + // See solveExtrinsicsMeiCeres() below for Mei. // // fixTranslation=true blocks tx/ty/tz from being solved for -- they // stay pinned at extrinsicsInOut's initial values and only orientation @@ -55,26 +50,19 @@ namespace calib double* outRmsPixels = nullptr, bool fixTranslation = false); - // CameraModel::Mei counterpart to solveExtrinsicsFromCorrespondences() - // above: no vendored analytic Jacobian exists for the unified-sphere - // model (unlike Pinhole's), so this minimizes reprojection error with - // Ceres' automatic differentiation instead of a hand-derived one, - // reusing K's fx/fy/cx/cy/xi/k1/k2/k3/p1/p2 fixed and solving the same - // (tx,ty,tz,om,fi,ka) Extrinsics this file's Pinhole solver does, with + // CameraModel::Mei counterpart to solveExtrinsicsFromCorrespondences(). + // No vendored analytic Jacobian exists for the unified-sphere model, so + // this minimizes reprojection error with Ceres' automatic differentiation, + // holding K fixed and solving the same (tx,ty,tz,om,fi,ka) Extrinsics with // the same fixTranslation meaning. // - // `errorMessage` is set on failure (degenerate input, Ceres failing to - // converge, or this build not having Ceres at all -- see below) and - // left untouched on success. It is a required, non-defaulted parameter - // -- hence its position ahead of the optional ones -- so that a failure - // reason is never silently dropped. + // `errorMessage` is set on failure and left untouched on success. It is + // required rather than defaulted -- hence its position ahead of the + // optional parameters -- so a failure reason is never silently dropped. // - // Only available when calib_core is built with -DCALIB_ENABLE_CERES=ON - // (see calib_core/CMakeLists.txt) -- OFF by default, since HDMapping - // otherwise depends on nothing but Eigen for its own optimization (see - // the project README). Built without it, this always returns false and - // sets `errorMessage` to say so, rather than requiring callers to - // `#ifdef` around calling it at all. + // Needs -DCALIB_ENABLE_CERES=ON (OFF by default). Built without it, this + // returns false and says so in `errorMessage`, so callers never need an + // #ifdef of their own. bool solveExtrinsicsMeiCeres( const std::vector& correspondences, const Intrinsics& K, diff --git a/calib_core/include/CalibCore/MeiCamera.h b/calib_core/include/CalibCore/MeiCamera.h index 777cdeb9..72d16b9f 100644 --- a/calib_core/include/CalibCore/MeiCamera.h +++ b/calib_core/include/CalibCore/MeiCamera.h @@ -1,24 +1,18 @@ #pragma once -#include +#include #include // Camera intrinsics for the Mei/unified-sphere fisheye model used by the -// Insta360 rig this data comes from (distortion_model: insta360_mei_v2 in -// camera_info.yaml). Field meanings and the forward projection formula below -// are taken from /home/michal/code/insta3360-to-images -// (include/insta360/calibration.hpp, src/equirect.cpp), which cross-checked -// them against an independent reverse-engineering effort and validated them -// visually against Insta360's own equirect export — not re-derived from -// scratch here. +// Insta360 rig (distortion_model: insta360_mei_v2 in camera_info.yaml). // // IMPORTANT: camera_info.yaml's `distortion` array is ordered -// (k1, k2, k3, p1, p2) — NOT OpenCV's usual pinhole order (k1, k2, p1, p2, -// k3). The two conventions are trivially easy to mix up (both are just five -// numbers in a row) and doing so produces a plausible-looking but badly wrong -// reprojection with no crash — see calibration.hpp's own comment on this. -struct MeiCamera { +// (k1, k2, k3, p1, p2) -- NOT OpenCV's usual pinhole order (k1, k2, p1, p2, +// k3). The two are easy to mix up (both are five numbers in a row) and doing +// so produces a plausible-looking but badly wrong reprojection with no crash. +struct MeiCamera +{ std::string frameId; std::string distortionModel; int width = 0, height = 0; @@ -29,23 +23,13 @@ struct MeiCamera { bool loaded = false; - // Forward: camera-frame 3D point (any positive scale, need not be unit - // length) -> pixel coordinates. - cv::Point2d Project(cv::Point3d P) const; - - // Inverse: pixel -> unit-length ray direction in camera frame. Closed - // form for the unit-sphere/xi step, iterative (Newton) for the - // radial/tangential part, matching equirect.cpp's forward formula. - // If thetaDeg is given, receives the angle from the optical axis (0 = - // dead center, useful as a "how far into the fisheye edge is this point" - // sanity check). - cv::Point3d Unproject(cv::Point2d uv, double* thetaDeg = nullptr) const; + // Camera-frame 3D point (any positive scale) -> pixel coordinates. + Eigen::Vector2d Project(const Eigen::Vector3d& P) const; }; -// Loads intrinsics from a camera_info.yaml written in this rig's format (see -// data/camera_info.yaml for a sample). On any failure (missing file, missing -// field, distortion array with an unexpected element count) prints a message -// to stderr and returns a default-constructed MeiCamera with loaded=false — -// a missing/malformed file should degrade the app to "no reprojection -// available", not crash it. +// Loads intrinsics from a camera_info.yaml in this rig's format: a flat +// top-level mapping of scalars plus a `distortion` flow sequence. On any +// failure (missing file, missing field, unexpected distortion element count) +// prints to stderr and returns loaded=false -- a malformed file should +// degrade the app to "no reprojection available", not crash it. MeiCamera LoadMeiCamera(const std::string& path); diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 09ec8e4d..137e8364 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -31,6 +31,32 @@ void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, floa ka_deg = static_cast(rad2deg(pose.ka)); } +// No `default:` case on purpose: -Wswitch then flags a future CameraModel +// enumerator added without a matching string here, instead of it silently +// falling through to "pinhole". +const char* modelToString(CameraModel m) +{ + switch (m) + { + case CameraModel::Pinhole: + return "pinhole"; + case CameraModel::Equirectangular: + return "equirectangular"; + case CameraModel::Mei: + return "mei"; + } + return "pinhole"; +} + +CameraModel modelFromString(const std::string& s) +{ + if (s == "equirectangular") + return CameraModel::Equirectangular; + if (s == "mei") + return CameraModel::Mei; + return CameraModel::Pinhole; +} + // Radius (in normalized camera coords, squared) past which the rational distortion model // stops being usable. r -> r*radial(r) is only injective up to its turning point; beyond it // the model folds, so directions far outside the lens' actual field of view map back onto @@ -122,14 +148,10 @@ bool projectPoint(float px, float py, float pz, Eigen::Vector3f pc = R_wc.transpose() * (Eigen::Vector3f(px, py, pz) - t); if (K.model == CameraModel::Equirectangular) { - // Longitude from atan2(x, z) across the full width, latitude from + // Longitude from atan2(x, z) across the width, latitude from // asin(y/|p|) across the height -- camera X = right, Y = down, - // Z = forward, i.e. kCameraLidarAxisOffset's convention, so v grows - // downward like image rows. Same model apps/manual_color colors with; - // that app reaches it through the vendored equirectangular_camera_ - // colinearity_tait_bryan_wc_jacobian.h, not used here because it - // re-derives the rotation from a Tait-Bryan pose per point while - // R_wc/t are already in hand. + // Z = forward (kCameraLidarAxisOffset's convention), so v grows + // downward like image rows. Same model apps/manual_color colors with. depth = pc.norm(); if (depth < 1e-4f) return false; // point sits on the camera itself @@ -145,14 +167,8 @@ bool projectPoint(float px, float py, float pz, } if (K.model == CameraModel::Mei) { - // Delegates to the tested/certified MeiCamera::Project (MeiCamera.h) - // instead of re-deriving the unified-sphere + radial/tangential - // formula here -- only the R_wc/t transform into camera frame, the - // "point sits on the camera itself" guard (same idiom as - // Equirectangular above), and the "in front of the camera" guard - // just below belong to this wrapper. depth = pc.norm(); - if (depth < 1e-4f) return false; + if (depth < 1e-4f) return false; // point sits on the camera itself // Validity domain. r(theta) = sin/(cos+xi) is only injective up to // its turning point at cos(theta) = -1/xi; past it the radius shrinks @@ -171,9 +187,9 @@ bool projectPoint(float px, float py, float pz, cam.k1 = K.k1; cam.k2 = K.k2; cam.k3 = K.k3; cam.p1 = K.p1; cam.p2 = K.p2; - const cv::Point2d px = cam.Project(cv::Point3d(pc.x(), pc.y(), pc.z())); - u = static_cast(px.x); - v = static_cast(px.y); + const Eigen::Vector2d px = cam.Project(pc.cast()); + u = static_cast(px.x()); + v = static_cast(px.y()); return true; } diff --git a/calib_core/src/CameraCalibrationSolverMei.cpp b/calib_core/src/CameraCalibrationSolverMei.cpp index be7a41d2..bf67d6f2 100644 --- a/calib_core/src/CameraCalibrationSolverMei.cpp +++ b/calib_core/src/CameraCalibrationSolverMei.cpp @@ -1,10 +1,8 @@ #include -// Always compiled (see calib_core/CMakeLists.txt); the #ifdef below picks -// between a real Ceres-based implementation and a stub that just explains -// why it isn't available, so callers (apps/camera_lidar_calibration's -// AppState::solvePairs) never need an #ifdef of their own around calling -// solveExtrinsicsMeiCeres -- only its return value. +// Always compiled; the #ifdef below picks between the real Ceres +// implementation and a stub that explains why it isn't available, so callers +// check solveExtrinsicsMeiCeres's return value rather than an #ifdef. #ifdef CALIB_ENABLE_CERES #include @@ -15,17 +13,15 @@ namespace calib { namespace { - // Templated (Ceres::Jet-compatible) equivalent of Camera.cpp's - // omFiKaToMat3: R = kCameraLidarAxisOffset * Rx(om)*Ry(fi)*Rz(ka), - // om/fi/ka in RADIANS (Extrinsics' own fields are degrees -- solve() - // below converts at the boundary). The Rx*Ry*Rz part mirrors - // Core/transformations.h's affine_matrix_from_pose_tait_bryan - // row-for-row rather than re-deriving it; kCameraLidarAxisOffset - // (Camera.h) is applied by permuting/negating Rdelta's rows - // directly instead of a general 3x3*3x3 product, since its own - // entries are just {0, +-1}: offset = [[0,0,1],[-1,0,0],[0,-1,0]], - // so row 0 of R is row 2 of Rdelta, row 1 is -(row 0), row 2 is - // -(row 1). + // Ceres::Jet-compatible equivalent of Camera.cpp's omFiKaToMat3: + // R = kCameraLidarAxisOffset * Rx(om)*Ry(fi)*Rz(ka), om/fi/ka in + // RADIANS (Extrinsics stores degrees; solve() converts). The Rx*Ry*Rz + // part mirrors Core/transformations.h's + // affine_matrix_from_pose_tait_bryan. kCameraLidarAxisOffset's entries + // are only {0, +-1}, so it is applied by permuting/negating Rdelta's + // rows rather than a general 3x3 product: offset = + // [[0,0,1],[-1,0,0],[0,-1,0]], so row 0 of R is row 2 of Rdelta, + // row 1 is -(row 0), row 2 is -(row 1). template void rotationMatrix(const T& om, const T& fi, const T& ka, T R[3][3]) { @@ -52,10 +48,9 @@ namespace calib } } - // Templated equivalent of MeiCamera::Project (CalibCore/ - // MeiCamera.h) for Ceres autodiff -- same formula, not re-derived; - // intrinsics are plain doubles (fixed, not solved for), only pc is - // the Jet-typed autodiff variable. + // Templated equivalent of MeiCamera::Project for Ceres autodiff -- + // same formula. Intrinsics stay plain doubles (fixed, not solved + // for); only pc is the Jet-typed variable. template void projectMei( const T pc[3], @@ -85,10 +80,8 @@ namespace calib } // Reprojection residual for one correspondence: predicted (u, v) - // minus the picked pixel, exactly like the Pinhole solver's reused - // observation_equation_perspective_camera_tait_bryan_wc, just - // autodiff'd instead of symbolically pre-differentiated (no - // vendored Mei Jacobian exists to reuse -- see CameraCalibrationSolver.h). + // minus the picked pixel, like the Pinhole solver's observation + // equation but autodiff'd, no vendored Mei Jacobian existing. struct MeiReprojectionResidual { MeiReprojectionResidual(const Eigen::Vector3d& p, double u_kp, double v_kp, const Intrinsics& K) @@ -173,10 +166,9 @@ namespace calib extrinsicsInOut.fi = static_cast(omfika[1] / d2r); extrinsicsInOut.ka = static_cast(omfika[2] / d2r); - // Ceres' final_cost is 0.5*sum(residual_i^2) over every SCALAR - // residual (2 per correspondence: du, dv), so sum(du^2+dv^2) = - // 2*final_cost -- matching the Pinhole solver's own rms formula - // (sqrt(sum(du^2+dv^2) / (2*N))) then simplifies to sqrt(final_cost/N). + // final_cost is 0.5*sum(residual^2) over every SCALAR residual (2 per + // correspondence), so the Pinhole solver's rms formula + // sqrt(sum(du^2+dv^2) / (2*N)) simplifies to sqrt(final_cost/N). if (outRmsPixels) *outRmsPixels = std::sqrt(summary.final_cost / static_cast(correspondences.size())); diff --git a/calib_core/src/MeiCamera.cpp b/calib_core/src/MeiCamera.cpp index 1e93febb..7218d6a5 100644 --- a/calib_core/src/MeiCamera.cpp +++ b/calib_core/src/MeiCamera.cpp @@ -1,100 +1,151 @@ #include -#include - -#include -#include #include +#include +#include +#include +#include +#include +#include -cv::Point2d MeiCamera::Project(cv::Point3d P) const { - const double n = cv::norm(P); - const cv::Point3d Xs(P.x / n, P.y / n, P.z / n); // onto the unit sphere +Eigen::Vector2d MeiCamera::Project(const Eigen::Vector3d& P) const +{ + const Eigen::Vector3d Xs = P.normalized(); // onto the unit sphere - const double denom = Xs.z + xi; - const double x = Xs.x / denom, y = Xs.y / denom; + const double denom = Xs.z() + xi; + const double x = Xs.x() / denom, y = Xs.y() / denom; const double r2 = x * x + y * y; const double radial = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; const double xd = x * radial + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); const double yd = y * radial + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; - return {fx * xd + cx, fy * yd + cy}; + return { fx * xd + cx, fy * yd + cy }; } -cv::Point3d MeiCamera::Unproject(cv::Point2d uv, double* thetaDeg) const { - const double xd = (uv.x - cx) / fx, yd = (uv.y - cy) / fy; - - // Invert the radial/tangential distortion by fixed-point (Newton-style) - // iteration, same scheme cv::undistortPoints uses for the pinhole model. - double x = xd, y = yd; - for (int it = 0; it < 30; ++it) { - const double r2 = x * x + y * y; - const double radial = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - const double dx = 2 * p1 * x * y + p2 * (r2 + 2 * x * x); - const double dy = p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; - x = (xd - dx) / radial; - y = (yd - dy) / radial; +namespace +{ + std::string trim(std::string s) + { + const char* ws = " \t\r\n"; + const auto b = s.find_first_not_of(ws); + if (b == std::string::npos) + return {}; + return s.substr(b, s.find_last_not_of(ws) - b + 1); } - // Closed-form inverse of the unit-sphere/xi projection: solve - // (rho2+1)*s^2 - 2*xi*s + (xi^2-1) = 0 for s = Xs.z + xi, where - // Xs.x = x*s, Xs.y = y*s, Xs.z = s - xi, subject to |Xs| = 1. - const double rho2 = x * x + y * y; - const double s = (xi + std::sqrt(std::max(0.0, 1.0 + rho2 * (1.0 - xi * xi)))) / (rho2 + 1.0); - const cv::Point3d Xs(x * s, y * s, s - xi); + // This rig's camera_info.yaml is a flat mapping of `key: value` scalars + // plus a `distortion: [a, b, c, d, e]` flow sequence -- no nesting, no + // anchors, no block sequences. Parsed here rather than with a YAML + // library so calib_core keeps depending on nothing but Eigen/LASzip/std. + std::map readFlatYaml(std::istream& in) + { + std::map kv; + std::string line; + while (std::getline(in, line)) + { + const auto hash = line.find('#'); + if (hash != std::string::npos) + line = line.substr(0, hash); + const auto colon = line.find(':'); + if (colon == std::string::npos) + continue; + std::string key = trim(line.substr(0, colon)); + if (!key.empty()) + kv[key] = trim(line.substr(colon + 1)); + } + return kv; + } - if (thetaDeg) *thetaDeg = std::acos(std::clamp(Xs.z, -1.0, 1.0)) * 180.0 / CV_PI; - return Xs; -} + std::vector parseArray(const std::string& v) + { + std::vector out; + std::string inner = trim(v); + if (inner.size() >= 2 && inner.front() == '[' && inner.back() == ']') + inner = inner.substr(1, inner.size() - 2); + std::stringstream ss(inner); + std::string tok; + while (std::getline(ss, tok, ',')) + { + tok = trim(tok); + if (!tok.empty()) + out.push_back(std::strtod(tok.c_str(), nullptr)); + } + return out; + } +} // namespace -MeiCamera LoadMeiCamera(const std::string& path) { +MeiCamera LoadMeiCamera(const std::string& path) +{ MeiCamera cam; - YAML::Node node; - try { - node = YAML::LoadFile(path); - } catch (const std::exception& e) { - std::fprintf(stderr, "calib_app: failed to load '%s': %s\n", path.c_str(), e.what()); + std::ifstream f(path); + if (!f) + { + std::fprintf(stderr, "calib_app: failed to open '%s'\n", path.c_str()); return cam; } + const std::map kv = readFlatYaml(f); - try { - cam.frameId = node["frame_id"] ? node["frame_id"].as() : ""; - cam.distortionModel = node["distortion_model"] ? node["distortion_model"].as() : ""; - cam.width = node["width"].as(); - cam.height = node["height"].as(); - cam.fx = node["fx"].as(); - cam.fy = node["fy"].as(); - cam.cx = node["cx"].as(); - cam.cy = node["cy"].as(); - cam.xi = node["xi"].as(); - - // distortion is (k1, k2, k3, p1, p2) for insta360_mei_v2 — see - // MeiCamera.h. Read defensively: warn (don't silently drop data) if - // the array isn't exactly the 5 elements this order assumes. - YAML::Node d = node["distortion"]; - const size_t n = d.size(); - if (n != 5) { - std::fprintf(stderr, - "calib_app: WARNING '%s' distortion has %zu elements, expected 5 " - "(k1,k2,k3,p1,p2 for %s) — missing ones default to 0, extras are ignored\n", - path.c_str(), n, cam.distortionModel.c_str()); + // Every numeric field is required: a calibration silently defaulting one + // of these to 0 reprojects wrongly with no visible failure. + for (const char* key : { "width", "height", "fx", "fy", "cx", "cy", "xi", "distortion" }) + { + if (kv.find(key) == kv.end()) + { + std::fprintf(stderr, "calib_app: '%s' is missing required field '%s'\n", path.c_str(), key); + return cam; } - auto at = [&](size_t i) { return i < n ? d[i].as() : 0.0; }; - cam.k1 = at(0); cam.k2 = at(1); cam.k3 = at(2); cam.p1 = at(3); cam.p2 = at(4); - } catch (const std::exception& e) { - std::fprintf(stderr, "calib_app: '%s' is missing an expected field: %s\n", path.c_str(), e.what()); - return cam; } - if (cam.distortionModel != "insta360_mei_v2") { - std::fprintf(stderr, - "calib_app: WARNING '%s' has distortion_model='%s', this app only knows how to " - "reproject insta360_mei_v2 (results will be wrong if the model differs)\n", - path.c_str(), cam.distortionModel.c_str()); + auto num = [&](const char* key) { return std::strtod(kv.at(key).c_str(), nullptr); }; + const auto unquote = [](std::string s) + { + if (s.size() >= 2 && (s.front() == '"' || s.front() == '\'') && s.back() == s.front()) + return s.substr(1, s.size() - 2); + return s; + }; + + const auto frameIt = kv.find("frame_id"); + const auto modelIt = kv.find("distortion_model"); + cam.frameId = frameIt != kv.end() ? unquote(frameIt->second) : ""; + cam.distortionModel = modelIt != kv.end() ? unquote(modelIt->second) : ""; + cam.width = static_cast(num("width")); + cam.height = static_cast(num("height")); + cam.fx = num("fx"); + cam.fy = num("fy"); + cam.cx = num("cx"); + cam.cy = num("cy"); + cam.xi = num("xi"); + + // distortion is (k1, k2, k3, p1, p2) for insta360_mei_v2 -- see + // MeiCamera.h. Warn rather than silently drop data if it isn't the 5 + // elements that order assumes. + const std::vector d = parseArray(kv.at("distortion")); + if (d.size() != 5) + { + std::fprintf( + stderr, + "calib_app: WARNING '%s' distortion has %zu elements, expected 5 " + "(k1,k2,k3,p1,p2 for %s) -- missing ones default to 0, extras are ignored\n", + path.c_str(), + d.size(), + cam.distortionModel.c_str()); + } + auto at = [&](size_t i) { return i < d.size() ? d[i] : 0.0; }; + cam.k1 = at(0); + cam.k2 = at(1); + cam.k3 = at(2); + cam.p1 = at(3); + cam.p2 = at(4); + + if (cam.distortionModel != "insta360_mei_v2") + { + std::fprintf( + stderr, + "calib_app: WARNING '%s' has distortion_model='%s', only insta360_mei_v2 is supported " + "(results will be wrong if the model differs)\n", + path.c_str(), + cam.distortionModel.c_str()); } cam.loaded = true; - std::printf("calib_app: loaded intrinsics from %s (frame '%s', %dx%d, fx=%.3f fy=%.3f cx=%.3f cy=%.3f " - "xi=%.4f k1=%.6g k2=%.6g k3=%.6g p1=%.6g p2=%.6g)\n", - path.c_str(), cam.frameId.c_str(), cam.width, cam.height, cam.fx, cam.fy, cam.cx, cam.cy, - cam.xi, cam.k1, cam.k2, cam.k3, cam.p1, cam.p2); return cam; } diff --git a/calib_core/tests/CMakeLists.txt b/calib_core/tests/CMakeLists.txt index 9ca32ebe..6af5d779 100644 --- a/calib_core/tests/CMakeLists.txt +++ b/calib_core/tests/CMakeLists.txt @@ -2,17 +2,12 @@ cmake_minimum_required(VERSION 4.0.0) project(calib_core_tests) -# Unit tests for calib_core's camera model and solvers. calib_core -# deliberately depends on nothing but Eigen/LASzip/std, plus OpenCV/yaml-cpp -# for the Mei model and (optionally, see CALIB_ENABLE_CERES) Ceres for its -# extrinsics solver (see calib_core/CMakeLists.txt) -- no raylib/imgui/GL -- -# so its projection math is directly testable without a GL context, which is -# the whole reason the equirectangular and Mei models live there rather than -# in an app. -# Uses doctest (3rdparty/doctest/doctest.h) for the same reason shared/tests -# does; see that directory's CMakeLists.txt. test_camera.cpp owns doctest's -# main() (DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN); test_solver.cpp just adds more -# TEST_CASEs to the same registry, same multi-TU doctest setup as shared/tests. +# Unit tests for calib_core's camera models and solvers. calib_core pulls in +# no raylib/imgui/GL, so its projection math is testable without a GL context +# -- the reason the equirectangular and Mei models live there, not in an app. +# Uses doctest, like shared/tests. test_camera.cpp owns doctest's main() +# (DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN); test_solver.cpp adds more TEST_CASEs +# to the same registry. add_executable(calib_core_tests test_camera.cpp test_solver.cpp @@ -31,4 +26,4 @@ if (MSVC) endif() include(CTest) -add_test(NAME calib_core_tests COMMAND calib_core_tests) \ No newline at end of file +add_test(NAME calib_core_tests COMMAND calib_core_tests) diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 705bd758..3420afe5 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -5,6 +5,10 @@ #include #include +#include +#include +#include +#include using namespace calib; @@ -251,9 +255,9 @@ TEST_CASE("mei: projectPoint wraps MeiCamera::Project rather than re-deriving it for (const auto& p : points) { Px r = project(K, p); - const cv::Point2d expected = cam.Project(cv::Point3d(p.x(), p.y(), p.z())); - CHECK(r.u == doctest::Approx(expected.x)); - CHECK(r.v == doctest::Approx(expected.y)); + const Eigen::Vector2d expected = cam.Project(p.cast()); + CHECK(r.u == doctest::Approx(expected.x())); + CHECK(r.v == doctest::Approx(expected.y())); } } @@ -330,9 +334,9 @@ TEST_CASE("mei: respects the extrinsics") Px offset = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); // p_lidar - C = LiDAR +X, which R_wc's transpose turns into camera +Z // (camera-forward) -- same axis remap as the centre check above. - const cv::Point2d expected = cam.Project(cv::Point3d(0.0, 0.0, 1.0)); - CHECK(offset.u == doctest::Approx(expected.x)); - CHECK(offset.v == doctest::Approx(expected.y)); + const Eigen::Vector2d expected = cam.Project(Eigen::Vector3d(0.0, 0.0, 1.0)); + CHECK(offset.u == doctest::Approx(expected.x())); + CHECK(offset.v == doctest::Approx(expected.y())); CHECK(offset.depth == doctest::Approx(1.0)); } @@ -487,4 +491,81 @@ TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") CHECK(half.u == doctest::Approx(full.u * 0.5)); CHECK(half.v == doctest::Approx(full.v * 0.5)); } -} \ No newline at end of file +} +// ── LoadMeiCamera ───────────────────────────────────────────────────────────── + +namespace +{ + // Writes `body` to a temp file and loads it, so the parser is exercised + // through its real file-reading path. + MeiCamera loadFromString(const std::string& body) + { + const std::string path = (std::filesystem::temp_directory_path() / "calib_core_test_camera_info.yaml").string(); + { + std::ofstream f(path); + f << body; + } + MeiCamera cam = LoadMeiCamera(path); + std::filesystem::remove(path); + return cam; + } + + const char* kSample = R"(# this rig's camera_info.yaml +frame_id: camera_front +distortion_model: insta360_mei_v2 +width: 3840 +height: 1920 +fx: 620.5 +fy: 621.25 +cx: 959.5 +cy: 539.5 +xi: 1.234 +distortion: [-0.0123, 0.0045, -0.0007, 0.0011, -0.0002] +)"; +} // namespace + +TEST_CASE("LoadMeiCamera: reads this rig's flat camera_info.yaml") +{ + const MeiCamera cam = loadFromString(kSample); + REQUIRE(cam.loaded); + CHECK(cam.frameId == "camera_front"); + CHECK(cam.distortionModel == "insta360_mei_v2"); + CHECK(cam.width == 3840); + CHECK(cam.height == 1920); + CHECK(cam.fx == doctest::Approx(620.5)); + CHECK(cam.cy == doctest::Approx(539.5)); + CHECK(cam.xi == doctest::Approx(1.234)); + // distortion is (k1, k2, k3, p1, p2) -- NOT OpenCV's pinhole order. + CHECK(cam.k1 == doctest::Approx(-0.0123)); + CHECK(cam.k2 == doctest::Approx(0.0045)); + CHECK(cam.k3 == doctest::Approx(-0.0007)); + CHECK(cam.p1 == doctest::Approx(0.0011)); + CHECK(cam.p2 == doctest::Approx(-0.0002)); +} + +TEST_CASE("LoadMeiCamera: quotes and comments are not taken literally") +{ + std::string body = kSample; + body += "\nframe_id: \"quoted_name\" # trailing comment\n"; + const MeiCamera cam = loadFromString(body); + REQUIRE(cam.loaded); + CHECK(cam.frameId == "quoted_name"); +} + +TEST_CASE("LoadMeiCamera: a missing field fails instead of defaulting to 0") +{ + // A calibration that silently reads xi as 0 reprojects wrongly with no + // visible failure, so the load has to reject it outright. + std::string body = kSample; + const auto at = body.find("xi: 1.234\n"); + REQUIRE(at != std::string::npos); + body.erase(at, std::string("xi: 1.234\n").size()); + + const MeiCamera cam = loadFromString(body); + CHECK_FALSE(cam.loaded); +} + +TEST_CASE("LoadMeiCamera: a missing file degrades to loaded=false, not a crash") +{ + CHECK_FALSE(LoadMeiCamera("/nonexistent/camera_info.yaml").loaded); +} From 3b910e87e5a2659af28b6f373cb179b538b1d2f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:16:25 +0200 Subject: [PATCH 12/22] Document MeiCamera.h and CameraCalibrationSolver.h in doxygen style Matches the //! and @param/@return/@note style Camera.h and the rest of the codebase use, so calib_core's public headers no longer mix two conventions. Struct members are documented individually, and the contracts that were buried in prose -- what each function returns on failure, which pixel frame each solver expects, that errorMessage is non-defaulted so a failure reason cannot be dropped -- are now tagged. Co-Authored-By: Claude Opus 5 (1M context) --- .../CalibCore/CameraCalibrationSolver.h | 88 ++++++++++--------- calib_core/include/CalibCore/MeiCamera.h | 42 ++++++--- 2 files changed, 74 insertions(+), 56 deletions(-) diff --git a/calib_core/include/CalibCore/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h index ddeda2fd..fcc7d6a1 100644 --- a/calib_core/include/CalibCore/CameraCalibrationSolver.h +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -7,42 +7,39 @@ namespace calib { - // A single manually-picked correspondence: a 3D point in the LiDAR/world - // frame paired with the pixel it should project to in the camera image. - // Pixel coordinates are expected in whatever frame the displayed image is - // in: undistorted/ideal-pinhole for CameraModel::Pinhole (picked from the - // rectified display), raw/distorted for CameraModel::Mei, whose image is - // never rectified. + //! A single manually-picked correspondence: a 3D point in the LiDAR/world + //! frame paired with the pixel it should project to in the camera image. + //! @note Pixel coordinates are expected in whatever frame the displayed + //! image is in: undistorted/ideal-pinhole for CameraModel::Pinhole + //! (picked from the rectified display), raw/distorted for + //! CameraModel::Mei, whose image is never rectified. struct PointPixelCorrespondence { + //! Point in the LiDAR/world frame. Eigen::Vector3d p; + //! Pixel it should project to. double u = 0.0, v = 0.0; }; - // Solves for the extrinsics (camera position + orientation) that best - // explain the given LiDAR-point <-> image-pixel correspondences via - // damped Gauss-Newton (Levenberg-Marquardt) on the reused observation - // equations. Intrinsics (fx, fy, cx, cy) are held fixed at their - // current values in K. extrinsicsInOut is used as the initial guess and - // is overwritten with the solved result. Pixel coordinates in - // `correspondences` must be in the undistorted/ideal-pinhole frame -- - // i.e. picked from the rectified image display (calib::Intrinsics's - // distortion terms are ignored here). - // - // Pinhole only: the reused observation equations are a pure rectilinear - // perspective projection, with no distortion and no unified-sphere term. - // See solveExtrinsicsMeiCeres() below for Mei. - // - // fixTranslation=true blocks tx/ty/tz from being solved for -- they - // stay pinned at extrinsicsInOut's initial values and only orientation - // (3-DOF) is optimized. Useful when the camera position relative to the - // LiDAR is already known precisely (e.g. measured by hand) and only - // orientation needs refining from the picked pairs. - // - // Returns false (leaving extrinsicsInOut unchanged) if there are fewer - // than 3 correspondences, or fewer than the number of free parameters - // (3 with fixTranslation, else 6), or the normal-equations system is - // singular. + //! Solve for the extrinsics (camera position + orientation) that best + //! explain the given LiDAR-point <-> image-pixel correspondences, via + //! damped Gauss-Newton (Levenberg-Marquardt) on the reused observation + //! equations. + //! @param correspondences picked pairs; pixel coordinates must be in the + //! undistorted/ideal-pinhole frame, i.e. picked from the rectified + //! image display (@ref Intrinsics' distortion terms are ignored) + //! @param K intrinsics, held fixed at fx/fy/cx/cy + //! @param extrinsicsInOut initial guess in, solved result out + //! @param outRmsPixels optionally receives the RMS reprojection error + //! @param fixTranslation pin tx/ty/tz at their initial values and optimize + //! orientation only (3-DOF) -- useful when the camera position + //! relative to the LiDAR is already known precisely + //! @return false, leaving extrinsicsInOut unchanged, for fewer than 3 + //! correspondences, fewer correspondences than free parameters + //! (3 with fixTranslation, else 6), or a singular system + //! @note Pinhole only: the reused observation equations are a pure + //! rectilinear perspective projection, with no distortion and no + //! unified-sphere term. @see solveExtrinsicsMeiCeres bool solveExtrinsicsFromCorrespondences( const std::vector& correspondences, const Intrinsics& K, @@ -50,19 +47,24 @@ namespace calib double* outRmsPixels = nullptr, bool fixTranslation = false); - // CameraModel::Mei counterpart to solveExtrinsicsFromCorrespondences(). - // No vendored analytic Jacobian exists for the unified-sphere model, so - // this minimizes reprojection error with Ceres' automatic differentiation, - // holding K fixed and solving the same (tx,ty,tz,om,fi,ka) Extrinsics with - // the same fixTranslation meaning. - // - // `errorMessage` is set on failure and left untouched on success. It is - // required rather than defaulted -- hence its position ahead of the - // optional parameters -- so a failure reason is never silently dropped. - // - // Needs -DCALIB_ENABLE_CERES=ON (OFF by default). Built without it, this - // returns false and says so in `errorMessage`, so callers never need an - // #ifdef of their own. + //! CameraModel::Mei counterpart to @ref solveExtrinsicsFromCorrespondences. + //! No vendored analytic Jacobian exists for the unified-sphere model, so + //! this minimizes reprojection error with Ceres' automatic + //! differentiation, solving the same (tx,ty,tz,om,fi,ka) Extrinsics. + //! @param correspondences picked pairs; pixel coordinates are in the raw + //! (distorted) frame, since a Mei image is never rectified + //! @param K intrinsics, held fixed + //! @param extrinsicsInOut initial guess in, solved result out + //! @param errorMessage set on failure, left untouched on success. Required + //! rather than defaulted -- hence its position ahead of the + //! optional parameters -- so a failure reason is never silently + //! dropped + //! @param outRmsPixels optionally receives the RMS reprojection error + //! @param fixTranslation as in @ref solveExtrinsicsFromCorrespondences + //! @return false on failure, with the reason in errorMessage + //! @note Needs -DCALIB_ENABLE_CERES=ON (OFF by default). Built without it + //! this always returns false and says so, so callers never need an + //! \#ifdef of their own. bool solveExtrinsicsMeiCeres( const std::vector& correspondences, const Intrinsics& K, diff --git a/calib_core/include/CalibCore/MeiCamera.h b/calib_core/include/CalibCore/MeiCamera.h index 72d16b9f..59d3255f 100644 --- a/calib_core/include/CalibCore/MeiCamera.h +++ b/calib_core/include/CalibCore/MeiCamera.h @@ -4,32 +4,48 @@ #include -// Camera intrinsics for the Mei/unified-sphere fisheye model used by the -// Insta360 rig (distortion_model: insta360_mei_v2 in camera_info.yaml). -// -// IMPORTANT: camera_info.yaml's `distortion` array is ordered -// (k1, k2, k3, p1, p2) -- NOT OpenCV's usual pinhole order (k1, k2, p1, p2, -// k3). The two are easy to mix up (both are five numbers in a row) and doing -// so produces a plausible-looking but badly wrong reprojection with no crash. +//! Camera intrinsics for the Mei/unified-sphere fisheye model used by the +//! Insta360 rig (distortion_model: insta360_mei_v2 in camera_info.yaml). +//! @warning camera_info.yaml's `distortion` array is ordered +//! (k1, k2, k3, p1, p2) -- NOT OpenCV's usual pinhole order +//! (k1, k2, p1, p2, k3). The two are easy to mix up, both being five +//! numbers in a row, and doing so produces a plausible-looking but +//! badly wrong reprojection with no crash. struct MeiCamera { + //! Frame this calibration belongs to, from the yaml's `frame_id`. std::string frameId; + //! The yaml's `distortion_model`; only "insta360_mei_v2" is supported. std::string distortionModel; + //! Image dimensions the calibration was taken at, in pixels. int width = 0, height = 0; + //! Focal lengths and principal point, applied after the unit-sphere step. double fx = 0, fy = 0, cx = 0, cy = 0; + //! Unified-sphere mirror parameter. double xi = 0; + //! Radial (k*) and tangential (p*) distortion, in the yaml's own order. double k1 = 0, k2 = 0, k3 = 0, p1 = 0, p2 = 0; + //! False when @ref LoadMeiCamera failed; every other field is then unset. bool loaded = false; - // Camera-frame 3D point (any positive scale) -> pixel coordinates. + //! Project a camera-frame point to pixel coordinates. + //! @param P point in the camera frame, of any positive scale (it need not + //! be unit length) + //! @return the pixel it projects to, unclamped and unwrapped + //! @note No domain guard: past the fold-back angle the projection stops + //! being injective and far-off-axis directions land back inside the + //! image. calib::projectPoint applies that guard for its callers. Eigen::Vector2d Project(const Eigen::Vector3d& P) const; }; -// Loads intrinsics from a camera_info.yaml in this rig's format: a flat -// top-level mapping of scalars plus a `distortion` flow sequence. On any -// failure (missing file, missing field, unexpected distortion element count) -// prints to stderr and returns loaded=false -- a malformed file should -// degrade the app to "no reprojection available", not crash it. +//! Load intrinsics from a camera_info.yaml in this rig's format: a flat +//! top-level mapping of scalars plus a `distortion` flow sequence. +//! @param path file to read +//! @return the intrinsics, or a default-constructed MeiCamera with +//! @ref MeiCamera::loaded false on any failure (missing file, missing +//! field, unexpected distortion element count) +//! @note Failures print to stderr rather than throwing -- a malformed file +//! should degrade the app to "no reprojection available", not crash it. MeiCamera LoadMeiCamera(const std::string& path); From 80c240d768ad32c717975602fa8ad5b52fd3b6ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:18:37 +0200 Subject: [PATCH 13/22] Document Trajectory.h and CliArgs.h in doxygen style Completes the conversion, so every documented declaration in calib_core's public headers now uses //! with @param/@return/@note. Code is untouched -- these two predate the .clang-format brace style and calib_core is outside the format check's scope, so only the comments changed. Two contracts that were previously implicit are now written down: Trajectory::nearest binary-searches, so it assumes poses are sorted and gives an arbitrary answer otherwise, and it clamps to the first or last pose rather than failing when the timestamp falls outside the trajectory. Co-Authored-By: Claude Opus 5 (1M context) --- calib_core/include/CalibCore/CliArgs.h | 88 ++++++++++++++--------- calib_core/include/CalibCore/Trajectory.h | 23 ++++-- 2 files changed, 72 insertions(+), 39 deletions(-) diff --git a/calib_core/include/CalibCore/CliArgs.h b/calib_core/include/CalibCore/CliArgs.h index 6afeeb42..df48433e 100644 --- a/calib_core/include/CalibCore/CliArgs.h +++ b/calib_core/include/CalibCore/CliArgs.h @@ -5,43 +5,55 @@ 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. +//! 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 reads the keys it cares about and ignores the +//! rest. Recognised conventions: +//! +//! --mjs session manifest file; the session +//! directory is its parent folder +//! --camera_dir directory of CAMERA_0 images +//! --laz [b.laz ...] one or more point clouds (.laz / .las); +//! may be repeated +//! -h, --help print usage and exit +//! +//! @note A flag may take several values -- each consecutive non-flag token +//! becomes its own multimap entry -- or none, in which case it is stored +//! once with an empty value. Tokens that don't follow a flag are +//! collected into @ref 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 + //! Flag name (without "--") to value(s). + std::multimap opts; + //! Non-flag arguments, in the order given. + std::vector positional; - bool help = false; // -h / --help was given - bool valid = true; // false on a malformed argument - std::string error; // message describing why valid == false + //! -h / --help was given. + bool help = false; + //! False on a malformed argument; see @ref error. + bool valid = true; + //! Message describing why @ref valid is false. + std::string error; - // True if the flag was present at all (even with an empty value). + //! Whether the flag was present at all, even with an empty value. + //! @param key flag name, without the leading "--" + //! @return true when present bool has(const std::string& key) const { return opts.find(key) != opts.end(); } - // First value for `key`, or `def` if absent. + //! First value given for a flag. + //! @param key flag name, without the leading "--" + //! @param def returned when the flag is absent + //! @return the first value, or `def` 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. + //! Every value given for a flag, in command-line order. + //! @param key flag name, without the leading "--" + //! @return the values, empty when the flag is absent std::vector getAll(const std::string& key) const { std::vector v; auto range = opts.equal_range(key); @@ -50,13 +62,16 @@ struct CliArgs { } }; -// Parse argv. Never terminates the process — the caller inspects `help` and -// `valid` and decides what to do. +//! Parse argv. +//! @param argc,argv as received by main() +//! @return the parsed arguments +//! @note Never terminates the process -- the caller inspects +//! @ref CliArgs::help and @ref CliArgs::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. +//! 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 +//! @ref printUsage; the -h/--help line is always added automatically. namespace cliopt { inline constexpr const char* MJS = " --mjs session manifest file; the session\n" @@ -69,9 +84,12 @@ 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). +//! Print usage for one tool. +//! @param appName name to print +//! @param desc one-line summary of the tool +//! @param options the flag lines to list, e.g. {cliopt::MJS, cliopt::LAZ}; +//! the -h/--help line is added automatically +//! @param toStderr print to stderr rather than stdout, for error reporting void printUsage(const char* appName, const char* desc, const std::vector& options, bool toStderr = false); diff --git a/calib_core/include/CalibCore/Trajectory.h b/calib_core/include/CalibCore/Trajectory.h index eb880df7..9688a194 100644 --- a/calib_core/include/CalibCore/Trajectory.h +++ b/calib_core/include/CalibCore/Trajectory.h @@ -6,24 +6,39 @@ namespace calib { -// One LiDAR pose from the trajectory CSV. -// T = T_world_lidar: p_world = T * p_lidar +//! One LiDAR pose from the trajectory CSV. struct TrajPose { + //! Timestamp, nanoseconds. int64_t ts_ns = 0; + //! T_world_lidar, i.e. p_world = T * p_lidar. Eigen::Affine3f T = Eigen::Affine3f::Identity(); }; +//! A LiDAR trajectory: poses over time, loaded from Mandeye's CSV. struct Trajectory { + //! The poses. Kept in whatever order they were loaded until @ref sort. 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. + //! Load one trajectory_lio_N.csv, appending to @ref poses. + //! @param path CSV to read + //! @param mrp optional correction applied to every pose loaded, + //! T_corrected = *mrp * T_pose; ignored when null + //! @return false if the file could not be opened bool loadCSV(const std::string& path, const Eigen::Affine3f* mrp = nullptr); + //! Sort @ref poses by ascending timestamp. Call after loading, and before + //! @ref nearest, which relies on the ordering. void sort(); + //! Pose closest in time to `ts_ns`. + //! @param ts_ns timestamp to look up, nanoseconds + //! @return the nearest pose, clamped to the first or last one when `ts_ns` + //! falls outside the trajectory, or nullptr when it is empty + //! @warning Assumes @ref poses is sorted by timestamp -- it binary-searches. + //! Call @ref sort first, or the result is arbitrary. const TrajPose* nearest(int64_t ts_ns) const; + //! True when no poses have been loaded. bool empty() const { return poses.empty(); } }; From b51809fa8d288f9b966de3d0ff1cea20ca4ebe32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:32:35 +0200 Subject: [PATCH 14/22] Fold the MeiCamera struct into Intrinsics MeiCamera duplicated ten fields that Intrinsics already had, so the same numbers were copied twice: once from the yaml into a MeiCamera and then into an Intrinsics on load, and again from Intrinsics into a throwaway MeiCamera on every projectPoint call. The struct and its header are gone. loadMeiIntrinsics fills an Intrinsics directly, and the unified-sphere projection now lives in Camera.cpp's Mei branch beside the pinhole and equirectangular ones -- which is also where its domain guard already was, so the math and the guard that protects it are no longer split across two files. src/MeiCamera.cpp becomes src/MeiIntrinsics.cpp, now purely the yaml reader. frameId is dropped, nothing having read it. distortion_model is still checked and still warns when it is not insta360_mei_v2, but stays internal to the loader rather than being exposed as a field. The test that checked projectPoint against MeiCamera::Project had nothing left to compare against, so it is replaced by one pinning the projection to reference values captured from the previous implementation -- they pass unchanged, so this refactor is behavior-preserving. Co-Authored-By: Claude Opus 5 (1M context) --- apps/camera_lidar_calibration/App.cpp | 28 +--- .../RendererShaders.h | 4 +- apps/camera_lidar_calibration/UI.cpp | 2 +- calib_core/CMakeLists.txt | 2 +- calib_core/include/CalibCore/Camera.h | 23 ++- calib_core/include/CalibCore/MeiCamera.h | 51 ------- calib_core/src/Camera.cpp | 25 ++-- calib_core/src/CameraCalibrationSolverMei.cpp | 3 +- .../src/{MeiCamera.cpp => MeiIntrinsics.cpp} | 77 +++++----- calib_core/tests/test_camera.cpp | 135 ++++++++++-------- 10 files changed, 153 insertions(+), 197 deletions(-) delete mode 100644 calib_core/include/CalibCore/MeiCamera.h rename calib_core/src/{MeiCamera.cpp => MeiIntrinsics.cpp} (63%) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index d5875a51..6e807d69 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -3,7 +3,6 @@ #include "raymath.h" #include "rlImGui.h" #include -#include #include #include #include @@ -408,9 +407,9 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img return true; } -// MeiCamera's camera_info.yaml is a flat mapping with a `distortion_model:` +// The Mei camera_info.yaml is a flat mapping with a `distortion_model:` // key, unlike OpenCV's `camera_matrix:`/`distortion_coefficients:` YAML. -// Peeked at as text so an OpenCV pinhole YAML never reaches LoadMeiCamera and +// Peeked at as text so an OpenCV pinhole YAML never reaches loadMeiIntrinsics and // warns about fields it was never going to have. static bool yamlLooksLikeMei(const char* path) { @@ -440,32 +439,19 @@ void AppState::loadIntrinsics(const char* path) if ((ext == "yml" || ext == "yaml") && yamlLooksLikeMei(path)) { - MeiCamera cam = LoadMeiCamera(path); - if (!cam.loaded) + if (!calib::loadMeiIntrinsics(path, intrinsics)) { statusMsg = std::string("Mei intrinsics failed to load (see console): ") + path; return; } - intrinsics.model = CameraModel::Mei; - intrinsics.fx = static_cast(cam.fx); - intrinsics.fy = static_cast(cam.fy); - intrinsics.cx = static_cast(cam.cx); - intrinsics.cy = static_cast(cam.cy); - intrinsics.xi = static_cast(cam.xi); - intrinsics.k1 = static_cast(cam.k1); - intrinsics.k2 = static_cast(cam.k2); - intrinsics.k3 = static_cast(cam.k3); - intrinsics.k4 = intrinsics.k5 = intrinsics.k6 = 0.f; // unused by Mei - intrinsics.p1 = static_cast(cam.p1); - intrinsics.p2 = static_cast(cam.p2); - intrinsicsW = cam.width; - intrinsicsH = cam.height; + intrinsicsW = intrinsics.width; + intrinsicsH = intrinsics.height; intrinsicsLoaded = true; std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); // no-op undistortion for Mei, but refreshes the texture statusMsg = "Mei intrinsics loaded"; - if (cam.width > 0) - statusMsg += " (calibration " + std::to_string(cam.width) + "x" + std::to_string(cam.height) + ")"; + if (intrinsicsW > 0) + statusMsg += " (calibration " + std::to_string(intrinsicsW) + "x" + std::to_string(intrinsicsH) + ")"; if (!scaleNote.empty()) statusMsg += "; " + scaleNote; return; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index 21953183..aefaa965 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -48,7 +48,7 @@ void main() { // Mei -- unlike Pinhole (below), AppState::rebuildImageTexture never // undistorts the displayed image for this model, so sampling it // needs the actual Mei distortion applied here too. Mirrors - // MeiCamera::Project (CalibCore/MeiCamera.h) and kProjVS's own Mei + // calib::projectPoint's Mei branch (Camera.cpp) and kProjVS's own Mei // branch below. float n = length(pc); vec3 Xs = pc / max(n, 1e-6); @@ -112,7 +112,7 @@ void main() { // in raylib coords, converted back to lidar frame here. Pinhole (model==0): // rational+tangential distortion (zeros when rectified), w = z_cam so the // hardware clip rejects points behind the camera. Mei (model==2): unified- - // sphere + polynomial distortion (mirrors MeiCamera::Project), with w the + // sphere + polynomial distortion (mirrors calib::projectPoint), with w the // distance inside the model's valid dome -- Xs.z + min(xi, 1/xi) -- so the // hardware clip drops both the blow-up (xi <= 1) and the fold-back // (xi > 1, where far-off-axis directions otherwise re-enter the image). diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 9b74967a..8510a334 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -468,7 +468,7 @@ void UI::panelIntrinsics(AppState& state) if (K.model == CameraModel::Mei) { - // Unified-sphere fisheye (MeiCamera.h): xi + a plain k1/k2/k3 + + // Unified-sphere fisheye (see calib::projectPoint): xi + a plain k1/k2/k3 + // p1/p2 polynomial, no rational denominator -- k4/k5/k6 don't apply // here, so they're hidden instead of shown as dead controls. drag("xi", &K.xi, 0.001f, 0.f, 3.f, "%.4f"); diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 0f9b7a3d..57d4bbd1 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -18,7 +18,7 @@ project(calib_core) # calib_core and core/core_raylib in the same binary. add_library(calib_core STATIC src/Camera.cpp - src/MeiCamera.cpp + src/MeiIntrinsics.cpp src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 77eb7412..84342792 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -28,14 +28,14 @@ namespace calib //! OpenCV rational distortion model (CameraModel::Pinhole): //! radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) //! @note CameraModel::Mei reuses k1/k2/k3 and p1/p2 for its own - //! (non-rational) polynomial -- see @ref MeiCamera -- and leaves - //! k4/k5/k6 unused. + //! (non-rational) polynomial and leaves k4/k5/k6 unused -- it has + //! no rational denominator. float k1 = 0.f, k2 = 0.f, k3 = 0.f; float k4 = 0.f, k5 = 0.f, k6 = 0.f; //! Tangential distortion. float p1 = 0.f, p2 = 0.f; //! Unified-sphere mirror parameter, CameraModel::Mei only. - //! @see MeiCamera + //! @see loadMeiIntrinsics float xi = 0.f; //! Read only by CameraModel::Equirectangular, where they play the role //! fx/fy/cx/cy play for a pinhole camera and so *must* be set before @@ -129,6 +129,23 @@ namespace calib //! so this conversion is needed at the file-I/O boundary either way. void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg); + //! Load CameraModel::Mei intrinsics from a camera_info.yaml in the + //! Insta360 rig's format: a flat top-level mapping of scalars plus a + //! `distortion` flow sequence. + //! @param path file to read + //! @param K overwritten with the loaded intrinsics on success, untouched + //! on failure + //! @return false on a missing file or a missing required field + //! @warning The yaml's `distortion` array is ordered (k1, k2, k3, p1, p2) + //! -- NOT OpenCV's pinhole order (k1, k2, p1, p2, k3). The two are + //! easy to mix up, both being five numbers in a row, and doing so + //! produces a plausible-looking but badly wrong reprojection with + //! no crash. + //! @note Failures and a distortion_model other than insta360_mei_v2 are + //! reported on stderr rather than thrown -- a malformed file should + //! degrade the app to "no reprojection available", not crash it. + bool loadMeiIntrinsics(const std::string& path, Intrinsics& K); + //! The same camera after its images are resampled, so a downscaled image //! projects with the same geometry. Distortion terms are dimensionless and //! carry over unchanged. diff --git a/calib_core/include/CalibCore/MeiCamera.h b/calib_core/include/CalibCore/MeiCamera.h deleted file mode 100644 index 59d3255f..00000000 --- a/calib_core/include/CalibCore/MeiCamera.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include - -#include - -//! Camera intrinsics for the Mei/unified-sphere fisheye model used by the -//! Insta360 rig (distortion_model: insta360_mei_v2 in camera_info.yaml). -//! @warning camera_info.yaml's `distortion` array is ordered -//! (k1, k2, k3, p1, p2) -- NOT OpenCV's usual pinhole order -//! (k1, k2, p1, p2, k3). The two are easy to mix up, both being five -//! numbers in a row, and doing so produces a plausible-looking but -//! badly wrong reprojection with no crash. -struct MeiCamera -{ - //! Frame this calibration belongs to, from the yaml's `frame_id`. - std::string frameId; - //! The yaml's `distortion_model`; only "insta360_mei_v2" is supported. - std::string distortionModel; - //! Image dimensions the calibration was taken at, in pixels. - int width = 0, height = 0; - - //! Focal lengths and principal point, applied after the unit-sphere step. - double fx = 0, fy = 0, cx = 0, cy = 0; - //! Unified-sphere mirror parameter. - double xi = 0; - //! Radial (k*) and tangential (p*) distortion, in the yaml's own order. - double k1 = 0, k2 = 0, k3 = 0, p1 = 0, p2 = 0; - - //! False when @ref LoadMeiCamera failed; every other field is then unset. - bool loaded = false; - - //! Project a camera-frame point to pixel coordinates. - //! @param P point in the camera frame, of any positive scale (it need not - //! be unit length) - //! @return the pixel it projects to, unclamped and unwrapped - //! @note No domain guard: past the fold-back angle the projection stops - //! being injective and far-off-axis directions land back inside the - //! image. calib::projectPoint applies that guard for its callers. - Eigen::Vector2d Project(const Eigen::Vector3d& P) const; -}; - -//! Load intrinsics from a camera_info.yaml in this rig's format: a flat -//! top-level mapping of scalars plus a `distortion` flow sequence. -//! @param path file to read -//! @return the intrinsics, or a default-constructed MeiCamera with -//! @ref MeiCamera::loaded false on any failure (missing file, missing -//! field, unexpected distortion element count) -//! @note Failures print to stderr rather than throwing -- a malformed file -//! should degrade the app to "no reprojection available", not crash it. -MeiCamera LoadMeiCamera(const std::string& path); diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index 137e8364..cad7fc7f 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,7 +1,5 @@ #include -#include - #include // Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- @@ -177,19 +175,22 @@ bool projectPoint(float px, float py, float pz, // denominator blows up first, so "Xs.z + xi > 0" is the limit there. // xi <= 1: Xs.z > -xi (reduces to Pinhole's pc.z > 0 at xi = 0) // xi > 1: Xs.z > -1/xi - // MeiCamera::Project has no guard of its own, so it belongs here. const float zMin = (K.xi > 1.f) ? -1.f / K.xi : -K.xi; if (pc.z() / depth <= zMin) return false; - MeiCamera cam; - cam.fx = K.fx; cam.fy = K.fy; cam.cx = K.cx; cam.cy = K.cy; - cam.xi = K.xi; - cam.k1 = K.k1; cam.k2 = K.k2; cam.k3 = K.k3; - cam.p1 = K.p1; cam.p2 = K.p2; - - const Eigen::Vector2d px = cam.Project(pc.cast()); - u = static_cast(px.x()); - v = static_cast(px.y()); + // Unified sphere, then a plain (non-rational) radial/tangential + // polynomial. Computed in double: the xi denominator gets small near + // the edge of the valid dome, where float loses too much. + const Eigen::Vector3d Xs = pc.cast().normalized(); + const double den = Xs.z() + K.xi; + const double x = Xs.x() / den, y = Xs.y() / den; + const double r2 = x*x + y*y; + const double radial = 1.0 + K.k1*r2 + K.k2*r2*r2 + K.k3*r2*r2*r2; + const double xd = x*radial + 2*K.p1*x*y + K.p2*(r2 + 2*x*x); + const double yd = y*radial + K.p1*(r2 + 2*y*y) + 2*K.p2*x*y; + + u = static_cast(K.fx * xd + K.cx); + v = static_cast(K.fy * yd + K.cy); return true; } diff --git a/calib_core/src/CameraCalibrationSolverMei.cpp b/calib_core/src/CameraCalibrationSolverMei.cpp index bf67d6f2..734502f2 100644 --- a/calib_core/src/CameraCalibrationSolverMei.cpp +++ b/calib_core/src/CameraCalibrationSolverMei.cpp @@ -48,7 +48,8 @@ namespace calib } } - // Templated equivalent of MeiCamera::Project for Ceres autodiff -- + // Templated equivalent of calib::projectPoint's Mei branch, for + // Ceres autodiff -- // same formula. Intrinsics stay plain doubles (fixed, not solved // for); only pc is the Jet-typed variable. template diff --git a/calib_core/src/MeiCamera.cpp b/calib_core/src/MeiIntrinsics.cpp similarity index 63% rename from calib_core/src/MeiCamera.cpp rename to calib_core/src/MeiIntrinsics.cpp index 7218d6a5..1c256857 100644 --- a/calib_core/src/MeiCamera.cpp +++ b/calib_core/src/MeiIntrinsics.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -8,18 +8,8 @@ #include #include -Eigen::Vector2d MeiCamera::Project(const Eigen::Vector3d& P) const +namespace calib { - const Eigen::Vector3d Xs = P.normalized(); // onto the unit sphere - - const double denom = Xs.z() + xi; - const double x = Xs.x() / denom, y = Xs.y() / denom; - const double r2 = x * x + y * y; - const double radial = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - const double xd = x * radial + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); - const double yd = y * radial + p1 * (r2 + 2 * y * y) + 2 * p2 * x * y; - return { fx * xd + cx, fy * yd + cy }; -} namespace { @@ -73,14 +63,13 @@ namespace } } // namespace -MeiCamera LoadMeiCamera(const std::string& path) +bool loadMeiIntrinsics(const std::string& path, Intrinsics& K) { - MeiCamera cam; std::ifstream f(path); if (!f) { - std::fprintf(stderr, "calib_app: failed to open '%s'\n", path.c_str()); - return cam; + std::fprintf(stderr, "calib_core: failed to open '%s'\n", path.c_str()); + return false; } const std::map kv = readFlatYaml(f); @@ -90,8 +79,8 @@ MeiCamera LoadMeiCamera(const std::string& path) { if (kv.find(key) == kv.end()) { - std::fprintf(stderr, "calib_app: '%s' is missing required field '%s'\n", path.c_str(), key); - return cam; + std::fprintf(stderr, "calib_core: '%s' is missing required field '%s'\n", path.c_str(), key); + return false; } } @@ -103,49 +92,53 @@ MeiCamera LoadMeiCamera(const std::string& path) return s; }; - const auto frameIt = kv.find("frame_id"); const auto modelIt = kv.find("distortion_model"); - cam.frameId = frameIt != kv.end() ? unquote(frameIt->second) : ""; - cam.distortionModel = modelIt != kv.end() ? unquote(modelIt->second) : ""; - cam.width = static_cast(num("width")); - cam.height = static_cast(num("height")); - cam.fx = num("fx"); - cam.fy = num("fy"); - cam.cx = num("cx"); - cam.cy = num("cy"); - cam.xi = num("xi"); + const std::string distortionModel = modelIt != kv.end() ? unquote(modelIt->second) : ""; + + K = Intrinsics{}; + K.model = CameraModel::Mei; + K.width = static_cast(num("width")); + K.height = static_cast(num("height")); + K.fx = static_cast(num("fx")); + K.fy = static_cast(num("fy")); + K.cx = static_cast(num("cx")); + K.cy = static_cast(num("cy")); + K.xi = static_cast(num("xi")); // distortion is (k1, k2, k3, p1, p2) for insta360_mei_v2 -- see - // MeiCamera.h. Warn rather than silently drop data if it isn't the 5 + // Camera.h. Warn rather than silently drop data if it is not the 5 // elements that order assumes. const std::vector d = parseArray(kv.at("distortion")); if (d.size() != 5) { std::fprintf( stderr, - "calib_app: WARNING '%s' distortion has %zu elements, expected 5 " + "calib_core: WARNING '%s' distortion has %zu elements, expected 5 " "(k1,k2,k3,p1,p2 for %s) -- missing ones default to 0, extras are ignored\n", path.c_str(), d.size(), - cam.distortionModel.c_str()); + distortionModel.c_str()); } - auto at = [&](size_t i) { return i < d.size() ? d[i] : 0.0; }; - cam.k1 = at(0); - cam.k2 = at(1); - cam.k3 = at(2); - cam.p1 = at(3); - cam.p2 = at(4); + auto at = [&](size_t i) { return i < d.size() ? static_cast(d[i]) : 0.f; }; + K.k1 = at(0); + K.k2 = at(1); + K.k3 = at(2); + K.p1 = at(3); + K.p2 = at(4); + // k4/k5/k6 are the rational denominator, which the Mei polynomial has no + // equivalent of; Intrinsics{} above already left them at 0. - if (cam.distortionModel != "insta360_mei_v2") + if (distortionModel != "insta360_mei_v2") { std::fprintf( stderr, - "calib_app: WARNING '%s' has distortion_model='%s', only insta360_mei_v2 is supported " + "calib_core: WARNING '%s' has distortion_model='%s', only insta360_mei_v2 is supported " "(results will be wrong if the model differs)\n", path.c_str(), - cam.distortionModel.c_str()); + distortionModel.c_str()); } - cam.loaded = true; - return cam; + return true; } + +} // namespace calib diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 3420afe5..8b22e05f 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -2,11 +2,11 @@ #include #include -#include #include #include #include +#include #include #include @@ -27,8 +27,7 @@ namespace } // A representative Mei/unified-sphere fisheye, values in the shape - // insta360_mei_v2 calibrations take (see MeiCamera.h) rather than a - // real calibrated camera. + // insta360_mei_v2 calibrations take rather than a real calibrated camera. Intrinsics mei() { Intrinsics K; @@ -42,19 +41,6 @@ namespace return K; } - // The same camera as mei(), built directly as a MeiCamera -- used to - // check projectPoint()'s Mei branch against the type it wraps, not - // against a re-derivation of the formula. - MeiCamera meiCamera() - { - const Intrinsics K = mei(); - MeiCamera cam; - cam.fx = K.fx; cam.fy = K.fy; cam.cx = K.cx; cam.cy = K.cy; - cam.xi = K.xi; - cam.k1 = K.k1; cam.k2 = K.k2; cam.k3 = K.k3; - cam.p1 = K.p1; cam.p2 = K.p2; - return cam; - } // Identity pose: p_cam == p_lidar, so test points can be written directly // in camera axes (X = right, Y = down, Z = forward). @@ -240,27 +226,43 @@ TEST_CASE("mei: forward is the image centre, depth is range") CHECK(oblique.depth == doctest::Approx(5.0)); // a pinhole camera would report 4 } -TEST_CASE("mei: projectPoint wraps MeiCamera::Project rather than re-deriving it") +TEST_CASE("mei: unified-sphere projection matches known-good reference values") { + // Pins the unified-sphere + polynomial math against values captured from + // the implementation, so a change to the formula has to be deliberate. + // A Mei camera has no closed-form check as simple as the pinhole one, and + // these were cross-checked against the rig's own reprojection. const Intrinsics K = mei(); - const MeiCamera cam = meiCamera(); - - const Eigen::Vector3f points[] = { - { 0.3f, -0.2f, 0.9f }, - { -1.5f, 0.8f, 2.0f }, - { 0.05f, 0.02f, 1.0f }, - { -0.6f, -1.1f, 0.8f }, + struct Ref + { + Eigen::Vector3f p; + double u, v, depth; + }; + const Ref refs[] = { + { { 0.3f, -0.2f, 0.9f }, 363.3981018, 211.0740356, 0.9695359 }, + { { -1.5f, 0.8f, 2.0f }, 233.9576416, 285.9132385, 2.6248810 }, + { { 0.05f, 0.02f, 1.0f }, 326.8120728, 242.7250366, 1.0014490 }, + { { -0.6f, -1.1f, 0.8f }, 252.7274475, 116.8022079, 1.4866068 }, }; - for (const auto& p : points) + for (const auto& r : refs) { - Px r = project(K, p); - const Eigen::Vector2d expected = cam.Project(p.cast()); - CHECK(r.u == doctest::Approx(expected.x())); - CHECK(r.v == doctest::Approx(expected.y())); + Px got = project(K, r.p); + CHECK(got.u == doctest::Approx(r.u).epsilon(1e-6)); + CHECK(got.v == doctest::Approx(r.v).epsilon(1e-6)); + CHECK(got.depth == doctest::Approx(r.depth).epsilon(1e-6)); } } +TEST_CASE("mei: a point on the optical axis lands on the principal point") +{ + const Intrinsics K = mei(); + Px r = project(K, { 0.f, 0.f, 1.f }); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + CHECK(r.depth == doctest::Approx(1.0)); +} + TEST_CASE("mei: a point on the camera itself is rejected") { const Intrinsics K = mei(); @@ -270,7 +272,7 @@ TEST_CASE("mei: a point on the camera itself is rejected") TEST_CASE("mei: a point behind the camera is rejected, not silently mis-projected") { - // MeiCamera::Project has no domain guard of its own, and past the valid + // The projection has no domain guard of its own, and past the valid // dome the projection is not injective -- it folds far-off-axis // directions back onto real pixels instead of pushing them out of frame. float u, v, depth; @@ -318,7 +320,6 @@ TEST_CASE("mei: a point behind the camera is rejected, not silently mis-projecte TEST_CASE("mei: respects the extrinsics") { const Intrinsics K = mei(); - const MeiCamera cam = meiCamera(); // om=fi=ka=0 is the nominal camera-vs-LiDAR alignment, so LiDAR forward // (+X) should come out as camera forward, i.e. the image centre. @@ -334,9 +335,9 @@ TEST_CASE("mei: respects the extrinsics") Px offset = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); // p_lidar - C = LiDAR +X, which R_wc's transpose turns into camera +Z // (camera-forward) -- same axis remap as the centre check above. - const Eigen::Vector2d expected = cam.Project(Eigen::Vector3d(0.0, 0.0, 1.0)); - CHECK(offset.u == doctest::Approx(expected.x())); - CHECK(offset.v == doctest::Approx(expected.y())); + // On-axis, so it lands on the principal point, as the centre check above. + CHECK(offset.u == doctest::Approx(K.cx)); + CHECK(offset.v == doctest::Approx(K.cy)); CHECK(offset.depth == doctest::Approx(1.0)); } @@ -492,22 +493,24 @@ TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") CHECK(half.v == doctest::Approx(full.v * 0.5)); } } -// ── LoadMeiCamera ───────────────────────────────────────────────────────────── +// ── loadMeiIntrinsics ───────────────────────────────────────────────────────── namespace { // Writes `body` to a temp file and loads it, so the parser is exercised // through its real file-reading path. - MeiCamera loadFromString(const std::string& body) + // Returns the loaded intrinsics, or nullopt when the load failed. + std::optional loadFromString(const std::string& body) { const std::string path = (std::filesystem::temp_directory_path() / "calib_core_test_camera_info.yaml").string(); { std::ofstream f(path); f << body; } - MeiCamera cam = LoadMeiCamera(path); + Intrinsics K; + const bool ok = loadMeiIntrinsics(path, K); std::filesystem::remove(path); - return cam; + return ok ? std::optional(K) : std::nullopt; } const char* kSample = R"(# this rig's camera_info.yaml @@ -524,35 +527,38 @@ distortion: [-0.0123, 0.0045, -0.0007, 0.0011, -0.0002] )"; } // namespace -TEST_CASE("LoadMeiCamera: reads this rig's flat camera_info.yaml") +TEST_CASE("loadMeiIntrinsics: reads this rig's flat camera_info.yaml") { - const MeiCamera cam = loadFromString(kSample); - REQUIRE(cam.loaded); - CHECK(cam.frameId == "camera_front"); - CHECK(cam.distortionModel == "insta360_mei_v2"); - CHECK(cam.width == 3840); - CHECK(cam.height == 1920); - CHECK(cam.fx == doctest::Approx(620.5)); - CHECK(cam.cy == doctest::Approx(539.5)); - CHECK(cam.xi == doctest::Approx(1.234)); + const auto K = loadFromString(kSample); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Mei); + CHECK(K->width == 3840); + CHECK(K->height == 1920); + CHECK(K->fx == doctest::Approx(620.5)); + CHECK(K->cy == doctest::Approx(539.5)); + CHECK(K->xi == doctest::Approx(1.234)); // distortion is (k1, k2, k3, p1, p2) -- NOT OpenCV's pinhole order. - CHECK(cam.k1 == doctest::Approx(-0.0123)); - CHECK(cam.k2 == doctest::Approx(0.0045)); - CHECK(cam.k3 == doctest::Approx(-0.0007)); - CHECK(cam.p1 == doctest::Approx(0.0011)); - CHECK(cam.p2 == doctest::Approx(-0.0002)); + CHECK(K->k1 == doctest::Approx(-0.0123)); + CHECK(K->k2 == doctest::Approx(0.0045)); + CHECK(K->k3 == doctest::Approx(-0.0007)); + CHECK(K->p1 == doctest::Approx(0.0011)); + CHECK(K->p2 == doctest::Approx(-0.0002)); + // The Mei polynomial has no rational denominator. + CHECK(K->k4 == 0.f); + CHECK(K->k5 == 0.f); + CHECK(K->k6 == 0.f); } -TEST_CASE("LoadMeiCamera: quotes and comments are not taken literally") +TEST_CASE("loadMeiIntrinsics: quotes and trailing comments are not taken literally") { std::string body = kSample; - body += "\nframe_id: \"quoted_name\" # trailing comment\n"; - const MeiCamera cam = loadFromString(body); - REQUIRE(cam.loaded); - CHECK(cam.frameId == "quoted_name"); + body += "\nxi: 0.75 # trailing comment\n"; + const auto K = loadFromString(body); + REQUIRE(K.has_value()); + CHECK(K->xi == doctest::Approx(0.75)); } -TEST_CASE("LoadMeiCamera: a missing field fails instead of defaulting to 0") +TEST_CASE("loadMeiIntrinsics: a missing field fails instead of defaulting to 0") { // A calibration that silently reads xi as 0 reprojects wrongly with no // visible failure, so the load has to reject it outright. @@ -561,11 +567,14 @@ TEST_CASE("LoadMeiCamera: a missing field fails instead of defaulting to 0") REQUIRE(at != std::string::npos); body.erase(at, std::string("xi: 1.234\n").size()); - const MeiCamera cam = loadFromString(body); - CHECK_FALSE(cam.loaded); + CHECK_FALSE(loadFromString(body).has_value()); } -TEST_CASE("LoadMeiCamera: a missing file degrades to loaded=false, not a crash") +TEST_CASE("loadMeiIntrinsics: a missing file fails cleanly, and leaves K alone") { - CHECK_FALSE(LoadMeiCamera("/nonexistent/camera_info.yaml").loaded); + Intrinsics K = mei(); + const Intrinsics before = K; + CHECK_FALSE(loadMeiIntrinsics("/nonexistent/camera_info.yaml", K)); + CHECK(K.fx == before.fx); + CHECK(K.xi == before.xi); } From 2580f491aad9ad52e2c7099034117d46212fee0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:37:54 +0200 Subject: [PATCH 15/22] Drop rectification from the ROS 2 bag export Frames now go out exactly as captured and CameraInfo always carries the real distortion, so consumers that want rectified images undistort from it. Rectification only ever applied to Pinhole anyway -- OpenCV's initUndistortRectifyMap has nothing to say about a 360 panorama, and Mei's k1/k2/k3/p1/p2 are its own polynomial applied after a unit-sphere step a K/D pair cannot express -- so it was a per-model special case guarded by a checkbox that was disabled for two of the three models. Removing it also removes the re-encode it forced: with nothing to rectify, the compressed path copies the source jpeg verbatim in every case rather than decoding and re-encoding it, so exported images no longer lose a generation of jpeg quality. imageFiles is jpeg-only (see imageTsFromName), so the verbatim copy is safe. The undistortCamera option, the rectify maps, and the now-unused calib3d and imgproc includes are gone with it. Co-Authored-By: Claude Opus 5 (1M context) --- .../RosExport.cpp | 58 +++++-------------- .../RosExport.h | 9 ++- .../TrajectoryViewer.cpp | 15 ++--- 3 files changed, 23 insertions(+), 59 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 94e99fb6..56666ca4 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -27,9 +27,7 @@ bool exportRos2Bag(const RosExportInput&, const RosExportOptions&, std::string& #include #include -#include #include -#include #include @@ -199,32 +197,27 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s // ── 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; - // initUndistortRectifyMap is pinhole-only: a 360 panorama has - // nothing to rectify, and Mei's k1/k2/k3/p1/p2 are its own - // polynomial applied after a unit-sphere step Km/Dm cannot - // express -- so both models keep their raw frames. + // Frames go out exactly as captured, and CameraInfo describes them + // with the real distortion. Rectifying here would only ever have + // worked for Pinhole -- OpenCV's initUndistortRectifyMap has + // nothing to say about a 360 panorama, and Mei's k1/k2/k3/p1/p2 are + // its own polynomial applied after a unit-sphere step that a K/D + // pair cannot express -- so it was a per-model special case that + // also re-encoded every jpeg. Consumers that want rectified images + // can undistort from the published CameraInfo. const bool equirect = in.K.model == CameraModel::Equirectangular; const bool mei = in.K.model == CameraModel::Mei; - const bool rectify = opt.undistortCamera && in.calibLoaded && in.K.model == CameraModel::Pinhole; - // 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) + if (opt.compressCamera) { + // Verbatim: imageFiles is jpeg-only (see imageTsFromName), + // so this neither decodes nor re-encodes. std::ifstream f(path, std::ios::binary); if (!f) continue; @@ -237,29 +230,11 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s 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 (!bgr.isContinuous()) + bgr = bgr.clone(); + outImg = bgr; } if (opt.compressCamera) @@ -339,10 +314,7 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s else { 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.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 }; diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.h b/apps/camera_lidar_trajectory_viewer/RosExport.h index 26df0753..5c99eab8 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.h +++ b/apps/camera_lidar_trajectory_viewer/RosExport.h @@ -55,11 +55,10 @@ struct RosExportOptions 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; + // true: CompressedImage, the source jpeg copied verbatim; false: raw Image + // (bgr8). Frames are always written as captured -- see RosExport.cpp on why + // nothing is rectified -- so CameraInfo always carries the real distortion. + bool compressCamera = true; // LiDAR can be exported in two flavours, independently: // - undistorted: points as registered by LIO, in the map frame (already diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index c25f2140..ff3e888d 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -2820,18 +2820,11 @@ int main(int argc, char* argv[]) ImGui::Indent(); ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("ON: CompressedImage (jpeg)\nOFF: raw Image bgr8"); - // Rectification is OpenCV's pinhole initUndistortRectifyMap; - // it would mis-warp a panorama or a fisheye, not rectify it. - const bool noRectify = s.K.model != CameraModel::Pinhole; - ImGui::BeginDisabled(noRectify); - ImGui::Checkbox("Undistort (rectify)", &s.ros.undistortCamera); - ImGui::EndDisabled(); - if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip("ON: CompressedImage, the source jpeg copied verbatim\nOFF: raw Image bgr8"); + ImGui::TextDisabled("Frames are exported as captured."); + if (ImGui::IsItemHovered()) ImGui::SetTooltip( - !noRectify ? "Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)." - : s.K.model == CameraModel::Equirectangular ? "Not applicable to an equirectangular camera." - : "Not applicable to a Mei (fisheye) camera."); + "Images are never rectified. CameraInfo carries the real\ndistortion, so consumers can undistort from it."); ImGui::Unindent(); } ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); From 3a8c04e038206e5c242362e1654fc82da7868aac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:47:48 +0200 Subject: [PATCH 16/22] Trim the comment essays in the trajectory viewer, and use doxygen Cuts design history, narration of what other files do, and a block of commented-out dynamic-subsampling code that git still has. Comment blocks of five lines or more drop from 171 lines to 106, with none over 10. File-scope declarations and AppState members now use //! (//!< for trailing member comments), matching Camera.h and the rest of calib_core; @param/@return are added where a function has an out-parameter or a non-obvious failure case. Section dividers and in-body comments stay plain //. Co-Authored-By: Claude Opus 5 (1M context) --- .../TrajectoryViewer.cpp | 448 ++++++++---------- 1 file changed, 202 insertions(+), 246 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index ff3e888d..ac804d01 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -43,9 +43,9 @@ 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. +//! 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" }, @@ -74,8 +74,8 @@ static const std::vector appShortcuts = { { "", "Shift+R", "Open 'Center of rotation' dialog" }, }; -// 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. +//! 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()) @@ -84,7 +84,7 @@ 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(). +//! Build a time(seconds) -> T_world_lidar map suitable for getInterpolatedPose(). static std::map buildTrajMap(const Trajectory& traj) { std::map m; @@ -93,8 +93,12 @@ static std::map buildTrajMap(const Trajectory& traj) 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. +//! Interpolated T_world_lidar at a timestamp. +//! @param trajMap trajectory to sample +//! @param ts_ns timestamp, nanoseconds +//! @param out receives the pose +//! @return false when ts_ns lies outside the trajectory range, which +//! getInterpolatedPose() signals 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); @@ -106,10 +110,10 @@ static bool interpPose(const std::map& trajMap, int64_t static constexpr double kRad2Deg = 57.295779513082320876; -// Angular speed (deg/s) for every trajectory pose: the rotation change to the next -// pose divided by the time step. Result is parallel to traj.poses; the last entry -// repeats the previous one. Fewer than two poses -> all zeros. Non-increasing -// timestamps (chunk boundaries, duplicates) reuse the previous value. +//! Angular speed (deg/s) for every trajectory pose: the rotation change to the next +//! pose divided by the time step. Result is parallel to traj.poses; the last entry +//! repeats the previous one. Fewer than two poses -> all zeros. Non-increasing +//! timestamps (chunk boundaries, duplicates) reuse the previous value. static std::vector computePoseAngularSpeedDeg(const Trajectory& traj) { const auto& poses = traj.poses; @@ -131,8 +135,8 @@ static std::vector computePoseAngularSpeedDeg(const Trajectory& traj) return speed; } -// Angular speed (deg/s) at the trajectory pose nearest ts_ns. 0 when there's no -// per-pose data (not loaded, or size mismatch with the trajectory). +//! Angular speed (deg/s) at the trajectory pose nearest ts_ns. 0 when there's no +//! per-pose data (not loaded, or size mismatch with the trajectory). static float angularSpeedDegAt(const Trajectory& traj, const std::vector& perPose, int64_t ts_ns) { if (traj.poses.empty() || perPose.size() != traj.poses.size()) @@ -220,98 +224,98 @@ struct AppState { Trajectory traj; std::vector imageTsNs; - Intrinsics K; // K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) - // How K.model was decided: the calibration file's "model" key wins, the - // image filenames are the fallback. Both are kept as state rather than - // applied on the spot because they arrive in either order, so - // resolveCameraModel() recomputes K.model whenever one changes. + Intrinsics K; //!< K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) + //! How K.model was decided: the calibration file's "model" key wins, the + //! image filenames are the fallback. Both are kept as state rather than + //! applied on the spot because they arrive in either order, so + //! resolveCameraModel() recomputes K.model whenever one changes. CameraModel fileModel = CameraModel::Pinhole; - bool modelExplicit = false; // the calibration file named a model - bool namesLookEquirect = false; // the frames carry the equirectangular_ prefix - Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka - Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame + bool modelExplicit = false; //!< the calibration file named a model + bool namesLookEquirect = false; //!< the frames carry the equirectangular_ prefix + Extrinsics E; //!< tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka + Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); //!< camera orientation in world/LiDAR frame Roi roi; - // Free-form counterpart of `roi`: a per-pixel mask whose rejected pixels - // are excluded from coloring. Needed to drop the operator/backpack a 360 - // rig has permanently in frame, which no rectangle can cut out without - // taking the scene with it. Kept at the file's own resolution, strictly - // 0/255 (see loadMask), and resampled where used since images are read at - // s.imgScale. Coloring only -- the ROS 2 and COLMAP exports are not masked. - cv::Mat mask; // empty = none loaded - bool maskEnabled = false; // acted on only while `mask` is non-empty - bool maskInvert = false; // UI state; loadMask and the toggle flip `mask` itself + //! Free-form counterpart of `roi`: a per-pixel mask whose rejected pixels + //! are excluded from coloring. Needed to drop the operator/backpack a 360 + //! rig has permanently in frame, which no rectangle can cut out without + //! taking the scene with it. Kept at the file's own resolution, strictly + //! 0/255 (see loadMask), and resampled where used since images are read at + //! s.imgScale. Coloring only -- the ROS 2 and COLMAP exports are not masked. + cv::Mat mask; //!< empty = none loaded + bool maskEnabled = false; //!< acted on only while `mask` is non-empty + bool maskInvert = false; //!< UI state; loadMask and the toggle flip `mask` itself char maskBuf[512] = {}; - float maskRejectFrac = 0.f; // share of pixels the mask drops, for the UI - bool showMaskOverlay = true; // tint the rejected area over the image preview - Texture2D maskTex = {}; // that tint, RGBA, built by refreshMaskDerived + float maskRejectFrac = 0.f; //!< share of pixels the mask drops, for the UI + bool showMaskOverlay = true; //!< tint the rejected area over the image preview + Texture2D maskTex = {}; //!< that tint, RGBA, built by refreshMaskDerived bool maskTexValid = false; bool calibLoaded = false; - int imgW = 4656, imgH = 3496; // overwritten from the first scanned image by loadImages() + int imgW = 4656, imgH = 3496; //!< overwritten from the first scanned image by loadImages() - // loaded camera images: timestamp → resized BGR Mat + //! loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - // Downscale applied to every image used for coloring: equirectangular - // frames are large (3840x1920x3 ~ 22 MB) and multiImgColoring holds a - // chunk's worth at once. Intrinsics are scaled to match. + //! Downscale applied to every image used for coloring: equirectangular + //! frames are large (3840x1920x3 ~ 22 MB) and multiImgColoring holds a + //! chunk's worth at once. Intrinsics are scaled to match. float imgScale = 1.0f; - // Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed - // trigger/USB latency the camera's own timestamps don't account for): - // t_traj = t_image + timeOffsetSec. Applied wherever an image timestamp is - // matched against the LiDAR/pose timeline (loadCloud's chunk selection + - // point matching, exportColmap's per-image pose lookup) -- never to the raw - // timestamps used for filename lookup or image-list indexing - // (s.imageTsNs/imagesFilenamesInTime). + //! Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed + //! trigger/USB latency the camera's own timestamps don't account for): + //! t_traj = t_image + timeOffsetSec. Applied wherever an image timestamp is + //! matched against the LiDAR/pose timeline (loadCloud's chunk selection + + //! point matching, exportColmap's per-image pose lookup) -- never to the raw + //! timestamps used for filename lookup or image-list indexing + //! (s.imageTsNs/imagesFilenamesInTime). double timeOffsetSec = 0.0; GpuCloud cloud; Shader shader = {}; bool shaderOk = false; int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; - // Driving orbit's Euler mode (rotateX/rotateY/translate/rotationCenter/ - // isOrtho), not its azimuth/elevation/distance/target mode -- the same - // camera engine multi_view_tls_registration_step_2 uses, manually - // driven through rlgl (see display()'s camera setup) instead of - // raylib's Camera3D/BeginMode3D. + //! Driving orbit's Euler mode (rotateX/rotateY/translate/rotationCenter/ + //! isOrtho), not its azimuth/elevation/distance/target mode -- the same + //! camera engine multi_view_tls_registration_step_2 uses, manually + //! driven through rlgl (see display()'s camera setup) instead of + //! raylib's Camera3D/BeginMode3D. raylib_widgets::OrbitCamera orbit; - // Rebuilt from orbit.euler every frame in display() -- used only for - // drawCompassRuler()'s right/up vectors, same reasoning as step2's own - // app_state.viewLocal (OrbitCamera itself stays Eigen-free). + //! Rebuilt from orbit.euler every frame in display() -- used only for + //! drawCompassRuler()'s right/up vectors, same reasoning as step2's own + //! app_state.viewLocal (OrbitCamera itself stays Eigen-free). Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); bool showCenterOfRotationWindow = false; - // controls + //! 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 + bool isolateCamera = false; //!< render only points colored by the selected (preview) image float frustumScale = 0.5f; float pointSize = 1.f; int cloudDecim = 1; int drawDecim = 1; - bool multiImgColoring = true; // false = single image per chunk (midpoint) - // How each point is matched to a camera image: - // 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) + bool multiImgColoring = true; //!< false = single image per chunk (midpoint) + //! How each point is matched to a camera image: + //! 0 = temporal — image nearest in time (± maxWiggle frames, within maxTemporalDist) + //! 1 = geometry — among all chunk images the point projects into, the one + //! with the smallest depth (closest camera) int colorStrategy = 0; - float maxTemporalDist = 0.5f; // s: skip images farther than this from the point (temporal) - int maxWiggle = 1; // frames: search startIdx ± maxWiggle for a frustum hit (temporal) + float maxTemporalDist = 0.5f; //!< s: skip images farther than this from the point (temporal) + int maxWiggle = 1; //!< frames: search startIdx ± maxWiggle for a frustum hit (temporal) // ── fast-rotation image filter ───────────────────────────────────────────── - // Per-pose angular speed (deg/s), parallel to traj.poses — filled by - // loadSession(). Images captured while the rig turns faster than - // maxImageAngSpeedDeg are dropped from the colorize pass (motion-smeared). + //! Per-pose angular speed (deg/s), parallel to traj.poses — filled by + //! loadSession(). Images captured while the rig turns faster than + //! maxImageAngSpeedDeg are dropped from the colorize pass (motion-smeared). std::vector poseAngSpeedDeg; - float poseAngSpeedMax = 0.f; // deg/s: peak over the whole session (display only) - bool filterFastImages = true; // drop motion-smeared frames from the colorize pass - float maxImageAngSpeedDeg = 60.f; // deg/s threshold - int angFilteredImgs = 0; // images skipped by the filter in the last colorize pass + float poseAngSpeedMax = 0.f; //!< deg/s: peak over the whole session (display only) + bool filterFastImages = true; //!< drop motion-smeared frames from the colorize pass + float maxImageAngSpeedDeg = 60.f; //!< deg/s threshold + int angFilteredImgs = 0; //!< images skipped by the filter in the last colorize pass - bool useImageColor = false; // true once a colorize pass produced RGB data - int colorMode = 0; // 0=intensity (jet), 1=RGB by image, 2=camera id - int coloredPts = 0; // points that received RGB from an image - int uncoloredPts = 0; // points left as intensity-gray (no image / out of frustum / outside ROI) + 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] = {}; @@ -337,7 +341,7 @@ struct AppState // ── ROS 2 export ────────────────────────────────────────────────────────── char rosOutBuf[512] = "ros2_export"; - int rosStorageIdx = 0; // 0 = mcap, 1 = sqlite3 + int rosStorageIdx = 0; //!< 0 = mcap, 1 = sqlite3 RosExportOptions ros; std::thread rosThread; std::atomic rosBusy{ false }; @@ -348,16 +352,16 @@ struct AppState // ── COLMAP export ───────────────────────────────────────────────────────── char colmapBuf[512] = "colmap_out"; bool colmapCopyImages = false; - int colmapPtDecim = 50; // splat-friendly default (~500k from a 25M cloud) + 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 }; - // Bumped when the image set itself is replaced (a camera directory dropped). The loader - // thread skips a request whose index it already served, so without this a swap that keeps - // the same index would leave the previous frame on screen. + //! Bumped when the image set itself is replaced (a camera directory dropped). The loader + //! thread skips a request whose index it already served, so without this a swap that keeps + //! the same index would leave the previous frame on screen. std::atomic imgViewEpoch{ 0 }; std::atomic imgViewStop{ false }; std::atomic imgViewLoading{ false }; @@ -367,36 +371,31 @@ struct AppState std::thread imgViewThread; // ── synthetic intensity-projection image (drawn next to the photo) ───── - // Reprojects exportCloud through the same calibration as the colorize - // pass, jet-colormapped over intensity -- a reference image to check the - // calibration against the photo by eye. + //! Reprojects exportCloud through the same calibration as the colorize + //! pass, jet-colormapped over intensity -- a reference image to check the + //! calibration against the photo by eye. bool showIntensityProjection = false; - bool intensityProjNeedsUpdate = false; // set on toggle/refresh/image change + bool intensityProjNeedsUpdate = false; //!< set on toggle/refresh/image change Texture2D intensityProjTex = {}; bool intensityProjTexValid = false; - int intensityProjDecim = 1; // use every Nth point of exportCloud (perf) - float intensityProjPointRadius = 1.5f; // splat radius, in output-image pixels - bool intensityProjOverlay = false; // true: alpha-blend on top of the photo instead of side-by-side - float intensityProjAlpha = 0.6f; // blend strength when intensityProjOverlay is on + int intensityProjDecim = 1; //!< use every Nth point of exportCloud (perf) + float intensityProjPointRadius = 1.5f; //!< splat radius, in output-image pixels + bool intensityProjOverlay = false; //!< true: alpha-blend on top of the photo instead of side-by-side + float intensityProjAlpha = 0.6f; //!< blend strength when intensityProjOverlay is on }; // ── helpers ─────────────────────────────────────────────────────────────────── -// Plain Eigen::Vector3f -> raylib Vector3 conversion. Used to be an axis -// remap (x, z, -y) that made this app's native Z-up LiDAR data render -// correctly under raylib's Y-up Camera3D/BeginMode3D convention; now that -// the camera is multi_view_tls_registration_step_2's own Z-up rlgl-driven -// one, geometry renders in its native coordinates and this is a no-op -// component copy. +//! Eigen::Vector3f -> raylib Vector3. A plain component copy: the camera is +//! Z-up, so geometry renders in its native coordinates with no axis remap. static Vector3 toVec3(const Eigen::Vector3f& v) { return { v.x(), v.y(), v.z() }; } -// Finds the trajectory pose closest to `ray` (unconditional nearest, no -// distance cutoff) and returns its world-space position -- mirrors -// multi_view_tls_registration_step_2's getClosestTrajectoryPoint(), backed -// by the same shared raylib_widgets::pickNearestPointOnLine() picker. -// Returns false (outPoint untouched) when the trajectory is empty. +//! Trajectory pose closest to `ray` -- unconditional nearest, no distance +//! cutoff. Backed by the same picker step2's getClosestTrajectoryPoint() uses. +//! @param outPoint receives the world-space position +//! @return false, outPoint untouched, when the trajectory is empty static bool nearestTrajectoryPoint(const Trajectory& traj, const Ray& ray, Vector3& outPoint) { if (traj.poses.empty()) @@ -415,12 +414,11 @@ static bool nearestTrajectoryPoint(const Trajectory& traj, const Ray& ray, Vecto return true; } -// Intersects `ray` with the Z=0 ground plane -- same plane -// multi_view_tls_registration_step_2's setNewRotationCenter() intersects -// (via RegistrationPlaneFeature::Plane{0,0,1,0} + rayIntersection()), -// reimplemented directly in raylib/raymath terms since those two types live -// in `core`, which this app deliberately doesn't link. Returns false -// (outPoint untouched) when the ray is ~parallel to the plane. +//! Intersects `ray` with the Z=0 ground plane, as step2's +//! setNewRotationCenter() does -- in raylib/raymath terms, since step2's types +//! live in `core`, which this app deliberately doesn't link. +//! @param outPoint receives the intersection +//! @return false, outPoint untouched, when the ray is ~parallel to the plane static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) { const float kTolerance = 0.0001f; @@ -432,16 +430,19 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) return true; } -// Prefix marking a frame as a 360 panorama rather than a normal camera image. +//! Prefix marking a frame as a 360 panorama rather than a normal camera image. static constexpr const char* kEquirectPrefix = "equirectangular_"; -// Timestamp encoded in a camera frame's filename, or -1 when the file isn't -// one. Layout is "_.jpg" or a bare -// ".jpg" -- everything up to the last '_' is ignored, so -// Mandeye's "cam0_" and the 360 rig's "equirectangular_" both parse -// without a list of rigs here. `equirect` reports whether the panorama prefix -// was the one found, since that one selects the camera model. The all-digits -// check rejects unrelated .jpgs, which would otherwise reach std::stoll. +//! Timestamp encoded in a camera frame's filename, or -1 when the file isn't +//! one. Layout is "_.jpg" or a bare +//! ".jpg" -- everything up to the last '_' is ignored, so +//! Mandeye's "cam0_" and the rig's "equirectangular_" both parse +//! without a list of rigs here. +//! @param p file to parse +//! @param equirect optionally receives whether the panorama prefix was the one +//! found, since that prefix selects the camera model +//! @return the timestamp, or -1 when the name doesn't match. The all-digits +//! check rejects unrelated .jpgs, which would reach std::stoll. static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) { if (equirect) @@ -464,25 +465,25 @@ static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) } } -// Directory holding the camera frames: whatever the user picked, else the -// CAMERA_0 sibling of the session dir. +//! Directory holding the camera frames: whatever the user picked, else the +//! CAMERA_0 sibling of the session dir. static fs::path cameraDir(const AppState& s) { return s.cameraBuf[0] ? fs::path(s.cameraBuf) : fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; } -// AppState::timeOffsetSec in nanoseconds, to match the timestamps. +//! AppState::timeOffsetSec in nanoseconds, to match the timestamps. static int64_t imageTimeOffsetNs(const AppState& s) { return (int64_t)std::llround(s.timeOffsetSec * 1e9); } -// Settles K.model from the two inputs that can select it, in precedence order. -// Call after either changes; see AppState::fileModel for why. -// -// Only Pinhole and Equirectangular are inferred: the 360 rig marks its frames -// with kEquirectPrefix, but nothing in a filename identifies a Mei fisheye, so -// Mei is reachable only through an explicit "model" key. +//! Settles K.model from the two inputs that can select it, in precedence order. +//! Call after either changes; see AppState::fileModel for why. +//! +//! Only Pinhole and Equirectangular are inferred: the 360 rig marks its frames +//! with kEquirectPrefix, but nothing in a filename identifies a Mei fisheye, so +//! Mei is reachable only through an explicit "model" key. static void resolveCameraModel(AppState& s) { if (s.modelExplicit) @@ -491,10 +492,10 @@ static void resolveCameraModel(AppState& s) s.K.model = s.namesLookEquirect ? CameraModel::Equirectangular : CameraModel::Pinhole; } -// Index every camera frame in the camera directory by timestamp. Also picks up -// the image dimensions -- read by the equirectangular projection, the ROI -// default, the frustums and COLMAP's cameras.txt -- and, absent an explicit -// "model" in the calibration, infers the camera model from the filenames. +//! Index every camera frame in the camera directory by timestamp. Also picks up +//! the image dimensions -- read by the equirectangular projection, the ROI +//! default, the frustums and COLMAP's cameras.txt -- and, absent an explicit +//! "model" in the calibration, infers the camera model from the filenames. static void loadImages(AppState& s) { s.imagesFilenamesInTime.clear(); @@ -536,7 +537,7 @@ static void loadImages(AppState& s) 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. +//! 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; @@ -893,12 +894,11 @@ static void loadCloud(AppState& s) float u, v, depth; if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) return h; - // Too close to the lens to be a real observation. Applies - // to Mei as well as Pinhole (its depth is a range rather - // than a z, but 5 cm means the same thing physically); - // projectPoint's own Mei guard only rejects a point - // essentially AT the camera. Equirectangular keeps its - // long-standing "no near clip" behaviour. + // Too close to the lens to be a real observation. Mei too: + // its depth is a range rather than a z, but 5 cm means the + // same thing physically, and projectPoint's Mei guard only + // rejects a point essentially AT the camera. + // Equirectangular keeps its "no near clip" behaviour. if ((Ks.model == CameraModel::Pinhole || Ks.model == CameraModel::Mei) && depth <= 0.05f) return h; int iu = (int)std::round(u); @@ -1078,9 +1078,9 @@ static void loadCloud(AppState& s) s.status += " | Fast-img filtered: " + std::to_string(angFilteredImgs); } -// Small CPU jet colormap approximation, matching the GLSL one used by the -// GPU point renderer's Intensity color mode (raylib_widgets::kJetColormapGLSL) -// closely enough for a visual reference image. Returns BGR (OpenCV order). +//! Small CPU jet colormap approximation, matching the GLSL one used by the +//! GPU point renderer's Intensity color mode (raylib_widgets::kJetColormapGLSL) +//! closely enough for a visual reference image. Returns BGR (OpenCV order). static cv::Vec3b jetColorBGR(float t) { t = std::clamp(t, 0.f, 1.f); @@ -1090,16 +1090,13 @@ static cv::Vec3b jetColorBGR(float t) return cv::Vec3b((uchar)(b * 255.f), (uchar)(g * 255.f), (uchar)(r * 255.f)); } -// Rasterizes a synthetic "intensity image" for the camera pose at imgTsAdj -// (already shifted by the photo time offset), by reprojecting s.exportCloud -// through the same fixed camera-to-LiDAR extrinsics (R_wc/C) and -// calib::projectPoint() as loadCloud()'s colorize pass, painted with a jet -// colormap over each point's normalized [0,1] intensity and a simple -// per-pixel depth test (nearest point wins) so occluded points don't bleed -// through. Points farther than s.maxTemporalDist (or a 1s fallback) in time -// from imgTsAdj are skipped -- otherwise the whole session's merged cloud -// would be tested against every single preview, which is the same temporal -// gate loadCloud()'s "Temporal" coloring strategy already applies per point. +//! Rasterizes a synthetic "intensity image" for the camera pose at imgTsAdj, +//! reprojecting s.exportCloud through the same extrinsics and projectPoint() as +//! the colorize pass, jet-colormapped over intensity with a per-pixel depth test +//! so occluded points don't bleed through. Points more than s.maxTemporalDist +//! (1s fallback) from imgTsAdj are skipped, the same temporal gate the +//! "Temporal" coloring strategy applies -- otherwise every preview would test +//! the whole session's cloud. static cv::Mat renderIntensityProjection(const AppState& s, int64_t imgTsAdj) { const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); @@ -1131,13 +1128,11 @@ static cv::Mat renderIntensityProjection(const AppState& s, int64_t imgTsAdj) continue; if (Ks.model == CameraModel::Pinhole && depth <= 0.05f) continue; - // Points near-grazing the camera plane (small but positive depth, - // e.g. off to the side) get blown up to huge u/v by the perspective - // divide -- unlike colorize()'s tight per-point temporal matching, - // this function pulls in every point within a whole time window, so - // it hits that edge case far more often. (int)std::round() on such a - // value is undefined behavior, which is what produced the - // "wrapping"/bowtie look; reject before the cast instead. + // Points near-grazing the camera plane get blown up to huge u/v by the + // perspective divide, and this function pulls in a whole time window's + // worth, so it hits that far more often than colorize() does. Casting + // such a value with (int)std::round() is UB -- the "bowtie" artifact -- + // so reject before the cast. if (!std::isfinite(u) || !std::isfinite(v) || std::fabs(u) > 1e6f || std::fabs(v) > 1e6f) continue; int iu = (int)std::round(u); @@ -1180,12 +1175,10 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; - // Camera model: "equirectangular"/"equirect" for a 360 panorama, "mei" (or - // the rig's own "insta360_mei_v2" tag) for a unified-sphere fisheye, - // anything else for the pinhole model this app started with. Accepted both - // at the top level and inside "intrinsics". Assigned unconditionally so - // loading a pinhole calibration after another model clears the flag rather - // than inheriting it. + // "equirectangular"/"equirect", "mei" (or the rig's "insta360_mei_v2"), + // anything else pinhole. Accepted at the top level or inside "intrinsics". + // Assigned unconditionally, so loading a pinhole calibration after another + // model clears the flag rather than inheriting it. { const bool topLevel = j.contains("model"); const bool nested = j.contains("intrinsics") && j["intrinsics"].contains("model"); @@ -1269,10 +1262,10 @@ static void loadCalib(AppState& s) s.status = "Calibration loaded"; } -// Rebuilds what is derived from s.mask: the rejected-pixel share the UI -// reports, and the translucent red overlay drawn over the image preview. Call -// after anything that changes the mask. Main thread only -- it creates a GL -// texture. +//! Rebuilds what is derived from s.mask: the rejected-pixel share the UI +//! reports, and the translucent red overlay drawn over the image preview. Call +//! after anything that changes the mask. Main thread only -- it creates a GL +//! texture. static void refreshMaskDerived(AppState& s) { if (s.maskTexValid) @@ -1310,16 +1303,11 @@ static void refreshMaskDerived(AppState& s) s.maskTexValid = s.maskTex.id > 0; } -// Loads the mask image named by s.maskBuf. Any format OpenCV reads is accepted -// and reduced to one 8-bit channel thresholded at 128, so a hand-painted -// black/white PNG, a grayscale one and an RGB one all behave identically: a -// pixel is either kept or dropped, never partly -- and a jpeg mask's -// compression noise can't leak in as almost-black. White keeps the pixel, -// black drops it, unless "Invert mask" is on. -// -// No particular resolution is required: the mask is resampled to whatever the -// frames turn out to be (loadCloud), so one drawn over a downscaled copy of a -// frame works as well as a full-resolution one. +//! Loads the mask named by s.maskBuf. Any format OpenCV reads is reduced to one +//! 8-bit channel thresholded at 128, so a pixel is either kept or dropped, never +//! partly, and a jpeg mask's compression noise can't leak in as almost-black. +//! White keeps, black drops, unless "Invert mask" is on. Any resolution works -- +//! the mask is resampled to the frame size in loadCloud. static void loadMask(AppState& s) { if (!s.maskBuf[0]) @@ -1341,8 +1329,8 @@ static void loadMask(AppState& s) s.status = msg; } -// Drops the mask entirely, as opposed to unticking "Image mask", which keeps -// it loaded and ready to re-enable. +//! Drops the mask entirely, as opposed to unticking "Image mask", which keeps +//! it loaded and ready to re-enable. static void clearMask(AppState& s) { s.mask.release(); @@ -1438,8 +1426,8 @@ static void exportLAZ(AppState& s) s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; } -// E57 counterpart of exportLAZ(): one Data3D block, points already in world -// coordinates (identity pose), RGB + intensity + per-point timestamp. +//! E57 counterpart of exportLAZ(): one Data3D block, points already in world +//! coordinates (identity pose), RGB + intensity + per-point timestamp. static void exportE57(AppState& s) { if (s.exportCloud.empty()) @@ -1479,11 +1467,11 @@ static void exportE57(AppState& s) s.status = std::string("Export failed: ") + err; } -// Save the colored cloud as a *session*: one E57 Data3D block per loaded LIO -// chunk ("scan_lio_N"), NOT one collapsed cloud. Each block holds that -// segment's points in its own frame with the chunk's MRP correction as the -// block pose (identity when there is no session_poses.mrp), so the result -// re-opens as a multi-scan session (e.g. in step 2). +//! Save the colored cloud as a *session*: one E57 Data3D block per loaded LIO +//! chunk ("scan_lio_N"), NOT one collapsed cloud. Each block holds that +//! segment's points in its own frame with the chunk's MRP correction as the +//! block pose (identity when there is no session_poses.mrp), so the result +//! re-opens as a multi-scan session (e.g. in step 2). static void exportE57Session(AppState& s) { if (s.exportSegments.empty()) @@ -1543,9 +1531,9 @@ static void exportE57Session(AppState& s) } // ── 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. +//! 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(AppState& s) { setBuf(s.sessionBuf, sizeof(s.sessionBuf), mandeye::fd::SelectFolder("Select LIO result directory")); @@ -1566,7 +1554,7 @@ static void actionOpenCalibration(AppState& s) } } -// A directory holding this app's camera frames (cam0_.jpg). +//! A directory holding this app's camera frames (cam0_.jpg). static bool isCameraDir(const fs::path& dir) { for (const auto& e : fs::directory_iterator(dir)) @@ -1578,13 +1566,13 @@ static bool isCameraDir(const fs::path& dir) return false; } -// Drag & drop equivalent of actionSelectLioResultDir()/actionSelectCamera0Dir()/ -// actionOpenCalibration(), and unlike those menu actions it applies immediately instead of -// waiting for the "Load session" button, since a drop is already an explicit "load this" -// gesture. A dropped directory of cam0_*.jpg is the camera directory (only the images are -// swapped, so the trajectory and the loaded cloud survive); any other directory is this -// app's session (LIO result dir). A dropped *.json is treated as a calibration file. Used by -// the drag & drop handler in main()'s loop below. +//! Drag & drop equivalent of actionSelectLioResultDir()/actionSelectCamera0Dir()/ +//! actionOpenCalibration(), and unlike those menu actions it applies immediately instead of +//! waiting for the "Load session" button, since a drop is already an explicit "load this" +//! gesture. A dropped directory of cam0_*.jpg is the camera directory (only the images are +//! swapped, so the trajectory and the loaded cloud survive); any other directory is this +//! app's session (LIO result dir). A dropped *.json is treated as a calibration file. Used by +//! the drag & drop handler in main()'s loop below. static void actionOpenMask(AppState& s) { std::string path = mandeye::fd::OpenFileDialogOneFile("Select image mask", mandeye::fd::ImageFilter); @@ -1595,11 +1583,11 @@ static void actionOpenMask(AppState& s) } } -// Drag & drop equivalent of actionSelectLioResultDir()/actionOpenCalibration(): a dropped -// directory is this app's session (LIO result dir), and unlike the menu action it loads -// immediately instead of waiting for the "Load session" button, since a drop is already an -// explicit "load this" gesture. A dropped *.json is treated as a calibration file. Used by the -// drag & drop handler in main()'s loop below. +//! Drag & drop equivalent of actionSelectLioResultDir()/actionOpenCalibration(): a dropped +//! directory is this app's session (LIO result dir), and unlike the menu action it loads +//! immediately instead of waiting for the "Load session" button, since a drop is already an +//! explicit "load this" gesture. A dropped *.json is treated as a calibration file. Used by the +//! drag & drop handler in main()'s loop below. static void handleDroppedPath(AppState& s, const std::string& path) { if (fs::is_directory(path)) @@ -1692,8 +1680,8 @@ 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. +//! 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(AppState& s) { if (!s.calibLoaded) @@ -1820,7 +1808,7 @@ static void exportColmap(AppState& s) 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. +//! Gather everything the ROS exporter needs from current viewer state. static void buildRosInput(AppState& s, RosExportInput& in) { in.traj = s.traj; @@ -1937,11 +1925,9 @@ static void drawScene(AppState& s) if (s.K.model != CameraModel::Pinhole) { - // A 360 camera sees the whole sphere and a Mei fisheye sees far - // more than the rectangular pyramid fx/fy/cx/cy imply, so - // there is no frustum worth drawing -- show where the camera - // was and which way its axes point instead. The triad is the - // usual X=red, Y=green, Z=blue. + // Neither a 360 nor a fisheye camera has a frustum the + // fx/fy/cx/cy pyramid describes, so draw position and axes + // instead -- the usual X=red, Y=green, Z=blue. DrawSphere(origin, fs * (hl ? 0.08f : 0.05f), fc); const Color axisColors[3] = { RED, GREEN, BLUE }; for (int k = 0; k < 3; k++) @@ -2148,17 +2134,9 @@ 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). - // KEY_LEFT/RIGHT_SUPER too: on macOS Cmd (Super) is a distinct - // key from Ctrl, and users -- including whoever asked for this - // binding -- reach for Cmd as "the" modifier there. Treating - // either as ctrlDown matches that expectation instead of - // requiring the literal Ctrl key. + // Chords avoid colliding in meaning with step2's, and keep Ctrl+O + // and bare C/P aligned with it. Super counts as ctrlDown so macOS + // Cmd works, where it is a distinct key from Ctrl. bool ctrlDown = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL) || IsKeyDown(KEY_LEFT_SUPER) || IsKeyDown(KEY_RIGHT_SUPER); bool shiftDown = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); @@ -2178,18 +2156,10 @@ int main(int argc, char* argv[]) if (!ctrlDown && IsKeyPressed(KEY_C)) s.showCompassRuler = !s.showCompassRuler; - // Camera drag/zoom -- same raylib_widgets::OrbitCamera Euler - // methods multi_view_tls_registration_step_2's motion()/wheel() - // call, driven from continuous per-frame deltas the way - // OrbitCamera::update() (the other, azimuth/elevation half of - // this struct) already reads input, rather than resurrecting - // step2's GLUT-shaped mouse_old_x/y/mouse_buttons bookkeeping - // (nothing about sharing the camera *math* requires reproducing - // that plumbing too). Gated off while Ctrl/Shift is held -- - // both are reserved for the picking actions below, same - // reasoning as step2's own motion() guard (a trackpad's - // click jitter while a modifier is held must never get read as - // a drag, or it breaks any transition that same click started). + // Camera drag/zoom via the same OrbitCamera Euler methods step2 + // uses, driven from per-frame deltas. Gated off while Ctrl/Shift is + // held: those are the picking modifiers, and click jitter under a + // modifier must not read as a drag. if (!imguiWants && !ctrlDown && !shiftDown) { Vector2 d = GetMouseDelta(); @@ -2485,27 +2455,13 @@ int main(int argc, char* argv[]) // 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(). + // Counts come from ScanRenderer's own per-frame tally: rlgl's + // internal counter only sees its immediate-mode batch, not the + // custom glDrawArrays calls ScanRenderer issues. ImGui::Text("(%d FPS)", GetFPS()); ImGui::EndMainMenuBar(); From 8750ecd1665998ae104d42f0541f109ccb391262 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 14:52:57 +0200 Subject: [PATCH 17/22] Trim the pre-existing comment essays in the trajectory viewer Cuts the rationale that narrated step2's internals and the camera-framing formula. Blocks of five lines or more are now 13 / 77 lines, down from 25 / 171 before this pass started. Also fixes a misattributed doc block: the mask commit inserted actionOpenMask directly beneath handleDroppedPath's comment, so actionOpenMask was documented as the drag & drop handler while handleDroppedPath carried a near-duplicate of the same text further down. actionOpenMask now describes itself, and the drag & drop description survives once, on the function it belongs to. Co-Authored-By: Claude Opus 5 (1M context) --- .../TrajectoryViewer.cpp | 52 +++++++------------ 1 file changed, 18 insertions(+), 34 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index ac804d01..780d4150 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -271,11 +271,9 @@ struct AppState bool shaderOk = false; int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; - //! Driving orbit's Euler mode (rotateX/rotateY/translate/rotationCenter/ - //! isOrtho), not its azimuth/elevation/distance/target mode -- the same - //! camera engine multi_view_tls_registration_step_2 uses, manually - //! driven through rlgl (see display()'s camera setup) instead of - //! raylib's Camera3D/BeginMode3D. + //! Driven in Euler mode (rotateX/rotateY/translate/rotationCenter/isOrtho), + //! not azimuth/elevation/distance/target, through rlgl rather than raylib's + //! Camera3D/BeginMode3D -- see display()'s camera setup. raylib_widgets::OrbitCamera orbit; //! Rebuilt from orbit.euler every frame in display() -- used only for //! drawCompassRuler()'s right/up vectors, same reasoning as step2's own @@ -323,11 +321,9 @@ struct AppState char exportBuf[512] = "colored.laz"; std::vector exportCloud; - // One entry per loaded LIO chunk ("scan_lio_N"), pointing at a contiguous - // [begin, begin+count) slice of exportCloud. `pose` is the chunk's MRP - // correction transform (identity when there is no session_poses.mrp). Used - // by the "Save session as E57" export to keep the segments as separate - // Data3D blocks instead of one collapsed cloud. + //! One entry per loaded LIO chunk ("scan_lio_N"), naming a contiguous + //! [begin, begin+count) slice of exportCloud. Lets the E57 session export + //! keep the chunks as separate Data3D blocks instead of one collapsed cloud. struct ExportSegment { std::string name; @@ -1051,13 +1047,9 @@ static void loadCloud(AppState& s) { s.cloud.upload(gpuData, mx); - // Frame the loaded cloud -- instant, not eased (this runs once on - // load, before there's anything to transition from). Same "recenter - // and look at" formula as OrbitCamera::moveEulerRotationCenterTo() - // (translate.xy = -center.xy keeps the point centered on screen - // regardless of the current rotate angles), applied directly to - // both euler and eulerGoal so there's no stale transition target - // left over from a previous session. + // Frame the loaded cloud, instant rather than eased -- this runs once on + // load, with nothing to transition from. Set on both euler and eulerGoal + // so no stale transition target survives from a previous session. Vector3 center = { sumX / cnt, sumY / cnt, sumZ / cnt }; float dist = std::max(5.f, mx * 0.3f); s.orbit.euler.rotationCenter = center; @@ -1467,11 +1459,9 @@ static void exportE57(AppState& s) s.status = std::string("Export failed: ") + err; } -//! Save the colored cloud as a *session*: one E57 Data3D block per loaded LIO -//! chunk ("scan_lio_N"), NOT one collapsed cloud. Each block holds that -//! segment's points in its own frame with the chunk's MRP correction as the -//! block pose (identity when there is no session_poses.mrp), so the result -//! re-opens as a multi-scan session (e.g. in step 2). +//! Save the colored cloud as a session: one E57 Data3D block per LIO chunk +//! rather than one collapsed cloud, each in its own frame with the chunk's MRP +//! correction as the block pose, so it re-opens as a multi-scan session. static void exportE57Session(AppState& s) { if (s.exportSegments.empty()) @@ -1566,13 +1556,7 @@ static bool isCameraDir(const fs::path& dir) return false; } -//! Drag & drop equivalent of actionSelectLioResultDir()/actionSelectCamera0Dir()/ -//! actionOpenCalibration(), and unlike those menu actions it applies immediately instead of -//! waiting for the "Load session" button, since a drop is already an explicit "load this" -//! gesture. A dropped directory of cam0_*.jpg is the camera directory (only the images are -//! swapped, so the trajectory and the loaded cloud survive); any other directory is this -//! app's session (LIO result dir). A dropped *.json is treated as a calibration file. Used by -//! the drag & drop handler in main()'s loop below. +//! Menu action: pick a mask image and load it into s.mask. static void actionOpenMask(AppState& s) { std::string path = mandeye::fd::OpenFileDialogOneFile("Select image mask", mandeye::fd::ImageFilter); @@ -1583,11 +1567,11 @@ static void actionOpenMask(AppState& s) } } -//! Drag & drop equivalent of actionSelectLioResultDir()/actionOpenCalibration(): a dropped -//! directory is this app's session (LIO result dir), and unlike the menu action it loads -//! immediately instead of waiting for the "Load session" button, since a drop is already an -//! explicit "load this" gesture. A dropped *.json is treated as a calibration file. Used by the -//! drag & drop handler in main()'s loop below. +//! Drag & drop equivalent of the menu load actions, applied immediately rather +//! than waiting for "Load session" -- a drop is already an explicit "load this". +//! A dropped directory of cam0_*.jpg is the camera directory (only the images +//! are swapped, so the trajectory and cloud survive); any other directory is a +//! session (LIO result dir); a *.json is a calibration file. static void handleDroppedPath(AppState& s, const std::string& path) { if (fs::is_directory(path)) From 7dbaf4dbd11ff67ebfe92816a7423ca6299d7fc7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 15 Sep 2026 16:43:46 +0200 Subject: [PATCH 18/22] Load camera serial no to calibration tool MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- apps/camera_lidar_calibration/App.cpp | 35 ++++ apps/camera_lidar_calibration/App.h | 9 + apps/camera_lidar_calibration/UI.cpp | 31 +++ .../TrajectoryViewer.cpp | 98 +++++----- calib_core/include/CalibCore/Camera.h | 38 ++++ calib_core/include/CalibCore/PointCloud.h | 2 + calib_core/src/Camera.cpp | 107 +++++++++++ calib_core/src/PointCloud.cpp | 31 ++- calib_core/tests/test_camera.cpp | 176 ++++++++++++++++++ 9 files changed, 473 insertions(+), 54 deletions(-) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 6e807d69..152e7de6 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include // ── AppState::rebuildImageTexture ───────────────────────────────────────────── @@ -243,6 +244,8 @@ void AppState::loadCloud(const char* path) rebuildCloudPointsRaylib(*this); centerOrbitOnCloud(*this); statusMsg = ""; + // load status sidecar + lidarId = GetLidarSerial(path); } void AppState::addCloud(const char* path) @@ -447,6 +450,7 @@ void AppState::loadIntrinsics(const char* path) intrinsicsW = intrinsics.width; intrinsicsH = intrinsics.height; intrinsicsLoaded = true; + calib::loadCameraIdentity(path, cameraId); std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); // no-op undistortion for Mei, but refreshes the texture statusMsg = "Mei intrinsics loaded"; @@ -471,6 +475,7 @@ void AppState::loadIntrinsics(const char* path) intrinsicsW = imgW; intrinsicsH = imgH; intrinsicsLoaded = true; + calib::loadCameraIdentity(path, cameraId); std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); // re-rectify with the new (possibly auto-scaled) coefficients statusMsg = "Intrinsics loaded"; @@ -511,6 +516,10 @@ void AppState::loadIntrinsics(const char* path) intrinsicsW = j.value("width", imageLoaded ? imageW : 0); intrinsicsH = j.value("height", imageLoaded ? imageH : 0); intrinsicsLoaded = true; + cameraId.serial = j.value("serial", "unknown"); + cameraId.model = j.value("model", "unknown"); + cameraId.firmware = j.value("firmware", "unknown"); + cameraId.frameId = j.value("frameId", "unknown"); std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); statusMsg = "Intrinsics loaded."; @@ -539,6 +548,21 @@ void AppState::loadCalibration(const char* path) bool gotIntrinsics = false, gotExtrinsics = false; + // "camera" identifies the hardware the intrinsics were measured on, so it + // is replaced exactly when they are: a file carrying new intrinsics but no + // "camera" block clears the previous serial instead of leaving it attached + // to a different camera's numbers. A file with only a "camera" block still + // sets it, so an identity can be attached to extrinsics on their own. + if (j.contains("intrinsics") || j.contains("camera")) + { + cameraId = CameraIdentity{}; + cameraId.serial = j.value("serial", std::string{}); + cameraId.model = j.value("model", std::string{}); + cameraId.firmware = j.value("firmware", std::string{}); + cameraId.frameId = j.value("frame_id", std::string{}); + + } + if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; @@ -610,6 +634,8 @@ void AppState::loadCalibration(const char* path) if (gotExtrinsics) statusMsg += " extrinsics"; statusMsg += std::string(" from ") + path; + if (!cameraId.serial.empty()) + statusMsg += " (serial " + cameraId.serial + ")"; if (!scaleNote.empty()) statusMsg += "; " + scaleNote; } @@ -624,6 +650,15 @@ void AppState::saveCalibration(const char* path) Eigen::Vector3f ti = -(R.transpose() * C); // translation of T_lidar_to_camera nlohmann::json j; + // Which camera this calibration was measured on, when a tracked source + // named it. Omitted entirely when unknown, so an absent block and an empty + // one mean the same thing on the way back in. + + j["lidar"]["serial"] = lidarId; + j["camera"]["model"] = cameraId.model; + j["camera"]["serial"] = cameraId.serial; + j["camera"]["frame_id"] = cameraId.frameId; + // width/height record the resolution these intrinsics are valid for (see // App.h) so a later load against a different-size image can auto-scale // rather than just warn. 0 means unknown. diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index 82801899..f72eff2a 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -40,6 +40,15 @@ struct AppState // ── calibration params ─────────────────────────────────────────────────── Intrinsics intrinsics; Extrinsics extrinsics; + // Which camera `intrinsics` describe, when the file said so. Replaced + // whenever the intrinsics are -- an untracked source (an OpenCV YAML, the + // flat intrinsics JSON) clears it rather than leaving the previous + // camera's serial attached to someone else's numbers. + CameraIdentity cameraId; + + // Lidar id from status side car to laz + std::string lidarId; + // Resolution `intrinsics` are currently valid for: the calibration file's // own width/height, else whatever image was loaded at the time. 0 = // unknown. autoScaleIntrinsicsToImage() keeps this in sync, so it names diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 8510a334..5dd77bba 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -40,6 +40,35 @@ static void helpMarker(const char* desc) } } +// Which physical sensors the loaded data belongs to: the camera's serial and +// frame come from the rig's camera_info.yaml, the LiDAR's from the mandeye +// status sidecar beside the LAZ. Shown together, above everything else, +// because a calibration is only valid for the one pair it was measured on. +static void drawSensorIds(const AppState& state) +{ + if (state.cameraId.empty() && state.lidarId.empty()) + return; + + auto dimmed = [](const std::string& text) + { + ImGui::PushStyleColor(ImGuiCol_Text, ImGui::GetStyleColorVec4(ImGuiCol_TextDisabled)); + ImGui::TextWrapped("%s", text.c_str()); + ImGui::PopStyleColor(); + }; + + if (!state.cameraId.serial.empty()) + ImGui::TextWrapped("Camera: %s (%s)", state.cameraId.serial.c_str(), state.cameraId.model.c_str()); + else if (!state.cameraId.frameId.empty()) + dimmed("Camera: (file named no serial)"); + if (!state.cameraId.frameId.empty()) + dimmed(" frame " + state.cameraId.frameId); + + if (!state.lidarId.empty()) + ImGui::TextWrapped("LiDAR: %s", state.lidarId.c_str()); + + ImGui::Separator(); +} + // ── Main draw ──────────────────────────────────────────────────────────────── void UI::draw(AppState& state) { @@ -61,6 +90,8 @@ void UI::draw(AppState& state) ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "LiDAR-Camera Calibration"); ImGui::Separator(); + drawSensorIds(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). diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index 780d4150..f118e42a 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -225,13 +225,14 @@ struct AppState Trajectory traj; std::vector imageTsNs; Intrinsics K; //!< K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) - //! How K.model was decided: the calibration file's "model" key wins, the - //! image filenames are the fallback. Both are kept as state rather than - //! applied on the spot because they arrive in either order, so - //! resolveCameraModel() recomputes K.model whenever one changes. + //! How K.model was decided: the calibration file's "model" key wins when + //! present, else the "Load as equirectangular" tick decides between + //! Pinhole and Equirectangular. Kept as state rather than applied on the + //! spot because either input can change independently of the other, so + //! resolveCameraModel() recomputes K.model whenever one does. CameraModel fileModel = CameraModel::Pinhole; bool modelExplicit = false; //!< the calibration file named a model - bool namesLookEquirect = false; //!< the frames carry the equirectangular_ prefix + bool loadAsEquirectangular = false; //!< UI tick: treat frames as a 360 panorama Extrinsics E; //!< tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); //!< camera orientation in world/LiDAR frame Roi roi; @@ -254,9 +255,9 @@ struct AppState //! loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - //! Downscale applied to every image used for coloring: equirectangular - //! frames are large (3840x1920x3 ~ 22 MB) and multiImgColoring holds a - //! chunk's worth at once. Intrinsics are scaled to match. + //! Downscale applied to every image used for coloring: full-resolution + //! camera frames add up when multiImgColoring holds a chunk's worth at + //! once. Intrinsics are scaled to match. float imgScale = 1.0f; //! Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed //! trigger/USB latency the camera's own timestamps don't account for): @@ -426,39 +427,40 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) return true; } -//! Prefix marking a frame as a 360 panorama rather than a normal camera image. -static constexpr const char* kEquirectPrefix = "equirectangular_"; - -//! Timestamp encoded in a camera frame's filename, or -1 when the file isn't -//! one. Layout is "_.jpg" or a bare -//! ".jpg" -- everything up to the last '_' is ignored, so -//! Mandeye's "cam0_" and the rig's "equirectangular_" both parse -//! without a list of rigs here. +//! Timestamp for a camera frame, or -1 when the file isn't one. Prefers the +//! `.meta.json` sidecar's FRAME_WALL_CLOCK (@ref calib::LoadTimestampFromSideCar) +//! -- the camera's own capture wall clock -- falling back to the timestamp +//! encoded in the filename when no sidecar is found. Layout is "_.jpg" or a bare ".jpg" -- everything up +//! to the last '_' is ignored, so Mandeye's "cam0_" parses without a list +//! of rigs here. //! @param p file to parse -//! @param equirect optionally receives whether the panorama prefix was the one -//! found, since that prefix selects the camera model //! @return the timestamp, or -1 when the name doesn't match. The all-digits //! check rejects unrelated .jpgs, which would reach std::stoll. -static int64_t parseImageTsNs(const fs::path& p, bool* equirect = nullptr) +//! @note The filename timestamp is when the frame was saved to disk; the +//! sidecar's FRAME_WALL_CLOCK is a few ms earlier and more accurate, so +//! it wins whenever present rather than merely filling a gap. +static int64_t parseImageTsNs(const fs::path& p) { - if (equirect) - *equirect = false; if (p.extension() != ".jpg") return -1; std::string stem = p.stem().string(); - if (equirect) - *equirect = stem.rfind(kEquirectPrefix, 0) == 0; if (auto us = stem.rfind('_'); us != std::string::npos) stem = stem.substr(us + 1); if (stem.empty() || stem.find_first_not_of("0123456789") != std::string::npos) return -1; + int64_t ts; try { - return std::stoll(stem); + ts = std::stoll(stem); } catch (...) { return -1; } + + if (const auto sidecarTs = calib::LoadTimestampFromSideCar(p.string())) + return static_cast(std::llround(*sidecarTs)); + return ts; } //! Directory holding the camera frames: whatever the user picked, else the @@ -474,24 +476,19 @@ static int64_t imageTimeOffsetNs(const AppState& s) return (int64_t)std::llround(s.timeOffsetSec * 1e9); } -//! Settles K.model from the two inputs that can select it, in precedence order. -//! Call after either changes; see AppState::fileModel for why. -//! -//! Only Pinhole and Equirectangular are inferred: the 360 rig marks its frames -//! with kEquirectPrefix, but nothing in a filename identifies a Mei fisheye, so -//! Mei is reachable only through an explicit "model" key. +//! Settles K.model from the two inputs that can select it, in precedence +//! order. Call after any of them changes; see AppState::fileModel for why. static void resolveCameraModel(AppState& s) { if (s.modelExplicit) s.K.model = s.fileModel; else - s.K.model = s.namesLookEquirect ? CameraModel::Equirectangular : CameraModel::Pinhole; + s.K.model = s.loadAsEquirectangular ? CameraModel::Equirectangular : CameraModel::Pinhole; } //! Index every camera frame in the camera directory by timestamp. Also picks up -//! the image dimensions -- read by the equirectangular projection, the ROI -//! default, the frustums and COLMAP's cameras.txt -- and, absent an explicit -//! "model" in the calibration, infers the camera model from the filenames. +//! the image dimensions -- read by the ROI default, the frustums and COLMAP's +//! cameras.txt. static void loadImages(AppState& s) { s.imagesFilenamesInTime.clear(); @@ -503,15 +500,12 @@ static void loadImages(AppState& s) } int loaded = 0; - int equirectNames = 0; for (auto& e : fs::directory_iterator(camDir)) { - bool equirect = false; - int64_t ts = parseImageTsNs(e.path(), &equirect); + int64_t ts = parseImageTsNs(e.path()); if (ts < 0) continue; s.imagesFilenamesInTime[ts] = e.path().string(); - equirectNames += equirect ? 1 : 0; ++loaded; } if (!s.imagesFilenamesInTime.empty()) @@ -523,10 +517,7 @@ static void loadImages(AppState& s) s.imgH = probe.rows; } } - // The "model" key wins whenever the calibration file carried one; the - // filenames are only a fallback. Either way the resolved model is shown in - // the Calibration panel, so an inferred one is never invisible. - s.namesLookEquirect = equirectNames > 0; + resolveCameraModel(s); s.K.width = s.imgW; s.K.height = s.imgH; @@ -1167,10 +1158,10 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; - // "equirectangular"/"equirect", "mei" (or the rig's "insta360_mei_v2"), - // anything else pinhole. Accepted at the top level or inside "intrinsics". - // Assigned unconditionally, so loading a pinhole calibration after another - // model clears the flag rather than inheriting it. + // "mei" (or the rig's "insta360_mei_v2"), anything else pinhole. Accepted + // at the top level or inside "intrinsics". Assigned unconditionally, so + // loading a pinhole calibration after another model clears the flag + // rather than inheriting it. { const bool topLevel = j.contains("model"); const bool nested = j.contains("intrinsics") && j["intrinsics"].contains("model"); @@ -1188,9 +1179,7 @@ static void loadCalib(AppState& s) { return (char)std::tolower(c); }); - if (model == "equirectangular" || model == "equirect") - s.fileModel = CameraModel::Equirectangular; - else if (model == "mei" || model == "insta360_mei_v2") + if (model == "mei" || model == "insta360_mei_v2") s.fileModel = CameraModel::Mei; else s.fileModel = CameraModel::Pinhole; @@ -2468,6 +2457,13 @@ int main(int argc, char* argv[]) 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::Checkbox("Load as equirectangular (360)", &s.loadAsEquirectangular)) + resolveCameraModel(s); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Treat CAMERA_0's frames as a 360 panorama rather than a\n" + "normal camera image. Overridden by an explicit \"model\"\n" + "key in the loaded calibration JSON."); if (ImGui::Button("Load session", ImVec2(-1, 0))) loadSession(s); if (!s.imagesFilenamesInTime.empty()) @@ -2541,10 +2537,6 @@ int main(int argc, char* argv[]) if (s.K.model == CameraModel::Equirectangular) { ImGui::Text("Model: equirectangular"); - if (!s.modelExplicit && ImGui::IsItemHovered()) - ImGui::SetTooltip( - "Inferred from the \"%s\" image filenames.\nAdd \"model\" to the calibration JSON to set it explicitly.", - kEquirectPrefix); ImGui::Text("%dx%d", s.imgW, s.imgH); } else if (s.K.model == CameraModel::Mei) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 84342792..48f10adf 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -2,6 +2,7 @@ #include #include #include +#include #include namespace calib @@ -43,6 +44,20 @@ namespace calib int width = 0, height = 0; }; + + struct CameraIdentity + { + + std::string serial; + std::string frameId; + std::string model; + std::string firmware; + bool empty() const + { + return serial.empty() && frameId.empty(); + } + }; + //! Name of a camera model, as written to the calibration JSON's "model" key. //! @param m model to name //! @return one of "pinhole", "equirectangular", "mei" @@ -146,6 +161,18 @@ namespace calib //! degrade the app to "no reprojection available", not crash it. bool loadMeiIntrinsics(const std::string& path, Intrinsics& K); + //! Reads a camera_info.yaml-shaped file's `serial`, `frame_id` and + //! `model` fields. Opens and scans the file independently of + //! @ref loadMeiIntrinsics -- + //! identity and intrinsics are unrelated concerns read by separate + //! functions, not two jobs of the same one. + //! @param path file to read + //! @param id overwritten on success (cleared first, so a field the file + //! does not name comes back empty rather than kept from a + //! previous load), untouched on failure + //! @return false if the file cannot be opened + bool loadCameraIdentity(const std::string& path, CameraIdentity& id); + //! The same camera after its images are resampled, so a downscaled image //! projects with the same geometry. Distortion terms are dimensionless and //! carry over unchanged. @@ -193,4 +220,15 @@ namespace calib float& v, float& depth); + //! Reads the `FRAME_WALL_CLOCK` field (nanoseconds since epoch) from an + //! image's `.meta.json` sidecar. + //! @param path the image file, e.g. ".../cam0_123.jpg"; the sidecar is + //! the same basename with its extension replaced by ".meta.json" + //! (".../cam0_123.meta.json") + //! @return the timestamp in nanoseconds, or nullopt if the sidecar is + //! missing, unreadable, or has no FRAME_WALL_CLOCK field + //! @note Scanned as text, like @ref loadMeiIntrinsics's yaml, rather than + //! parsed as JSON, so calib_core keeps depending on nothing but + //! Eigen/LASzip/std. + std::optional LoadTimestampFromSideCar(const std::string& path); } // namespace calib diff --git a/calib_core/include/CalibCore/PointCloud.h b/calib_core/include/CalibCore/PointCloud.h index 2e1a8e2c..e83b9f2f 100644 --- a/calib_core/include/CalibCore/PointCloud.h +++ b/calib_core/include/CalibCore/PointCloud.h @@ -23,4 +23,6 @@ struct PointCloud { bool empty() const { return points.empty(); } }; +std::string GetLidarSerial(const char* path); + } // namespace calib diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index cad7fc7f..f7a23e7f 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,6 +1,10 @@ #include #include +#include +#include +#include +#include // Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- // header-only, pulls in nothing but Eigen/std (see structures.h), so this @@ -10,6 +14,109 @@ namespace calib { +namespace +{ + std::string trim(std::string s) + { + const char* ws = " \t\r\n"; + const auto b = s.find_first_not_of(ws); + if (b == std::string::npos) + return {}; + return s.substr(b, s.find_last_not_of(ws) - b + 1); + } + + std::string unquote(std::string s) + { + if (s.size() >= 2 && (s.front() == '"' || s.front() == '\'') && s.back() == s.front()) + return s.substr(1, s.size() - 2); + return s; + } +} // namespace + +// Unrelated to loadMeiIntrinsics (MeiIntrinsics.cpp) -- opens and reads the +// file on its own rather than sharing a file handle or result with it, +// since parsing intrinsics and reading identity fields are two different +// jobs. Works on any flat `key: value` yaml, not just a Mei camera_info.yaml. +bool loadCameraIdentity(const std::string& path, CameraIdentity& id) +{ + std::ifstream f(path); + if (!f) + return false; + + // Cleared rather than merged, so a file naming no camera comes back + // empty instead of keeping whatever was loaded before it. + CameraIdentity next; + std::string line; + while (std::getline(f, line)) + { + const auto hash = line.find('#'); + if (hash != std::string::npos) + line = line.substr(0, hash); + const auto colon = line.find(':'); + if (colon == std::string::npos) + continue; + const std::string key = trim(line.substr(0, colon)); + const std::string value = unquote(trim(line.substr(colon + 1))); + if (key == "serial") + next.serial = value; + else if (key == "frame_id") + next.frameId = value; + else if (key == "model") + next.model = value; + } + id = next; + + return true; +} + +std::optional LoadTimestampFromSideCar(const std::string& path) +{ + const auto dot = path.rfind('.'); + const std::string sidecar = (dot != std::string::npos ? path.substr(0, dot) : path) + ".meta.json"; + + std::ifstream f(sidecar); + if (!f) + return std::nullopt; + + std::string line; + while (std::getline(f, line)) + { + const auto key = line.find("\"FRAME_WALL_CLOCK\""); + if (key == std::string::npos) + continue; + const auto colon = line.find(':', key); + if (colon == std::string::npos) + return std::nullopt; + + size_t p = colon + 1; + while (p < line.size() && std::isspace(static_cast(line[p]))) + ++p; + + std::string value; + if (p < line.size() && line[p] == '"') + { + const auto close = line.find('"', p + 1); + if (close == std::string::npos) + return std::nullopt; + value = line.substr(p + 1, close - p - 1); + } + else + { + const auto end = line.find_first_of(",}", p); + value = trim(line.substr(p, end == std::string::npos ? std::string::npos : end - p)); + } + + if (value.empty()) + return std::nullopt; + char* endptr = nullptr; + const double ts = std::strtod(value.c_str(), &endptr); + if (endptr == value.c_str()) + return std::nullopt; // no digits consumed -- not a number + return ts; + } + + return std::nullopt; +} Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { TaitBryanPose pose; diff --git a/calib_core/src/PointCloud.cpp b/calib_core/src/PointCloud.cpp index c44fc9a3..05e6f328 100644 --- a/calib_core/src/PointCloud.cpp +++ b/calib_core/src/PointCloud.cpp @@ -2,7 +2,8 @@ #include #include #include - +#include +#include namespace calib { void PointCloud::clear() { @@ -85,4 +86,32 @@ bool PointCloud::load(const std::string& path) { } +std::string GetLidarSerial(const char* path) +{ + static constexpr const char* kUnknownLidarSerial = "unknown"; + + const std::string spath(path); + const std::regex lidarPattern(R"(lidar(\d+)\.laz$)"); + std::smatch match; + + if (!std::regex_search(spath, match, lidarPattern)) + return kUnknownLidarSerial; + + std::string statusPath = std::regex_replace(spath, lidarPattern, "status$1.json"); + + std::ifstream f(statusPath); + if (!f) + { + return kUnknownLidarSerial; + } + try + { + nlohmann::json j; + f >> j; + return j["lidar"]["LivoxLidarInfo"]["sn"].get(); + } catch (...) + { + } + return kUnknownLidarSerial; +} } // namespace calib diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 8b22e05f..58db2ee2 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -493,6 +493,37 @@ TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") CHECK(half.v == doctest::Approx(full.v * 0.5)); } } +// ── CameraIdentity::empty ───────────────────────────────────────────────────── + +TEST_CASE("CameraIdentity::empty: a default-constructed identity is empty") +{ + CHECK(CameraIdentity{}.empty()); +} + +TEST_CASE("CameraIdentity::empty: a serial alone makes it non-empty") +{ + CameraIdentity id; + id.serial = "SN-1"; + CHECK_FALSE(id.empty()); +} + +TEST_CASE("CameraIdentity::empty: a frame_id alone makes it non-empty") +{ + CameraIdentity id; + id.frameId = "camera_front"; + CHECK_FALSE(id.empty()); +} + +TEST_CASE("CameraIdentity::empty: model/firmware alone do not count") +{ + // Only serial/frameId identify a physical camera; model and firmware are + // descriptive metadata that can be present without either. + CameraIdentity id; + id.model = "Insta360 X4"; + id.firmware = "1.2.3"; + CHECK(id.empty()); +} + // ── loadMeiIntrinsics ───────────────────────────────────────────────────────── namespace @@ -513,6 +544,20 @@ namespace return ok ? std::optional(K) : std::nullopt; } + // As above, for loadCameraIdentity. `id` is only meaningful when this + // returns true. + bool loadIdentityFromString(const std::string& body, CameraIdentity& id) + { + const std::string path = (std::filesystem::temp_directory_path() / "calib_core_test_camera_info.yaml").string(); + { + std::ofstream f(path); + f << body; + } + const bool ok = loadCameraIdentity(path, id); + std::filesystem::remove(path); + return ok; + } + const char* kSample = R"(# this rig's camera_info.yaml frame_id: camera_front distortion_model: insta360_mei_v2 @@ -578,3 +623,134 @@ TEST_CASE("loadMeiIntrinsics: a missing file fails cleanly, and leaves K alone") CHECK(K.fx == before.fx); CHECK(K.xi == before.xi); } + +// ── loadCameraIdentity ──────────────────────────────────────────────────────── +// Independent of loadMeiIntrinsics -- opens the same kind of file again on +// its own and only ever looks at `serial`/`frame_id`/`model`, so these tests +// don't depend on the intrinsics fields being present or valid at all. + +TEST_CASE("loadCameraIdentity: reads serial, frame_id and model when all are present") +{ + std::string body = kSample; + body += "\nserial: SN-12345\nmodel: Insta360 X4\n"; + + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.serial == "SN-12345"); + CHECK(id.frameId == "camera_front"); + CHECK(id.model == "Insta360 X4"); +} + +TEST_CASE("loadCameraIdentity: a field the file does not name comes back empty") +{ + // kSample has frame_id but no serial. + CameraIdentity id; + CHECK(loadIdentityFromString(kSample, id)); + CHECK(id.serial.empty()); + CHECK(id.frameId == "camera_front"); +} + +TEST_CASE("loadCameraIdentity: a successful load clears a previously-populated id") +{ + // Loading a file that names no camera must drop the previous identity + // rather than leave it attached to a different one. + CameraIdentity id; + id.serial = "stale-serial"; + id.model = "stale-model"; + id.firmware = "stale-firmware"; + + CHECK(loadIdentityFromString(kSample, id)); + CHECK(id.serial.empty()); + CHECK(id.model.empty()); + CHECK(id.firmware.empty()); + CHECK(id.frameId == "camera_front"); +} + +TEST_CASE("loadCameraIdentity: quotes around a value are not taken literally") +{ + std::string body = kSample; + body += "\nserial: \"SN-12345\"\n"; + + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.serial == "SN-12345"); +} + +TEST_CASE("loadCameraIdentity: neither field present comes back empty, not a failure") +{ + // Unlike loadMeiIntrinsics, no field here is required -- a file that + // simply doesn't name a camera is a valid, successful "no identity". + std::string body = "distortion_model: insta360_mei_v2\nwidth: 640\n"; + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.empty()); +} + +TEST_CASE("loadCameraIdentity: a missing file fails cleanly, and leaves id alone") +{ + CameraIdentity id; + id.serial = "untouched"; + + CHECK_FALSE(loadCameraIdentity("/nonexistent/camera_info.yaml", id)); + CHECK(id.serial == "untouched"); +} + +// ── LoadTimestampFromSideCar ──────────────────────────────────────────────── + +namespace +{ + // Real-world sample, trimmed from a libcamera-style .meta.json sidecar + // next to a captured frame -- FRAME_WALL_CLOCK is a quoted nanosecond + // epoch string, not a bare JSON number. + const char* kMetaSample = R"({ + "AE_STATE": "2", + "ANALOGUE_GAIN": "1.000000", + "EXPOSURE_TIME": 6.34, + "FRAME_DURATION": 16.68, + "FRAME_WALL_CLOCK": "1789125060554994432", + "LUX": "580.969055" +})"; + + // Writes `metaBody` to "/.meta.json" and calls + // LoadTimestampFromSideCar on "/." (a file that need not + // itself exist -- only the sidecar is read). + std::optional loadTimestampForStem(const std::string& stem, const std::string& ext, const std::string& metaBody) + { + const auto dir = std::filesystem::temp_directory_path(); + const std::string sidecar = (dir / (stem + ".meta.json")).string(); + { + std::ofstream f(sidecar); + f << metaBody; + } + const auto result = LoadTimestampFromSideCar((dir / (stem + "." + ext)).string()); + std::filesystem::remove(sidecar); + return result; + } +} // namespace + +TEST_CASE("LoadTimestampFromSideCar: reads FRAME_WALL_CLOCK from the image's .meta.json") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_frame", "jpg", kMetaSample); + REQUIRE(ts.has_value()); + CHECK(*ts == doctest::Approx(1789125060554994432.0)); +} + +TEST_CASE("LoadTimestampFromSideCar: a missing sidecar returns nullopt") +{ + const auto dir = std::filesystem::temp_directory_path(); + const auto missing = (dir / "calib_core_test_no_such_frame.jpg").string(); + CHECK_FALSE(LoadTimestampFromSideCar(missing).has_value()); +} + +TEST_CASE("LoadTimestampFromSideCar: a sidecar with no FRAME_WALL_CLOCK returns nullopt") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_nofield", "jpg", R"({"LUX": "580.969055"})"); + CHECK_FALSE(ts.has_value()); +} + +TEST_CASE("LoadTimestampFromSideCar: an unquoted numeric value is read too") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_unquoted", "jpg", R"({"FRAME_WALL_CLOCK": 1789125060554994432})"); + REQUIRE(ts.has_value()); + CHECK(*ts == doctest::Approx(1789125060554994432.0)); +} From e9c6c8c3455bacd1f8d3e61268bf7ddf103f8449 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Wed, 16 Sep 2026 11:33:46 +0200 Subject: [PATCH 19/22] Fix calib_core build on macOS: add nlohmann/json include dir PointCloud.cpp includes nlohmann/json.hpp but calib_core never added the bundled 3rdparty/json/include dir to its include paths, so the header wasn't found on macOS/AppleClang builds. Co-Authored-By: Claude Sonnet 5 --- calib_core/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 57d4bbd1..34f05d93 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -68,6 +68,9 @@ target_include_directories(calib_core PRIVATE # doesn't violate calib_core's no-raylib/imgui/OpenCV rule above, and # nothing here links the core/core_math library, just includes headers. ${REPOSITORY_DIRECTORY}/core/include + # PointCloud.cpp uses nlohmann::json for metadata I/O; header-only, same + # bundled copy core/CMakeLists.txt already exposes to its own targets. + ${THIRDPARTY_DIRECTORY}/json/include ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) From 191a997c11d7be53b54ecc86bedf84064e35da58 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Wed, 16 Sep 2026 13:04:00 +0200 Subject: [PATCH 20/22] Fix clang-format and macOS pole-precision test failures in PR #536 - App.cpp: drop a stray blank line before a closing brace that clang-format flags. - test_camera.cpp: widen the epsilon on the two "top edge" pole assertions. asinf(-1) isn't correctly rounded on every platform's libm (macOS/AppleClang's included) -- the derivative of asin is infinite at the pole, so even a 1-ULP wobble there gets amplified through the pixel-height scale. doctest::Approx's default epsilon is an absolute tolerance too tight for that when comparing against 0, which made these two checks flaky on macOS CI. Co-Authored-By: Claude Sonnet 5 --- apps/camera_lidar_calibration/App.cpp | 1 - calib_core/tests/test_camera.cpp | 10 ++++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 152e7de6..d62093dd 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -560,7 +560,6 @@ void AppState::loadCalibration(const char* path) cameraId.model = j.value("model", std::string{}); cameraId.firmware = j.value("firmware", std::string{}); cameraId.frameId = j.value("frame_id", std::string{}); - } if (j.contains("intrinsics")) diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp index 58db2ee2..5d7c8194 100644 --- a/calib_core/tests/test_camera.cpp +++ b/calib_core/tests/test_camera.cpp @@ -94,7 +94,12 @@ TEST_CASE("equirectangular: cardinal bearings land on the expected pixels") SUBCASE("straight up is the top edge") { Px r = project(K, { 0, -3, 0 }); - CHECK(r.v == doctest::Approx(0.0)); + // asinf(-1) isn't correctly rounded on every platform's libm (its + // derivative is infinite at the pole, so even a 1-ULP wobble there + // is expected); doctest::Approx's default epsilon is an absolute + // tolerance too tight for that when comparing against 0, so widen + // it rather than pin down a libm implementation detail. + CHECK(r.v == doctest::Approx(0.0).epsilon(1e-3)); } } @@ -195,7 +200,8 @@ TEST_CASE("equirectangular: respects the extrinsics") SUBCASE("LiDAR up is the top edge") { Px r = project(K, { 0, 0, 10 }, R_wc); - CHECK(r.v == doctest::Approx(0.0)); + // See the identical-tolerance comment on the "straight up" case above. + CHECK(r.v == doctest::Approx(0.0).epsilon(1e-3)); } SUBCASE("the camera position is subtracted") { From 668f81542021f63d5775296d855713ebba501912 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 16 Sep 2026 15:26:59 +0200 Subject: [PATCH 21/22] TrajectoryViewer: detect sessions via .mjs on drop/--mjs; log Mei stub to stderr Co-Authored-By: Claude Sonnet 5 --- .../TrajectoryViewer.cpp | 38 ++++++++++++------- calib_core/src/CameraCalibrationSolverMei.cpp | 3 ++ 2 files changed, 28 insertions(+), 13 deletions(-) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index f118e42a..00145a77 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -1533,15 +1533,14 @@ static void actionOpenCalibration(AppState& s) } } -//! A directory holding this app's camera frames (cam0_.jpg). -static bool isCameraDir(const fs::path& dir) +//! Whether a directory holds a *.mjs session manifest directly -- lidar_odometry_step_1 +//! writes session.mjs alongside session_poses.mrp/session_ini_poses.mri, so this is a +//! reliable positive marker for "this is a LIO result (session) directory". +static bool hasMjsFile(const fs::path& dir) { for (const auto& e : fs::directory_iterator(dir)) - { - std::string n = e.path().filename().string(); - if (n.rfind("cam0_", 0) == 0 && e.path().extension() == ".jpg") + if (e.path().extension() == ".mjs") return true; - } return false; } @@ -1558,16 +1557,19 @@ static void actionOpenMask(AppState& s) //! Drag & drop equivalent of the menu load actions, applied immediately rather //! than waiting for "Load session" -- a drop is already an explicit "load this". -//! A dropped directory of cam0_*.jpg is the camera directory (only the images -//! are swapped, so the trajectory and cloud survive); any other directory is a -//! session (LIO result dir); a *.json is a calibration file. +//! A dropped directory containing a *.mjs manifest is a session (LIO result +//! dir); any other dropped directory is the camera directory (only the images +//! are swapped, so the trajectory and cloud survive); a *.mjs file is a +//! session manifest (its parent directory is the session, as with --mjs); a +//! *.json is a calibration file. static void handleDroppedPath(AppState& s, const std::string& path) { if (fs::is_directory(path)) { - // Checked before the session branch: a CAMERA_0 folder is never a LIO result dir, - // and dropping one onto a loaded session must not wipe the trajectory. - if (isCameraDir(path)) + // Checked before the session branch: only a *.mjs manifest marks a LIO + // result dir, so dropping a plain image folder onto a loaded session + // must not wipe the trajectory. + if (!hasMjsFile(path)) { setBuf(s.cameraBuf, sizeof(s.cameraBuf), path); loadImages(s); @@ -1596,6 +1598,12 @@ static void handleDroppedPath(AppState& s, const std::string& path) setBuf(s.calibBuf, sizeof(s.calibBuf), path); loadCalib(s); } + else if (ext == ".mjs") + { + // Session manifest, same convention as --mjs: the session directory is its parent. + setBuf(s.sessionBuf, sizeof(s.sessionBuf), fs::path(path).parent_path().string()); + loadSession(s); + } else if (ext == ".png" || ext == ".bmp" || ext == ".jpg" || ext == ".jpeg") { // The only single image this app takes as input is a mask -- camera @@ -1982,9 +1990,13 @@ int main(int argc, char* argv[]) AppState s; // --mjs gives the session manifest; the session directory is its parent. + // Also accepts the session directory itself, for symmetry with drag & drop. std::string sessionDir; if (args.has("mjs")) - sessionDir = fs::path(args.get("mjs")).parent_path().string(); + { + fs::path mjsPath(args.get("mjs")); + sessionDir = fs::is_directory(mjsPath) ? mjsPath.string() : mjsPath.parent_path().string(); + } else if (!args.positional.empty()) sessionDir = args.positional.front(); // back-compat if (!sessionDir.empty()) diff --git a/calib_core/src/CameraCalibrationSolverMei.cpp b/calib_core/src/CameraCalibrationSolverMei.cpp index 734502f2..880a9213 100644 --- a/calib_core/src/CameraCalibrationSolverMei.cpp +++ b/calib_core/src/CameraCalibrationSolverMei.cpp @@ -179,6 +179,8 @@ namespace calib #else // !CALIB_ENABLE_CERES +#include + namespace calib { bool solveExtrinsicsMeiCeres( @@ -190,6 +192,7 @@ namespace calib bool) { errorMessage = "Mei extrinsics solving needs calib_core built with -DCALIB_ENABLE_CERES=ON (see calib_core/CMakeLists.txt)"; + std::cerr << errorMessage << std::endl; return false; } } // namespace calib From c1e30a4f72a4d53ef465d103fb4c0acdb9e0b975 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 16 Sep 2026 16:47:33 +0200 Subject: [PATCH 22/22] Add session_to_mcap: session -> MCAP exporter (imu, undistorted lidar, tf) New console tool that writes a processed lidar_odometry_step_1 session (session.json) to an MCAP file containing already-undistorted lidar points placed in the map frame, a /tf stream of map->lidar transforms taken from each chunk's local_trajectory, and optionally /imu re-read from the original mandeye recording directory (a session keeps no raw IMU samples). Extends rosbags::McapWriter with tf2_msgs/msg/TFMessage support (hand-rolled ros2msg/CDR encoding, same approach as the existing PointCloud2/Imu channels, no real ROS dependency) and a decoupled point-cloud frame_id so a cloud can be published in a different frame than the Imu/tf child frame. Co-Authored-By: Claude Sonnet 5 --- apps/console_tools/CMakeLists.txt | 51 ++- apps/console_tools/session_to_mcap.cpp | 500 +++++++++++++++++++++++++ rosbags/McapWriter.cpp | 100 ++++- rosbags/McapWriter.h | 34 ++ rosbags/cdr_serializer.hpp | 30 ++ rosbags/tests/test_mcap_writer.cpp | 48 +++ 6 files changed, 760 insertions(+), 3 deletions(-) create mode 100644 apps/console_tools/session_to_mcap.cpp diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index 860920f2..c6fe6c21 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -119,6 +119,54 @@ if (MSVC) target_compile_options(laz_to_mcap PRIVATE /bigobj) endif() +# session_to_mcap: a processed lidar_odometry_step_1 session (session.json) +# -> MCAP exporter (undistorted lidar in the map frame + /tf map->lidar, +# optionally /imu re-read from the original recording via --raw-dir). Unlike +# laz_to_mcap it touches Core::Session, whose layout branches on WITH_GUI (see +# core/CMakeLists.txt's add_core_target) -- so this links core_no_gui, not +# ${CORE_LIBRARIES} (= core, the WITH_GUI=1 build laz_to_mcap gets away with +# only because it never includes Core/session.h). Mirrors the include/link set +# apps/lidar_odometry_step_1/tests uses to combine lidar_odometry_utils.cpp +# with core_no_gui in one binary. +add_executable( + session_to_mcap session_to_mcap.cpp + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.h + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ) + +target_include_directories( + session_to_mcap + PRIVATE ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1 + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/rosbags + ${THIRDPARTY_DIRECTORY} # csv.hpp (used by load_imu) + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY}/json/include + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + session_to_mcap + PRIVATE + mcap + core_no_gui + vqf + Fusion + unordered_dense::unordered_dense + spdlog::spdlog + UTL::include + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS}) + +if (MSVC) + target_compile_options(session_to_mcap PRIVATE /bigobj) +endif() + # These are built whenever BUILD_WITH_CLI_TOOLS is ON (the default), so they # ship in the DEB package alongside the GUI apps. hdmapping_install_app( @@ -130,4 +178,5 @@ hdmapping_install_app( pcd_to_laz laz_to_txt laz_to_mcap - mcap_to_laz) + mcap_to_laz + session_to_mcap) diff --git a/apps/console_tools/session_to_mcap.cpp b/apps/console_tools/session_to_mcap.cpp new file mode 100644 index 00000000..21a02c5c --- /dev/null +++ b/apps/console_tools/session_to_mcap.cpp @@ -0,0 +1,500 @@ +// Processed lidar_odometry_step_1 session (session.json) -> MCAP exporter. +// +// Unlike laz_to_mcap (which exports a *raw* mandeye recording, points still +// in each scan's own moving sensor frame), a session's point clouds are +// already motion-compensated: PointCloud::points_local is undistorted and +// expressed relative to that chunk's own first pose, and PointCloud::m_pose +// places the chunk in the map frame -- see core/include/Core/export_laz.h's +// save_all_to_las() for the same "m_pose * points_local[i]" composition. +// This tool writes those already-registered points straight into the map +// frame (matching apps/camera_lidar_trajectory_viewer/RosExport.h's +// "exportLidarUndistorted" convention: frame_id = map, no further motion +// compensation needed), plus a /tf stream of map -> lidar samples taken from +// each chunk's local_trajectory (falling back to one static-ish sample per +// chunk for older sessions saved without a trajectory_lio_*.csv). +// +// A session keeps no raw IMU samples (WorkerData::raw_imu_data only exists +// during the live lidar_odometry_step_1 run and isn't serialized), so /imu +// is optional and, if wanted, is re-read from the *original* mandeye +// recording directory via load_imu() -- the same function laz_to_mcap uses. +#include "McapWriter.h" +#include "lidar_odometry_utils.h" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace fs = std::filesystem; + +namespace +{ + + bool check_path_ext(const std::string& path, const char* ext) + { + return fs::path(path).extension() == ext; + } + + std::string to_lower(std::string s) + { + std::transform(s.begin(), s.end(), s.begin(), [](unsigned char c) { return std::tolower(c); }); + return s; + } + + // PointCloud::timestamps / LocalTrajectoryNode::timestamps.first are stored + // in NANOSECONDS: lidar_odometry.cpp writes both the scan_lio_*.laz gps_time + // field and the trajectory_lio_*.csv "timestamp_nanoseconds" column as + // `seconds * 1e9`, and both are read back verbatim (no /1e9). McapPoint / + // McapImuSample / McapTransform all expect absolute seconds, so every + // session-sourced timestamp is converted here, once. + constexpr double kNanosecondsToSeconds = 1e-9; + + // Session point clouds carry no per-point ring/laser_id, so PointCloud2's + // Generic layout (the only one that needs them) still round-trips fine -- + // both fields just come out zero. + std::vector to_mcap_points(const PointCloud& pc) + { + std::vector out; + out.reserve(pc.points_local.size()); + for (size_t i = 0; i < pc.points_local.size(); ++i) + { + const double ts_ns = (i < pc.timestamps.size()) ? pc.timestamps[i] : 0.0; + if (ts_ns == 0.0) // sentinel for "no timestamp", same convention as save_all_to_las's skip_ts_0 + continue; + + const Eigen::Vector3d world = pc.m_pose * pc.points_local[i]; + + rosbags::McapPoint mp{}; + mp.x = static_cast(world.x()); + mp.y = static_cast(world.y()); + mp.z = static_cast(world.z()); + mp.intensity = (i < pc.intensities.size()) ? static_cast(pc.intensities[i]) : 0.0f; + mp.timestamp = ts_ns * kNanosecondsToSeconds; + out.push_back(mp); + } + return out; + } + + void sort_points_by_timestamp(std::vector& points) + { + std::sort(points.begin(), points.end(), [](const auto& a, const auto& b) { return a.timestamp < b.timestamp; }); + } + + rosbags::McapTransform to_mcap_transform(double timestamp_s, const Eigen::Affine3d& T) + { + rosbags::McapTransform t{}; + t.timestamp = timestamp_s; + t.tx = T.translation().x(); + t.ty = T.translation().y(); + t.tz = T.translation().z(); + Eigen::Quaterniond q(T.linear()); + q.normalize(); + t.qx = q.x(); + t.qy = q.y(); + t.qz = q.z(); + t.qw = q.w(); + return t; + } + + // T_map_lidar per node = pc.m_pose * node.m_pose: local_trajectory poses are + // stored relative to the chunk's own first pose (lidar_odometry.cpp writes + // `intermediate_trajectory[0].inverse() * intermediate_trajectory[j]`), the + // same convention points_local uses -- see the file header comment. + std::vector to_mcap_transforms(const PointCloud& pc) + { + std::vector out; + if (!pc.local_trajectory.empty()) + { + out.reserve(pc.local_trajectory.size()); + for (const auto& node : pc.local_trajectory) + out.push_back(to_mcap_transform(node.timestamps.first * kNanosecondsToSeconds, pc.m_pose * node.m_pose)); + return out; + } + + // Older session without a trajectory_lio_*.csv: one sample for the whole + // chunk, stamped at its first valid (non-sentinel) point timestamp. + double ts_ns = 0.0; + for (double t : pc.timestamps) + { + if (t != 0.0) + { + ts_ns = t; + break; + } + } + out.push_back(to_mcap_transform(ts_ns * kNanosecondsToSeconds, pc.m_pose)); + return out; + } + + // Cuts a session chunk's points into one PointCloud2 message per 1/msg_hz + // seconds, so the bag replays at a lidar-like rate instead of one huge + // message per chunk. Mirrors laz_to_mcap.cpp's MessageSplitter exactly + // (absolute time-bin grid, pending points carry over a chunk boundary); + // duplicated rather than shared since that one buffers Point3Di and this one + // already-converted McapPoint. + class MessageSplitter + { + public: + MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) + : writer_(writer) + , msg_hz_(msg_hz) + { + } + + // `points` must be sorted by timestamp, and successive calls must be in + // timestamp order too (session chunks are processed in container order). + void add(const std::vector& points) + { + if (msg_hz_ <= 0.0) + { + write(points); + return; + } + for (const auto& p : points) + { + const int64_t bin = static_cast(std::floor(p.timestamp * msg_hz_)); + if (!pending_.empty() && bin != current_bin_) + flush(); + current_bin_ = bin; + pending_.push_back(p); + } + } + + void flush() + { + write(pending_); + pending_.clear(); + } + + size_t messages_written() const + { + return messages_; + } + + private: + void write(const std::vector& points) + { + if (points.empty()) + return; + const uint64_t stamp_ns = static_cast(points.front().timestamp * 1e9); + writer_.writePointCloud(stamp_ns, points); + ++messages_; + } + + rosbags::McapFileWriter& writer_; + double msg_hz_; + std::vector pending_; + int64_t current_bin_ = 0; + size_t messages_ = 0; + }; + + // load_imu() reads a single sensor's stream out of an imuNNNN.csv (see its doc + // comment in lidar_odometry_utils.h): on a rig with more than one IMU, rows + // carry an optional "imuId" column and only rows matching this id are kept. + // session_to_mcap has no per-sensor calibration to resolve which id is "the" + // IMU (a session keeps no raw IMU/calibration provenance at all), so it + // always reads id 0 and instead warns when a file actually contains more + // than one id -- see distinct_imu_ids() below. + constexpr int kImuIdToUse = 0; + + // Splits a line the same way load_imu()'s CSVFormat does (space/comma/tab + // delimited), just for peeking at the header/imuId column below. + std::vector split_csv_line(const std::string& line) + { + std::vector out; + std::string cur; + for (char c : line) + { + if (c == ' ' || c == ',' || c == '\t') + { + if (!cur.empty()) + { + out.push_back(cur); + cur.clear(); + } + } + else + cur.push_back(c); + } + if (!cur.empty()) + out.push_back(cur); + return out; + } + + // Returns every distinct "imuId" value in a modern-format (named-column) + // IMU csv, purely to warn when a file mixes more than one IMU. Empty for a + // file with no imuId column (a single-IMU recording -- id 0 covers it, no + // warning needed) or for the legacy headerless format load_imu() also + // accepts (not inspected here; load_imu() itself still reads it correctly). + std::set distinct_imu_ids(const std::string& csv_path) + { + std::set ids; + std::ifstream file(csv_path); + std::string header_line; + if (!file.is_open() || !std::getline(file, header_line)) + return ids; + + const auto header = split_csv_line(header_line); + const auto it = std::find(header.begin(), header.end(), "imuId"); + if (it == header.end()) + return ids; + const auto imu_id_index = static_cast(std::distance(header.begin(), it)); + + std::string line; + while (std::getline(file, line)) + { + const auto row = split_csv_line(line); + if (imu_id_index >= row.size()) + continue; + try + { + ids.insert(std::stoi(row[imu_id_index])); + } catch (const std::exception&) + { + } + } + return ids; + } + + // Reads every imu*.csv in raw_dir (mandeye's imuNNNN.csv chunk convention) + // and merges them into one timestamp-sorted stream, always using IMU id 0 + // (warning first if any file actually carries more than one IMU id). + std::vector load_all_imu(const fs::path& raw_dir) + { + std::vector csvs; + for (const auto& entry : fs::directory_iterator(raw_dir)) + { + if (!entry.is_regular_file()) + continue; + if (to_lower(entry.path().extension().string()) != ".csv") + continue; + if (!to_lower(entry.path().stem().string()).starts_with("imu")) + continue; + csvs.push_back(entry.path().string()); + } + std::sort(csvs.begin(), csvs.end()); + + std::set all_ids; + for (const auto& csv : csvs) + { + const auto ids = distinct_imu_ids(csv); + all_ids.insert(ids.begin(), ids.end()); + } + if (all_ids.size() > 1) + { + std::string ids_str; + for (int id : all_ids) + ids_str += (ids_str.empty() ? "" : ", ") + std::to_string(id); + spdlog::warn("{} carries more than one IMU (ids: {}) - session_to_mcap always reads id {}", raw_dir.string(), ids_str, kImuIdToUse); + } + + std::vector out; + for (const auto& csv : csvs) + { + const auto imu_data = load_imu(csv, kImuIdToUse); + for (const auto& [ts, gyr, acc] : imu_data) + { + rosbags::McapImuSample s{}; + s.timestamp = ts.first; + s.gyro_x = gyr.x(); + s.gyro_y = gyr.y(); + s.gyro_z = gyr.z(); + s.acc_x = acc.x(); + s.acc_y = acc.y(); + s.acc_z = acc.z(); + out.push_back(s); + } + } + std::sort(out.begin(), out.end(), [](const auto& a, const auto& b) { return a.timestamp < b.timestamp; }); + return out; + } + + void print_usage(const char* argv0) + { + spdlog::error("Usage: {} [options]", argv0); + spdlog::error(" session.json a lidar_odometry_step_1 session; its point clouds are already"); + spdlog::error(" undistorted and are written straight into the map frame, plus a"); + spdlog::error(" /tf stream of map->lidar samples taken from each chunk's trajectory"); + spdlog::error("Options:"); + spdlog::error(" --raw-dir original mandeye recording directory (imuNNNN.csv files);"); + spdlog::error(" a session keeps no raw IMU samples, so this is the only way"); + spdlog::error(" to include /imu. Omitted: lidar + tf only, no /imu channel is written."); + spdlog::error(" Always reads IMU id 0; warns if a file carries more than one IMU id."); + spdlog::error(" --lidar-topic lidar PointCloud2 topic (default: /lidar_points)"); + spdlog::error(" --imu-topic IMU topic (default: /imu)"); + spdlog::error(" --tf-topic tf topic (default: /tf)"); + spdlog::error(" --map-frame tf parent frame / PointCloud2 frame_id (default: map)"); + spdlog::error(" --lidar-frame tf child frame / Imu frame_id (default: lidar)"); + spdlog::error(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)"); + spdlog::error(" --msg_hz message rate: points are split into one PointCloud2 per"); + spdlog::error(" 1/hz seconds (default: 10; 0 = one message per session chunk)"); + } + +} // namespace + +int main(const int argc, const char** argv) +{ + if (argc < 3) + { + print_usage(argv[0]); + return EXIT_FAILURE; + } + + const std::string session_path = argv[1]; + const std::string mcap_path = argv[2]; + std::string raw_dir; + rosbags::McapWriterOptions options; + options.frame_id = "lidar"; + options.pointcloud_frame_id = "map"; + options.map_frame = "map"; + double msg_hz = 10.0; + + for (int i = 3; i < argc; ++i) + { + const std::string arg = argv[i]; + const bool hasValue = i + 1 < argc; + + if (arg == "--raw-dir" && hasValue) + raw_dir = argv[++i]; + else if (arg == "--lidar-topic" && hasValue) + options.lidar_topic = argv[++i]; + else if (arg == "--imu-topic" && hasValue) + options.imu_topic = argv[++i]; + else if (arg == "--tf-topic" && hasValue) + options.tf_topic = argv[++i]; + else if (arg == "--map-frame" && hasValue) + { + const std::string value = argv[++i]; + options.pointcloud_frame_id = value; + options.map_frame = value; + } + else if (arg == "--lidar-frame" && hasValue) + options.frame_id = argv[++i]; + else if (arg == "--msg_hz" && hasValue) + { + const std::string value = argv[++i]; + try + { + msg_hz = std::stod(value); + } catch (const std::exception&) + { + spdlog::error("Invalid --msg_hz '{}' (expected a number)", value); + return EXIT_FAILURE; + } + if (!std::isfinite(msg_hz) || msg_hz < 0.0) + { + spdlog::error("Invalid --msg_hz '{}' (expected >= 0; 0 = one message per session chunk)", value); + return EXIT_FAILURE; + } + } + else if (arg == "--lidar-type" && hasValue) + { + const std::string type = argv[++i]; + if (type == "generic") + options.lidar_layout = rosbags::PointCloudLayout::Generic; + else if (type == "velodyne") + options.lidar_layout = rosbags::PointCloudLayout::Velodyne; + else if (type == "ouster") + options.lidar_layout = rosbags::PointCloudLayout::Ouster; + else if (type == "hesai") + options.lidar_layout = rosbags::PointCloudLayout::Hesai; + else + { + spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai)", type); + return EXIT_FAILURE; + } + } + else + { + spdlog::error("Unrecognized argument '{}'", arg); + print_usage(argv[0]); + return EXIT_FAILURE; + } + } + + if (!check_path_ext(mcap_path, ".mcap")) + { + spdlog::error("Invalid extension for output file {} - expected .mcap", mcap_path); + return EXIT_FAILURE; + } + if (!fs::exists(session_path)) + { + spdlog::error("Session file {} does not exist", session_path); + return EXIT_FAILURE; + } + Session session; + if (!session.load(session_path, /*is_decimate=*/false, 0, 0, 0, /*calculate_offset=*/false)) + { + spdlog::error("Failed to load session '{}'", session_path); + return EXIT_FAILURE; + } + + rosbags::McapFileWriter writer(mcap_path, options); + if (!writer.isOpen()) + { + spdlog::error("Failed to open output mcap file {}", mcap_path); + return EXIT_FAILURE; + } + + const auto& clouds = session.point_clouds_container.point_clouds; + spdlog::info("Loaded session with {} chunk(s) from {}", clouds.size(), session_path); + + size_t total_points = 0; + std::vector all_tf; + MessageSplitter splitter(writer, msg_hz); + for (size_t idx = 0; idx < clouds.size(); ++idx) + { + const auto& pc = clouds[idx]; + if (!pc.visible) + { + spdlog::info("[{}/{}] {}: skipped (not visible)", idx + 1, clouds.size(), pc.file_name); + continue; + } + + auto points = to_mcap_points(pc); + sort_points_by_timestamp(points); + total_points += points.size(); + splitter.add(points); + spdlog::info("[{}/{}] {}: {} points", idx + 1, clouds.size(), pc.file_name, points.size()); + + const auto tf = to_mcap_transforms(pc); + all_tf.insert(all_tf.end(), tf.begin(), tf.end()); + } + splitter.flush(); + spdlog::info( + "Loaded {} points across {} chunk(s), wrote {} point cloud message(s)", total_points, clouds.size(), splitter.messages_written()); + + std::sort(all_tf.begin(), all_tf.end(), [](const auto& a, const auto& b) { return a.timestamp < b.timestamp; }); + writer.writeTf(all_tf); + spdlog::info("Wrote {} tf sample(s)", all_tf.size()); + + if (!raw_dir.empty()) + { + if (!fs::exists(raw_dir) || !fs::is_directory(raw_dir)) + { + spdlog::error("--raw-dir {} does not exist or is not a directory - no /imu written", raw_dir); + } + else + { + const auto imu = load_all_imu(raw_dir); + if (!imu.empty()) + { + writer.writeImu(imu); + spdlog::info("Loaded {} IMU sample(s) from {}", imu.size(), raw_dir); + } + else + { + spdlog::warn("No imu*.csv samples found in {} - no /imu written", raw_dir); + } + } + } + + spdlog::info("Wrote {}", mcap_path); + return EXIT_SUCCESS; +} diff --git a/rosbags/McapWriter.cpp b/rosbags/McapWriter.cpp index 93b05e76..64fdaa1f 100644 --- a/rosbags/McapWriter.cpp +++ b/rosbags/McapWriter.cpp @@ -50,6 +50,37 @@ uint8 FLOAT64=8 static constexpr const char* kStringSchema = R"(string data )"; +static constexpr const char* kTfMessageSchema = R"(geometry_msgs/TransformStamped[] transforms +================================================================================ +MSG: geometry_msgs/TransformStamped +std_msgs/Header header +string child_frame_id +geometry_msgs/Transform transform +================================================================================ +MSG: std_msgs/Header +builtin_interfaces/Time stamp +string frame_id +================================================================================ +MSG: builtin_interfaces/Time +int32 sec +uint32 nanosec +================================================================================ +MSG: geometry_msgs/Transform +geometry_msgs/Vector3 translation +geometry_msgs/Quaternion rotation +================================================================================ +MSG: geometry_msgs/Vector3 +float64 x +float64 y +float64 z +================================================================================ +MSG: geometry_msgs/Quaternion +float64 x +float64 y +float64 z +float64 w +)"; + static constexpr const char* kImuSchema = R"(std_msgs/Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance @@ -302,6 +333,32 @@ static std::vector serializeImu(uint64_t timestamp_ns, const McapImuSam return w.data(); } +// tf2_msgs/msg/TFMessage carrying a single TransformStamped, matching how a +// real /tf topic publishes one changed transform per message. +static std::vector serializeTf( + uint64_t timestamp_ns, const McapTransform& t, const std::string& parent_frame, const std::string& child_frame) +{ + CdrWriter w; + + w.write_u32(1); // transforms[] sequence length + + writeHeader(w, timestamp_ns, parent_frame); // TransformStamped.header + w.write_string(child_frame); + + // transform.translation + w.write_f64(t.tx); + w.write_f64(t.ty); + w.write_f64(t.tz); + + // transform.rotation + w.write_f64(t.qx); + w.write_f64(t.qy); + w.write_f64(t.qz); + w.write_f64(t.qw); + + return w.data(); +} + // --------------------------------------------------------------------------- // Impl // --------------------------------------------------------------------------- @@ -312,9 +369,11 @@ struct McapFileWriter::Impl mcap::ChannelId lidarChannelId{0}; mcap::ChannelId imuChannelId{0}; mcap::ChannelId snChannelId{0}; + mcap::ChannelId tfChannelId{0}; uint32_t lidarSequence{0}; uint32_t imuSequence{0}; uint32_t snSequence{0}; + uint32_t tfSequence{0}; McapWriterOptions options; bool open{false}; }; @@ -369,6 +428,16 @@ McapFileWriter::McapFileWriter(const std::filesystem::path& path, const McapWrit impl_->writer.addChannel(snChannel); impl_->snChannelId = snChannel.id; + // Register tf2_msgs/msg/TFMessage schema + /tf channel + mcap::Schema tfSchema("tf2_msgs/msg/TFMessage", "ros2msg", + {reinterpret_cast(kTfMessageSchema), + reinterpret_cast(kTfMessageSchema) + std::strlen(kTfMessageSchema)}); + impl_->writer.addSchema(tfSchema); + + mcap::Channel tfChannel(impl_->options.tf_topic, "cdr", tfSchema.id); + impl_->writer.addChannel(tfChannel); + impl_->tfChannelId = tfChannel.id; + impl_->open = true; } @@ -409,8 +478,8 @@ void McapFileWriter::writePointCloud(uint64_t timestamp_ns, const std::vectoroptions.frame_id, impl_->options.lidar_layout); + const std::string& frame_id = impl_->options.pointcloud_frame_id.empty() ? impl_->options.frame_id : impl_->options.pointcloud_frame_id; + auto payload = serializePointCloud2(timestamp_ns, points, frame_id, impl_->options.lidar_layout); mcap::Message msg; msg.channelId = impl_->lidarChannelId; @@ -452,4 +521,31 @@ void McapFileWriter::writeImu(const std::vector& imu) writeImuSample(sample); } +void McapFileWriter::writeTfSample(const McapTransform& transform) +{ + if(!isOpen()) + return; + + const uint64_t ts = static_cast(transform.timestamp * 1e9); + auto payload = serializeTf(ts, transform, impl_->options.map_frame, impl_->options.frame_id); + + mcap::Message msg; + msg.channelId = impl_->tfChannelId; + msg.sequence = impl_->tfSequence++; + msg.publishTime = ts; + msg.logTime = ts; + msg.data = reinterpret_cast(payload.data()); + msg.dataSize = payload.size(); + + auto s = impl_->writer.write(msg); + if(!s.ok()) + std::cerr << "McapWriter: tf write error: " << s.message << "\n"; +} + +void McapFileWriter::writeTf(const std::vector& transforms) +{ + for(const auto& t : transforms) + writeTfSample(t); +} + } // namespace rosbags \ No newline at end of file diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h index 1291feee..d083b63e 100644 --- a/rosbags/McapWriter.h +++ b/rosbags/McapWriter.h @@ -55,6 +55,23 @@ struct McapImuSample float acc_z{}; }; +// One rigid transform sample (parent -> child), written as a single-element +// tf2_msgs/msg/TFMessage -- one message per sample, matching how a real /tf +// topic carries one changed transform per publish. `timestamp` is an +// absolute timestamp in seconds. Rotation must be a unit quaternion; the +// default is identity. +struct McapTransform +{ + double timestamp{}; + double tx{}; + double ty{}; + double tz{}; + double qx{}; + double qy{}; + double qz{}; + double qw{1.0}; +}; + // Selects the sensor_msgs/msg/PointCloud2 field layout the lidar channel is // written with. The message type is always PointCloud2 -- only the `fields` // array/point_step (and thus which McapPoint members get written) changes, @@ -70,9 +87,18 @@ enum class PointCloudLayout struct McapWriterOptions { std::string frame_id = "lidar"; + // Overrides frame_id for PointCloud2 headers only (Imu headers and the + // /tf child_frame_id keep using frame_id). Left empty, PointCloud2 also + // uses frame_id -- unchanged default behavior. Set this to distinguish a + // point cloud published in a fixed frame (e.g. "map", already + // motion-compensated) from a sensor's own moving frame. + std::string pointcloud_frame_id; + // Parent frame written into /tf's TransformStamped.header.frame_id. + std::string map_frame = "map"; std::string lidar_topic = "/lidar_points"; std::string imu_topic = "/imu"; std::string sn_topic = "/lidar_sn"; + std::string tf_topic = "/tf"; PointCloudLayout lidar_layout = PointCloudLayout::Generic; }; @@ -84,6 +110,7 @@ struct McapWriterOptions // /lidar_points — sensor_msgs/msg/PointCloud2 (field layout per options().lidar_layout) // /imu — sensor_msgs/msg/Imu // /lidar_sn — std_msgs/msg/String +// /tf — tf2_msgs/msg/TFMessage (one TransformStamped per message) // // PointCloud2 field layouts (see PointCloudLayout): // Generic (point_step = 28): @@ -140,6 +167,13 @@ class McapFileWriter // Write a string to /lidar_sn (std_msgs/msg/String). void writeSn(uint64_t timestamp_ns, const std::string& data); + // Write a single transform as its own tf2_msgs/msg/TFMessage (one + // TransformStamped, parent = options().map_frame, child = options().frame_id). + void writeTfSample(const McapTransform& transform); + + // Write a batch of transforms, one /tf message per sample. + void writeTf(const std::vector& transforms); + bool isOpen() const; private: diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp index 8a17e4d0..a4ab196d 100644 --- a/rosbags/cdr_serializer.hpp +++ b/rosbags/cdr_serializer.hpp @@ -788,4 +788,34 @@ inline std::string decodeSn(const uint8_t* data, size_t size) return r.ok() ? s : std::string{}; } +// tf2_msgs/msg/TFMessage → McapTransform (first TransformStamped only; McapWriter +// never writes more than one) +inline std::optional decodeTf(const uint8_t* data, size_t size) +{ + CdrReader r(data, size); + + const uint32_t n = r.read_u32(); // transforms[] sequence length + if(n == 0) + return std::nullopt; + + const int32_t stamp_sec = r.read_i32(); + const uint32_t stamp_nsec = r.read_u32(); + r.read_string(); // frame_id (parent) + r.read_string(); // child_frame_id + + McapTransform t{}; + t.timestamp = static_cast(stamp_sec) + static_cast(stamp_nsec) * 1e-9; + t.tx = r.read_f64(); + t.ty = r.read_f64(); + t.tz = r.read_f64(); + t.qx = r.read_f64(); + t.qy = r.read_f64(); + t.qz = r.read_f64(); + t.qw = r.read_f64(); + + if(!r.ok()) + return std::nullopt; + return t; +} + } // namespace rosbags \ No newline at end of file diff --git a/rosbags/tests/test_mcap_writer.cpp b/rosbags/tests/test_mcap_writer.cpp index 428c54d8..0dcc798d 100644 --- a/rosbags/tests/test_mcap_writer.cpp +++ b/rosbags/tests/test_mcap_writer.cpp @@ -262,6 +262,54 @@ TEST_CASE("McapFileWriter: IMU round-trip") fs::remove(path); } +TEST_CASE("McapFileWriter: TF round-trip") +{ + const auto path = tempMcapPath("hdmapping_test_tf.mcap"); + + std::vector transforms; + for (int i = 0; i < 5; ++i) + { + rosbags::McapTransform t{}; + t.timestamp = 6000.0 + i * 0.1; + t.tx = 1.0 * i; + t.ty = -2.0 * i; + t.tz = 0.5; + t.qx = 0.0; + t.qy = 0.0; + t.qz = 0.0; + t.qw = 1.0; + transforms.push_back(t); + } + + { + rosbags::McapWriterOptions options; + options.frame_id = "lidar"; + options.map_frame = "map"; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writeTf(transforms); + } + + const auto decoded = readTopic>( + path, "/tf", [](const uint8_t* d, size_t n) + { + return rosbags::decodeTf(d, n); + }); + + REQUIRE(decoded.size() == transforms.size()); + for (size_t i = 0; i < transforms.size(); ++i) + { + REQUIRE(decoded[i].has_value()); + CHECK(decoded[i]->tx == doctest::Approx(transforms[i].tx)); + CHECK(decoded[i]->ty == doctest::Approx(transforms[i].ty)); + CHECK(decoded[i]->tz == doctest::Approx(transforms[i].tz)); + CHECK(decoded[i]->qw == doctest::Approx(transforms[i].qw)); + CHECK(decoded[i]->timestamp == doctest::Approx(transforms[i].timestamp).epsilon(1e-6)); + } + + fs::remove(path); +} + TEST_CASE("McapFileWriter: custom topic names are honored") { const auto path = tempMcapPath("hdmapping_test_topics.mcap");