Skip to content

Honor "Show with initial pose" in step 2 raylib renderer #520

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JanuszBedkowski merged 1 commit into from
Sep 8, 2026
+72 −17
Merged

Honor "Show with initial pose" in step 2 raylib renderer #520

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>(session.point_clouds_container.intersection_width));
static_cast<float>(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);

Expand Down
14 changes: 12 additions & 2 deletions core/include/Core/raylib_render.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<PointCloud>& pointClouds,
float pointSize,
Expand All @@ -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
Expand Down Expand Up @@ -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<PointCloud>& 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
Expand Down
69 changes: 56 additions & 13 deletions core/src/raylib_render.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,14 @@ 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.
Matrix toRaylibMatrix(const Eigen::Affine3d& t);
} // namespace

void ScanRenderer::draw(
const std::vector<PointCloud>& pointClouds,
float pointSize,
Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -322,6 +342,11 @@ void ScanRenderer::draw(
rlDisableVertexArray();
}

if (useInitialPose)
{
rlSetUniformMatrix(locMVP_, mvp); // undo the last scan's per-scan fold
}

rlDisableShader();
}

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;

Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -698,38 +732,47 @@ 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<float>(pc.line_width);
rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1);
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);
}
}

Expand Down
Loading