From df8e063de310eb79fe96ec5a530b4ea271ba3b1d Mon Sep 17 00:00:00 2001 From: yuckinus <40965122+yuckinus@users.noreply.github.com> Date: Thu, 4 Dec 2025 17:16:49 +0200 Subject: [PATCH] Fixes and updates - small optimizations for info_window and drawMiniCompassWithRuler - unify display execution order of frame render - simplify camera transformations - calibration, step3 / fix bugs with compass not working - step2 / add Open from las/laz, fix behaviour after open from las/laz - viewer / add properties window with session/cloud metrics (timestamp) --- .../lidar_odometry_gui.cpp | 74 +- .../mandeye_mission_recorder_calibration.cpp | 104 +-- .../mandeye_single_session_viewer.cpp | 342 +++++-- .../multi_session_registration.cpp | 850 +++++++++--------- .../multi_view_tls_registration_gui.cpp | 803 +++++++++-------- core/include/utils.hpp | 13 +- core/src/utils.cpp | 125 +-- 7 files changed, 1259 insertions(+), 1052 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index b74365bf..2c8fc8da 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -681,7 +681,7 @@ void save_results(bool info, double elapsed_seconds) } } -void lidar_odometry_gui() +void project_gui() { if (ImGui::Begin("full_lidar_odometry_gui", &full_lidar_odometry_gui)) { @@ -1620,40 +1620,37 @@ void save_results(bool info, double elapsed_seconds, std::string &working_direct } void display() -{ - updateCameraTransition(); - - ImGuiIO &io = ImGui::GetIO(); - +{ + ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glMatrixMode(GL_PROJECTION); glLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - glClearColor(params.clear_color.x * params.clear_color.w, params.clear_color.y * params.clear_color.w, params.clear_color.z * params.clear_color.w, params.clear_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); + updateCameraTransition(); reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + viewLocal = Eigen::Affine3f::Identity(); + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); if (!lock_z) viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); - - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; + viewLocal.translate(-rotation_center); glMatrixMode(GL_MODELVIEW); - glLoadMatrixf(result.matrix().data()); + glLoadMatrixf(viewLocal.matrix().data()); showAxes(); @@ -2117,6 +2114,8 @@ void display() ImGui::ColorEdit3("Background color", (float *)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); + bg_color = params.clear_color; + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -2132,9 +2131,8 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - { info_gui = !info_gui; - } + ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -2142,22 +2140,6 @@ void display() } } - if (full_lidar_odometry_gui) - { - lastPar = 0; - - lidar_odometry_gui(); - } - - if (loRunning) - progress_window(); - - if (info_gui) - { - infoLines[infoLines.size() - 2] = "It saves session file in " + working_directory + "\\lidar_odometry_result_*"; - info_window(infoLines, appShortcuts, &info_gui); - } - if (initial_transformation_gizmo) { ImGuiIO &io = ImGui::GetIO(); @@ -2226,8 +2208,26 @@ void display() stretch_gizmo_m(3, 3) = m_gizmo[15]; } + if (full_lidar_odometry_gui) + { + lastPar = 0; + + project_gui(); + } + + if (loRunning) + progress_window(); + + cor_window(); + + if (info_gui) + { + infoLines[infoLines.size() - 2] = "It saves session file in " + working_directory + "\\lidar_odometry_result_*"; + info_window(infoLines, appShortcuts); + } + if (compass_ruler) - drawMiniCompassWithRuler(viewLocal, fabs(translate_z), params.clear_color); + drawMiniCompassWithRuler(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp index 6cfba692..7a50e13f 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -626,24 +626,47 @@ void project_gui() void display() { - updateCameraTransition(); - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glMatrixMode(GL_PROJECTION); glLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - if (is_ortho) + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + viewLocal.translate(-rotation_center); + + glLoadMatrixf(viewLocal.matrix().data()); + } + else { 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, + camera_ortho_xy_view_zoom / ratio, -100000, 100000); 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, + camera_ortho_xy_view_zoom / ratio, -100, 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -663,47 +686,13 @@ 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()); + 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())); + 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); - } - glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); - - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - - if (!is_ortho) - { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - - if (!lock_z) - viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - else - viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); - - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; - - glLoadMatrixf(result.matrix().data()); - /* glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ - } - else - { glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } @@ -838,6 +827,20 @@ void display() } ImGui::EndDisabled(); + ImGui::MenuItem("Orthographic", "key O", &is_ortho); + if (is_ortho) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); + ImGui::MenuItem("Show grid", "key G", &show_grid); ImGui::MenuItem("Show axes", "key X", &show_axes); @@ -875,9 +878,8 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - { info_gui = !info_gui; - } + ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -885,10 +887,12 @@ void display() ImGui::EndMainMenuBar(); } - info_window(infoLines, appShortcuts, &info_gui); + cor_window(); + + info_window(infoLines, appShortcuts); if (compass_ruler) - drawMiniCompassWithRuler(viewLocal, fabs(translate_z), bg_color); + drawMiniCompassWithRuler(); project_gui(); 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 610a0dfc..1728674b 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -77,7 +77,7 @@ static const std::vector appShortcuts = { {"", "O", ""}, {"", "Ctrl+O", "Open session"}, {"", "P", ""}, - {"", "Ctrl+P", ""}, + {"", "Ctrl+P", "Properties"}, {"", "Q", ""}, {"", "Ctrl+Q", ""}, {"", "R", ""}, @@ -152,9 +152,16 @@ int colorScheme = 0; //0=solid color; gradients:1=intensity, 2=cloud height int oldcolorScheme = 0; bool usePose = false; +bool is_properties_gui = false; +bool is_session_gui = true; +bool is_index_gui = false; + Session session; bool session_loaded = false; +int session_total_number_of_points = 0; +PointClouds::PointCloudDimensions session_dims; + //built in console output redirection to imgui window /////////////////////////////////////////////////////////////////////////////////// @@ -695,12 +702,20 @@ void openSession() if (session_file_name.size() > 0) { - std::string newTitle = winTitle + " - " + truncPath(session_file_name); - glutSetWindowTitle(newTitle.c_str()); - session_loaded = session.load(fs::path(session_file_name).string(), false, 0.0, 0.0, 0.0, false); index_rendered_points_local = 0; + if (session_loaded) + { + std::string newTitle = winTitle + " - " + truncPath(session_file_name); + glutSetWindowTitle(newTitle.c_str()); + + for (const auto& pc : session.point_clouds_container.point_clouds) + session_total_number_of_points += pc.points_local.size(); + + session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + } + if (gl_useVBOs) { //clearing previous data @@ -763,8 +778,180 @@ void openSession() } } +void session_gui() +{ + ImGui::Text("File name:"); + ImGui::Text(session.session_file_name.c_str()); + ImGui::Text("Working directory:"); + ImGui::Text(session.working_directory.c_str()); + + ImGui::Separator(); + + ImGui::Text("Is ground truth: %s", session.is_ground_truth ? "yes" : "no"); + ImGui::Text("Number of clouds: %zu", session.point_clouds_container.point_clouds.size()); + ImGui::Text("Number of points: %zu", session_total_number_of_points); + + ImGui::Separator(); + + ImGui::Text("Dimensions:"); + if (ImGui::BeginTable("Dimensions", 4)) + { + ImGui::TableSetupColumn("Coord [m]"); + ImGui::TableSetupColumn("min"); + ImGui::TableSetupColumn("max"); + ImGui::TableSetupColumn("size"); + ImGui::TableHeadersRow(); + + 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 + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.x_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.x_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.length); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.y_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.y_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.width); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.z_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.z_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.height); + + ImGui::EndTable(); + } + +} + +void index_gui() +{ + ImGui::Text("File name:"); + ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); + + ImGui::Text("Current index: %zu / %zu", index_rendered_points_local, session.point_clouds_container.point_clouds.size()); + + ImGui::Separator(); + + ImGui::Text("Vector sizes:"); + + 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("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()); + ImGui::Text("timestamps : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.size()); + + ImGui::Separator(); + + ImGui::Text("Timestamps:"); + double t = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.front(); + + ImGui::Text("First point: %.0f [ns]", t); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Click to copy to clipboard"); + if (ImGui::IsItemClicked()) + { + char tmp[64]; + snprintf(tmp, sizeof(tmp), "%.0f", t); + ImGui::SetClipboardText(tmp); + } + + t = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps.back(); + ImGui::Text("Last point : %.0f [ns]", t); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Click to copy to clipboard"); + if (ImGui::IsItemClicked()) + { + char tmp[64]; + snprintf(tmp, sizeof(tmp), "%.0f", t); + ImGui::SetClipboardText(tmp); + } +} + +void properties_gui() +{ + ImGui::Begin("Properties", &is_properties_gui, ImGuiWindowFlags_MenuBar); + { + if (ImGui::BeginMenuBar()) + { + bool justPushed = false; + + if (is_session_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (ImGui::Button("Session")) + { + if (!is_session_gui) + { + is_session_gui = true; + is_index_gui = false; + justPushed = true; + } + } + 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 (ImGui::Button("Index")) + { + if (!is_index_gui) + { + is_session_gui = false; + is_index_gui = true; + justPushed = true; + } + } + if (is_index_gui && !justPushed) ImGui::PopStyleColor(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Properties related to current cloud index"); + + ImGui::EndMenuBar(); + } + + if (is_session_gui) + session_gui(); + if (is_index_gui) + index_gui(); + } + + ImGui::End(); +} + + void display() { + ImGuiIO& io = ImGui::GetIO(); + glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); @@ -772,29 +959,44 @@ void display() 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(); } - updateCameraTransition(); - - ImGuiIO& io = ImGui::GetIO(); - - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glMatrixMode(GL_PROJECTION); glLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - if (is_ortho) + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + if (!is_ortho) { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + viewLocal.translate(rotation_center); + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + viewLocal.translate(-rotation_center); + + glLoadMatrixf(viewLocal.matrix().data()); + } + else + { 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, + camera_ortho_xy_view_zoom / ratio, -100000, 100000); 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, + camera_ortho_xy_view_zoom / ratio, -100, 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -814,42 +1016,13 @@ 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()); + 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())); + 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); - } - - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - - if (!is_ortho) - { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - - if (!lock_z) - viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - else - viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; - - glLoadMatrixf(result.matrix().data()); - /* glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ - } - else - { glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } @@ -858,20 +1031,20 @@ void display() 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; - if (usePose == false) + if (usePose == false) pose.translation().setZero(); glPointSize(point_size); glBegin(GL_POINTS); for (size_t i = 0; i < cloud.points_local.size(); i++) { - if (colorScheme == 0) //solid color + if (colorScheme == 0) //solid color { glColor3f(cloud.render_color[0], cloud.render_color[1], cloud.render_color[2]); } @@ -879,22 +1052,22 @@ void display() { 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()); + 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()){ + 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; @@ -908,8 +1081,8 @@ void display() for (int 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()); + iCloud.points_local[i].y(), + iCloud.points_local[i].z()); p = pose * p; glVertex3f(p.x(), p.y(), p.z()); } @@ -930,12 +1103,24 @@ void display() if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) { openSession(); - - //workaround + + //workaround io.AddKeyEvent(ImGuiKey_O, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } + if (session_loaded) + { + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_P, false)) + { + is_properties_gui = !is_properties_gui; + + //workaround + io.AddKeyEvent(ImGuiKey_P, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + } + if (session.point_clouds_container.point_clouds.size() > 0) { if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_N, false)) @@ -1056,7 +1241,6 @@ void display() ImGui::Separator(); - if (ImGui::BeginMenu("Colors")) { ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); @@ -1117,6 +1301,8 @@ void display() ImGui::Separator(); + ImGui::MenuItem("Properties", "Ctrl+P", &is_properties_gui, session_loaded); + if (ImGui::BeginMenu("Console")) { #ifdef _WIN32 @@ -1172,18 +1358,31 @@ void display() ImGui::SameLine(); ImGui::PushItemWidth(ImGuiNumberWidth); ImGui::SliderInt("##irpls", &tempIndex, 0, static_cast(session.point_clouds_container.point_clouds.size() - 1)); + if (ImGui::IsItemHovered()) + { + 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; + ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); + ImGui::NewLine(); + ImGui::Text("Check Properties (Ctrl+P) for more info"); + ImGui::EndTooltip(); + } ImGui::SameLine(); ImGui::InputInt("##irpli", &tempIndex, 1, 10); - ImGui::PopItemWidth(); if (ImGui::IsItemHovered()) { 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; + - 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"); ImGui::EndTooltip(); } + ImGui::PopItemWidth(); if ((tempIndex >= 0) && (tempIndex < session.point_clouds_container.point_clouds.size())) index_rendered_points_local = tempIndex; @@ -1200,16 +1399,17 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - { info_gui = !info_gui; - } + ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); - ImGui::EndMainMenuBar(); } + if (is_properties_gui) + properties_gui(); + if (consImGui) { if (!consHooked) @@ -1222,10 +1422,12 @@ void display() ConsoleUnhook(); } - info_window(infoLines, appShortcuts, &info_gui); + cor_window(); + + info_window(infoLines, appShortcuts); if (compass_ruler) - drawMiniCompassWithRuler(viewLocal, fabs(translate_z), bg_color); + drawMiniCompassWithRuler(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index b4647f1f..f691835d 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -2841,14 +2841,12 @@ void project_gui() { ImGui::SameLine(); - std::string ts_begin = std::to_string(sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first); - int index_last = sessions[i].point_clouds_container.point_clouds.size() - 1; int index_last2 = sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory.size() - 1; - std::string ts_end = std::to_string(sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory[index_last2].timestamps.first); - - ImGui::Text(("Timestamp range: <" + ts_begin + ", " + ts_end + ">").c_str()); + ImGui::Text("Timestamp range: <%.0f, %.0f>", + sessions[i].point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first, + sessions[i].point_clouds_container.point_clouds[index_last].local_trajectory[index_last2].timestamps.first); } } } @@ -2968,57 +2966,20 @@ void project_gui() void display() { - updateCameraTransition(); - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glMatrixMode(GL_PROJECTION); glLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - - if (is_ortho) - { - - 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); - - 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); - - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); - - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); - - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * DEG_TO_RAD; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - - 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()), - 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); - } + updateCameraTransition(); - glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); + viewLocal = Eigen::Affine3f::Identity(); if (!is_ortho) { @@ -3065,30 +3026,54 @@ void display() }*/ } - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + viewLocal.translate(rotation_center); + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); if (!lock_z) viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); - - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; + viewLocal.translate(-rotation_center); - glLoadMatrixf(result.matrix().data()); - // reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - // glTranslatef(translate_x, translate_y, translate_z); - // glRotatef(rotate_x, 1.0, 0.0, 0.0); - // glRotatef(rotate_y, 0.0, 0.0, 1.0); + glLoadMatrixf(viewLocal.matrix().data()); } else { + 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); + + 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); + + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); + + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * DEG_TO_RAD; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + + 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()), + 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); + glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } @@ -3186,13 +3171,9 @@ void display() int index_session_to = edges[i].index_session_to; if (sessions[index_session_from].is_ground_truth || sessions[index_session_to].is_ground_truth) - { glColor3f(0.0f, 1.0f, 1.0f); - } else - { glColor3f(1.0f, 1.0f, 0.0f); - } glBegin(GL_LINES); auto v1 = sessions[index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose.translation(); @@ -3317,14 +3298,359 @@ void display() glVertex3f(c.x() + 10, c.y(), c.z()); glEnd(); - glRasterPos3f(c.x() + 10, c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); - } - } - } - }*/ + glRasterPos3f(c.x() + 10, c.y(), c.z()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); + } + } + } + }*/ + + // gnss.render(session.point_clouds_container); + + if (!is_loop_closure_gui) + { + Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity(); + Eigen::Affine3d prev_pose_after_gismo = Eigen::Affine3d::Identity(); + + for (size_t i = 0; i < sessions.size(); i++) + { + // guizmo_all_sessions; + if (sessions[i].is_gizmo && !sessions[i].is_ground_truth) + { + if (sessions[i].point_clouds_container.point_clouds.size() > 0) + { + prev_pose_manipulated = sessions[i].point_clouds_container.point_clouds[0].m_pose; + std::vector all_m_poses; + for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + // if (all_m_poses.size() > 1) + //{ + ImGuiIO& io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + // std::cout << "3" << std::endl; + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(modelview, projection, ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + sessions[i].point_clouds_container.point_clouds[0].m_pose = + Eigen::Map(m_gizmo).cast(); + prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; + sessions[i].point_clouds_container.point_clouds[0].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); + + sessions[i].point_clouds_container.point_clouds[0].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; + sessions[i].point_clouds_container.point_clouds[0].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; + sessions[i].point_clouds_container.point_clouds[0].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; + + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + + Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; + for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + sessions[i].point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; + + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + //} + } + } + } + if (gizmo_all_sessions) + { + for (size_t i = 0; i < sessions.size(); i++) + { + // guizmo_all_sessions; + if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth) + { + std::vector all_m_poses; + for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose; + + Eigen::Affine3d m_new = prev_pose_after_gismo * m_rel_org; + + sessions[i].point_clouds_container.point_clouds[0].m_pose = m_new; + sessions[i].point_clouds_container.point_clouds[0].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); + + sessions[i].point_clouds_container.point_clouds[i].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; + sessions[i].point_clouds_container.point_clouds[i].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; + sessions[i].point_clouds_container.point_clouds[i].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; + + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + + Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; + for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + sessions[i].point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); + + sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; + + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + } + else + { + // ImGuizmo ----------------------------------------------- + if (edge_gizmo && edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g.matrix() = Eigen::Map(m_gizmo).cast(); + + const int& index_src = edges[index_active_edge].index_from; + + const Eigen::Affine3d& m_src = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } + } + + /*if (!is_loop_closure_gui) +{ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].gizmo) + { + std::vector all_m_poses; + for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); + + ImGuiIO &io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; + session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; + session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; + session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; + session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; + session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; + session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; + session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; + session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; + session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; + session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; + session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; + session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; + session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; + session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; + session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; + session.point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); + + session.point_clouds_container.point_clouds[i].gui_translation[0] = (float)session.point_clouds_container.point_clouds[i].pose.px; + session.point_clouds_container.point_clouds[i].gui_translation[1] = (float)session.point_clouds_container.point_clouds[i].pose.py; + session.point_clouds_container.point_clouds[i].gui_translation[2] = (float)session.point_clouds_container.point_clouds[i].pose.pz; + + session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); + + if (!manipulate_only_marked_gizmo) + { + Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; + for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + session.point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); + + session.point_clouds_container.point_clouds[j].gui_translation[0] = (float)session.point_clouds_container.point_clouds[j].pose.px; + session.point_clouds_container.point_clouds[j].gui_translation[1] = (float)session.point_clouds_container.point_clouds[j].pose.py; + session.point_clouds_container.point_clouds[j].gui_translation[2] = (float)session.point_clouds_container.point_clouds[j].pose.pz; + + session.point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + + session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); + observation_picking.render(); + + glPushAttrib(GL_ALL_ATTRIB_BITS); + glPointSize(5); + for (const auto &obs : observation_picking.observations) + { + for (const auto &[key1, value1] : obs) + { + for (const auto &[key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + if (session.point_clouds_container.show_with_initial_pose) + { + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + } + else + { + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + } + glColor3f(0, 1, 0); + glBegin(GL_POINTS); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + glColor3f(1, 0, 0); + glBegin(GL_LINES); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + } + } + } + } + glPopAttrib(); + + for (const auto &obs : observation_picking.observations) + { + Eigen::Vector3d mean(0, 0, 0); + int counter = 0; + for (const auto &[key1, value1] : obs) + { + mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + counter++; + } + if (counter > 0) + { + mean /= counter; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glEnd();F + } + } + + glColor3f(1, 0, 1); + glBegin(GL_POINTS); + for (auto p : picked_points) + { + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); +} +else +{ + // ImGuizmo ----------------------------------------------- + if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - // gnss.render(session.point_clouds_container); + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g(0, 0) = m_gizmo[0]; + m_g(1, 0) = m_gizmo[1]; + m_g(2, 0) = m_gizmo[2]; + m_g(3, 0) = m_gizmo[3]; + m_g(0, 1) = m_gizmo[4]; + m_g(1, 1) = m_gizmo[5]; + m_g(2, 1) = m_gizmo[6]; + m_g(3, 1) = m_gizmo[7]; + m_g(0, 2) = m_gizmo[8]; + m_g(1, 2) = m_gizmo[9]; + m_g(2, 2) = m_gizmo[10]; + m_g(3, 2) = m_gizmo[11]; + m_g(0, 3) = m_gizmo[12]; + m_g(1, 3) = m_gizmo[13]; + m_g(2, 3) = m_gizmo[14]; + m_g(3, 3) = m_gizmo[15]; + + const int &index_src = session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; + + const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; + session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } +}*/ ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); @@ -3952,9 +4278,8 @@ void display() ImGui::SameLine(); if (viewer_decimate_point_cloud < 1) - { viewer_decimate_point_cloud = 1; - } + ImGui::SameLine(); ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); } @@ -3972,9 +4297,8 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - { info_gui = !info_gui; - } + ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -4041,10 +4365,18 @@ void display() ImGui::EndPopup(); } - info_window(infoLines, appShortcuts, &info_gui); + if (is_ndt_gui) + ndt_gui(); + + if (is_loop_closure_gui) + loop_closure_gui(); + + cor_window(); + + info_window(infoLines, appShortcuts); if (compass_ruler) - drawMiniCompassWithRuler(viewLocal, fabs(translate_z), bg_color); + drawMiniCompassWithRuler(); // my_display_code(); /*if (is_ndt_gui) @@ -4058,367 +4390,9 @@ void display() if (is_manual_analisys) observation_picking_gui();*/ // if (is_loop_closure_gui) - //{ // manual_pose_graph_loop_closure.Gui(); - // } - project_gui(); - - if (!is_loop_closure_gui) - { - Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity(); - Eigen::Affine3d prev_pose_after_gismo = Eigen::Affine3d::Identity(); - - for (size_t i = 0; i < sessions.size(); i++) - { - // guizmo_all_sessions; - if (sessions[i].is_gizmo && !sessions[i].is_ground_truth) - { - if (sessions[i].point_clouds_container.point_clouds.size() > 0) - { - prev_pose_manipulated = sessions[i].point_clouds_container.point_clouds[0].m_pose; - std::vector all_m_poses; - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - // if (all_m_poses.size() > 1) - //{ - ImGuiIO &io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - // std::cout << "3" << std::endl; - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(modelview, projection, ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - sessions[i].point_clouds_container.point_clouds[0].m_pose = - Eigen::Map(m_gizmo).cast(); - prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; - sessions[i].point_clouds_container.point_clouds[0].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); - - sessions[i].point_clouds_container.point_clouds[0].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; - sessions[i].point_clouds_container.point_clouds[0].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; - sessions[i].point_clouds_container.point_clouds[0].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; - - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); - - Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - sessions[i].point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; - - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - //} - } - } - } - if (gizmo_all_sessions) - { - for (size_t i = 0; i < sessions.size(); i++) - { - // guizmo_all_sessions; - if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth) - { - std::vector all_m_poses; - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose; - - Eigen::Affine3d m_new = prev_pose_after_gismo * m_rel_org; - - sessions[i].point_clouds_container.point_clouds[0].m_pose = m_new; - sessions[i].point_clouds_container.point_clouds[0].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); - - sessions[i].point_clouds_container.point_clouds[i].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; - sessions[i].point_clouds_container.point_clouds[i].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; - sessions[i].point_clouds_container.point_clouds[i].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; - - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); - - Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (int j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - sessions[i].point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; - - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - } - } - } - } - else - { - // ImGuizmo ----------------------------------------------- - if (edge_gizmo && edges.size() > 0) - { - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); - - m_g.matrix() = Eigen::Map(m_gizmo).cast(); - - const int &index_src = edges[index_active_edge].index_from; - - const Eigen::Affine3d &m_src = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); - } - } - - if (is_ndt_gui) - ndt_gui(); - - if (is_loop_closure_gui) - loop_closure_gui(); - - /*if (!is_loop_closure_gui) - { - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - if (session.point_clouds_container.point_clouds[i].gizmo) - { - std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - { - all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); - } - - ImGuiIO &io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - { - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - } - - session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; - session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; - session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; - session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; - session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; - session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; - session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; - session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; - session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; - session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; - session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; - session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; - session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; - session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; - session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; - session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; - session.point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); - - session.point_clouds_container.point_clouds[i].gui_translation[0] = (float)session.point_clouds_container.point_clouds[i].pose.px; - session.point_clouds_container.point_clouds[i].gui_translation[1] = (float)session.point_clouds_container.point_clouds[i].pose.py; - session.point_clouds_container.point_clouds[i].gui_translation[2] = (float)session.point_clouds_container.point_clouds[i].pose.pz; - - session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om * RAD_TO_DEG); - session.point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); - - if (!manipulate_only_marked_gizmo) - { - Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - session.point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); - - session.point_clouds_container.point_clouds[j].gui_translation[0] = (float)session.point_clouds_container.point_clouds[j].pose.px; - session.point_clouds_container.point_clouds[j].gui_translation[1] = (float)session.point_clouds_container.point_clouds[j].pose.py; - session.point_clouds_container.point_clouds[j].gui_translation[2] = (float)session.point_clouds_container.point_clouds[j].pose.pz; - - session.point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - } - } - } - - session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); - observation_picking.render(); - - glPushAttrib(GL_ALL_ATTRIB_BITS); - glPointSize(5); - for (const auto &obs : observation_picking.observations) - { - for (const auto &[key1, value1] : obs) - { - for (const auto &[key2, value2] : obs) - { - if (key1 != key2) - { - Eigen::Vector3d p1, p2; - if (session.point_clouds_container.show_with_initial_pose) - { - p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; - } - else - { - p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; - } - glColor3f(0, 1, 0); - glBegin(GL_POINTS); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - glColor3f(1, 0, 0); - glBegin(GL_LINES); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - } - } - } - } - glPopAttrib(); - - for (const auto &obs : observation_picking.observations) - { - Eigen::Vector3d mean(0, 0, 0); - int counter = 0; - for (const auto &[key1, value1] : obs) - { - mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - counter++; - } - if (counter > 0) - { - mean /= counter; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glEnd();F - } - } - glColor3f(1, 0, 1); - glBegin(GL_POINTS); - for (auto p : picked_points) - { - glVertex3f(p.x(), p.y(), p.z()); - } - glEnd(); - } - else - { - // ImGuizmo ----------------------------------------------- - if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) - { - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - { - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - } - - Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); - - m_g(0, 0) = m_gizmo[0]; - m_g(1, 0) = m_gizmo[1]; - m_g(2, 0) = m_gizmo[2]; - m_g(3, 0) = m_gizmo[3]; - m_g(0, 1) = m_gizmo[4]; - m_g(1, 1) = m_gizmo[5]; - m_g(2, 1) = m_gizmo[6]; - m_g(3, 1) = m_gizmo[7]; - m_g(0, 2) = m_gizmo[8]; - m_g(1, 2) = m_gizmo[9]; - m_g(2, 2) = m_gizmo[10]; - m_g(3, 2) = m_gizmo[11]; - m_g(0, 3) = m_gizmo[12]; - m_g(1, 3) = m_gizmo[13]; - m_g(2, 3) = m_gizmo[14]; - m_g(3, 3) = m_gizmo[15]; - - const int &index_src = session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; - - const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; - session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); - } - }*/ + project_gui(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); 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 a21328c7..9a649f88 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 @@ -230,8 +230,6 @@ bool session_loaded = false; int num_edge_extended_before = 0; int num_edge_extended_after = 0; -ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); - void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session); void reshape(int w, int h); @@ -446,22 +444,19 @@ void icp_gui() tls_registration.icp.is_rodrigues = (ICP3dSelection == 2); if (ImGui::Button("Optimization point to point source to target")) - { tls_registration.icp.optimization_point_to_point_source_to_target(session.point_clouds_container); - } + ImGui::Separator(); ImGui::Text("Optimization source to target Lie-algebra:"); ImGui::SameLine(); if (ImGui::Button("left Jacobian")) - { tls_registration.icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); - } + ImGui::SameLine(); if (ImGui::Button("right Jacobian")) - { tls_registration.icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); - } + ImGui::Separator(); if (ImGui::Button("Compute rms (optimization_point_to_point_source_to_target)")) @@ -822,7 +817,6 @@ void observation_picking_gui() observation_picking.point_size = 1; if (observation_picking.point_size > 20) observation_picking.point_size = 20; - ImGui::PopItemWidth(); if (ImGui::Button("Accept current observation")) { @@ -1101,7 +1095,7 @@ void lio_segments_gui() static double angle_diff = 5.0; - if (ImGui::Button("Set those that sattisfy acceptable angle")) + if (ImGui::Button("Set those that satisfy acceptable angle")) { for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) { @@ -1417,6 +1411,45 @@ void openSession() } } +void openLaz() +{ + session.point_clouds_container.point_clouds.clear(); + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load las/laz files", mandeye::fd::LAS_LAZ_filter, true); + if (input_file_names.size() > 0) + { + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + + std::cout << "Creating session from las/laz files:" << std::endl; + for (size_t i = 0; i < input_file_names.size(); i++) + std::cout << input_file_names[i] << std::endl; + + if (!session.point_clouds_container.load_whu_tls(input_file_names, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset)) + std::cout << "Problem creating! Check input files laz/las" << std::endl; + else + std::cout << "Loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + + session_loaded = true; + index_begin = 0; + index_end = session.point_clouds_container.point_clouds.size() - 1; + + std::string newTitle = winTitle + " - " + fs::path(input_file_names[0]).parent_path().string(); + glutSetWindowTitle(newTitle.c_str()); + + for (const auto& pc : session.point_clouds_container.point_clouds) + session_total_number_of_points += pc.points_local.size(); + + session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + + [[maybe_unused]] + pfd::message message( + "Information", + "If you can not see point cloud --> 1. Change 'Points render subsampling', 2. Check console 'min max coordinates should be small numbers to see points in our local coordinate system'. 3. Set checkbox 'calculate_offset for WHU-TLS'. 4. Later on You can change offset directly in session json file.", + pfd::choice::ok, pfd::icon::info); + message.result(); + } +} + void saveSession() { const auto output_file_name = mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); @@ -1494,6 +1527,8 @@ void saveSession() std::cerr << "Error copying poses file: " << e.what() << '\n'; } } + else + std::cout << "saving canceled" << std::endl; } void saveSubsession() @@ -1509,12 +1544,8 @@ void saveSubsession() } for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { if (session.point_clouds_container.point_clouds[i].visible) - { inx_end = i; - } - } // creating filename proposal based on current selection fs::path path(session_file_name); @@ -1550,9 +1581,9 @@ void saveSubsession() catch (const fs::filesystem_error& e) { std::cerr << "Error copying poses file: " << e.what() << '\n'; } - }else{ - std::cout << "saving canceled" << std::endl; } + else + std::cout << "saving canceled" << std::endl; } void project_gui() @@ -1636,6 +1667,8 @@ void project_gui() ImGui::SameLine(); ImGuiHyperlink("https://3d.bk.tudelft.nl/liangliang/publications/2019/plade/resso.html"); + ImGui::NewLine(); + if (ImGui::Button("Load ETH file (pairs.txt)")) { std::string input_file_name = ""; @@ -1657,40 +1690,21 @@ void project_gui() ImGui::SameLine(); ImGuiHyperlink("https://prs.igp.ethz.ch/research/completed_projects/automatic_registration_of_point_clouds.html"); - if (ImGui::Button("Load AlignedPointCloud from WHU-TLS (select all *.las or *.laz files in folder 2-AlignedPointCloud)")) - { - session.point_clouds_container.point_clouds.clear(); - std::vector input_file_names; - input_file_names = mandeye::fd::OpenFileDialog("Load las files", {}, true); - if (input_file_names.size() > 0) - { - session.working_directory = fs::path(input_file_names[0]).parent_path().string(); - - std::cout << "Las/Laz files:" << std::endl; - for (size_t i = 0; i < input_file_names.size(); i++) - std::cout << input_file_names[i] << std::endl; - - if (!session.point_clouds_container.load_whu_tls(input_file_names, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset)) - std::cout << "Check input files laz/las" << std::endl; - else - std::cout << "Loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; - } + ImGui::NewLine(); - session.point_clouds_container.print_point_cloud_dimension(); + if (ImGui::Button("Load aligned point cloud from WHU-TLS")) + openLaz(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select all *.las/*.laz files in folder 2-AlignedPointCloud"); - [[maybe_unused]] - pfd::message message( - "Information", - "If you can not see point cloud --> 1. Change 'Points render subsampling', 2. Check console 'min max coordinates should be small numbers to see points in our local coordinate system'. 3. Set checkbox 'calculate_offset for WHU-TLS'. 4. Later on You can change offset directly in session json file.", - pfd::choice::ok, pfd::icon::info); - message.result(); - } ImGui::SameLine(); ImGui::Checkbox("Calculate_offset for WHU-TLS", &tls_registration.calculate_offset); ImGui::Text("WHU-TLS dataset: "); ImGui::SameLine(); ImGuiHyperlink("http://3s.whu.edu.cn/ybs/en/benchmark.htm"); + ImGui::NewLine(); + if (ImGui::Button("Load 3DTK files (select all *.txt files)")) { session.point_clouds_container.point_clouds.clear(); @@ -1718,6 +1732,8 @@ void project_gui() ImGui::SameLine(); ImGuiHyperlink("http://kos.informatik.uni-osnabrueck.de/3Dscans/"); + ImGui::NewLine(); + if (ImGui::Button("Update initial poses from RESSO file")) { std::string input_file_name; @@ -1880,10 +1896,8 @@ void project_gui() ImGui::SetTooltip(zText); ImGui::PopItemWidth(); - if (ImGui::Button("Set offset_to_apply --> from session (offset_to_apply_x, offset_to_apply_y, offset_to_apply_z)")) - { + if (ImGui::Button("Set offset_to_apply --> from session (X, Y, Z)")) session.point_clouds_container.offset = session.point_clouds_container.offset_to_apply; - } if (!simple_gui) { @@ -1914,63 +1928,25 @@ void project_gui() void display() { - updateCameraTransition(); - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + glMatrixMode(GL_PROJECTION); glLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - - if (is_ortho) - { - 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); - - 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); - - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); - - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); - - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * DEG_TO_RAD; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - - 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()), - 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); - } + updateCameraTransition(); - glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); + viewLocal = Eigen::Affine3f::Identity(); if (!is_ortho) { reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - // janusz if (is_loop_closure_gui) { @@ -2010,29 +1986,54 @@ void display() } } - viewTranslation.translate(rotation_center); + viewLocal.translate(rotation_center); viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - if (!lock_z) viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); else viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - - viewTranslation2.translate(-rotation_center); + viewLocal.translate(-rotation_center); - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; - - glLoadMatrixf(result.matrix().data()); - /* glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ + glLoadMatrixf(viewLocal.matrix().data()); } else { + 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); + + 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); + + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); + + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * DEG_TO_RAD; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + + 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()), + 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); + glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } @@ -2051,62 +2052,302 @@ void display() session.ground_control_points.render(session.point_clouds_container); } - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); - - ShowMainDockSpace(); - - view_kbd_shortcuts(); + int prev_index_pose = session.control_points.index_pose; + if (session.control_points.is_imgui) + session.control_points.imgui(session.point_clouds_container, rotation_center); - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) + if (prev_index_pose != session.control_points.index_pose) { - is_pca_gui = !is_pca_gui; - - //workaround - io.AddKeyEvent(ImGuiKey_A, false); - io.AddKeyEvent(ImGuiMod_Ctrl, false); - } + session.control_points.index_picked_point = -1; // reset picked point when pose changes - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_C, false)) - { - session.control_points.is_imgui = !session.control_points.is_imgui; + new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); + new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); + new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); - //workaround - io.AddKeyEvent(ImGuiKey_C, false); - io.AddKeyEvent(ImGuiMod_Ctrl, false); + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + if (session.control_points.track_pose_with_camera) + { + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + } + else + { + new_translate_x = translate_x; + new_translate_y = translate_y; + } + new_translate_z = translate_z; + camera_transition_active = true; } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_E, false)) + if (session.ground_control_points.is_imgui) + session.ground_control_points.imgui(session.point_clouds_container); + + if (!session.control_points.is_imgui) { - is_lio_segments_gui = !is_lio_segments_gui; + if (!is_loop_closure_gui) + { + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].gizmo) + { + std::vector all_m_poses; + for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + { + all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); + } - //workaround - io.AddKeyEvent(ImGuiKey_E, false); - io.AddKeyEvent(ImGuiMod_Ctrl, false); - } + ImGuiIO& io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_G, false)) - { - session.ground_control_points.is_imgui = !session.ground_control_points.is_imgui; + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); - //workaround - io.AddKeyEvent(ImGuiKey_G, false); - io.AddKeyEvent(ImGuiMod_Ctrl, false); - } + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_L, false)) - { - is_loop_closure_gui = !is_loop_closure_gui; + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - //workaround - io.AddKeyEvent(ImGuiKey_L, false); - io.AddKeyEvent(ImGuiMod_Ctrl, false); - } + session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; + session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; + session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; + session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; + session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; + session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; + session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; + session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; + session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; + session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; + session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; + session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; + session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; + session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; + session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; + session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; + session.point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) - { - openSession(); + session.point_clouds_container.point_clouds[i].gui_translation[0] = (float)session.point_clouds_container.point_clouds[i].pose.px; + session.point_clouds_container.point_clouds[i].gui_translation[1] = (float)session.point_clouds_container.point_clouds[i].pose.py; + session.point_clouds_container.point_clouds[i].gui_translation[2] = (float)session.point_clouds_container.point_clouds[i].pose.pz; + + session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); + + if (!manipulate_only_marked_gizmo) + { + Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; + for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); + session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; + session.point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); + + session.point_clouds_container.point_clouds[j].gui_translation[0] = (float)session.point_clouds_container.point_clouds[j].pose.px; + session.point_clouds_container.point_clouds[j].gui_translation[1] = (float)session.point_clouds_container.point_clouds[j].pose.py; + session.point_clouds_container.point_clouds[j].gui_translation[2] = (float)session.point_clouds_container.point_clouds[j].pose.pz; + + session.point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + } + } + } + } + + 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; + + observation_picking.render(); + + glPushAttrib(GL_ALL_ATTRIB_BITS); + glPointSize(5); + for (const auto& obs : observation_picking.observations) + { + for (const auto& [key1, value1] : obs) + { + for (const auto& [key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + if (session.point_clouds_container.show_with_initial_pose) + { + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + } + else + { + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + } + glColor3f(0, 1, 0); + glBegin(GL_POINTS); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + glColor3f(1, 0, 0); + glBegin(GL_LINES); + glVertex3f(p1.x(), p1.y(), p1.z()); + glVertex3f(p2.x(), p2.y(), p2.z()); + glEnd(); + } + } + } + } + glPopAttrib(); + + for (const auto& obs : observation_picking.observations) + { + Eigen::Vector3d mean(0, 0, 0); + int counter = 0; + for (const auto& [key1, value1] : obs) + { + mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + counter++; + } + if (counter > 0) + { + mean /= counter; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + glEnd(); + } + } + + glColor3f(1, 0, 1); + glBegin(GL_POINTS); + for (auto p : picked_points) + { + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); + } + else + { + // ImGuizmo ----------------------------------------------- + if (session.pose_graph_loop_closure.gizmo && session.pose_graph_loop_closure.edges.size() > 0) + { + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + if (!is_ortho) + { + GLfloat projection[16]; + glGetFloatv(GL_PROJECTION_MATRIX, projection); + + GLfloat modelview[16]; + glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + + ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); + } + else + ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); + + Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); + + m_g(0, 0) = m_gizmo[0]; + m_g(1, 0) = m_gizmo[1]; + m_g(2, 0) = m_gizmo[2]; + m_g(3, 0) = m_gizmo[3]; + m_g(0, 1) = m_gizmo[4]; + m_g(1, 1) = m_gizmo[5]; + m_g(2, 1) = m_gizmo[6]; + m_g(3, 1) = m_gizmo[7]; + m_g(0, 2) = m_gizmo[8]; + m_g(1, 2) = m_gizmo[9]; + m_g(2, 2) = m_gizmo[10]; + m_g(3, 2) = m_gizmo[11]; + m_g(0, 3) = m_gizmo[12]; + m_g(1, 3) = m_gizmo[13]; + m_g(2, 3) = m_gizmo[14]; + m_g(3, 3) = m_gizmo[15]; + + const int& index_src = session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from; + + const Eigen::Affine3d& m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; + session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); + } + } + } + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + + view_kbd_shortcuts(); + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) + { + is_pca_gui = !is_pca_gui; + + //workaround + io.AddKeyEvent(ImGuiKey_A, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_C, false)) + { + session.control_points.is_imgui = !session.control_points.is_imgui; + + //workaround + io.AddKeyEvent(ImGuiKey_C, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_E, false)) + { + is_lio_segments_gui = !is_lio_segments_gui; + + //workaround + io.AddKeyEvent(ImGuiKey_E, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_G, false)) + { + session.ground_control_points.is_imgui = !session.ground_control_points.is_imgui; + + //workaround + io.AddKeyEvent(ImGuiKey_G, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_L, false)) + { + is_loop_closure_gui = !is_loop_closure_gui; + + //workaround + io.AddKeyEvent(ImGuiKey_L, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) + { + openSession(); //workaround io.AddKeyEvent(ImGuiKey_O, false); @@ -2154,11 +2395,31 @@ void display() { if (!session_loaded) { - if (ImGui::Button("Load session")) + if (ImGui::Button("Open session")) openSession(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Select session to open (Ctrl+O)"); + ImGui::SameLine(); + + if (ImGui::ArrowButton("##menuArrow", ImGuiDir_Down)) + { + ImGui::OpenPopup("OpenMenu"); + } + + if (ImGui::BeginPopup("OpenMenu")) + { + if (ImGui::MenuItem("Open las/laz")) + openLaz(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Create session from las/laz files"); + + + ImGui::MenuItem("Calculate_offset", nullptr, &tls_registration.calculate_offset); + + ImGui::EndPopup(); + } + ImGui::SameLine(); ImGui::Dummy(ImVec2(20, 0)); ImGui::SameLine(); @@ -2571,7 +2832,7 @@ void display() } ImGui::Separator(); ImGui::MenuItem("Pose Graph SLAM", "Ctrl+P", &is_pose_graph_slam); - ImGui::MenuItem("Manual Analysis", nullptr, &is_manual_analisys); + ImGui::MenuItem("Observations", nullptr, &is_manual_analisys); ImGui::Separator(); @@ -2828,9 +3089,8 @@ void display() //ImGui::Text("(avg %.1f)", fps_avg); if (viewer_decimate_point_cloud < 1) - { viewer_decimate_point_cloud = 1; - } + ImGui::SameLine(); ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); } @@ -2845,9 +3105,8 @@ void display() ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); if (ImGui::SmallButton("Info")) - { info_gui = !info_gui; - } + ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); @@ -2874,250 +3133,12 @@ void display() if (is_lio_segments_gui) lio_segments_gui(); - int prev_index_pose = session.control_points.index_pose; - if (session.control_points.is_imgui) - session.control_points.imgui(session.point_clouds_container, rotation_center); - - if (prev_index_pose != session.control_points.index_pose) - { - session.control_points.index_picked_point = -1; // reset picked point when pose changes - - new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); - new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); - new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - if (session.control_points.track_pose_with_camera) - { - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - } - else - { - new_translate_x = translate_x; - new_translate_y = translate_y; - } - new_translate_z = translate_z; - camera_transition_active = true; - } - - if (session.ground_control_points.is_imgui) - session.ground_control_points.imgui(session.point_clouds_container); + cor_window(); - info_window(infoLines, appShortcuts, &info_gui); + info_window(infoLines, appShortcuts); if (compass_ruler) - drawMiniCompassWithRuler(viewLocal, fabs(translate_z), bg_color); - - if (!session.control_points.is_imgui) - { - if (!is_loop_closure_gui) - { - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - if (session.point_clouds_container.point_clouds[i].gizmo) - { - std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - { - all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); - } - - ImGuiIO &io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; - session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; - session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; - session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; - session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; - session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; - session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; - session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; - session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; - session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; - session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; - session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; - session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; - session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; - session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; - session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; - session.point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); - - session.point_clouds_container.point_clouds[i].gui_translation[0] = (float)session.point_clouds_container.point_clouds[i].pose.px; - session.point_clouds_container.point_clouds[i].gui_translation[1] = (float)session.point_clouds_container.point_clouds[i].pose.py; - session.point_clouds_container.point_clouds[i].gui_translation[2] = (float)session.point_clouds_container.point_clouds[i].pose.pz; - - session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om * RAD_TO_DEG); - session.point_clouds_container.point_clouds[i].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[i].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); - - if (!manipulate_only_marked_gizmo) - { - Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (int j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - session.point_clouds_container.point_clouds[j].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); - - session.point_clouds_container.point_clouds[j].gui_translation[0] = (float)session.point_clouds_container.point_clouds[j].pose.px; - session.point_clouds_container.point_clouds[j].gui_translation[1] = (float)session.point_clouds_container.point_clouds[j].pose.py; - session.point_clouds_container.point_clouds[j].gui_translation[2] = (float)session.point_clouds_container.point_clouds[j].pose.pz; - - session.point_clouds_container.point_clouds[j].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - } - } - } - - 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; - - observation_picking.render(); - - glPushAttrib(GL_ALL_ATTRIB_BITS); - glPointSize(5); - for (const auto &obs : observation_picking.observations) - { - for (const auto &[key1, value1] : obs) - { - for (const auto &[key2, value2] : obs) - { - if (key1 != key2) - { - Eigen::Vector3d p1, p2; - if (session.point_clouds_container.show_with_initial_pose) - { - p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; - } - else - { - p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; - } - glColor3f(0, 1, 0); - glBegin(GL_POINTS); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - glColor3f(1, 0, 0); - glBegin(GL_LINES); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - } - } - } - } - glPopAttrib(); - - for (const auto &obs : observation_picking.observations) - { - Eigen::Vector3d mean(0, 0, 0); - int counter = 0; - for (const auto &[key1, value1] : obs) - { - mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - counter++; - } - if (counter > 0) - { - mean /= counter; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glEnd(); - } - } - - glColor3f(1, 0, 1); - glBegin(GL_POINTS); - for (auto p : picked_points) - { - glVertex3f(p.x(), p.y(), p.z()); - } - glEnd(); - } - else - { - // ImGuizmo ----------------------------------------------- - if (session.pose_graph_loop_closure.gizmo && session.pose_graph_loop_closure.edges.size() > 0) - { - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); - - m_g(0, 0) = m_gizmo[0]; - m_g(1, 0) = m_gizmo[1]; - m_g(2, 0) = m_gizmo[2]; - m_g(3, 0) = m_gizmo[3]; - m_g(0, 1) = m_gizmo[4]; - m_g(1, 1) = m_gizmo[5]; - m_g(2, 1) = m_gizmo[6]; - m_g(3, 1) = m_gizmo[7]; - m_g(0, 2) = m_gizmo[8]; - m_g(1, 2) = m_gizmo[9]; - m_g(2, 2) = m_gizmo[10]; - m_g(3, 2) = m_gizmo[11]; - m_g(0, 3) = m_gizmo[12]; - m_g(1, 3) = m_gizmo[13]; - m_g(2, 3) = m_gizmo[14]; - m_g(3, 3) = m_gizmo[15]; - - const int &index_src = session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from; - - const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; - session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); - } - } - } + drawMiniCompassWithRuler(); ImGui::Render(); ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); diff --git a/core/include/utils.hpp b/core/include/utils.hpp index 4b4d4c82..3b3e209c 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -11,6 +11,8 @@ const float DEG_TO_RAD = M_PI / 180.0f; const float RAD_TO_DEG = 180.0f / M_PI; +const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); + const std::string out_fn = "Output file name"; constexpr float ImGuiNumberWidth = 120.0f; @@ -55,6 +57,8 @@ extern int point_size; extern bool info_gui; extern bool compass_ruler; +extern Eigen::Affine3f viewLocal; + extern Eigen::Vector3f rotation_center; extern float rotate_x, rotate_y; extern float translate_x, translate_y; @@ -100,15 +104,12 @@ void breakCameraTransition(); void setCameraPreset(CameraPreset preset); void camMenu(); void view_kbd_shortcuts(); +void cor_window(); void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); -void info_window(const std::vector& infoLines, const std::vector& appShortcuts, bool* open); +void info_window(const std::vector& infoLines, const std::vector& appShortcuts); -void drawMiniCompassWithRuler( - const Eigen::Affine3f& viewLocal, - float translate_z, - const ImVec4& bg_color, - ImVec2 compassSize = ImVec2(200, 200)); +void drawMiniCompassWithRuler(); Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); LaserBeam GetLaserBeam(int x, int y); diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 617051ca..12d6e12a 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -35,6 +35,8 @@ int point_size = 1; bool info_gui = false; bool compass_ruler = true; +Eigen::Affine3f viewLocal; + Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); float rotate_x = 0.0, rotate_y = 0.0; float translate_x, translate_y = 0.0; @@ -403,7 +405,7 @@ void ShowMainDockSpace() auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); - ImGui::DockBuilderDockWindow("Settings", dock_id_left); + //ImGui::DockBuilderDockWindow("Settings", dock_id_left); ImGui::DockBuilderDockWindow("Console", dock_id_bottom); ImGui::DockBuilderFinish(dockspace_id); } @@ -842,6 +844,59 @@ void view_kbd_shortcuts() +void cor_window() +{ + //temporary location for new rotation center (minimal impact change in all apps) + if (cor_gui) + { + ImGui::OpenPopup("Center of rotation"); + cor_gui = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; + + ImGui::CloseCurrentPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } +} + + + void ImGuiHyperlink(const char* url, ImVec4 color) { ImGui::PushStyleColor(ImGuiCol_Text, color); @@ -937,59 +992,11 @@ void ShowShortcutsTable(const std::vector appShortcuts) -void info_window(const std::vector& infoLines, const std::vector& appShortcuts, bool* open) +void info_window(const std::vector& infoLines, const std::vector& appShortcuts) { - //temporary location for new rotation center (minimal impact change in all apps) - if (cor_gui) - { - ImGui::OpenPopup("Center of rotation"); - cor_gui = false; - } + if (!info_gui) return; - if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) - { - ImGui::Text("Select new center of rotation [m]:"); - ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(xText); - ImGui::SameLine(); - ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(yText); - ImGui::SameLine(); - ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(zText); - ImGui::PopItemWidth(); - - ImGui::Separator(); - - if (ImGui::Button("Set")) - { - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - - camera_transition_active = true; - - ImGui::CloseCurrentPopup(); - } - - ImGui::SameLine(); - if (ImGui::Button("Cancel")) - { - ImGui::CloseCurrentPopup(); - } - - ImGui::EndPopup(); - } - - if (!open || !*open) return; - - if (ImGui::Begin("Info", open, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking)) + if (ImGui::Begin("Info", &info_gui, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) { bool firstLine = true; for (const auto& line : infoLines) @@ -1041,19 +1048,17 @@ void info_window(const std::vector& infoLines, const std::vector