Step 2: clip loop-closure target preview by intersection slab

apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp

@@ -3751,7 +3751,19 @@ void renderLoopClosure(
                              static_cast<unsigned char>(pointClouds[idx].render_color[2] * 255.f),
                              255 };
                 scan_renderer.drawCachedWithTransform(
-                    static_cast<size_t>(idx), delta, c, static_cast<float>(pointClouds[idx].point_size), ScanColorMode::Flat);
+                    static_cast<size_t>(idx),
+                    delta,
+                    c,
+                    static_cast<float>(pointClouds[idx].point_size),
+                    ScanColorMode::Flat,
+                    0.f,
+                    1.f,
+                    Eigen::Vector3d::Zero(),
+                    1.f,
+                    point_clouds_container.xz_intersection,
+                    point_clouds_container.yz_intersection,
+                    point_clouds_container.xy_intersection,
+                    static_cast<float>(point_clouds_container.intersection_width));
             }
         }
     }

core/include/Core/raylib_render.hpp

@@ -113,8 +113,9 @@ class ScanRenderer
     // useInitialPose draws every scan at its m_initial_pose instead of its
     // current m_pose (ports the legacy PointCloud::render()'s
     // show_with_initial_pose branch). The cached GPU buffers stay baked at
-    // m_pose; the per-scan delta (m_initial_pose * m_pose^-1) is folded into
-    // the MVP, the same trick drawCachedWithTransform() uses to preview a pose.
+    // m_pose; the per-scan delta (m_initial_pose * m_pose^-1) is applied in
+    // the vertex shader, the same trick drawCachedWithTransform() uses to
+    // preview a pose, so the slab filter and gradients follow the shown pose.
     void draw(
         const std::vector<PointCloud>& pointClouds,
         float pointSize,
@@ -198,8 +199,9 @@ class ScanRenderer
     // re-upload -- displaced by an extra transform on top of the pose it
     // was cached at. extraTransform is the delta from the scan's current
     // m_pose to the desired preview pose (i.e. previewPose * m_pose.inverse());
-    // it's folded into the MVP matrix for this draw call rather than applied
-    // to the points themselves. For overlays that need to preview a scan at
+    // it's applied in the vertex shader for this draw call rather than to the
+    // cached points themselves, ahead of the slab filter and Elevation/Distance
+    // gradients, so both follow the preview pose. For overlays that need to preview a scan at
     // a pose other than its stored one without paying for a CPU retransform
     // every frame (e.g. a loop-closure edge's in-progress relative pose, or a
     // gizmo-drag preview). colorMode selects the shader's gradient exactly as
@@ -297,6 +299,7 @@ class ScanRenderer
     Shader shader_{};
     bool shaderValid_ = false;
     int locMVP_ = -1;
+    int locModel_ = -1;
     int locPointSize_ = -1;
     int locColor_ = -1;
     int locColorMode_ = -1;

core/src/raylib_render.cpp

@@ -49,6 +49,7 @@ void ScanRenderer::init()
     if (shaderValid_)
     {
         locMVP_ = rlGetLocationUniform(shader_.id, "mvp");
+        locModel_ = rlGetLocationUniform(shader_.id, "model");
         locPointSize_ = rlGetLocationUniform(shader_.id, "pointSize");
         locColor_ = rlGetLocationUniform(shader_.id, "pointColor");
         locColorMode_ = rlGetLocationUniform(shader_.id, "colorMode");
@@ -198,8 +199,8 @@ void ScanRenderer::syncPoses(const std::vector<PointCloud>& pointClouds)
 namespace
 {
     // Defined lower down with the other file-local helpers; forward declared
-    // here so draw()/drawTrajectories() can fold a per-scan initial-pose delta
-    // into the MVP.
+    // here so draw()/drawTrajectories() can set a per-scan initial-pose delta
+    // as the shader's model matrix.
     Matrix toRaylibMatrix(const Eigen::Affine3d& t);
 } // namespace
 
@@ -234,6 +235,7 @@ void ScanRenderer::draw(
 
     rlEnableShader(shader_.id);
     rlSetUniformMatrix(locMVP_, mvp);
+    rlSetUniformMatrix(locModel_, MatrixIdentity());
     rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
     rlSetUniform(locElevMin_, &elevationMin, RL_SHADER_UNIFORM_FLOAT, 1);
     rlSetUniform(locElevMax_, &elevationMax, RL_SHADER_UNIFORM_FLOAT, 1);
@@ -281,14 +283,14 @@ void ScanRenderer::draw(
         }
 
         // Points are cached in world space at m_pose (see rebuild()); to show
-        // them at m_initial_pose instead, prepend the delta
-        // (m_initial_pose * m_pose^-1) to the shared MVP for this scan only --
-        // same technique as drawCachedWithTransform(). With useInitialPose off
-        // the MVP set once above is left untouched.
+        // them at m_initial_pose instead, set the delta
+        // (m_initial_pose * m_pose^-1) as this scan's model matrix -- same
+        // technique as drawCachedWithTransform(). With useInitialPose off the
+        // identity model set once above is left untouched.
         if (useInitialPose)
         {
             const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse();
-            rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp));
+            rlSetUniformMatrix(locModel_, toRaylibMatrix(delta));
         }
 
         float color[4];
@@ -336,11 +338,6 @@ void ScanRenderer::draw(
         rlDisableVertexArray();
     }
 
-    if (useInitialPose)
-    {
-        rlSetUniformMatrix(locMVP_, mvp); // undo the last scan's per-scan fold
-    }
-
     rlDisableShader();
 }
 
