Rework of camera-lidar calibration, point picking, optimization

.gitignore

@@ -75,3 +75,5 @@ imgui.ini
 
 # deploy_mandeye.bat output
 /deploy
+
+.cache/

apps/camera_lidar_calibration/App.h

@@ -10,6 +10,15 @@
 
 using namespace calib;
 
+// One manually-picked correspondence between a LiDAR point and the image
+// pixel it should project to. u/v are in the (undistorted) displayed-image
+// pixel frame; px/py/pz are in the raw LiDAR frame.
+struct CorrespondencePair
+{
+    float u = 0.f, v = 0.f;
+    float px = 0.f, py = 0.f, pz = 0.f;
+};
+
 struct AppState
 {
     // ── loaded data ──────────────────────────────────────────────────────────
@@ -22,6 +31,11 @@ struct AppState
     int imageW = 0, imageH = 0;
     std::string imagePath;
     std::vector<std::string> cloudPaths;
+    // `cloud.points` converted to raylib world-space (x, z, -y), 1:1 index
+    // match with `cloud.points` -- kept in sync by loadCloud()/addCloud() so
+    // 3D point picking (raylib_widgets::pickNearestPoint) doesn't have to
+    // re-convert the whole cloud every frame.
+    std::vector<Vector3> cloudPointsRaylib;
 
     // ── calibration params ───────────────────────────────────────────────────
     Intrinsics intrinsics;
@@ -41,6 +55,33 @@ struct AppState
     // ── misc ──────────────────────────────────────────────────────────────────
     std::string statusMsg;
 
+    // ── LiDAR↔image correspondence picking ──────────────────────────────────
+    // Hold Shift and click in either the Image View or the 3D View to set
+    // the pending pick for that side (each click overwrites the previous
+    // pending pick for that side, in either order); once both sides are set
+    // the pair is completed automatically.
+    std::vector<CorrespondencePair> pairs;
+    CorrespondencePair pendingPair;
+    bool pendingHasImage = false;
+    bool pendingHasCloud = false;
+    double lastSolveRmsPixels = -1.0; // < 0 = no solve run yet
+    // Row selected in the Correspondences panel list (-1 = none); drawn as
+    // a large cross in both the 3D view and the image view.
+    int selectedPairIndex = -1;
+    // When set, "Solve Extrinsics from Pairs" refines orientation only and
+    // leaves tx/ty/tz exactly as they are -- for when the camera's position
+    // relative to the LiDAR is already known (e.g. measured by hand) and
+    // only orientation needs calibrating from the picked pairs. Also
+    // disables the tx/ty/tz drag sliders in the Extrinsics panel so they
+    // can't be nudged by accident while locked.
+    bool lockTranslation = false;
+
+    void setPendingImagePoint(float u, float v); // completes the pair if a cloud point is already pending
+    void setPendingCloudPoint(float px, float py, float pz); // completes the pair if an image point is already pending
+    void clearPending(); // discards the in-progress pending pick, if any
+    void removePair(int index);
+    bool solvePairs(); // solves extrinsics from `pairs`, updates statusMsg
+
     // ── operations ────────────────────────────────────────────────────────────
     void loadImage(const char* path);
     void loadCloud(const char* path); // clear + load

apps/camera_lidar_calibration/Renderer.cpp

@@ -24,11 +24,11 @@ Color jetColor(float t)
 // those definitions, which became a duplicate-symbol link error once
 // raylib_widgets/CMakeLists.txt started actually compiling that .cpp.
 
-// World-frame convention: E.rx/ry/rz = camera orientation in world (R_wc, ZYX Euler).
+// World-frame convention: E.om/fi/ka = camera orientation in world (R_wc = Rx*Ry*Rz).
 // E.tx/ty/tz = camera position in world.  p_cam = R_wc^T * (p_lidar - C).
 static Matrix buildLidarToCamMatrix(const Extrinsics& E)
 {
-    Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz);
+    Eigen::Matrix3f R = omFiKaToMat3(E.om, E.fi, E.ka);
     Eigen::Vector3f ti = -(R.transpose() * Eigen::Vector3f(E.tx, E.ty, E.tz));
     // Raylib Matrix struct fields: m0,m4,m8,m12 / m1,m5,m9,m13 / m2,m6,m10,m14 / m3,m7,m11,m15
     // We store R^T with translation ti (lidar→cam transform).
@@ -80,6 +80,7 @@ void Renderer::initPointShader()
     {
         locMVP = rlGetLocationUniform(pointShader.id, "mvp");
         locPointSize = rlGetLocationUniform(pointShader.id, "pointSize");
+        locDecim = rlGetLocationUniform(pointShader.id, "drawDecim");
         locColorMode = rlGetLocationUniform(pointShader.id, "colorMode");
         locHeightRange = rlGetLocationUniform(pointShader.id, "heightRange");
         locMaxDist = rlGetLocationUniform(pointShader.id, "maxDist");
@@ -108,6 +109,7 @@ void Renderer::initPointShader()
         locPrjOpacity = rlGetLocationUniform(projShader.id, "opacity");
         locPrjPointSize = rlGetLocationUniform(projShader.id, "pointSize");
         locPrjColorMode = rlGetLocationUniform(projShader.id, "colorMode");
+        locPrjDecim = rlGetLocationUniform(projShader.id, "drawDecim");
     }
 
     // Allow gl_PointSize from the vertex shader (core profile requires this)
@@ -162,7 +164,14 @@ void Renderer::unloadCloudGPU()
 }
 
 void Renderer::renderImageOverlay(
-    const Texture2D& img, int imgW, int imgH, const Intrinsics& K, const Extrinsics& E, bool applyDistortion, const VisualizationParams& vp)
+    const Texture2D& img,
+    int imgW,
+    int imgH,
+    const Intrinsics& K,
+    const Extrinsics& E,
+    bool applyDistortion,
+    const VisualizationParams& vp,
+    bool showOverlay)
 {
     if (!imageTexValid)
         return;
@@ -173,7 +182,7 @@ void Renderer::renderImageOverlay(
     DrawTexturePro(
         img, Rectangle{ 0, 0, (float)imgW, (float)imgH }, Rectangle{ 0, 0, (float)texW, (float)texH }, Vector2{ 0, 0 }, 0.f, WHITE);
 
-    if (cloudCount > 0 && projShaderValid)
+    if (showOverlay && cloudCount > 0 && projShaderValid)
     {
         rlDrawRenderBatchActive(); // flush the image quad before raw GL draw
 
@@ -208,6 +217,7 @@ void Renderer::renderImageOverlay(
         rlSetUniform(locPrjOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1);
         rlSetUniform(locPrjPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
         rlSetUniform(locPrjColorMode, &vp.colorMode, RL_SHADER_UNIFORM_INT, 1);
+        rlSetUniform(locPrjDecim, &vp.drawDecim, RL_SHADER_UNIFORM_INT, 1);
 
         rlEnableVertexArray(cloudVAO);
         glDrawArrays(GL_POINTS, 0, cloudCount);
@@ -256,6 +266,7 @@ void Renderer::draw3DCloud(
     rlEnableShader(pointShader.id);
     rlSetUniformMatrix(locMVP, mvp);
     rlSetUniform(locPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
+    rlSetUniform(locDecim, &vp.drawDecim, RL_SHADER_UNIFORM_INT, 1);
     rlSetUniform(locColorMode, &colorMode, RL_SHADER_UNIFORM_INT, 1);
     rlSetUniform(locHeightRange, heightRange, RL_SHADER_UNIFORM_VEC2, 1);
     rlSetUniform(locMaxDist, &maxDist, RL_SHADER_UNIFORM_FLOAT, 1);
@@ -281,7 +292,7 @@ void Renderer::draw3DCloud(
 void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, int imgW, int imgH, float scale)
 {
     // World-frame convention: R_wc = camera orientation in world, C = camera position in world
-    Eigen::Matrix3f R = eulerZYXtoMat3(E.rx, E.ry, E.rz);
+    Eigen::Matrix3f R = omFiKaToMat3(E.om, E.fi, E.ka);
 
     // Camera position in LiDAR frame is directly (E.tx, E.ty, E.tz)
     Vector3 origin = { E.tx, E.tz, -E.ty }; // LiDAR→raylib

apps/camera_lidar_calibration/Renderer.h

@@ -15,6 +15,9 @@ struct VisualizationParams
     float depthMax = 50.f;
     float opacity = 1.f;
     int colorMode = 0; // 0=depth(jet), 1=intensity, 2=height(z), 3=Camera RGB
+    // Draw only every Nth point (GPU-side, like camera_lidar_trajectory_viewer's
+    // "Draw decimation") -- 1 = draw all points.
+    int drawDecim = 1;
 };
 
 Color jetColor(float t); // t in [0,1]
@@ -38,14 +41,17 @@ class Renderer
 
     // Render image + GPU-projected point overlay into imageTex.
     // If the displayed image is rectified, pass applyDistortion=false.
+    // showOverlay=false draws just the plain image (e.g. while picking, so
+    // projected points don't obscure the pixel the user is aiming for).
     void renderImageOverlay(
         const Texture2D& img,
         int imgW,
         int imgH,
         const Intrinsics& K,
         const Extrinsics& E,
         bool applyDistortion,
-        const VisualizationParams& vp);
+        const VisualizationParams& vp,
+        bool showOverlay = true);
 
     // Draw 3D point cloud into current BeginMode3D context (GPU shader path).
     // For colorMode 3 (camera RGB) pass the displayed image texture and the
@@ -77,7 +83,7 @@ class Renderer
     int cloudCount = 0;
     // 3D view shader uniforms
     int locMVP = -1, locColorMode = -1, locHeightRange = -1;
-    int locMaxDist = -1, locOpacity = -1, locPointSize = -1;
+    int locMaxDist = -1, locOpacity = -1, locPointSize = -1, locDecim = -1;
     int locCamXform = -1, locCamK = -1, locCamImgSize = -1, locCamTex = -1;
 
     // 2D image-projection shader
@@ -86,5 +92,5 @@ class Renderer
     int locPrjXform = -1, locPrjK = -1, locPrjImgSize = -1;
     int locPrjRad1 = -1, locPrjRad2 = -1, locPrjTan = -1;
     int locPrjDepthRange = -1, locPrjOpacity = -1;
-    int locPrjPointSize = -1, locPrjColorMode = -1;
+    int locPrjPointSize = -1, locPrjColorMode = -1, locPrjDecim = -1;
 };

apps/camera_lidar_calibration/RendererShaders.h

@@ -18,6 +18,7 @@ layout(location = 0) in vec3 vertexPosition;
 layout(location = 1) in float vertexIntensity;
 uniform mat4 mvp;
 uniform float pointSize;
+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;
@@ -26,6 +27,11 @@ out float fragIntensity;
 out vec2 fragUV;
 out float fragCamDepth;
 void main() {
+    if (drawDecim > 1 && (gl_VertexID % drawDecim) != 0) {
+        gl_Position = vec4(2.0, 2.0, 2.0, 1.0);
+        gl_PointSize = 0.0;
+        return;
+    }
     fragPos = vertexPosition;
     fragIntensity = vertexIntensity;
     gl_Position = mvp * vec4(vertexPosition, 1.0);
@@ -90,9 +96,15 @@ uniform vec3 kRad1;        // k1 k2 k3
 uniform vec3 kRad2;        // k4 k5 k6
 uniform vec2 pTan;         // p1 p2
 uniform float pointSize;
+uniform int drawDecim;     // draw only every Nth point; 1 = draw all
 out float fragDepth;
 out float fragIntensity;
 void main() {
+    if (drawDecim > 1 && (gl_VertexID % drawDecim) != 0) {
+        gl_Position = vec4(2.0, 2.0, 2.0, 1.0);
+        gl_PointSize = 0.0;
+        return;
+    }
     // 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;