Skip to content

Draw trajtectory as points #470

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 2 commits into from
Jul 31, 2026
+157 −31
Merged

Draw trajtectory as points #470

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2cb4502
Draw trajtetory as points
Jul 30, 2026
57f4fb1
fix for PFD, clang
Jul 31, 2026
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
14 changes: 7 additions & 7 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 @@ -3941,7 +3941,7 @@ void display()
if (ImGui::MenuItem("Load GNSS files and convert WGS84 to PUWG92"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3964,7 +3964,7 @@ void display()
if (ImGui::MenuItem("Load GNSS (deprecated)"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3981,7 +3981,7 @@ void display()
if (ImGui::MenuItem("Load GNSS"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -4001,7 +4001,7 @@ void display()
if (ImGui::MenuItem("Load NMEA (deprecated)"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", mandeye::fd::Nmea_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -4018,7 +4018,7 @@ void display()
if (ImGui::MenuItem("Load NMEA"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", mandeye::fd::Nmea_filter, true);

if (input_file_names.size() > 0)
{
Expand Down Expand Up @@ -4462,8 +4462,8 @@ void display()

if (tmp < 0)
tmp = 0;
else if (tmp > 5)
tmp = 5;
else if (tmp > 50)
tmp = 50;

if (tmp != session.point_clouds_container.point_clouds[0].line_width)
for (auto& point_cloud : session.point_clouds_container.point_clouds)
Expand Down
10 changes: 5 additions & 5 deletions apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3072,7 +3072,7 @@ void display()
if (ImGui::MenuItem("Load GNSS files and convert WGS84 to PUWG92"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3095,7 +3095,7 @@ void display()
if (ImGui::MenuItem("Load GNSS (deprecated)"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3112,7 +3112,7 @@ void display()
if (ImGui::MenuItem("Load GNSS"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", mandeye::fd::Gnss_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3132,7 +3132,7 @@ void display()
if (ImGui::MenuItem("Load NMEA (deprecated)"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", mandeye::fd::Nmea_filter, true);

if (input_file_names.size() > 0)
{
Expand All @@ -3149,7 +3149,7 @@ void display()
if (ImGui::MenuItem("Load NMEA"))
{
std::vector<std::string> input_file_names;
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true);
input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", mandeye::fd::Nmea_filter, true);

if (input_file_names.size() > 0)
{
Expand Down
3 changes: 3 additions & 0 deletions core/include/Core/pfd_wrapper.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@ namespace mandeye::fd
const std::vector<std::string> json_filter = { "Calibration file (*.json)", "*.json", "All files", "*" };
const std::vector<std::string> sn_filter = { "SN file (*.sn)", "*.sn", "All files", "*" };

const std::vector<std::string> Nmea_filter = { "NMEA file (*.nmea)", "*.nmea", "All files", "*" };
const std::vector<std::string> Gnss_filter = { "GNSS file (*.gnss)", "*.gnss", "All files", "*" };

std::string OpenFileDialogOneFile(const std::string& title, const std::vector<std::string>& filter);
std::vector<std::string> OpenFileDialog(const std::string& title, const std::vector<std::string>& filter, bool multiselect);
std::string SaveFileDialog(
Expand Down
44 changes: 36 additions & 8 deletions core/include/Core/raylib_render.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,14 +139,19 @@ class ScanRenderer
void clearMarks();

// Ported from PointCloud::render()'s trajectory section (core/src/point_cloud.cpp):
// draws each visible scan's local_trajectory (posed by m_pose) as a line
// strip in its traj_color (skipped if pc.line_width <= 0), decimated by
// reduceRenderedTrajectory (mirrors "viewer_reduce_rendered_trajectory"),
// plus the fuse-inclination-from-IMU quad markers, the fixed-om/fi rings,
// and the show_IMU/show_pose orientation crosses. If visibleImuDiff is
// set, also draws the IMU-vs-LIO angular-difference debug lines.
// Intersection-slab gating (xz/yz/xy) is not implemented so these always
// draw when the relevant per-scan flag is set.
// draws each visible scan's local_trajectory (posed by m_pose) as points
// (GL_POINTS, sized via pc.line_width -- skipped entirely if
// pc.line_width <= 0) in its traj_color, decimated by
// reduceRenderedTrajectory (mirrors "viewer_reduce_rendered_trajectory").
// Positions are cached per scan (see TrajGPU below) and only rebuilt
// when a scan's pose, trajectory point count or the stride changes --
// line_width only affects the draw call's point-size uniform, not the
// cached geometry, so changing it alone needs no rebuild.
// Also draws the fuse-inclination-from-IMU quad markers, the fixed-om/fi
// rings, and the show_IMU/show_pose orientation crosses. If
// visibleImuDiff is set, also draws the IMU-vs-LIO angular-difference
// debug lines. Intersection-slab gating (xz/yz/xy) is not implemented so
// these always draw when the relevant per-scan flag is set.
void drawTrajectories(const std::vector<PointCloud>& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const;

// Draws a single already-cached scan (see rebuild()) straight from its
Expand Down Expand Up @@ -179,6 +184,29 @@ class ScanRenderer

void unload(CloudGPU& cloud);

// GPU cache for one scan's trajectory point positions (drawTrajectories())
// -- a position-only VBO drawn with GL_POINTS, rebuilt only when stale.
struct TrajGPU
{
unsigned int vao = 0;
unsigned int vbo = 0;
int vertexCount = 0;
Eigen::Affine3d lastPose = Eigen::Affine3d::Identity();
bool hasPose = false;
int builtStride = -1;
size_t builtPointCount = 0;
};

// Both take the cache entry by reference rather than an index so they
// can be called from the const drawTrajectories() (lazy rebuild-if-stale
// on an otherwise-const draw path, same rationale as lastDrawCallCount_/
// lastVertexCount_ below being mutable) without needing non-const
// access to *this itself.
void unloadTraj(TrajGPU& traj) const;
void rebuildTrajectoryGPU(TrajGPU& traj, const PointCloud& pc, int stride) const;

mutable std::vector<TrajGPU> trajClouds_;

std::vector<CloudGPU> clouds_;
Shader shader_{};
bool shaderValid_ = false;
Expand Down
117 changes: 106 additions & 11 deletions core/src/raylib_render.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,12 @@ void ScanRenderer::shutdown()
}
clouds_.clear();

for (auto& t : trajClouds_)
{
unloadTraj(t);
}
trajClouds_.clear();

if (shaderValid_)
{
UnloadShader(shader_);
Expand Down Expand Up @@ -515,12 +521,90 @@ namespace
}
} // namespace

void ScanRenderer::unloadTraj(TrajGPU& traj) const
{
if (traj.vao)
{
rlUnloadVertexArray(traj.vao);
traj.vao = 0;
}
if (traj.vbo)
{
rlUnloadVertexBuffer(traj.vbo);
traj.vbo = 0;
}
traj.vertexCount = 0;
}

void ScanRenderer::rebuildTrajectoryGPU(TrajGPU& traj, const PointCloud& pc, int stride) const
{
unloadTraj(traj);

traj.lastPose = pc.m_pose;
traj.hasPose = true;
traj.builtStride = stride;
traj.builtPointCount = pc.local_trajectory.size();

if (pc.local_trajectory.empty())
{
return;
}

// Drawn as points (GL_POINTS, sized via gl_PointSize in the shared point
// shader) rather than a line/ribbon/cylinder between them -- simplest
// possible trajectory geometry, one vertex per decimated pose, no
// connecting geometry to regenerate or to zigzag with sensor jitter.
std::vector<float> data;
data.reserve((pc.local_trajectory.size() / stride + 1) * 3);
for (size_t i = 0; i < pc.local_trajectory.size(); i += stride)
{
Vector3 p = toVec3((pc.m_pose * pc.local_trajectory[i].m_pose).translation());
data.push_back(p.x);
data.push_back(p.y);
data.push_back(p.z);
}

if (data.empty())
{
return;
}

traj.vao = rlLoadVertexArray();
rlEnableVertexArray(traj.vao);
traj.vbo = rlLoadVertexBuffer(data.data(), static_cast<int>(data.size() * sizeof(float)), false);
rlSetVertexAttribute(0, 3, RL_FLOAT, false, 3 * sizeof(float), 0);
rlEnableVertexAttribute(0);
rlDisableVertexArray();

traj.vertexCount = static_cast<int>(data.size() / 3);
}

void ScanRenderer::drawTrajectories(const std::vector<PointCloud>& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const
{
int stride = reduceRenderedTrajectory < 1 ? 1 : reduceRenderedTrajectory;

for (const auto& pc : pointClouds)
if (trajClouds_.size() > pointClouds.size())
{
for (size_t i = pointClouds.size(); i < trajClouds_.size(); ++i)
{
unloadTraj(trajClouds_[i]);
}
}
trajClouds_.resize(pointClouds.size());

if (shaderValid_)
{
rlDrawRenderBatchActive();
Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection());
rlEnableShader(shader_.id);
rlSetUniformMatrix(locMVP_, mvp);
int colorModeFlat = 0;
rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1);
}

for (size_t idx = 0; idx < pointClouds.size(); ++idx)
{
const PointCloud& pc = pointClouds[idx];
if (!pc.visible || pc.local_trajectory.empty())
{
continue;
Expand All @@ -539,19 +623,25 @@ void ScanRenderer::drawTrajectories(const std::vector<PointCloud>& pointClouds,
}
}

if (pc.line_width > 0 && pc.local_trajectory.size() >= 2)
if (shaderValid_ && pc.line_width > 0)
{
Color c = Color{ static_cast<unsigned char>(pc.traj_color[0] * 255.f),
static_cast<unsigned char>(pc.traj_color[1] * 255.f),
static_cast<unsigned char>(pc.traj_color[2] * 255.f),
255 };
TrajGPU& traj = trajClouds_[idx];
bool stale = !traj.hasPose || !traj.lastPose.isApprox(pc.m_pose, 1e-9) || traj.builtStride != stride ||
traj.builtPointCount != pc.local_trajectory.size();
if (stale)
{
rebuildTrajectoryGPU(traj, pc, stride);
}

Vector3 prev = toVec3((pc.m_pose * pc.local_trajectory[0].m_pose).translation());
for (size_t i = stride; i < pc.local_trajectory.size(); i += stride)
if (traj.vertexCount > 0)
{
Vector3 cur = toVec3((pc.m_pose * pc.local_trajectory[i].m_pose).translation());
DrawLine3D(prev, cur, c);
prev = cur;
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();
}
}

Expand Down Expand Up @@ -579,6 +669,11 @@ void ScanRenderer::drawTrajectories(const std::vector<PointCloud>& pointClouds,
drawOrientationCross(pc.m_pose);
}
}

if (shaderValid_)
{
rlDisableShader();
}
}

// Mini compass + ruler: ported to gui.cpp's own drawMiniCompassWithRuler()
Expand Down
Loading