diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index b7d495c4..72e48ebd 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -4622,14 +4622,16 @@ void display() session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, - static_cast(session.point_clouds_container.intersection_width)); + static_cast(session.point_clouds_container.intersection_width), + session.point_clouds_container.show_with_initial_pose); scan_renderer.drawTrajectories( session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff, session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection); + session.point_clouds_container.xy_intersection, + session.point_clouds_container.show_with_initial_pose); observationPickingRender(observation_picking); diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index 4e4787b1..09fe75fc 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -106,6 +106,12 @@ class ScanRenderer // three false (the default), every point draws, matching today's // behavior. Applied in the fragment shader (a discard), not a CPU-side // filter -- see raylib_render_shaders.hpp. + // + // 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. void draw( const std::vector& pointClouds, float pointSize, @@ -118,7 +124,8 @@ class ScanRenderer bool xzIntersection = false, bool yzIntersection = false, bool xyIntersection = false, - float intersectionWidth = 0.1f) const; + float intersectionWidth = 0.1f, + bool useInitialPose = false) const; // Number of glDrawArrays calls draw() issued the last time it ran (one // per visible scan) -- raylib/rlgl don't expose a draw-call counter for @@ -168,13 +175,16 @@ class ScanRenderer // per-point like draw() above) whenever any of xzIntersection/ // yzIntersection/xyIntersection is set, so a cross-section view isn't // cluttered by trajectory points/markers outside the slab. + // useInitialPose: see draw() -- draws each scan's trajectory/markers at + // m_initial_pose instead of m_pose. void drawTrajectories( const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff, bool xzIntersection = false, bool yzIntersection = false, - bool xyIntersection = false) const; + bool xyIntersection = false, + bool useInitialPose = false) const; // Draws a single already-cached scan (see rebuild()) straight from its // persistent, full-resolution GPU buffer -- no CPU re-transform or diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index ddae1265..7d3bc493 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -208,6 +208,14 @@ void ScanRenderer::syncPoses(const std::vector& 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. + Matrix toRaylibMatrix(const Eigen::Affine3d& t); +} // namespace + void ScanRenderer::draw( const std::vector& pointClouds, float pointSize, @@ -220,7 +228,8 @@ void ScanRenderer::draw( bool xzIntersection, bool yzIntersection, bool xyIntersection, - float intersectionWidth) const + float intersectionWidth, + bool useInitialPose) const { lastDrawCallCount_ = 0; lastVertexCount_ = 0; @@ -280,6 +289,17 @@ void ScanRenderer::draw( continue; } + // 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. + if (useInitialPose) + { + const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse(); + rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp)); + } + float color[4]; int colorModeInt; if (gpu.hasMarkColor) @@ -322,6 +342,11 @@ void ScanRenderer::draw( rlDisableVertexArray(); } + if (useInitialPose) + { + rlSetUniformMatrix(locMVP_, mvp); // undo the last scan's per-scan fold + } + rlDisableShader(); } @@ -546,12 +571,12 @@ namespace // and show_IMU) mirrors the local pose's translation with orientation taken // from the first local_trajectory node's raw IMU om/fi/ka instead of the // LIO-optimized pose. - Eigen::Affine3d imuOrientationAtPose(const PointCloud& pc) + Eigen::Affine3d imuOrientationAtPose(const PointCloud& pc, const Eigen::Affine3d& poseForTranslation) { TaitBryanPose tb; - tb.px = pc.m_pose(0, 3); - tb.py = pc.m_pose(1, 3); - tb.pz = pc.m_pose(2, 3); + tb.px = poseForTranslation(0, 3); + tb.py = poseForTranslation(1, 3); + tb.pz = poseForTranslation(2, 3); tb.om = pc.local_trajectory[0].imu_om_fi_ka.x(); tb.fi = pc.local_trajectory[0].imu_om_fi_ka.y(); tb.ka = pc.local_trajectory[0].imu_om_fi_ka.z(); @@ -623,7 +648,8 @@ void ScanRenderer::drawTrajectories( bool visibleImuDiff, bool xzIntersection, bool yzIntersection, - bool xyIntersection) const + bool xyIntersection, + bool useInitialPose) const { int stride = reduceRenderedTrajectory < 1 ? 1 : reduceRenderedTrajectory; @@ -650,10 +676,13 @@ 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(); - Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); rlEnableShader(shader_.id); rlSetUniformMatrix(locMVP_, mvp); int colorModeFlat = 0; @@ -673,11 +702,16 @@ void ScanRenderer::drawTrajectories( continue; } + // 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. + const Eigen::Affine3d& shownPose = useInitialPose ? pc.m_initial_pose : pc.m_pose; + if (visibleImuDiff) { for (size_t i = 1; i < pc.local_trajectory.size(); ++i) { - Eigen::Affine3d m = pc.m_pose * pc.local_trajectory[i].m_pose; + Eigen::Affine3d m = shownPose * pc.local_trajectory[i].m_pose; Vector3 origin = toVec3(m.translation()); const auto& diff = pc.local_trajectory[i].imu_diff_angle_om_fi_ka_deg; DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(diff.x() * 10, 0, 0)), RED); @@ -698,6 +732,11 @@ void ScanRenderer::drawTrajectories( if (traj.vertexCount > 0) { + if (useInitialPose) + { + const Eigen::Affine3d delta = pc.m_initial_pose * pc.m_pose.inverse(); + rlSetUniformMatrix(locMVP_, MatrixMultiply(toRaylibMatrix(delta), mvp)); + } 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); float pointSize = static_cast(pc.line_width); @@ -705,31 +744,35 @@ void ScanRenderer::drawTrajectories( rlEnableVertexArray(traj.vao); glDrawArrays(GL_POINTS, 0, traj.vertexCount); rlDisableVertexArray(); + if (useInitialPose) + { + rlSetUniformMatrix(locMVP_, mvp); // restore for the next scan + } } } if (pc.fuse_inclination_from_IMU) { - drawSquareOutline(pc.m_pose, 0.2, GREEN); - drawSquareOutline(imuOrientationAtPose(pc), 0.2, RED); + drawSquareOutline(shownPose, 0.2, GREEN); + drawSquareOutline(imuOrientationAtPose(pc, shownPose), 0.2, RED); } if (pc.fixed_om && pc.fixed_fi) { for (double x = 0.4; x <= 1.0; x += 0.1) { - drawSquareOutline(pc.m_pose, x, RED); + drawSquareOutline(shownPose, x, RED); } } if (pc.show_IMU) { - drawOrientationCross(imuOrientationAtPose(pc)); + drawOrientationCross(imuOrientationAtPose(pc, shownPose)); } if (pc.show_pose) { - drawOrientationCross(pc.m_pose); + drawOrientationCross(shownPose); } }