diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 2ecf9ca8..558020c9 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -184,6 +184,8 @@ std::chrono::time_point loStartTime; std::atomic loElapsedSeconds{ 0.0 }; std::atomic loEstimatedTimeRemaining{ 0.0 }; +fs::path outwd; + #if _WIN32 #define DEFAULT_PATH "C:\\" #else @@ -670,7 +672,7 @@ void step2(const std::atomic& loPause) void save_results(bool info, double elapsed_seconds) { - fs::path outwd = get_next_result_path(working_directory); + outwd = get_next_result_path(working_directory); save_result(worker_data, params, outwd, elapsed_seconds); if (info) { @@ -883,9 +885,9 @@ void settings_gui() ImGui::NewLine(); - ImGui::InputDouble("Decimation", ¶ms.decimation, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Downsampling", ¶ms.decimation, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Larger value of decimation better performance, but worse accuracy"); + ImGui::SetTooltip("Larger value of downsampling better performance, but worse accuracy"); ImGui::InputDouble("Max distance of processed points [m]", ¶ms.max_distance_lidar, 0.0, 0.0, "%.2f"); if (ImGui::IsItemHovered()) @@ -1100,7 +1102,7 @@ void settings_gui() ImGui::SameLine(); ImGui::Checkbox("Show buckets", &show_reference_buckets); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Decimation###ref", &dec_reference_points); + ImGui::InputInt("Downsamplingn###ref", &dec_reference_points); if (ImGui::Button("Filter reference buckets")) { @@ -1564,9 +1566,9 @@ void openData() loRunning = false; std::ostringstream oss; - oss << "Data saved to folder:\n'" << working_directory << "\\lio_result_*'\n" - << "Calculated trajectory length: " << std::fixed << std::setprecision(1) - << params.total_length_of_calculated_trajectory << "[m]\n" + oss << "Data saved to folder:\n'" << outwd.string() << "'\n" + << "Calculated trajectory length: " + << std::fixed << std::setprecision(1) << params.total_length_of_calculated_trajectory << "[m]\n" << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str(); [[maybe_unused]] pfd::message message("Information", oss.str(), pfd::choice::ok, pfd::icon::info); diff --git a/apps/lidar_odometry_step_1/resource.rc b/apps/lidar_odometry_step_1/resource.rc index 6b4eb256..b18b318f 100644 --- a/apps/lidar_odometry_step_1/resource.rc +++ b/apps/lidar_odometry_step_1/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 5 -PRODUCTVERSION 0, 0, 9, 5 +FILEVERSION 0, 0, 9, 6 +PRODUCTVERSION 0, 0, 9, 6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 1\0" -VALUE "FileVersion", "0.9.5\0" +VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Odometry\0" -VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "lidar_odometry_step_1.exe\0" -VALUE "ProductVersion", "0.9.5\0" +VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/mandeye_mission_recorder_calibration/resource.rc b/apps/mandeye_mission_recorder_calibration/resource.rc index 6f2f975e..0ed87c81 100644 --- a/apps/mandeye_mission_recorder_calibration/resource.rc +++ b/apps/mandeye_mission_recorder_calibration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,5 - PRODUCTVERSION 0,0,9,5 + FILEVERSION 0,0,9,6 + PRODUCTVERSION 0,0,9,6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping MR Calibration\0" - VALUE "FileVersion", "0.9.5\0" + VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Mission Recorder Calibration\0" - VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" + VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_mission_recorder_calibration.exe\0" - VALUE "ProductVersion", "0.9.5\0" + VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/mandeye_raw_data_viewer/resource.rc b/apps/mandeye_raw_data_viewer/resource.rc index d8c77965..8ecd2dbf 100644 --- a/apps/mandeye_raw_data_viewer/resource.rc +++ b/apps/mandeye_raw_data_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,5 - PRODUCTVERSION 0,0,9,5 + FILEVERSION 0,0,9,6 + PRODUCTVERSION 0,0,9,6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Raw Data Viewer\0" - VALUE "FileVersion", "0.9.5\0" + VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Raw data viewer\0" - VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" + VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_raw_data_viewer.exe\0" - VALUE "ProductVersion", "0.9.5\0" + VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp index 2d803351..1d7c2ad3 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -1,4 +1,4 @@ -// clang-format off +// clang-format off #include #include // clang-format on @@ -9,7 +9,7 @@ #include #include - + #include #include @@ -29,9 +29,9 @@ #include #ifdef _WIN32 -#include -#include // <-- Required for ShellExecuteA -#include "resource.h" + #include + #include // <-- Required for ShellExecuteA + #include "resource.h" #endif #include @@ -39,92 +39,96 @@ std::string winTitle = std::string("Single session viewer ") + HDMAPPING_VERSION_STRING; -std::vector infoLines = { "This program is optional step in MANDEYE process", - "", - "It analyzes session created in step_1 for problems that need to be addressed further", - "Next step will be to load session file with 'multi_view_tls_registration_step_2' app" }; - -// App specific shortcuts (Type and Shortcut are just for easy reference) -static const std::vector appShortcuts = { { "Normal keys", "A", "" }, - { "", "Ctrl+A", "" }, - { "", "B", "" }, - { "", "Ctrl+B", "" }, - { "", "C", "" }, - { "", "Ctrl+C", "" }, - { "", "D", "" }, - { "", "Ctrl+D", "" }, - { "", "E", "" }, - { "", "Ctrl+E", "" }, - { "", "F", "" }, - { "", "Ctrl+F", "" }, - { "", "G", "" }, - { "", "Ctrl+G", "" }, - { "", "H", "" }, - { "", "Ctrl+H", "" }, - { "", "I", "" }, - { "", "Ctrl+I", "" }, - { "", "J", "" }, - { "", "Ctrl+K", "" }, - { "", "K", "" }, - { "", "Ctrl+K", "" }, - { "", "L", "" }, - { "", "Ctrl+L", "" }, - { "", "M", "" }, - { "", "Ctrl+M", "" }, - { "", "N", "" }, - { "", "Ctrl+N", "show Neightbouring scans" }, - { "", "O", "" }, - { "", "Ctrl+O", "Open session" }, - { "", "P", "" }, - { "", "Ctrl+P", "Properties" }, - { "", "Q", "" }, - { "", "Ctrl+Q", "" }, - { "", "R", "" }, - { "", "Ctrl+R", "" }, - { "", "Shift+R", "" }, - { "", "S", "" }, - { "", "Ctrl+S", "" }, - { "", "Ctrl+Shift+S", "" }, - { "", "T", "" }, - { "", "Ctrl+T", "" }, - { "", "U", "" }, - { "", "Ctrl+U", "" }, - { "", "V", "" }, - { "", "Ctrl+V", "" }, - { "", "W", "" }, - { "", "Ctrl+W", "" }, - { "", "X", "" }, - { "", "Ctrl+X", "" }, - { "", "Y", "" }, - { "", "Ctrl+Y", "" }, - { "", "Z", "" }, - { "", "Ctrl+Z", "" }, - { "", "Shift+Z", "" }, - { "", "1-9", "" }, - { "Special keys", "Up arrow", "Intensity offset +" }, - { "", "Shift + up arrow", "" }, - { "", "Ctrl + up arrow", "" }, - { "", "Down arrow", "Intensity offset -" }, - { "", "Shift + down arrow", "" }, - { "", "Ctrl + down arrow", "" }, - { "", "Left arrow", "Point cloud index -" }, - { "", "Shift + left arrow", "" }, - { "", "Ctrl + left arrow", "" }, - { "", "Right arrow", "Point cloud index +" }, - { "", "Shift + right arrow", "" }, - { "", "Ctrl + right arrow", "" }, - { "", "Pg down", "Point cloud index -" }, - { "", "Pg up", "Point cloud index +" }, - { "", "- key", "Point cloud index -" }, - { "", "+ key", "Point cloud index +" }, - { "Mouse related", "Left click + drag", "" }, - { "", "Right click + drag", "n" }, - { "", "Scroll", "" }, - { "", "Shift + scroll", "" }, - { "", "Shift + drag", "" }, - { "", "Ctrl + left click", "" }, - { "", "Ctrl + right click", "" }, - { "", "Ctrl + middle click", "" } }; +std::vector infoLines = { + "This program is optional step in MANDEYE process", + "", + "It analyzes session created in step_1 for problems that need to be addressed further", + "Next step will be to load session file with 'multi_view_tls_registration_step_2' app" +}; + +//App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { + {"Normal keys", "A", ""}, + {"", "Ctrl+A", ""}, + {"", "B", ""}, + {"", "Ctrl+B", ""}, + {"", "C", ""}, + {"", "Ctrl+C", ""}, + {"", "D", ""}, + {"", "Ctrl+D", ""}, + {"", "E", ""}, + {"", "Ctrl+E", ""}, + {"", "F", ""}, + {"", "Ctrl+F", ""}, + {"", "G", ""}, + {"", "Ctrl+G", ""}, + {"", "H", ""}, + {"", "Ctrl+H", ""}, + {"", "I", ""}, + {"", "Ctrl+I", ""}, + {"", "J", ""}, + {"", "Ctrl+K", ""}, + {"", "K", ""}, + {"", "Ctrl+K", ""}, + {"", "L", ""}, + {"", "Ctrl+L", ""}, + {"", "M", ""}, + {"", "Ctrl+M", ""}, + {"", "N", ""}, + {"", "Ctrl+N", "show Neightbouring scans"}, + {"", "O", ""}, + {"", "Ctrl+O", "Open session"}, + {"", "P", ""}, + {"", "Ctrl+P", "Properties"}, + {"", "Q", ""}, + {"", "Ctrl+Q", ""}, + {"", "R", ""}, + {"", "Ctrl+R", ""}, + {"", "Shift+R", ""}, + {"", "S", ""}, + {"", "Ctrl+S", ""}, + {"", "Ctrl+Shift+S", ""}, + {"", "T", ""}, + {"", "Ctrl+T", ""}, + {"", "U", ""}, + {"", "Ctrl+U", ""}, + {"", "V", ""}, + {"", "Ctrl+V", ""}, + {"", "W", ""}, + {"", "Ctrl+W", ""}, + {"", "X", ""}, + {"", "Ctrl+X", ""}, + {"", "Y", ""}, + {"", "Ctrl+Y", ""}, + {"", "Z", ""}, + {"", "Ctrl+Z", ""}, + {"", "Shift+Z", ""}, + {"", "1-9", ""}, + {"Special keys", "Up arrow", "Intensity offset +"}, + {"", "Shift + up arrow", ""}, + {"", "Ctrl + up arrow", ""}, + {"", "Down arrow", "Intensity offset -"}, + {"", "Shift + down arrow", ""}, + {"", "Ctrl + down arrow", ""}, + {"", "Left arrow", "Point cloud index -"}, + {"", "Shift + left arrow", ""}, + {"", "Ctrl + left arrow", ""}, + {"", "Right arrow", "Point cloud index +"}, + {"", "Shift + right arrow", ""}, + {"", "Ctrl + right arrow", ""}, + {"", "Pg down", "Point cloud index -"}, + {"", "Pg up", "Point cloud index +"}, + {"", "- key", "Point cloud index -"}, + {"", "+ key", "Point cloud index +"}, + {"Mouse related", "Left click + drag", ""}, + {"", "Right click + drag", "n"}, + {"", "Scroll", ""}, + {"", "Shift + scroll", ""}, + {"", "Shift + drag", ""}, + {"", "Ctrl + left click", ""}, + {"", "Ctrl + right click", ""}, + {"", "Ctrl + middle click", ""} +}; #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; @@ -132,16 +136,22 @@ namespace fs = std::filesystem; ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.0f); -float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_ortho_projection[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; -float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_ortho_gizmo_view[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; int index_rendered_points_local = -1; float offset_intensity = 0.0; bool show_neighbouring_scans = false; -int colorScheme = 0; // 0=solid color; gradients:1=intensity, 2=cloud height -int oldcolorScheme = 0; +ColorScheme colorScheme = CS_SOLID; +ColorScheme oldcolorScheme = CS_SOLID; bool usePose = false; bool is_properties_gui = false; @@ -154,7 +164,7 @@ bool session_loaded = false; int session_total_number_of_points = 0; PointClouds::PointCloudDimensions session_dims; -// built in console output redirection to imgui window +//built in console output redirection to imgui window /////////////////////////////////////////////////////////////////////////////////// #ifdef _WIN32 @@ -163,14 +173,10 @@ bool consWin = true; bool consImGui = false; bool consHooked = false; -class DualStreamBuf : public std::streambuf -{ +class DualStreamBuf : public std::streambuf { public: DualStreamBuf(std::streambuf* sb1, std::string* imguiBuffer, bool isCerr) - : consoleBuf(sb1) - , imguiBuffer(imguiBuffer) - , isCerr(isCerr) - { + : consoleBuf(sb1), imguiBuffer(imguiBuffer), isCerr(isCerr) { } protected: @@ -179,7 +185,7 @@ class DualStreamBuf : public std::streambuf int overflow(int c) override { - // handle EOF + //handle EOF if (c == traits_type::eof()) return traits_type::not_eof(c); @@ -221,7 +227,11 @@ class DualStreamBuf : public std::streambuf std::string* imguiBuffer; }; -static std::string g_ImGuiLog; // store text for ImGui + + + + +static std::string g_ImGuiLog; // store text for ImGui static DualStreamBuf* g_coutBuf = nullptr; static DualStreamBuf* g_cerrBuf = nullptr; static std::streambuf* g_origCoutBuf = nullptr; @@ -229,24 +239,22 @@ static std::streambuf* g_origCerrBuf = nullptr; void ConsoleHook() { - g_origCoutBuf = std::cout.rdbuf(); // save original buffer - g_origCerrBuf = std::cerr.rdbuf(); // save original buffer + g_origCoutBuf = std::cout.rdbuf(); // save original buffer + g_origCerrBuf = std::cerr.rdbuf(); // save original buffer g_coutBuf = new DualStreamBuf(g_origCoutBuf, &g_ImGuiLog, false); g_cerrBuf = new DualStreamBuf(g_origCerrBuf, &g_ImGuiLog, true); - std::cout.rdbuf(g_coutBuf); // replace - std::cerr.rdbuf(g_cerrBuf); // replace - - consHooked = true; + std::cout.rdbuf(g_coutBuf); // replace + std::cerr.rdbuf(g_cerrBuf); // replace + + consHooked = true; } void ConsoleUnhook() { - if (g_origCoutBuf) - std::cout.rdbuf(g_origCoutBuf); // restore original buffer - if (g_origCerrBuf) - std::cerr.rdbuf(g_origCerrBuf); // restore original buffer - delete g_coutBuf; // delete custom buffers - delete g_cerrBuf; // delete custom buffers + if (g_origCoutBuf) std::cout.rdbuf(g_origCoutBuf); //restore original buffer + if (g_origCerrBuf) std::cerr.rdbuf(g_origCerrBuf); //restore original buffer + delete g_coutBuf; //delete custom buffers + delete g_cerrBuf; //delete custom buffers g_coutBuf = g_cerrBuf = nullptr; g_ImGuiLog.clear(); @@ -259,11 +267,11 @@ void ImGuiConsole(bool* p_open) { if (ImGui::Begin("Console", p_open)) { - if (ImGui::Button("Clear")) - g_ImGuiLog.clear(); - // ImGui::SameLine(); - // if (ImGui::Button("Copy")) - // ImGui::LogToClipboard(); + if (ImGui::Button("Clear")) + g_ImGuiLog.clear(); + //ImGui::SameLine(); + //if (ImGui::Button("Copy")) + // ImGui::LogToClipboard(); ImGui::Separator(); @@ -281,14 +289,14 @@ void ImGuiConsole(bool* p_open) ImGui::PushStyleColor(ImGuiCol_FrameBgActive, ImVec4(0, 0, 0, 0)); ImGui::InputTextMultiline( "##console", - g_ImGuiLogBuf, - sizeof(g_ImGuiLogBuf), + g_ImGuiLogBuf, sizeof(g_ImGuiLogBuf), ImVec2(-1.0f, -1.0f), - ImGuiInputTextFlags_ReadOnly | ImGuiInputTextFlags_AllowTabInput); + ImGuiInputTextFlags_ReadOnly | ImGuiInputTextFlags_AllowTabInput + ); ImGui::PopStyleColor(3); // auto-scroll - // if (ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) + //if (ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) // ImGui::SetScrollHereY(1.0f); } ImGui::EndChild(); @@ -297,15 +305,17 @@ void ImGuiConsole(bool* p_open) ImGui::End(); } -// VBO/VAO/SSBO proof of concept implementation through glew. openGL 4.6 required + + +//VBO/VAO/SSBO proof of concept implementation through glew. openGL 4.6 required /////////////////////////////////////////////////////////////////////////////////// // -// in order to test have to be enabled from View menu before loading session so buffers are created -// should be tested with smaller sessions since session is "doubled" with gl_Points vector -bool gl_useVBOs = false; + //in order to test have to be enabled from View menu before loading session so buffers are created + //should be tested with smaller sessions since session is "doubled" with gl_Points vector + bool gl_useVBOs = false; -// vertex shader -const char* pointsVertSource = R"glsl( + // vertex shader + const char* pointsVertSource = R"glsl( #version 460 core //SSBOs @@ -314,7 +324,7 @@ const char* pointsVertSource = R"glsl( }; struct Cloud { //order matters for std430 layout - int colorScheme; // 0=solid,1=intensity gradient, etc. + int colorScheme; // 0=solid,1=random,2=intensity gradient, etc. vec3 fixedColor; // fixed color mat4 pose; // cloud transformation }; @@ -352,8 +362,8 @@ const char* pointsVertSource = R"glsl( } )glsl"; -// fragment shader -const char* pointsFragSource = R"glsl( + // fragment shader + const char* pointsFragSource = R"glsl( #version 460 core in float vIntensity; @@ -370,7 +380,7 @@ const char* pointsFragSource = R"glsl( if (vColorScheme == 0) endColor = vFixedColor; // solid color - else if (vColorScheme == 1) + else if (vColorScheme == 2) endColor = vec3(norm, 0.0, 1.0 - norm); // blue–red gradient by intensity else endColor = vec3(0.5); // default gray @@ -379,293 +389,311 @@ const char* pointsFragSource = R"glsl( } )glsl"; -/////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////// -struct gl_point -{ - Eigen::Vector3f pos; - float intensity; -}; - -std::vector gl_Points; -/*= { - {{0.0f, 0.0f, 1.0f}, 1.0f}, - {{0.5f, 0.0f, 1.0f}, 0.7f}, - {{0.0f, 0.5f, 1.0f}, 0.5f}, - {{0.0f, 0.0f, 2.0f}, 0.2f}, -};*/ + struct gl_point { + Eigen::Vector3f pos; + float intensity; + }; -struct gl_cloud -{ - GLint offset; - GLsizei count; - bool visible; // show/hide -}; + std::vector gl_Points; + /*= { + {{0.0f, 0.0f, 1.0f}, 1.0f}, + {{0.5f, 0.0f, 1.0f}, 0.7f}, + {{0.0f, 0.5f, 1.0f}, 0.5f}, + {{0.0f, 0.0f, 2.0f}, 0.2f}, + };*/ + + struct gl_cloud { + GLint offset; + GLsizei count; + bool visible; // show/hide + }; -// Add to clouds vector -std::vector gl_clouds; -/*= { { - 0, //offset - static_cast(gl_Points.size()), //count - true, //visible - Eigen::Matrix4f::Identity(), //pose - 1, //colorScheme - Eigen::Vector3f(1.0f, 0.0f, 0.0f) //color = red, for example -} };*/ - -// CPU-side vectors for SSBO data -struct gl_cloudSSBO -{ - int colorScheme; // 4 bytes - float padding0[3]; // pad to vec4 alignment (std430 requires vec3 as 16 bytes) - float fixedColor[3]; // 12 bytes for vec3 - float padding1; // pad to 16 bytes - float pose[16]; // mat4 = 16 floats (16*4 = 64 bytes) -}; + // Add to clouds vector + std::vector gl_clouds; + /*= { { + 0, //offset + static_cast(gl_Points.size()), //count + true, //visible + Eigen::Matrix4f::Identity(), //pose + 1, //colorScheme + Eigen::Vector3f(1.0f, 0.0f, 0.0f) //color = red, for example + } };*/ + + // CPU-side vectors for SSBO data + struct gl_cloudSSBO { + int colorScheme; // 4 bytes + float padding0[3]; // pad to vec4 alignment (std430 requires vec3 as 16 bytes) + float fixedColor[3]; // 12 bytes for vec3 + float padding1; // pad to 16 bytes + float pose[16]; // mat4 = 16 floats (16*4 = 64 bytes) + }; -std::vector gl_cloudIndexSSBO; // size = gl_Points.size() -std::vector gl_cloudsSSBO; // Cloud is your struct from GLSL + std::vector gl_cloudIndexSSBO; // size = gl_Points.size() + std::vector gl_cloudsSSBO; // Cloud is your struct from GLSL -Eigen::Matrix4f gl_mvp; // Model-View-Projection matrix + Eigen::Matrix4f gl_mvp; //Model-View-Projection matrix -// Uniform locations -GLuint VAO, VBO = 0; + // Uniform locations + GLuint VAO, VBO = 0; -GLuint gl_uPointSize = 0; -GLuint gl_uMVP = 0; -GLuint gl_shaderProgram = 0; -GLuint gl_uIntensityScale = 0; -GLuint gl_uUsePose = 0; + GLuint gl_uPointSize = 0; + GLuint gl_uMVP = 0; + GLuint gl_shaderProgram = 0; + GLuint gl_uIntensityScale = 0; + GLuint gl_uUsePose = 0; -GLuint gl_ssboCloudIndex, gl_ssboClouds = 0; + GLuint gl_ssboCloudIndex, gl_ssboClouds = 0; -// gl_*** functions related to glew/openGL VBO/VAO -GLuint gl_compileShader(GLenum type, const char* source) -{ - GLuint shader = GL_CALL_RET(glCreateShader(type)); - GL_CALL(glShaderSource(shader, 1, &source, nullptr)); - GL_CALL(glCompileShader(shader)); - - // check compilation - GLint success; - GL_CALL(glGetShaderiv(shader, GL_COMPILE_STATUS, &success)); - if (!success) + //gl_*** functions related to glew/openGL VBO/VAO + GLuint gl_compileShader(GLenum type, const char* source) { - char infoLog[512]; - GL_CALL(glGetShaderInfoLog(shader, 512, nullptr, infoLog)); - std::cerr << "openGL shader compilation failed: " << infoLog << std::endl; + GLuint shader = GL_CALL_RET(glCreateShader(type)); + GL_CALL(glShaderSource(shader, 1, &source, nullptr)); + GL_CALL(glCompileShader(shader)); + + // check compilation + GLint success; + GL_CALL(glGetShaderiv(shader, GL_COMPILE_STATUS, &success)); + if (!success) + { + char infoLog[512]; + GL_CALL(glGetShaderInfoLog(shader, 512, nullptr, infoLog)); + std::cerr << "openGL shader compilation failed: " << infoLog << std::endl; + } + return shader; } - return shader; -} -GLuint gl_createShaderProgram(const char* vertSource, const char* fragSource) -{ - GLuint vertexShader = gl_compileShader(GL_VERTEX_SHADER, vertSource); - GLuint fragmentShader = gl_compileShader(GL_FRAGMENT_SHADER, fragSource); - - GLuint program = GL_CALL_RET(glCreateProgram()); - GL_CALL(glAttachShader(program, vertexShader)); - GL_CALL(glAttachShader(program, fragmentShader)); - GL_CALL(glLinkProgram(program)); - - // check linking - GLint success; - GL_CALL(glGetProgramiv(program, GL_LINK_STATUS, &success)); - if (!success) + GLuint gl_createShaderProgram(const char* vertSource, const char* fragSource) { - char infoLog[512]; - GL_CALL(glGetProgramInfoLog(program, 512, nullptr, infoLog)); - std::cerr << "openGL program linking failed: " << infoLog << std::endl; - } + GLuint vertexShader = gl_compileShader(GL_VERTEX_SHADER, vertSource); + GLuint fragmentShader = gl_compileShader(GL_FRAGMENT_SHADER, fragSource); + + GLuint program = GL_CALL_RET(glCreateProgram()); + GL_CALL(glAttachShader(program, vertexShader)); + GL_CALL(glAttachShader(program, fragmentShader)); + GL_CALL(glLinkProgram(program)); + + // check linking + GLint success; + GL_CALL(glGetProgramiv(program, GL_LINK_STATUS, &success)); + if (!success) + { + char infoLog[512]; + GL_CALL(glGetProgramInfoLog(program, 512, nullptr, infoLog)); + std::cerr << "openGL program linking failed: " << infoLog << std::endl; + } - GL_CALL(glDeleteShader(vertexShader)); - GL_CALL(glDeleteShader(fragmentShader)); + GL_CALL(glDeleteShader(vertexShader)); + GL_CALL(glDeleteShader(fragmentShader)); - return program; -} + return program; + } -void gl_loadPointCloudBuffer(const std::vector& points, GLuint& VAO, GLuint& VBO) -{ - // Create VAO + VBO - GL_CALL(glGenVertexArrays(1, &VAO)); - GL_CALL(glBindVertexArray(VAO)); - - GL_CALL(glGenBuffers(1, &VBO)); - GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, VBO)); - GL_CALL(glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(gl_point), points.data(), GL_STATIC_DRAW)); - - // --- Attribute 0: position --- - // layout(location = 0) in vec3 aPos; - GL_CALL(glEnableVertexAttribArray(0)); - GL_CALL(glVertexAttribPointer( - 0, // attribute index - 3, // vec3 - GL_FLOAT, - GL_FALSE, - sizeof(gl_point), // stride = full struct - (void*)offsetof(gl_point, pos) // offset of position field + void gl_loadPointCloudBuffer(const std::vector& points, GLuint& VAO, GLuint& VBO) + { + // Create VAO + VBO + GL_CALL(glGenVertexArrays(1, &VAO)); + GL_CALL(glBindVertexArray(VAO)); + + GL_CALL(glGenBuffers(1, &VBO)); + GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, VBO)); + GL_CALL(glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(gl_point), points.data(), GL_STATIC_DRAW)); + + // --- Attribute 0: position --- + // layout(location = 0) in vec3 aPos; + GL_CALL(glEnableVertexAttribArray(0)); + GL_CALL(glVertexAttribPointer( + 0, // attribute index + 3, // vec3 + GL_FLOAT, + GL_FALSE, + sizeof(gl_point), // stride = full struct + (void*)offsetof(gl_point, pos) // offset of position field )); - // --- Attribute 1: intensity --- - // layout(location = 1) in float aIntensity; - GL_CALL(glEnableVertexAttribArray(1)); - GL_CALL(glVertexAttribPointer(1, 1, GL_FLOAT, GL_FALSE, sizeof(gl_point), (void*)offsetof(gl_point, intensity))); - - // Unbind - GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, 0)); - GL_CALL(glBindVertexArray(0)); -} + // --- Attribute 1: intensity --- + // layout(location = 1) in float aIntensity; + GL_CALL(glEnableVertexAttribArray(1)); + GL_CALL(glVertexAttribPointer( + 1, + 1, + GL_FLOAT, + GL_FALSE, + sizeof(gl_point), + (void*)offsetof(gl_point, intensity) + )); -void gl_updateSSBOs() -{ - // Vertex -> Cloud index - if (gl_ssboCloudIndex == 0) - { - GL_CALL(glGenBuffers(1, &gl_ssboCloudIndex)); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); - GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, gl_cloudIndexSSBO.size() * sizeof(int), gl_cloudIndexSSBO.data(), GL_DYNAMIC_DRAW)); - } - else - { - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); - GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, gl_cloudIndexSSBO.size() * sizeof(int), gl_cloudIndexSSBO.data())); + // Unbind + GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, 0)); + GL_CALL(glBindVertexArray(0)); } - GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, gl_ssboCloudIndex)); + void gl_updateSSBOs() { + // Vertex -> Cloud index + if (gl_ssboCloudIndex == 0) + { + GL_CALL(glGenBuffers(1, &gl_ssboCloudIndex)); + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); + GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, + gl_cloudIndexSSBO.size() * sizeof(int), + gl_cloudIndexSSBO.data(), + GL_DYNAMIC_DRAW)); + } + else + { + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); + GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, + 0, + gl_cloudIndexSSBO.size() * sizeof(int), + gl_cloudIndexSSBO.data())); + } - // Cloud attributes - if (gl_ssboClouds == 0) - { - GL_CALL(glGenBuffers(1, &gl_ssboClouds)); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); - GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), gl_cloudsSSBO.data(), GL_DYNAMIC_DRAW)); + GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, gl_ssboCloudIndex)); + + // Cloud attributes + if (gl_ssboClouds == 0) + { + GL_CALL(glGenBuffers(1, &gl_ssboClouds)); + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); + GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, + gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), + gl_cloudsSSBO.data(), + GL_DYNAMIC_DRAW)); + } + else + { + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); + GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, + 0, + gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), + gl_cloudsSSBO.data())); + } + + GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 1, gl_ssboClouds)); + + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, 0)); } - else + + void gl_updateUserView() { - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); - GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), gl_cloudsSSBO.data())); + ImGuiIO& io = ImGui::GetIO(); + + Eigen::Affine3f viewLocal1 = Eigen::Affine3f::Identity(); + viewLocal1.translate(rotation_center); + + viewLocal1.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + viewLocal1.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal1.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + viewLocal1.translate(-rotation_center); + + //Get the projection matrix from your reshape() or manually rebuild it + float aspect = float(io.DisplaySize.x) / float(io.DisplaySize.y); + float fov = 60.0f * DEG_TO_RAD; + float nearf = 0.001f; //closest distance to camera objects are rendered [m] + float farf = 1000.0f; //furthest distance to camera objects are rendered [m] + + Eigen::Matrix4f projection = Eigen::Matrix4f::Zero(); + float f = 1.0f / tan(fov / 2.0f); + projection(0, 0) = f / aspect; + projection(1, 1) = f; + projection(2, 2) = (farf + nearf) / (nearf - farf); + projection(2, 3) = (2 * farf * nearf) / (nearf - farf); + projection(3, 2) = -1.0f; + + //Combine into a single MVP (model-view-projection) + gl_mvp = projection * viewLocal1.matrix(); } - GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 1, gl_ssboClouds)); + void gl_renderPointCloud() + { + GL_CALL(glUseProgram(gl_shaderProgram)); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, 0)); -} + //Set point size from GUI + GL_CALL(glUniform1f(gl_uPointSize, static_cast(point_size))); + GL_CALL(glUniform1f(gl_uIntensityScale, offset_intensity)); + GL_CALL(glUniform1i(gl_uUsePose, usePose)); + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); -void gl_updateUserView() -{ - ImGuiIO& io = ImGui::GetIO(); + if (oldcolorScheme != colorScheme) + { + if (colorScheme == CS_SOLID) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + { + gl_cloudsSSBO[i].colorScheme = CS_SOLID; + gl_cloudsSSBO[i].fixedColor[0] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; + gl_cloudsSSBO[i].fixedColor[1] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; + gl_cloudsSSBO[i].fixedColor[2] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; + } - Eigen::Affine3f viewLocal1 = Eigen::Affine3f::Identity(); - viewLocal1.translate(rotation_center); - - viewLocal1.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - viewLocal1.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal1.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - viewLocal1.translate(-rotation_center); - - // Get the projection matrix from your reshape() or manually rebuild it - float aspect = float(io.DisplaySize.x) / float(io.DisplaySize.y); - float fov = 60.0f * DEG_TO_RAD; - float nearf = 0.001f; // closest distance to camera objects are rendered [m] - float farf = 1000.0f; // furthest distance to camera objects are rendered [m] - - Eigen::Matrix4f projection = Eigen::Matrix4f::Zero(); - float f = 1.0f / tan(fov / 2.0f); - projection(0, 0) = f / aspect; - projection(1, 1) = f; - projection(2, 2) = (farf + nearf) / (nearf - farf); - projection(2, 3) = (2 * farf * nearf) / (nearf - farf); - projection(3, 2) = -1.0f; - - // Combine into a single MVP (model-view-projection) - gl_mvp = projection * viewLocal1.matrix(); -} + else if (colorScheme == CS_GRAD_INTENS) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + gl_cloudsSSBO[i].colorScheme = 1; -void gl_renderPointCloud() -{ - GL_CALL(glUseProgram(gl_shaderProgram)); + else if (colorScheme == CS_RANDOM) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + { + gl_cloudsSSBO[i].colorScheme = CS_SOLID; + gl_cloudsSSBO[i].fixedColor[0] = float(rand() % 255) / 255.0f; + gl_cloudsSSBO[i].fixedColor[1] = float(rand() % 255) / 255.0f; + gl_cloudsSSBO[i].fixedColor[2] = float(rand() % 255) / 255.0f; + } - // Set point size from GUI - GL_CALL(glUniform1f(gl_uPointSize, static_cast(point_size))); - GL_CALL(glUniform1f(gl_uIntensityScale, offset_intensity)); - GL_CALL(glUniform1i(gl_uUsePose, usePose)); - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); + oldcolorScheme = colorScheme; - if (oldcolorScheme != colorScheme) - { - if (colorScheme == 0) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) - { - gl_cloudsSSBO[i].colorScheme = 0; - gl_cloudsSSBO[i].fixedColor[0] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - gl_cloudsSSBO[i].fixedColor[1] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - gl_cloudsSSBO[i].fixedColor[2] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - } + gl_updateSSBOs(); + } + + GL_CALL(glBindVertexArray(VAO)); + /*for (size_t i = 0; i < gl_clouds.size(); i++) + { + if (!gl_clouds[i].visible) continue; - else if (colorScheme == 1) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) - gl_cloudsSSBO[i].colorScheme = 1; + // Send per-cloud uniforms - else if (colorScheme == 2) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + if (usePose) { - gl_cloudsSSBO[i].colorScheme = 0; - gl_cloudsSSBO[i].fixedColor[0] = float(rand() % 255) / 255.0f; - gl_cloudsSSBO[i].fixedColor[1] = float(rand() % 255) / 255.0f; - gl_cloudsSSBO[i].fixedColor[2] = float(rand() % 255) / 255.0f; + Eigen::Matrix4f cloudMVP = gl_mvp * gl_cloudsSSBO[i].pose; + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, cloudMVP.data())); } + else + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); + + // Draw only this cloud’s range + //GL_CALL(glDrawArrays(GL_POINTS, gl_clouds[i].offset, gl_clouds[i].count)); + }*/ - oldcolorScheme = colorScheme; + GL_CALL(glDrawArrays(GL_POINTS, 0, gl_cloudIndexSSBO.size())); - gl_updateSSBOs(); + GL_CALL(glBindVertexArray(0)); + GL_CALL(glUseProgram(0)); // back to fixed-function for legacy code } - GL_CALL(glBindVertexArray(VAO)); - /*for (size_t i = 0; i < gl_clouds.size(); i++) + void gl_init() { - if (!gl_clouds[i].visible) continue; - - // Send per-cloud uniforms - - if (usePose) + GLenum err = glewInit(); + if (err != GLEW_OK) { - Eigen::Matrix4f cloudMVP = gl_mvp * gl_cloudsSSBO[i].pose; - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, cloudMVP.data())); + std::cerr << "GLEW init failed: " << glewGetErrorString(err) << std::endl; + return; } - else - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); - // Draw only this cloud’s range - //GL_CALL(glDrawArrays(GL_POINTS, gl_clouds[i].offset, gl_clouds[i].count)); - }*/ + //Compile shaders and create shader program + gl_shaderProgram = gl_createShaderProgram(pointsVertSource, pointsFragSource); - GL_CALL(glDrawArrays(GL_POINTS, 0, gl_cloudIndexSSBO.size())); - - GL_CALL(glBindVertexArray(0)); - GL_CALL(glUseProgram(0)); // back to fixed-function for legacy code -} + //Get pointers + gl_uMVP = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uMVP"))); + gl_uPointSize = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uPointSize"))); + gl_uIntensityScale = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uIntensityScale"))); + gl_uUsePose = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uUsePose"))); -void gl_init() -{ - GLenum err = glewInit(); - if (err != GLEW_OK) - { - std::cerr << "GLEW init failed: " << glewGetErrorString(err) << std::endl; - return; + GL_CALL(glEnable(GL_PROGRAM_POINT_SIZE)); } +/////////////////////////////////////////////////////////////////////////////////// - // Compile shaders and create shader program - gl_shaderProgram = gl_createShaderProgram(pointsVertSource, pointsFragSource); - // Get pointers - gl_uMVP = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uMVP"))); - gl_uPointSize = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uPointSize"))); - gl_uIntensityScale = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uIntensityScale"))); - gl_uUsePose = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uUsePose"))); - GL_CALL(glEnable(GL_PROGRAM_POINT_SIZE)); -} -/////////////////////////////////////////////////////////////////////////////////// void openSession() { @@ -692,16 +720,16 @@ void openSession() if (gl_useVBOs) { - // clearing previous data + //clearing previous data GLint offset = 0; gl_clouds.clear(); gl_clouds.shrink_to_fit(); gl_cloudIndexSSBO.clear(); gl_cloudIndexSSBO.shrink_to_fit(); gl_cloudsSSBO.clear(); - gl_cloudsSSBO.shrink_to_fit(); + gl_cloudsSSBO.shrink_to_fit(); - // Convert point cloud data to gl_Points + //Convert point cloud data to gl_Points for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); ++j) { const auto& pc = session.point_clouds_container.point_clouds[j]; @@ -714,10 +742,10 @@ void openSession() gp.pos[0] = pt.x(); gp.pos[1] = pt.y(); gp.pos[2] = pt.z(); - gp.intensity = pc.intensities[i]; // same index + gp.intensity = pc.intensities[i]; // same index gl_Points.push_back(gp); - gl_cloudIndexSSBO.push_back(static_cast(j)); // cloud index + gl_cloudIndexSSBO.push_back(static_cast(j)); // cloud index } gl_cloud gc; @@ -729,26 +757,26 @@ void openSession() gl_cloudSSBO gcSSBO; Eigen::Matrix4f pose_f = pc.m_pose.matrix().cast(); std::memcpy(gcSSBO.pose, pose_f.data(), 16 * sizeof(float)); - // gcSSBO.colorScheme = colorScheme; - // gcSSBO.fixedColor[0] = pc.render_color[0]; - // gcSSBO.fixedColor[1] = pc.render_color[1]; - // gcSSBO.fixedColor[2] = pc.render_color[2]; - oldcolorScheme = -1; // force update + //gcSSBO.colorScheme = colorScheme; + //gcSSBO.fixedColor[0] = pc.render_color[0]; + //gcSSBO.fixedColor[1] = pc.render_color[1]; + //gcSSBO.fixedColor[2] = pc.render_color[2]; + oldcolorScheme = CS_FOLLOW; //force update with non-sense value gl_cloudsSSBO.push_back(gcSSBO); offset += static_cast(pc.points_local.size()); } - // Load to OpenGL buffers + //Load to OpenGL buffers gl_loadPointCloudBuffer(gl_Points, VAO, VBO); - // Clear CPU-side data after load to save memory + //Clear CPU-side data after load to save memory gl_Points.clear(); gl_Points.shrink_to_fit(); - // Prepare SSBO data + //Prepare SSBO data gl_updateSSBOs(); - } + } } } @@ -768,29 +796,19 @@ void session_gui() ImGui::Separator(); ImGui::Text("Offset [m]:"); - ImGui::Text( - "X: %.10f; Y: %.10f; Z: %.10f", - session.point_clouds_container.offset.x(), - session.point_clouds_container.offset.y(), - session.point_clouds_container.offset.z()); + ImGui::Text("X: %.10f; Y: %.10f; Z: %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Click to copy to clipboard"); if (ImGui::IsItemClicked()) { char tmp[64]; - snprintf( - tmp, - sizeof(tmp), - "%.10f %.10f %.10f", - session.point_clouds_container.offset.x(), - session.point_clouds_container.offset.y(), - session.point_clouds_container.offset.z()); + snprintf(tmp, sizeof(tmp), "%.10f %.10f %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); ImGui::SetClipboardText(tmp); } ImGui::Separator(); - ImGui::Text("Dimensions:"); + ImGui::Text("Dimensions:"); if (ImGui::BeginTable("Dimensions", 4)) { ImGui::TableSetupColumn("Coord [m]"); @@ -802,6 +820,7 @@ void session_gui() ImGui::TableNextRow(); ImGui::TableSetColumnIndex(0); + std::string text = "X"; float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; // Set cursor so text is centered @@ -842,6 +861,7 @@ void session_gui() ImGui::EndTable(); } + } void index_gui() @@ -858,8 +878,7 @@ void index_gui() ImGui::Text("index_pairs : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].index_pairs.size()); ImGui::Text("buckets : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].buckets.size()); ImGui::Text("points_local : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()); - ImGui::Text( - "normal_vectors_local: %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].normal_vectors_local.size()); + ImGui::Text("normal_vectors_local: %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].normal_vectors_local.size()); ImGui::Text("colors : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].colors.size()); ImGui::Text("points_type : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].points_type.size()); ImGui::Text("intensities : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].intensities.size()); @@ -900,8 +919,7 @@ void properties_gui() { bool justPushed = false; - if (is_session_gui) - ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_session_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Session")) { if (!is_session_gui) @@ -911,15 +929,13 @@ void properties_gui() justPushed = true; } } - if (is_session_gui && !justPushed) - ImGui::PopStyleColor(); + if (is_session_gui && !justPushed) ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Properties related to whole session"); ImGui::SameLine(); - if (is_index_gui) - ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_index_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Index")) { if (!is_index_gui) @@ -929,8 +945,7 @@ void properties_gui() justPushed = true; } } - if (is_index_gui && !justPushed) - ImGui::PopStyleColor(); + if (is_index_gui && !justPushed) ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Properties related to current cloud index"); @@ -946,6 +961,7 @@ void properties_gui() ImGui::End(); } + void display() { ImGuiIO& io = ImGui::GetIO(); @@ -955,9 +971,10 @@ void display() glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); + if (gl_useVBOs) { - gl_updateUserView(); // this can be optimized to be called only on change (camera movement, parameters, window resize) + gl_updateUserView(); //this can be optimized to be called only on change (camera movement, parameters, window resize) gl_renderPointCloud(); } @@ -988,21 +1005,13 @@ void display() } else { - glOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, + glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); + camera_ortho_xy_view_zoom / ratio, -100000, 100000); - glm::mat4 proj = glm::orthoLH_ZO( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, + glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100, - 100); + camera_ortho_xy_view_zoom / ratio, -100, 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -1021,9 +1030,10 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt( - glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), + v_center_t.x(), v_center_t.y(), v_center_t.z(), + v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); @@ -1034,13 +1044,11 @@ void display() showAxes(); - if (index_rendered_points_local >= 0 && - index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) + if (index_rendered_points_local >= 0 && index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) { - const auto& cloud = session.point_clouds_container.point_clouds[index_rendered_points_local]; // avoiding multiple indexing + const auto& cloud = session.point_clouds_container.point_clouds[index_rendered_points_local]; //avoiding multiple indexing - const double inv_max_intensity = 1.0 / *std::max_element(cloud.intensities.begin(), cloud.intensities.end()); // precompute for - // speed + const double inv_max_intensity = 1.0 / *std::max_element(cloud.intensities.begin(), cloud.intensities.end()); //precompute for speed Eigen::Affine3d pose = cloud.m_pose; @@ -1051,32 +1059,31 @@ void display() glBegin(GL_POINTS); for (size_t i = 0; i < cloud.points_local.size(); i++) { - if (colorScheme == 0) // solid color + if (colorScheme == CS_SOLID) //solid color { glColor3f(cloud.render_color[0], cloud.render_color[1], cloud.render_color[2]); } - else if (colorScheme == 1) // intensity gradient + else if (colorScheme == CS_GRAD_INTENS) //intensity gradient { const double norm = cloud.intensities[i] * inv_max_intensity + offset_intensity; glColor3f(norm, 0.0, 1.0 - norm); } - Eigen::Vector3d p(cloud.points_local[i].x(), cloud.points_local[i].y(), cloud.points_local[i].z()); + Eigen::Vector3d p(cloud.points_local[i].x(), + cloud.points_local[i].y(), + cloud.points_local[i].z()); p = pose * p; glVertex3f(p.x(), p.y(), p.z()); } glEnd(); - if (show_neighbouring_scans) - { + if (show_neighbouring_scans) { glColor3f(pc_neigbouring_color.x, pc_neigbouring_color.y, pc_neigbouring_color.z); glPointSize(point_size); glBegin(GL_POINTS); - for (int index = index_rendered_points_local - 20; index <= index_rendered_points_local + 20; index += 5) - { - if (index != index_rendered_points_local && index >= 0 && index < session.point_clouds_container.point_clouds.size()) - { - const auto& iCloud = session.point_clouds_container.point_clouds[index]; // avoiding multiple indexing + for (int index = index_rendered_points_local - 20; index <= index_rendered_points_local + 20; index += 5) { + if (index != index_rendered_points_local && index >= 0 && index < session.point_clouds_container.point_clouds.size()) { + const auto& iCloud = session.point_clouds_container.point_clouds[index]; //avoiding multiple indexing Eigen::Affine3d pose = iCloud.m_pose; if (usePose == false) @@ -1088,7 +1095,9 @@ void display() for (size_t i = 0; i < iCloud.points_local.size(); i++) { - Eigen::Vector3d p(iCloud.points_local[i].x(), iCloud.points_local[i].y(), iCloud.points_local[i].z()); + Eigen::Vector3d p(iCloud.points_local[i].x(), + iCloud.points_local[i].y(), + iCloud.points_local[i].z()); p = pose * p; glVertex3f(p.x(), p.y(), p.z()); } @@ -1110,7 +1119,7 @@ void display() { openSession(); - // workaround + //workaround io.AddKeyEvent(ImGuiKey_O, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1121,7 +1130,7 @@ void display() { is_properties_gui = !is_properties_gui; - // workaround + //workaround io.AddKeyEvent(ImGuiKey_P, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1133,7 +1142,7 @@ void display() { show_neighbouring_scans = !show_neighbouring_scans; - // workaround + //workaround io.AddKeyEvent(ImGuiKey_N, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1148,11 +1157,15 @@ void display() else if (offset_intensity > 1) offset_intensity = 1; - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) || - ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true)) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) + || ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true) + ) index_rendered_points_local += 1; - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) || - ImGui::IsKeyPressed(ImGuiKey_PageDown, true) || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true)) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + || ImGui::IsKeyPressed(ImGuiKey_PageDown, true) + || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true) + ) index_rendered_points_local -= 1; if (index_rendered_points_local < 0) @@ -1164,7 +1177,7 @@ void display() if (ImGui::BeginMainMenuBar()) { if (ImGui::Button("Open session")) - openSession(); + openSession(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Select session to open for analyze (Ctrl+O)"); @@ -1190,7 +1203,7 @@ void display() for (auto& point_cloud : session.point_clouds_container.point_clouds) point_cloud.point_size = point_size; - // ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + //ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); ImGui::Separator(); } @@ -1218,9 +1231,11 @@ void display() ImGui::MenuItem("Use segment pose", nullptr, &usePose); ImGui::MenuItem("Show neighbouring scans", "Ctrl+N", &show_neighbouring_scans); - // ImGui::MenuItem("show_covs", nullptr, &show_covs); + + //ImGui::MenuItem("show_covs", nullptr, &show_covs); ImGui::Separator(); + ImGui::MenuItem("VBO/VAO proof of concept", nullptr, &gl_useVBOs); if (ImGui::IsItemHovered()) @@ -1235,7 +1250,7 @@ void display() } if (gl_useVBOs) - usePose = true; // forces usePose when using VBOs for now + usePose = true; //forces usePose when using VBOs for now ImGui::Separator(); @@ -1253,7 +1268,7 @@ void display() ImGui::Separator(); - ImGui::Text("Point cloud:"); + ImGui::Text("Point cloud:"); float color[3]; if (session.point_clouds_container.point_clouds.size() > 0) @@ -1265,20 +1280,21 @@ void display() if (ImGui::ColorEdit3("", (float*)&color, ImGuiColorEditFlags_NoInputs)) { + colorScheme = CS_SOLID; + for (auto& pc : session.point_clouds_container.point_clouds) { pc.render_color[0] = color[0]; pc.render_color[1] = color[1]; pc.render_color[2] = color[2]; - pc.show_color = false; } } - ImGui::SameLine(); - if (ImGui::MenuItem("> Solid color", nullptr, (colorScheme == 0))) - colorScheme = 0; + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid color", nullptr, (colorScheme == CS_SOLID))) + colorScheme = CS_SOLID; - if (ImGui::MenuItem("> Intensity gradient", nullptr, (colorScheme == 1))) - colorScheme = 1; + if (ImGui::MenuItem("> Intensity gradient", nullptr, (colorScheme == CS_GRAD_INTENS))) + colorScheme = CS_GRAD_INTENS; ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputFloat("Offset intensity", &offset_intensity, 0.01, 0.1, "%.2f"); if (offset_intensity < 0) @@ -1288,8 +1304,9 @@ void display() if (ImGui::IsItemHovered()) ImGui::SetTooltip("keyboard up/down arrows"); - if (ImGui::MenuItem("> Random colors per segment", nullptr, (colorScheme == 2))) - colorScheme = 2; + if (ImGui::MenuItem("> Random color per segment", nullptr, (colorScheme == CS_RANDOM))) + colorScheme = CS_RANDOM; + } ImGui::EndDisabled(); @@ -1339,7 +1356,7 @@ void display() } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Scene view relevant parameters"); - + camMenu(); ImGui::SameLine(); @@ -1359,9 +1376,8 @@ void display() { ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / - 1e9; + double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] + - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); @@ -1373,9 +1389,8 @@ void display() { ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / - 1e9; + double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] + - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); @@ -1390,9 +1405,7 @@ void display() ImGui::SameLine(); ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); - ImGui::SameLine( - ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -1409,7 +1422,7 @@ void display() } if (is_properties_gui) - properties_gui(); + properties_gui(); if (consImGui) { @@ -1439,7 +1452,7 @@ void display() void mouse(int glut_button, int state, int x, int y) { - ImGuiIO& io = ImGui::GetIO(); + ImGuiIO &io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; @@ -1507,7 +1520,7 @@ void mouse(int glut_button, int state, int x, int y) } } -int main(int argc, char* argv[]) +int main(int argc, char *argv[]) { try { @@ -1518,17 +1531,20 @@ int main(int argc, char* argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - } catch (const std::bad_alloc& e) + } + catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } catch (const std::exception& e) + } + catch (const std::exception& e) { std::cout << e.what(); - } catch (...) + } + catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } return 0; -} \ No newline at end of file +} diff --git a/apps/mandeye_single_session_viewer/resource.rc b/apps/mandeye_single_session_viewer/resource.rc index ae9959ec..b477d6ac 100644 --- a/apps/mandeye_single_session_viewer/resource.rc +++ b/apps/mandeye_single_session_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,5 - PRODUCTVERSION 0,0,9,5 + FILEVERSION 0,0,9,6 + PRODUCTVERSION 0,0,9,6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Session Viewer\0" - VALUE "FileVersion", "0.9.5\0" + VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Single session viewer\0" - VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" + VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_single_session_viewer.exe\0" - VALUE "ProductVersion", "0.9.5\0" + VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 9aba0868..f9bf5971 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1990,6 +1990,21 @@ void loadSessions() { Session session; session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset); + + //making sure irelevant session specific settings that could affect rendering are off + session.point_clouds_container.xz_intersection = false; + session.point_clouds_container.yz_intersection = false; + session.point_clouds_container.xy_intersection = false; + session.point_clouds_container.xz_grid_10x10 = false; + session.point_clouds_container.xz_grid_1x1 = false; + session.point_clouds_container.xz_grid_01x01 = false; + session.point_clouds_container.yz_grid_10x10 = false; + session.point_clouds_container.yz_grid_1x1 = false; + session.point_clouds_container.yz_grid_01x01 = false; + session.point_clouds_container.xy_grid_10x10 = false; + session.point_clouds_container.xy_grid_1x1 = false; + session.point_clouds_container.xy_grid_01x01 = false; + sessions.push_back(session); if (session.is_ground_truth) index_gt = sessions.size() - 1; @@ -2169,25 +2184,12 @@ void settings_gui() ImGui::EndDisabled(); ImGui::SameLine(); - ImGui::Checkbox(("Show RGB##" + std::to_string(i)).c_str(), &sessions[i].show_rgb); - - if (!sessions[i].show_rgb) - { - ImGui::SameLine(); - ImGui::ColorEdit3( - ("Color##" + std::to_string(i)).c_str(), (float*)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); - for (auto& pc : sessions[i].point_clouds_container.point_clouds) - { - pc.render_color[0] = sessions[i].render_color[0]; - pc.render_color[1] = sessions[i].render_color[1]; - pc.render_color[2] = sessions[i].render_color[2]; - pc.show_color = sessions[i].show_rgb; - } - } - else + ImGui::ColorEdit3(("Color##" + std::to_string(i)).c_str(), (float *)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); + for (auto &pc : sessions[i].point_clouds_container.point_clouds) { - for (auto& pc : sessions[i].point_clouds_container.point_clouds) - pc.show_color = sessions[i].show_rgb; + pc.render_color[0] = sessions[i].render_color[0]; + pc.render_color[1] = sessions[i].render_color[1]; + pc.render_color[2] = sessions[i].render_color[2]; } } ImGui::EndDisabled(); @@ -2589,22 +2591,7 @@ void display() { if (session.visible) { - session.point_clouds_container.render( - observation_picking, - viewer_decimate_point_cloud, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 10000); + session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud); session.ground_control_points.render(session.point_clouds_container); session.control_points.render(session.point_clouds_container, false); @@ -3738,7 +3725,7 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render subsampling", &viewer_decimate_point_cloud, 10, 100); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); ImGui::SameLine(); diff --git a/apps/multi_session_registration/resource.rc b/apps/multi_session_registration/resource.rc index 89cc80ae..543c017d 100644 --- a/apps/multi_session_registration/resource.rc +++ b/apps/multi_session_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 5 -PRODUCTVERSION 0, 0, 9, 5 +FILEVERSION 0, 0, 9, 6 +PRODUCTVERSION 0, 0, 9, 6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 3\0" -VALUE "FileVersion", "0.9.5\0" +VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Multi session registration\0" -VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_session_registration_step_3.exe\0" -VALUE "ProductVersion", "0.9.5\0" +VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 c010159b..f11c2aac 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 @@ -204,7 +204,13 @@ int index_loop_closure_target = 0; int index_begin = 0; int index_end = 0; -float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +ColorScheme csPointCloud = CS_SOLID; +ColorScheme csTrajectory = CS_SOLID; + +float m_gizmo[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; @@ -1294,10 +1300,7 @@ void lio_segments_gui() if (session.point_clouds_container.point_clouds[i].visible) { ImGui::SameLine(); - ImGui::ColorEdit3( - ("pc_color##" + std::to_string(i)).c_str(), - session.point_clouds_container.point_clouds[i].render_color, - ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3(("color##" + std::to_string(i)).c_str(), session.point_clouds_container.point_clouds[i].render_color, ImGuiColorEditFlags_NoInputs); #if 0 ImGui::SameLine(); @@ -2446,26 +2449,9 @@ void display() } } - session.point_clouds_container.render( - observation_picking, - viewer_decimate_point_cloud, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, - session.point_clouds_container.xz_grid_10x10, - session.point_clouds_container.xz_grid_1x1, - session.point_clouds_container.xz_grid_01x01, - session.point_clouds_container.yz_grid_10x10, - session.point_clouds_container.yz_grid_1x1, - session.point_clouds_container.yz_grid_01x01, - session.point_clouds_container.xy_grid_10x10, - session.point_clouds_container.xy_grid_1x1, - session.point_clouds_container.xy_grid_01x01, - session.point_clouds_container.intersection_width, - session_dims); - - // std::cout << "session.point_clouds_container.xy_grid_10x10 " << (int)session.point_clouds_container.xy_grid_10x10 << - // std::endl; + session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, session_dims); + + // std::cout << "session.point_clouds_container.xy_grid_10x10 " << (int)session.point_clouds_container.xy_grid_10x10 << std::endl; observation_picking.render(); @@ -2672,7 +2658,13 @@ void display() pc.render_color[0] = float(rand() % 255) / 255.0f; pc.render_color[1] = float(rand() % 255) / 255.0f; pc.render_color[2] = float(rand() % 255) / 255.0f; - pc.show_color = false; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } } // workaround @@ -3322,19 +3314,153 @@ void display() { ImGui::BeginDisabled(!session_loaded); { - auto tmp = point_size; - ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points size", &point_size); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("keyboard 1-9 keys"); - if (point_size < 1) - point_size = 1; - else if (point_size > 10) - point_size = 10; + if (ImGui::BeginMenu("Point cloud")) + { + auto tmp = point_size; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points size", &point_size); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("keyboard 1-9 keys"); + if (point_size < 1) + point_size = 1; + else if (point_size > 10) + point_size = 10; + + if (tmp != point_size) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.point_size = point_size; + + ImGui::Separator(); + + ImGui::Text("Color:"); + + float color[3]; + if (session_loaded) + { + color[0] = session.point_clouds_container.point_clouds[0].render_color[0]; + color[1] = session.point_clouds_container.point_clouds[0].render_color[1]; + color[2] = session.point_clouds_container.point_clouds[0].render_color[2]; + } + + if (ImGui::ColorEdit3("", (float*)&color, ImGuiColorEditFlags_NoInputs)) + { + csPointCloud = CS_SOLID; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = color[0]; + pc.render_color[1] = color[1]; + pc.render_color[2] = color[2]; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + } + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid", nullptr, (csPointCloud == CS_SOLID))) + csPointCloud = CS_SOLID; + + if (ImGui::MenuItem("> Random per segment", "Ctrl+R", (csPointCloud == CS_RANDOM))) + { + csPointCloud = CS_RANDOM; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = float(rand() % 255) / 255.0f; + pc.render_color[1] = float(rand() % 255) / 255.0f; + pc.render_color[2] = float(rand() % 255) / 255.0f; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + } - if (tmp != point_size) - for (auto& point_cloud : session.point_clouds_container.point_clouds) - point_cloud.point_size = point_size; + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Trajectory")) + { + ImGui::BeginDisabled(!glLineWidthSupport); + { + auto tmp = session.point_clouds_container.point_clouds[0].line_width; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Line width", &tmp); + if (tmp < 0) + tmp = 0; + else if (tmp > 5) + tmp = 5; + + if (tmp != session.point_clouds_container.point_clouds[0].line_width) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.line_width = tmp; + } + ImGui::EndDisabled(); + + ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + + ImGui::Separator(); + + ImGui::Text("Color:"); + + float color[3]; + if (session_loaded) + { + color[0] = session.point_clouds_container.point_clouds[0].traj_color[0]; + color[1] = session.point_clouds_container.point_clouds[0].traj_color[1]; + color[2] = session.point_clouds_container.point_clouds[0].traj_color[2]; + } + + if (ImGui::ColorEdit3("", (float*)&color, ImGuiColorEditFlags_NoInputs)) + { + csTrajectory = CS_SOLID; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = color[0]; + pc.traj_color[1] = color[1]; + pc.traj_color[2] = color[2]; + } + } + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid", nullptr, (csTrajectory == CS_SOLID))) + csTrajectory = CS_SOLID; + + if (ImGui::MenuItem("> Random per segment", nullptr, (csTrajectory == CS_RANDOM))) + { + csTrajectory = CS_RANDOM; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = float(rand() % 255) / 255.0f; + pc.traj_color[1] = float(rand() % 255) / 255.0f; + pc.traj_color[2] = float(rand() % 255) / 255.0f; + } + } + + if (ImGui::MenuItem("> Follow cloud color", nullptr, (csTrajectory == CS_FOLLOW))) + { + csTrajectory = CS_FOLLOW; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + + ImGui::EndMenu(); + } + + ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); ImGui::BeginDisabled(tls_registration.gnss.gnss_poses.size() <= 0); { @@ -3342,8 +3468,6 @@ void display() } ImGui::EndDisabled(); - ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); - ImGui::Separator(); } ImGui::EndDisabled(); @@ -3369,46 +3493,6 @@ void display() ImGui::Separator(); - ImGui::Text("Colors:"); - - ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); - - ImGui::BeginDisabled(!session_loaded); - { - float color[3]; - if (session_loaded) - { - color[0] = session.point_clouds_container.point_clouds[0].render_color[0]; - color[1] = session.point_clouds_container.point_clouds[0].render_color[1]; - color[2] = session.point_clouds_container.point_clouds[0].render_color[2]; - } - - if (ImGui::ColorEdit3("Point cloud", (float*)&color, ImGuiColorEditFlags_NoInputs)) - { - for (auto& pc : session.point_clouds_container.point_clouds) - { - pc.render_color[0] = color[0]; - pc.render_color[1] = color[1]; - pc.render_color[2] = color[2]; - pc.show_color = false; - } - } - - if (ImGui::MenuItem("Random segments colors", "Ctrl+R")) - { - for (auto& pc : session.point_clouds_container.point_clouds) - { - pc.render_color[0] = float(rand() % 255) / 255.0f; - pc.render_color[1] = float(rand() % 255) / 255.0f; - pc.render_color[2] = float(rand() % 255) / 255.0f; - pc.show_color = false; - } - } - } - ImGui::EndDisabled(); - - ImGui::Separator(); - ImGui::MenuItem("Settings", nullptr, &is_settings_gui); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Show power user settings window with more parameters"); @@ -3461,7 +3545,7 @@ void display() ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render subsampling", &viewer_decimate_point_cloud, 10, 100); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); ImGui::SameLine(); diff --git a/apps/multi_view_tls_registration/resource.rc b/apps/multi_view_tls_registration/resource.rc index 1f965547..d12b43d0 100644 --- a/apps/multi_view_tls_registration/resource.rc +++ b/apps/multi_view_tls_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 5 -PRODUCTVERSION 0, 0, 9, 5 +FILEVERSION 0, 0, 9, 6 +PRODUCTVERSION 0, 0, 9, 6 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 2\0" -VALUE "FileVersion", "0.9.5\0" +VALUE "FileVersion", "0.9.6\0" VALUE "InternalName", "Multi view TLS registration\0" -VALUE "LegalCopyright", "(c) 2025 github.com/MapsHD/HDMapping\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_view_tls_registration_step_2.exe\0" -VALUE "ProductVersion", "0.9.5\0" +VALUE "ProductVersion", "0.9.6\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp index ce166575..2e892bd6 100644 --- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp +++ b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp @@ -860,8 +860,7 @@ void display() session.point_clouds_container.point_clouds[k].point_size = SystemData::point_size; } - session.point_clouds_container.render( - observation_picking, decimation_step, false, false, false, false, false, false, false, false, false, false, false, false, 10000); + session.point_clouds_container.render(observation_picking, decimation_step); // session.ground_control_points.render(session.point_clouds_container); /*session.point_clouds_container.render({}, {}, session.point_clouds_container.xz_intersection, diff --git a/core/include/point_cloud.h b/core/include/point_cloud.h index c55e2a43..2252201b 100644 --- a/core/include/point_cloud.h +++ b/core/include/point_cloud.h @@ -13,136 +13,130 @@ class PointCloud { public: - struct GridParameters - { - double bounding_box_min_X = 0.0; - double bounding_box_min_Y = 0.0; - double bounding_box_min_Z = 0.0; - double bounding_box_max_X = 0.0; - double bounding_box_max_Y = 0.0; - double bounding_box_max_Z = 0.0; - double bounding_box_extension = 1.0; - int number_of_buckets_X = 0; - int number_of_buckets_Y = 0; - int number_of_buckets_Z = 0; - long long unsigned int number_of_buckets = 0; - double resolution_X = 2.0; - double resolution_Y = 2.0; - double resolution_Z = 2.0; - }; - - struct PointBucketIndexPair - { - int index_of_point = 0; - long long unsigned int index_of_bucket = 0; - // int index_pose; - }; - - struct Bucket - { - long long unsigned int index_begin = 0; - long long unsigned int index_end = 0; - long long unsigned int number_of_points = 0; - }; - - struct Job - { - long long unsigned int index_begin_inclusive = 0; - long long unsigned int index_end_exclusive = 0; - }; - - struct NearestNeighbour - { - int index_source = 0; - int index_target = 0; - bool found = false; - }; - - struct LocalTrajectoryNode - { - std::pair timestamps; - Eigen::Affine3d m_pose; - Eigen::Vector3d imu_om_fi_ka; - Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; - }; - - PointCloud() - { - m_pose = Eigen::Affine3d::Identity(); - pose.px = pose.py = pose.pz = pose.om = pose.fi = pose.ka = 0.0; - gui_translation[0] = gui_translation[1] = gui_translation[2] = 0.0f; - gui_rotation[0] = gui_rotation[1] = gui_rotation[2] = 0.0; - - render_color[0] = (float(rand() % 255)) / 255.0f; - render_color[1] = (float(rand() % 255)) / 255.0f; - render_color[2] = (float(rand() % 255)) / 255.0f; - - visible = true; - gizmo = false; - }; - - ~PointCloud() = default; - - GridParameters rgd_params; - std::vector index_pairs; - std::vector buckets; - std::string file_name; - std::vector points_local; - std::vector normal_vectors_local; - std::vector colors; - std::vector points_type; - std::vector intensities; - std::vector timestamps; - Eigen::Affine3d m_pose_temp; - Eigen::Affine3d m_pose; - Eigen::Affine3d m_initial_pose; - Eigen::Matrix covariance_matrix_tait_bryan; - Eigen::Matrix information_matrix_tait_bryan; - int number_points_vertical = 0; - int number_points_horizontal = 0; - int point_size = 1; - // std::vector available_geo_points; - bool choosing_geo = false; - std::vector local_trajectory; - bool show_local_trajectory = false; - bool show_color = true; - bool fuse_inclination_from_IMU = false; - - bool show_IMU = false; - bool show_pose = false; - - TaitBryanPose pose; - float gui_translation[3]; - float gui_rotation[3]; - float render_color[3]; - bool visible; - // bool visible_imu_diff = false; - bool gizmo; - int num_threads = 16; - // bool fixed = false; - // double search_radious = 2.0; - bool fixed_x = false; - bool fixed_y = false; - bool fixed_z = false; - bool fixed_om = false; - bool fixed_fi = false; - bool fixed_ka = false; - - bool load(const std::string& file_name); - bool load_pc(std::string input_file_name, bool load_cache_mode); - void update_from_gui(); - bool save_as_global(std::string file_name); - // rgd - bool build_rgd(); - void grid_calculate_params(std::vector& points, GridParameters& params); - void reindex(std::vector& ip, std::vector& points, GridParameters rgd_params); - std::vector get_jobs(long long unsigned int size, int num_threads); - void cout_rgd(); - std::vector> nns(PointCloud& pc_target, double search_radious); - void clean(); - void compute_normal_vectors(double search_radious); - void decimate(double bucket_x, double bucket_y, double bucket_z); - void shift_to_center(); + struct GridParameters { + double bounding_box_min_X = 0.0; + double bounding_box_min_Y = 0.0; + double bounding_box_min_Z = 0.0; + double bounding_box_max_X = 0.0; + double bounding_box_max_Y = 0.0; + double bounding_box_max_Z = 0.0; + double bounding_box_extension = 1.0; + int number_of_buckets_X = 0; + int number_of_buckets_Y = 0; + int number_of_buckets_Z = 0; + long long unsigned int number_of_buckets = 0; + double resolution_X = 2.0; + double resolution_Y = 2.0; + double resolution_Z = 2.0; + }; + + struct PointBucketIndexPair { + int index_of_point = 0; + long long unsigned int index_of_bucket = 0; + //int index_pose; + }; + + struct Bucket { + long long unsigned int index_begin = 0; + long long unsigned int index_end = 0; + long long unsigned int number_of_points = 0; + }; + + struct Job { + long long unsigned int index_begin_inclusive = 0; + long long unsigned int index_end_exclusive = 0; + }; + + struct NearestNeighbour { + int index_source = 0; + int index_target = 0; + bool found = false; + }; + + struct LocalTrajectoryNode{ + std::pair timestamps; + Eigen::Affine3d m_pose; + Eigen::Vector3d imu_om_fi_ka; + Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; + }; + + PointCloud() { + m_pose = Eigen::Affine3d::Identity(); + pose.px = pose.py = pose.pz = pose.om = pose.fi = pose.ka = 0.0; + gui_translation[0] = gui_translation[1] = gui_translation[2] = 0.0f; + gui_rotation[0] = gui_rotation[1] = gui_rotation[2] = 0.0; + + render_color[0] = (float(rand() % 255)) / 255.0f; + render_color[1] = (float(rand() % 255)) / 255.0f; + render_color[2] = (float(rand() % 255)) / 255.0f; + + visible = true; + gizmo = false; + }; + ~PointCloud() { ; }; + + GridParameters rgd_params; + std::vector index_pairs; + std::vector < Bucket> buckets; + std::string file_name; + std::vector points_local; + std::vector normal_vectors_local; + std::vector colors; + std::vector points_type; + std::vector intensities; + std::vector timestamps; + Eigen::Affine3d m_pose_temp; + Eigen::Affine3d m_pose; + Eigen::Affine3d m_initial_pose; + Eigen::Matrix covariance_matrix_tait_bryan; + Eigen::Matrix information_matrix_tait_bryan; + int number_points_vertical = 0; + int number_points_horizontal = 0; + int point_size = 1; + int line_width = 1; + //std::vector available_geo_points; + bool choosing_geo = false; + std::vector local_trajectory; + bool show_local_trajectory = false; + bool show_color = true; + bool fuse_inclination_from_IMU = false; + + bool show_IMU = false; + bool show_pose = false; + + TaitBryanPose pose; + float gui_translation[3]; + float gui_rotation[3]; + float render_color[3]; + float traj_color[3]; + bool visible; + //bool visible_imu_diff = false; + bool gizmo; + int num_threads = 16; + //bool fixed = false; + //double search_radious = 2.0; + bool fixed_x = false; + bool fixed_y = false; + bool fixed_z = false; + bool fixed_om = false; + bool fixed_fi = false; + bool fixed_ka = false; + + bool load(const std::string& file_name); + bool load_pc(std::string input_file_name, bool load_cache_mode); + void update_from_gui(); + bool save_as_global(std::string file_name); + //rgd + bool build_rgd(); + void grid_calculate_params(std::vector& points, GridParameters& params); + void reindex(std::vector& ip, std::vector& points, GridParameters rgd_params); + std::vector get_jobs(long long unsigned int size, int num_threads); + void cout_rgd(); + std::vector> nns(PointCloud& pc_target, double search_radious); + void clean(); + void compute_normal_vectors(double search_radious); + void decimate(double bucket_x, double bucket_y, double bucket_z); + void shift_to_center(); #if WITH_GUI == 1 // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, // bool xz_intersection, bool yz_intersection, bool xy_intersection, diff --git a/core/include/point_clouds.h b/core/include/point_clouds.h index cc383681..0ff14644 100644 --- a/core/include/point_clouds.h +++ b/core/include/point_clouds.h @@ -65,34 +65,10 @@ class PointClouds double bucket_z); // std::vector load_points(const std::string& point_clouds_file_name); #if WITH_GUI == 1 - void draw_grids( - bool xz_grid_10x10, - bool xz_grid_1x1, - bool xz_grid_01x01, - bool yz_grid_10x10, - bool yz_grid_1x1, - bool yz_grid_01x01, - bool xy_grid_10x10, - bool xy_grid_1x1, - bool xy_grid_01x01, - PointClouds::PointCloudDimensions dims); - void render( - const ObservationPicking& observation_picking, - int viewer_decmiate_point_cloud, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - bool xz_grid_10x10, - bool xz_grid_1x1, - bool xz_grid_01x01, - bool yz_grid_10x10, - bool yz_grid_1x1, - bool yz_grid_01x01, - bool xy_grid_10x10, - bool xy_grid_1x1, - bool xy_grid_01x01, - double intersection_width, - PointClouds::PointCloudDimensions dims = {}); + void draw_grids(bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, + bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, + bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, PointClouds::PointCloudDimensions dims); + void render(const ObservationPicking &observation_picking, int viewer_decimate_point_cloud, PointClouds::PointCloudDimensions dims = {}); #endif // bool save_poses(); bool save_poses(const std::string file_name, bool is_subsession); diff --git a/core/include/session.h b/core/include/session.h index 2897a241..e2d06bed 100644 --- a/core/include/session.h +++ b/core/include/session.h @@ -26,7 +26,7 @@ class Session float render_color[3]; std::string session_file_name = ""; bool is_ground_truth = false; - bool show_rgb = true; + //bool show_rgb = true; bool load_cache_mode = false; #if WITH_GUI == 1 diff --git a/core/include/utils.hpp b/core/include/utils.hpp index fb316ae5..20cd9412 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -40,6 +40,15 @@ enum CameraPreset CAMERA_RESET }; +enum ColorScheme { + CS_SOLID, // fixed color + CS_RANDOM, // random + CS_GRAD_INTENS, // gradient based on intensity + CS_GRAD_ELEV, // gradient based on elevation + CS_GRAD_DIST, // gradient based on distance from rotation center + CS_FOLLOW // valid for trajectory +}; + /////////////////////////////////////////////////////////////////////////////////// extern int viewer_decimate_point_cloud; @@ -81,8 +90,9 @@ extern float new_translate_z; // Transition timing extern bool camera_transition_active; -struct ShortcutEntry -{ +extern bool glLineWidthSupport; + +struct ShortcutEntry { std::string type; std::string shortcut; std::string description; diff --git a/core/src/observation_picking.cpp b/core/src/observation_picking.cpp index b91bd984..6ead89c2 100644 --- a/core/src/observation_picking.cpp +++ b/core/src/observation_picking.cpp @@ -26,6 +26,7 @@ void ObservationPicking::render() } glEnd(); } + if (grid1x1m) { glColor3f(0.3, 0.3, 0.3); glBegin(GL_LINES); @@ -39,6 +40,7 @@ void ObservationPicking::render() } glEnd(); } + if (grid01x01m) { glColor3f(0.1, 0.1, 0.1); glBegin(GL_LINES); @@ -52,6 +54,7 @@ void ObservationPicking::render() } glEnd(); } + if (grid001x001m) { glColor3f(0.8, 0.8, 0.8); glBegin(GL_LINES); diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 7041055d..6d2f77f4 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -1613,255 +1613,256 @@ void PointCloud::render( double intersection_width, bool visible_imu_diff) { - // std::cout << (int)xz_grid_10x10 << std::endl; + if (!this->visible) + return; - glPointSize(observation_picking.point_size); - - if (this->visible) - { - glColor3f(render_color[0], render_color[1], render_color[2]); - glPointSize(point_size); - - // glColor3f(render_color[0], render_color[1], render_color[2]); - if (visible_imu_diff) - { - glBegin(GL_LINES); - for (int i = 1; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - - glColor3f(1, 0, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10, m(1, 3), m(2, 3)); - - glColor3f(0, 1, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10, m(2, 3)); - - glColor3f(0, 0, 1); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), m(1, 3), m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); - - // std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << - // this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << - // this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << std::endl; - } - glEnd(); - } - - // glBegin(GL_POINTS); - // for (const auto& p : this->points_local) { - - // std::cout << this->colors.size() << " " << this->points_local.size() << std::endl; - - glBegin(GL_POINTS); - for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) - { - const auto& p = this->points_local[i]; - Eigen::Vector3d vp; - if (show_with_initial_pose) - { - vp = this->m_initial_pose * p; - } - else - { - vp = this->m_pose * p; - } - if (this->show_color) - { - if (this->colors.size() == this->points_local.size()) - { - glColor3f(this->colors[i].x(), this->colors[i].y(), this->colors[i].z()); - } - } - // - - if (observation_picking.is_observation_picking_mode) - { - if (fabs(vp.z() - observation_picking.picking_plane_height) <= observation_picking.picking_plane_threshold) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - else - { - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - - if (!xz_intersection && !yz_intersection && !xy_intersection) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - // glEnd(); - } - glEnd(); - glPointSize(1); - - if (!xz_intersection && !yz_intersection && !xy_intersection) - { - glColor3f(render_color[0], render_color[1], render_color[2]); - glBegin(GL_LINE_STRIP); - for (int i = 0; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - } - glEnd(); - - if (this->local_trajectory.size() > 0 && this->fuse_inclination_from_IMU) - { - // double om = local_trajectory[0].imu_om_fi_ka.x() * 180.0 / M_PI; - // double fi = local_trajectory[0].imu_om_fi_ka.y() * 180.0 / M_PI; - - // if (fabs(om) > 5 || fabs(fi) > 5) - //{ - // } - // else - //{ - - Eigen::Vector3d a1(-0.2, -0.2, 0); - Eigen::Vector3d a2(0.2, -0.2, 0); - Eigen::Vector3d a3(0.2, 0.2, 0); - Eigen::Vector3d a4(-0.2, 0.2, 0); - - Eigen::Vector3d a1t = this->m_pose * a1; - Eigen::Vector3d a2t = this->m_pose * a2; - Eigen::Vector3d a3t = this->m_pose * a3; - Eigen::Vector3d a4t = this->m_pose * a4; - - glColor3f(0, 1, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - - TaitBryanPose tb; - tb.px = this->m_pose(0, 3); - tb.py = this->m_pose(1, 3); - tb.pz = this->m_pose(2, 3); - tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); - tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); - tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); - - auto m = affine_matrix_from_pose_tait_bryan(tb); - a1t = m * a1; - a2t = m * a2; - a3t = m * a3; - a4t = m * a4; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - //} - } - - if (this->fixed_om && this->fixed_fi) - { - for (double x = -1.0; x <= 1.0; x += 0.1) - { - if (fabs(x) < 0.3) - { - continue; - } - - Eigen::Vector3d a1(-x, -x, 0); - Eigen::Vector3d a2(x, -x, 0); - Eigen::Vector3d a3(x, x, 0); - Eigen::Vector3d a4(-x, x, 0); - - Eigen::Vector3d a1t = this->m_pose * a1; - Eigen::Vector3d a2t = this->m_pose * a2; - Eigen::Vector3d a3t = this->m_pose * a3; - Eigen::Vector3d a4t = this->m_pose * a4; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - } - } - - if (show_IMU) - { - TaitBryanPose tb; - tb.px = this->m_pose(0, 3); - tb.py = this->m_pose(1, 3); - tb.pz = this->m_pose(2, 3); - tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); - tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); - tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); - - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(tb); - - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - } - - if (show_pose) - { - Eigen::Affine3d m = this->m_pose; + if (visible_imu_diff) + { + glBegin(GL_LINES); + for (int i = 1; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + + glColor3f(1, 0, 0); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10, + m(1, 3), + m(2, 3)); + + glColor3f(0, 1, 0); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3), + m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10, + m(2, 3)); + + glColor3f(0, 0, 1); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3), + m(1, 3), + m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); + + // std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << std::endl; + } + glEnd(); + } - glLineWidth(2); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + glColor3f(render_color[0], render_color[1], render_color[2]); + if (observation_picking.is_observation_picking_mode) + glPointSize(observation_picking.point_size); + else + glPointSize(point_size); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + glBegin(GL_POINTS); + for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) + { + const auto &p = this->points_local[i]; + Eigen::Vector3d vp; + if (show_with_initial_pose) + { + vp = this->m_initial_pose * p; + } + else + { + vp = this->m_pose * p; + } + + bool render_point = false; + + if (observation_picking.is_observation_picking_mode) + { + if (fabs(vp.z() - observation_picking.picking_plane_height) <= observation_picking.picking_plane_threshold) + { + render_point = true; + } + } + else + { + if (xz_intersection) + { + if (fabs(vp.y()) < intersection_width) + { + render_point = true; + } + } + if (yz_intersection) + { + if (fabs(vp.x()) < intersection_width) + { + render_point = true; + } + } + if (xy_intersection) + { + if (fabs(vp.z()) < intersection_width) + { + render_point = true; + } + } + + if (!xz_intersection && !yz_intersection && !xy_intersection) + { + render_point = true; + } + } + + if (render_point) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + glEnd(); + glPointSize(1); // reset point size - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - glLineWidth(1); - } - } - } + //render trajectory + if (!xz_intersection && !yz_intersection && !xy_intersection) + { + if (line_width > 0) + { + glColor3f(traj_color[0], traj_color[1], traj_color[2]); + glLineWidth(line_width); + glBegin(GL_LINE_STRIP); + for (int i = 0; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + } + glEnd(); + glLineWidth(1); // reset line width + } + + if (this->local_trajectory.size() > 0 && this->fuse_inclination_from_IMU) + { + // double om = local_trajectory[0].imu_om_fi_ka.x() * 180.0 / M_PI; + // double fi = local_trajectory[0].imu_om_fi_ka.y() * 180.0 / M_PI; + + // if (fabs(om) > 5 || fabs(fi) > 5) + //{ + // } + // else + //{ + + glBegin(GL_LINE_STRIP); + + Eigen::Vector3d a1(-0.2, -0.2, 0); + Eigen::Vector3d a2(0.2, -0.2, 0); + Eigen::Vector3d a3(0.2, 0.2, 0); + Eigen::Vector3d a4(-0.2, 0.2, 0); + + Eigen::Vector3d a1t = this->m_pose * a1; + Eigen::Vector3d a2t = this->m_pose * a2; + Eigen::Vector3d a3t = this->m_pose * a3; + Eigen::Vector3d a4t = this->m_pose * a4; + + glColor3f(0, 1, 0); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + + TaitBryanPose tb; + tb.px = this->m_pose(0, 3); + tb.py = this->m_pose(1, 3); + tb.pz = this->m_pose(2, 3); + tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); + + auto m = affine_matrix_from_pose_tait_bryan(tb); + a1t = m * a1; + a2t = m * a2; + a3t = m * a3; + a4t = m * a4; + + glColor3f(1, 0, 0); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + + glEnd(); + //} + } + + if (this->fixed_om && this->fixed_fi) + { + for (double x = -1.0; x <= 1.0; x += 0.1) + { + if (fabs(x) < 0.3) + { + continue; + } + + Eigen::Vector3d a1(-x, -x, 0); + Eigen::Vector3d a2(x, -x, 0); + Eigen::Vector3d a3(x, x, 0); + Eigen::Vector3d a4(-x, x, 0); + + Eigen::Vector3d a1t = this->m_pose * a1; + Eigen::Vector3d a2t = this->m_pose * a2; + Eigen::Vector3d a3t = this->m_pose * a3; + Eigen::Vector3d a4t = this->m_pose * a4; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glEnd(); + } + } + + if (show_IMU) + { + TaitBryanPose tb; + tb.px = this->m_pose(0, 3); + tb.py = this->m_pose(1, 3); + tb.pz = this->m_pose(2, 3); + tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); + + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(tb); + + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + } + + if (show_pose) + { + Eigen::Affine3d m = this->m_pose; + + glLineWidth(2); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + glLineWidth(1); + } + } } void PointCloud::render(Eigen::Affine3d pose, int viewer_decimate_point_cloud) diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index b6d4bf96..38a4c9a3 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -803,52 +803,22 @@ void PointClouds::draw_grids( } } -void PointClouds::render( - const ObservationPicking& observation_picking, - int viewer_decmiate_point_cloud, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - bool xz_grid_10x10, - bool xz_grid_1x1, - bool xz_grid_01x01, - bool yz_grid_10x10, - bool yz_grid_1x1, - bool yz_grid_01x01, - bool xy_grid_10x10, - bool xy_grid_1x1, - bool xy_grid_01x01, - double intersection_width, - PointClouds::PointCloudDimensions dims) +void PointClouds::render(const ObservationPicking &observation_picking, int viewer_decimate_point_cloud, PointClouds::PointCloudDimensions dims) { - // Draw grids once for the scene - if (xz_grid_10x10 || xz_grid_1x1 || xz_grid_01x01 || yz_grid_10x10 || yz_grid_1x1 || yz_grid_01x01 || xy_grid_10x10 || xy_grid_1x1 || - xy_grid_01x01) - draw_grids( - xz_grid_10x10, - xz_grid_1x1, - xz_grid_01x01, - yz_grid_10x10, - yz_grid_1x1, - yz_grid_01x01, - xy_grid_10x10, - xy_grid_1x1, - xy_grid_01x01, - dims); - - // Render each point cloud (points + trajectories) - for (auto& p : point_clouds) - { - p.render( - this->show_with_initial_pose, - observation_picking, - viewer_decmiate_point_cloud, - xz_intersection, - yz_intersection, - xy_intersection, - intersection_width, - show_imu_to_lio_diff); - } + // Draw grids once for the scene + if (xz_grid_10x10 || xz_grid_1x1 || xz_grid_01x01 || + yz_grid_10x10 || yz_grid_1x1 || yz_grid_01x01 || + xy_grid_10x10 || xy_grid_1x1 || xy_grid_01x01) + draw_grids(xz_grid_10x10, xz_grid_1x1, xz_grid_01x01, + yz_grid_10x10, yz_grid_1x1, yz_grid_01x01, + xy_grid_10x10, xy_grid_1x1, xy_grid_01x01, dims); + + // Render each point cloud (points + trajectories) + for (auto &p : point_clouds) + { + p.render(this->show_with_initial_pose, observation_picking, viewer_decimate_point_cloud, + xz_intersection, yz_intersection, xy_intersection, intersection_width, show_imu_to_lio_diff); + } } #endif diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 46d28898..e0e97bfa 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -70,6 +70,8 @@ double scroll_hint_lastT = 0.0; bool show_about = false; +bool glLineWidthSupport = true; + // General shortcuts applicable to any app static const std::vector shortcuts = { { "Normal keys", "A", "" }, { "", "Ctrl+A", "" }, @@ -478,6 +480,15 @@ bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display) // glutSpecialFunc(specialDown); // glutSpecialUpFunc(specialUp); + //check line width range support + GLfloat range[2]; + glGetFloatv(GL_LINE_WIDTH_RANGE, range); + if (range[0] == range[1]) + { + std::cerr << "No line width support in this GPU/driver configuration, range: " << range[0] << " - " << range[1] << std::endl; + glLineWidthSupport = false; + } + return (glGetError() == GL_NO_ERROR); }