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 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_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 4ad06ec0..d62093dd 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 ───────────────────────────────────────────── @@ -27,7 +28,12 @@ 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 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); // OpenCV distCoeffs order: k1 k2 p1 p2 k3 k4 k5 k6 (rational model) @@ -61,6 +67,30 @@ void AppState::rebuildImageTexture() imageLoaded = true; } +// ── AppState::autoScaleIntrinsicsToImage ────────────────────────────────────── +std::string AppState::autoScaleIntrinsicsToImage() +{ + if (!intrinsicsLoaded || intrinsicsW <= 0 || imageW <= 0) + return ""; + if (intrinsicsW == imageW && intrinsicsH == imageH) + return ""; + + // 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); + 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 +169,15 @@ bool AppState::solvePairs() } double rms = -1.0; - bool ok = calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); + std::string solveErr; + // 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); if (!ok) { - statusMsg = "Solve failed (degenerate correspondences)"; + statusMsg = !solveErr.empty() ? ("Solve failed: " + solveErr) : "Solve failed (degenerate correspondences)"; return false; } @@ -168,9 +203,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 ─────────────────────────────────────────────────────── @@ -204,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) @@ -368,6 +410,27 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img return true; } +// 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 loadMeiIntrinsics and +// warns about 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 +440,27 @@ void AppState::loadIntrinsics(const char* path) for (auto& c : ext) c = static_cast(tolower(c)); + if ((ext == "yml" || ext == "yaml") && yamlLooksLikeMei(path)) + { + if (!calib::loadMeiIntrinsics(path, intrinsics)) + { + statusMsg = std::string("Mei intrinsics failed to load (see console): ") + path; + return; + } + 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"; + if (intrinsicsW > 0) + statusMsg += " (calibration " + std::to_string(intrinsicsW) + "x" + std::to_string(intrinsicsH) + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; + return; + } + if (ext == "yml" || ext == "yaml") { int imgW = 0, imgH = 0; @@ -386,17 +470,21 @@ 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 + calib::loadCameraIdentity(path, cameraId); + 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 +496,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 +510,21 @@ 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; + 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."; + if (!scaleNote.empty()) + statusMsg += " " + scaleNote; } // ── AppState::loadCalibration ───────────────────────────────────────────────── @@ -446,13 +548,29 @@ 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"]; + 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 +579,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 +618,12 @@ void AppState::loadCalibration(const char* path) return; } + std::string scaleNote; if (gotIntrinsics) + { + scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); + } statusMsg = "Loaded"; if (gotIntrinsics) @@ -509,6 +633,10 @@ 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; } // ── AppState::saveCalibration ───────────────────────────────────────────────── @@ -521,9 +649,34 @@ 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 } }; + // 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. + 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..f72eff2a 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -40,6 +40,20 @@ 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 + // the size the *current* intrinsics apply to, not the file's original. + int intrinsicsW = 0, intrinsicsH = 0; // ── visualization ───────────────────────────────────────────────────────── VisualizationParams vizParams; @@ -92,6 +106,12 @@ 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(); + // 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(); }; 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..aefaa965 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,35 @@ 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 + // 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); + float denom = Xs.z + xi; + // 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; + 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 +109,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 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). inline constexpr const char* kProjVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; @@ -93,8 +124,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 +141,40 @@ 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); + // >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); + 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..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). @@ -447,22 +478,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 (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"); + 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/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 584be0e7..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,26 +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; - const bool rectify = opt.undistortCamera && in.calibLoaded; - // Original jpeg bytes can be copied verbatim only when we neither - // rectify nor need to re-encode (compressed + no undistort). - const bool copyJpegBytes = opt.compressCamera && !rectify; + // 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; 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; @@ -231,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) @@ -299,14 +280,45 @@ 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. 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 }; + 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 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 (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 }; + 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"; 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.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/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 6b9fe3ba..00145a77 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 @@ -42,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" }, @@ -73,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()) @@ -83,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; @@ -92,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); @@ -105,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; @@ -130,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()) @@ -219,66 +224,97 @@ struct AppState { Trajectory traj; std::vector imageTsNs; - Intrinsics K; - 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 + Intrinsics K; //!< K.model selects pinhole / equirectangular / Mei (see CalibCore/Camera.h) + //! 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 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; + //! 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 + bool maskTexValid = false; 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 + //! loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - const float imgScale = 1.0f; + //! 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): + //! 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. + //! 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 - // 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] = {}; @@ -286,11 +322,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; @@ -304,7 +338,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 }; @@ -315,16 +349,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 }; @@ -332,25 +366,33 @@ struct AppState cv::Mat imgViewPending; bool imgViewHasNew = false; 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. + 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 }; // ── 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()) @@ -369,12 +411,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; @@ -386,45 +427,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) +//! 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 +//! @return the timestamp, or -1 when the name doesn't match. The all-digits +//! check rejects unrelated .jpgs, which would reach std::stoll. +//! @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) { - s.imagesFilenamesInTime.clear(); - fs::path camDir; - if (s.cameraBuf[0]) + if (p.extension() != ".jpg") + return -1; + std::string stem = p.stem().string(); + 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 { - camDir = fs::path(s.cameraBuf); - } - else + ts = std::stoll(stem); + } catch (...) { - camDir = fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; + 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 +//! 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. +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 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.loadAsEquirectangular ? CameraModel::Equirectangular : CameraModel::Pinhole; +} + +//! Index every camera frame in the camera directory by timestamp. Also picks up +//! the image dimensions -- read by the ROI default, the frustums and COLMAP's +//! cameras.txt. +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; for (auto& e : fs::directory_iterator(camDir)) { - std::string n = e.path().filename().string(); - if (n.rfind("cam0_", 0) != 0 || e.path().extension() != ".jpg") + int64_t ts = parseImageTsNs(e.path()); + 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(); + ++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; } } + + resolveCameraModel(s); + s.K.width = s.imgW; + s.K.height = s.imgH; 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; @@ -501,22 +601,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()); } @@ -525,40 +617,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) { @@ -589,25 +650,32 @@ 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 + // 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. - // 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; + const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + // 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 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 + // 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); - auto packGray = [](float intensity) -> float { uint8_t g = (uint8_t)(std::min(1.f, std::max(0.f, intensity)) * 255.f); @@ -675,12 +743,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; @@ -690,19 +761,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 = cv::imread(fnIt->second); + 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; @@ -718,12 +792,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 = 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 }); + chunkImgs.push_back({ imgTs + offNs, pose, std::move(img), gidx }); } } } @@ -792,6 +866,11 @@ static void loadCloud(AppState& s) float inRoiF = -1.f; // 1 inside ROI, 0 outside, -1 not in frustum int globalIdx = -1; }; + // 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; @@ -799,34 +878,54 @@ 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) + // 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; - 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 - // 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]); std::memcpy(&h.colorF, &p, 4); h.globalIdx = e.globalIdx; - h.depth = pc_.z(); + h.depth = depth; h.ok = true; return h; }; @@ -939,13 +1038,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; @@ -966,6 +1061,93 @@ 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, +//! 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); + 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 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); + 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); @@ -976,6 +1158,33 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; + // "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"); + 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); + }); + if (model == "mei" || model == "insta360_mei_v2") + s.fileModel = CameraModel::Mei; + else + s.fileModel = CameraModel::Pinhole; + resolveCameraModel(s); + } if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; @@ -983,7 +1192,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); @@ -992,6 +1204,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")) { @@ -1030,6 +1243,84 @@ 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 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]) + { + 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()) @@ -1116,8 +1407,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()) @@ -1157,11 +1448,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()) @@ -1221,9 +1510,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")); @@ -1244,32 +1533,43 @@ 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; } -// 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); + if (!path.empty()) + { + setBuf(s.maskBuf, sizeof(s.maskBuf), path); + loadMask(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 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); @@ -1298,6 +1598,20 @@ 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 + // 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; @@ -1347,8 +1661,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) @@ -1361,6 +1675,15 @@ static void exportColmap(AppState& s) s.status = "COLMAP: no images"; return; } + 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; + } fs::path out(s.colmapBuf); fs::path sparse = out / "sparse"; @@ -1397,11 +1720,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 = imageTimeOffsetNs(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 @@ -1465,11 +1789,14 @@ 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; - 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; @@ -1567,12 +1894,31 @@ 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; 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::Pinhole) + { + // 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++) + { + 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 +1926,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 @@ -1648,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()) @@ -1773,17 +2119,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); @@ -1803,18 +2141,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(); @@ -1887,11 +2217,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; } } @@ -1981,6 +2313,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] + imageTimeOffsetNs(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(); @@ -1995,6 +2344,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); @@ -2061,7 +2412,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(); @@ -2089,27 +2440,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(); @@ -2132,6 +2469,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()) @@ -2202,8 +2546,43 @@ 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"); + 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); + 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::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); ImGui::Separator(); if (ImGui::Checkbox("Region of interest", &s.roi.enabled)) @@ -2218,7 +2597,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(); @@ -2230,6 +2612,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(); } @@ -2252,8 +2665,14 @@ 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.timeOffsetSec != 0.0) + { + ImGui::SameLine(); + ImGui::TextDisabled("(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s))); + } { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); bool fast = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f && as > s.maxImageAngSpeedDeg; @@ -2270,6 +2689,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'."); + } } } @@ -2314,10 +2764,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"); - ImGui::Checkbox("Undistort (rectify)", &s.ros.undistortCamera); + 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("Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)."); + ImGui::SetTooltip( + "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); @@ -2366,8 +2817,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::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( + 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( @@ -2405,9 +2863,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; @@ -2415,6 +2877,32 @@ 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); + } + + // 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) { @@ -2429,6 +2917,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(); } @@ -2442,6 +2944,10 @@ int main(int argc, char* argv[]) s.rosThread.join(); if (s.imgViewTexValid) 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/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..1b8aa96f --- /dev/null +++ b/apps/console_tools/session_to_mcap.cpp @@ -0,0 +1,526 @@ +// 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/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 2c187642..df0d98d6 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,15 @@ 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); +bool loadCalibrationJson(const std::string& path); +bool saveCalibrationJson(const std::string& path); float imgui_co_size{ 1000.0f }; bool imgui_draw_co{ true }; @@ -134,10 +145,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 +217,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 +356,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 +474,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 +851,161 @@ 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); +} + +// 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 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); + 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(); + + // (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; +} + +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; + + // 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(); + 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; @@ -654,17 +1014,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 +1024,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 +1351,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 +1469,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) @@ -1247,6 +1591,28 @@ 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); @@ -1257,6 +1623,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 +1647,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 +1714,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 +1979,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); diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 99397ebc..34f05d93 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -18,12 +18,30 @@ project(calib_core) # calib_core and core/core_raylib in the same binary. add_library(calib_core STATIC src/Camera.cpp + src/MeiIntrinsics.cpp src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp src/CameraCalibrationSolver.cpp + src/CameraCalibrationSolverMei.cpp ) +# ── Optional Ceres-based Mei extrinsics solver ──────────────────────────────── +# 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 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() + message(STATUS "calib_core Mei Ceres solver: disabled (set -DCALIB_ENABLE_CERES=ON to enable)") +endif() + target_include_directories(calib_core PUBLIC include ) @@ -50,10 +68,19 @@ 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}) +if(CALIB_ENABLE_CERES) + # 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() + if(MSVC) target_compile_options(calib_core PRIVATE /W4) target_compile_definitions(calib_core PRIVATE _USE_MATH_DEFINES LASZIP_API_VERSION) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 65d3de88..48f10adf 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -2,103 +2,213 @@ #include #include #include +#include +#include namespace calib { + //! 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 + Equirectangular, // 360 panorama; width/height are the intrinsics, k*/p* unused + Mei // Insta 360 + }; + struct Intrinsics { + CameraModel model = CameraModel::Pinhole; float fx = 800.f, fy = 800.f; float cx = 640.f, cy = 360.f; - // OpenCV rational distortion model: - // radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + //! 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 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 + //! Tangential distortion. float p1 = 0.f, p2 = 0.f; + //! Unified-sphere mirror parameter, CameraModel::Mei only. + //! @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 + //! @ref projectPoint is called. + 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(); + } }; - // 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); - // 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). + //! 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); + + //! 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. + //! @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 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 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, @@ -110,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/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h index d476dad7..fcc7d6a1 100644 --- a/calib_core/include/CalibCore/CameraCalibrationSolver.h +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -1,42 +1,45 @@ #pragma once #include "Camera.h" #include +#include #include namespace calib { - // A single manually-picked correspondence: a 3D point in the LiDAR/world - // frame paired with the pixel it should project to in the camera image. - // Pixel coordinates are expected in the *undistorted* (ideal pinhole) - // frame -- i.e. picked from the rectified image display, see - // solveExtrinsicsFromCorrespondences() below. + //! 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). - // - // 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, @@ -44,4 +47,30 @@ namespace calib double* outRmsPixels = nullptr, bool fixTranslation = false); + //! 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, + Extrinsics& extrinsicsInOut, + std::string& errorMessage, + double* outRmsPixels = nullptr, + bool fixTranslation = false); + } // namespace calib 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/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/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(); } }; diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index c2fbabd0..86c7380a 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,5 +1,11 @@ #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 // doesn't violate calib_core's no-raylib/imgui/OpenCV design (see @@ -8,6 +14,99 @@ 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; + + nlohmann::json j; + try + { + f >> j; + } catch (const nlohmann::json::exception&) + { + return std::nullopt; + } + + const auto it = j.find("FRAME_WALL_CLOCK"); + if (it == j.end()) + return std::nullopt; + + // FRAME_WALL_CLOCK is nanoseconds since epoch, as a number or a numeric + // string -- returned as-is, matching the filename timestamps. + if (it->is_string()) + { + try + { + return std::stod(it->get()); + } catch (const std::exception&) + { + return std::nullopt; + } + } + if (it->is_number()) + return it->get(); + return std::nullopt; +} Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { TaitBryanPose pose; @@ -27,6 +126,114 @@ 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 +// 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; + 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; +} + +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, @@ -35,13 +242,68 @@ 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 width, latitude from + // asin(y/|p|) across the height -- camera X = right, Y = down, + // 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 + + 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; + } + + if (K.model == CameraModel::Mei) { + depth = pc.norm(); + 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 + // 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 + const float zMin = (K.xi > 1.f) ? -1.f / K.xi : -K.xi; + if (pc.z() / depth <= zMin) return false; + + // 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; + } + depth = pc.z(); if (depth <= 1e-4f) return false; 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) diff --git a/calib_core/src/CameraCalibrationSolverMei.cpp b/calib_core/src/CameraCalibrationSolverMei.cpp new file mode 100644 index 00000000..880a9213 --- /dev/null +++ b/calib_core/src/CameraCalibrationSolverMei.cpp @@ -0,0 +1,200 @@ +#include + +// 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 + +#include + +namespace calib +{ + namespace + { + // 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]) + { + 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 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 + 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, 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) + : 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); + + // 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())); + + return true; + } +} // namespace calib + +#else // !CALIB_ENABLE_CERES + +#include + +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)"; + std::cerr << errorMessage << std::endl; + return false; + } +} // namespace calib + +#endif \ No newline at end of file diff --git a/calib_core/src/MeiIntrinsics.cpp b/calib_core/src/MeiIntrinsics.cpp new file mode 100644 index 00000000..1c256857 --- /dev/null +++ b/calib_core/src/MeiIntrinsics.cpp @@ -0,0 +1,144 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +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); + } + + // 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; + } + + 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 + +bool loadMeiIntrinsics(const std::string& path, Intrinsics& K) +{ + std::ifstream f(path); + if (!f) + { + std::fprintf(stderr, "calib_core: failed to open '%s'\n", path.c_str()); + return false; + } + const std::map kv = readFlatYaml(f); + + // 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_core: '%s' is missing required field '%s'\n", path.c_str(), key); + return false; + } + } + + 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 modelIt = kv.find("distortion_model"); + 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 + // 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_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(), + distortionModel.c_str()); + } + 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 (distortionModel != "insta360_mei_v2") + { + std::fprintf( + stderr, + "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(), + distortionModel.c_str()); + } + + return true; +} + +} // namespace calib 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/CMakeLists.txt b/calib_core/tests/CMakeLists.txt new file mode 100644 index 00000000..6af5d779 --- /dev/null +++ b/calib_core/tests/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(calib_core_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 +) + +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) diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp new file mode 100644 index 00000000..5d7c8194 --- /dev/null +++ b/calib_core/tests/test_camera.cpp @@ -0,0 +1,762 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include + +#include +#include +#include +#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; + } + + // A representative Mei/unified-sphere fisheye, values in the shape + // insta360_mei_v2 calibrations take 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; + } + + + // 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 }); + // 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)); + } +} + +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); + // 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") + { + // 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)); + } +} + +// ── 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: 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(); + 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& r : refs) + { + 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(); + 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") +{ + // 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; + + // 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 + { + 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: the limit is where the denominator blows up, acos(-xi)") + { + Intrinsics K = mei(); + 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))); + } + + 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))); + } +} + +TEST_CASE("mei: respects the extrinsics") +{ + const Intrinsics K = mei(); + + // 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. + // 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)); +} + +// ── 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)); +} + +// ── 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") +{ + 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)); + } + 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)); + } +} +// ── 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 +{ + // Writes `body` to a temp file and loads it, so the parser is exercised + // through its real file-reading path. + // 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; + } + Intrinsics K; + const bool ok = loadMeiIntrinsics(path, K); + std::filesystem::remove(path); + 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 +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("loadMeiIntrinsics: reads this rig's flat camera_info.yaml") +{ + 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(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("loadMeiIntrinsics: quotes and trailing comments are not taken literally") +{ + std::string body = kSample; + 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("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. + 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()); + + CHECK_FALSE(loadFromString(body).has_value()); +} + +TEST_CASE("loadMeiIntrinsics: a missing file fails cleanly, and leaves K alone") +{ + 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); +} + +// ── 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)); +} 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 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_reader.cpp b/rosbags/tests/test_mcap_reader.cpp index 8ce96fb5..e2efc861 100644 --- a/rosbags/tests/test_mcap_reader.cpp +++ b/rosbags/tests/test_mcap_reader.cpp @@ -261,7 +261,7 @@ TEST_CASE("McapFileReader: topic resolution") CHECK(reader.topics().lidar == "/custom/points"); CHECK(reader.topics().imu == "/custom/imu"); CHECK(reader.topics().sn == "/custom/sn"); - CHECK(reader.channels().size() == 3); + CHECK(reader.channels().size() == 4); // lidar, imu, sn, tf are always registered } SUBCASE("an explicit topic is honored") @@ -280,7 +280,7 @@ TEST_CASE("McapFileReader: topic resolution") rosbags::McapFileReader reader(path, options); CHECK_FALSE(reader.isOpen()); CHECK(reader.error().find("/does/not/exist") != std::string::npos); - CHECK(reader.channels().size() == 3); // --list still works on an unresolvable file + CHECK(reader.channels().size() == 4); // --list still works on an unresolvable file } fs::remove(path); 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");