@@ -437,17 +434,16 @@ void ScanRenderer::drawCachedWithTransform(
 
     rlDrawRenderBatchActive();
 
-    Matrix mvpBase = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection());
-    // MatrixMultiply(A, B) composes as "apply A, then B" (see draw()'s own
-    // modelview-then-projection use above) -- so this applies the delta
-    // transform to the cached points first, then the normal view/projection.
-    Matrix mvp = MatrixMultiply(toRaylibMatrix(extraTransform), mvpBase);
+    Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection());
 
     float colorF[4] = { color.r / 255.f, color.g / 255.f, color.b / 255.f, color.a / 255.f };
     int colorModeInt = static_cast<int>(colorMode);
 
     rlEnableShader(shader_.id);
     rlSetUniformMatrix(locMVP_, mvp);
+    // The delta goes in the model matrix, not the MVP, so the slab filter and
+    // gradients see the displaced positions (see kPointVS).
+    rlSetUniformMatrix(locModel_, toRaylibMatrix(extraTransform));
     rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
     rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1);
     rlSetUniform(locColorMode_, &colorModeInt, RL_SHADER_UNIFORM_INT, 1);
@@ -532,6 +528,7 @@ void ScanRenderer::drawPoints(const PointsGPU& gpu, Color color, float pointSize
 
     rlEnableShader(shader_.id);
     rlSetUniformMatrix(locMVP_, mvp);
+    rlSetUniformMatrix(locModel_, MatrixIdentity());
     rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1);
     rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1);
     rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
@@ -706,15 +703,12 @@ void ScanRenderer::drawTrajectories(
         return;
     }
 
-    // Hoisted out of the `if (shaderValid_)` block below so the per-scan
-    // initial-pose MVP fold in the draw loop can use it as the base.
-    const Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection());
-
     if (shaderValid_)
     {
         rlDrawRenderBatchActive();
         rlEnableShader(shader_.id);
-        rlSetUniformMatrix(locMVP_, mvp);
+        rlSetUniformMatrix(locMVP_, MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()));
+        rlSetUniformMatrix(locModel_, MatrixIdentity());
         int colorModeFlat = 0;
         rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1);
         // Explicitly off -- see drawCachedWithTransform()'s comment on why.
@@ -734,8 +728,8 @@ void ScanRenderer::drawTrajectories(
         }
 
         // Same initial-pose handling as draw(): the trajectory GPU buffer is
-        // cached at m_pose, so fold (m_initial_pose * m_pose^-1) into the MVP
-        // for the polyline; the CPU-side overlays just use shownPose directly.
+        // cached at m_pose, so set (m_initial_pose * m_pose^-1) as the model
+        // matrix for the polyline; the CPU-side overlays just use shownPose directly.
         const Eigen::Affine3d& shownPose = useInitialPose ? pc.m_initial_pose : pc.m_pose;
 
         if (visibleImuDiff)
@@ -766,7 +760,7 @@ void ScanRenderer::drawTrajectories(
                 if (useInitialPose)
                 {
                     const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse();
-                    rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp));
+                    rlSetUniformMatrix(locModel_, toRaylibMatrix(delta));
                 }
                 float colorF[4] = { pc.traj_color[0], pc.traj_color[1], pc.traj_color[2], 1.0f };
                 rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1);
@@ -777,7 +771,7 @@ void ScanRenderer::drawTrajectories(
                 rlDisableVertexArray();
                 if (useInitialPose)
                 {
-                    rlSetUniformMatrix(locMVP_, mvp); // restore for the next scan
+                    rlSetUniformMatrix(locModel_, MatrixIdentity()); // restore for the next scan
                 }
             }
         }

core/src/raylib_render_shaders.hpp

@@ -16,20 +16,27 @@ namespace raylib_render_shaders
     // pointColor (0, the original app's only mode) and the ScanColorMode
     // gradients (1/2/3), matching the enum's Intensity/Elevation/Distance
     // ordering exactly (see ScanRenderer::draw()'s static_cast below).
+    // model displaces the cached world-space points to the pose actually shown
+    // (identity for a plain draw; a pose delta for drawCachedWithTransform() or
+    // useInitialPose). It is applied before fragWorldPos, not folded into mvp, so
+    // the intersection slab and the Elevation/Distance gradients follow the
+    // displaced points instead of the pose they were cached at.
     inline constexpr const char* kPointVS = R"(
 #version 330
 in vec3 vertexPosition;
 in float vertexIntensity;
 uniform mat4 mvp;
+uniform mat4 model;
 uniform float pointSize;
 out float fragIntensity;
 out vec3 fragWorldPos;
 void main()
 {
-    gl_Position = mvp * vec4(vertexPosition, 1.0);
+    vec4 worldPos = model * vec4(vertexPosition, 1.0);
+    gl_Position = mvp * worldPos;
     gl_PointSize = pointSize;
     fragIntensity = vertexIntensity;
-    fragWorldPos = vertexPosition;
+    fragWorldPos = worldPos.xyz;
 }
 )";