Support for fisheye (instaX5).

.gitignore

@@ -72,6 +72,10 @@ imgui.ini
 /.gitmodules
 /build2
 /build3
+/build-*
+
+# local test data
+/rosbags.zip
 
 # deploy_mandeye.bat output
 /deploy

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()

apps/camera_lidar_calibration/App.cpp

@@ -16,6 +16,7 @@
 #include <opencv2/calib3d.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc.hpp>
+#include <regex>
 #include <vector>
 
 // ── 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_<double>(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<float>(imageW) / static_cast<float>(intrinsicsW);
+    float sy = static_cast<float>(imageH) / static_cast<float>(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<double>(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<char>(tolower(static_cast<unsigned char>(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<char>(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

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