diff --git a/CMakeLists.txt b/CMakeLists.txt index b4147b5..6d72674 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.15.0) project(hd-mapping) set (HDMAPPING_VERSION_MAJOR 0) -set (HDMAPPING_VERSION_MINOR 68) +set (HDMAPPING_VERSION_MINOR 69) set (HDMAPPING_VERSION_PATCH 0) add_definitions(-DHDMAPPING_VERSION_MAJOR=${HDMAPPING_VERSION_MAJOR}) @@ -104,6 +104,7 @@ add_subdirectory(apps/mandeye_raw_data_viewer) add_subdirectory(apps/compare_trajectories) add_subdirectory(apps/quick_start_demo) add_subdirectory(apps/mandeye_mission_recorder_calibration) +add_subdirectory(apps/mandeye_single_session_viewer) # CPack configuration set(CPACK_PACKAGE_NAME "hd_mapping") diff --git a/apps/mandeye_single_session_viewer/CMakeLists.txt b/apps/mandeye_single_session_viewer/CMakeLists.txt new file mode 100644 index 0000000..5107c6d --- /dev/null +++ b/apps/mandeye_single_session_viewer/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.15.0) + +project(mandeye_raw_data_viewer) + +add_executable( + mandeye_single_session_viewer + mandeye_single_session_viewer.cpp + ../lidar_odometry_step_1/lidar_odometry_utils.cpp +) + +target_include_directories( + mandeye_single_session_viewer + PRIVATE include + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/core_hd_mapping/include + ${EXTERNAL_LIBRARIES_DIRECTORY} + ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui + ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends + ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo + ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-2.2.0/include + ${FREEGLUT_INCLUDE_DIR} + ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + mandeye_single_session_viewer + PRIVATE Fusion + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + OpenGL::GLU + ${PLATFORM_LASZIP_LIB} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES} + ${PLATFORM_MISCELLANEOUS_LIBS}) + +if(WIN32) + add_custom_command( + TARGET mandeye_single_session_viewer + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy $ + $ + COMMAND_EXPAND_LISTS) +endif() + +install(TARGETS mandeye_single_session_viewer DESTINATION bin) \ No newline at end of file diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp new file mode 100644 index 0000000..f70d88e --- /dev/null +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -0,0 +1,524 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include "../lidar_odometry_step_1/lidar_odometry_utils.h" + +#include + +#include + +#define SAMPLE_PERIOD (1.0 / 200.0) +namespace fs = std::filesystem; + +const unsigned int window_width = 800; +const unsigned int window_height = 600; +double camera_ortho_xy_view_zoom = 10; +double camera_ortho_xy_view_shift_x = 0.0; +double camera_ortho_xy_view_shift_y = 0.0; +double camera_mode_ortho_z_center_h = 0.0; +double camera_ortho_xy_view_rotation_angle_deg = 0; +bool is_ortho = false; +bool show_axes = true; +ImVec4 clear_color = ImVec4(0.8f, 0.8f, 0.8f, 1.00f); +ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); +ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.0f); + +int point_size = 1; +Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); +float translate_x, translate_y = 0.0; +float translate_z = -20.0; +float rotate_x = 0.0, rotate_y = 0.0; +int mouse_old_x, mouse_old_y; +int mouse_buttons = 0; +bool gui_mouse_down{false}; +float mouse_sensitivity = 1.0; + +float m_ortho_projection[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; + +float m_ortho_gizmo_view[] = {1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; + +int index_rendered_points_local = -1; +float offset_intensity = 0.0; +bool show_neighbouring_scans = false; +const std::vector + Session_filter = {"Session, json", "*.json"}; + +Session session; + +void reshape(int w, int h) +{ + glViewport(0, 0, (GLsizei)w, (GLsizei)h); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + if (!is_ortho) + { + gluPerspective(60.0, (GLfloat)w / (GLfloat)h, 0.01, 10000.0); + } + else + { + ImGuiIO &io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + 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); + // glOrtho(-translate_z, translate_z, -translate_z * (float)h / float(w), translate_z * float(h) / float(w), -10000, 10000); + } + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); +} + +void motion(int x, int y) +{ + ImGuiIO &io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + if (!io.WantCaptureMouse) + { + float dx, dy; + dx = (float)(x - mouse_old_x); + dy = (float)(y - mouse_old_y); + + if (is_ortho) + { + if (mouse_buttons & 1) + { + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + Eigen::Vector3d v(dx * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.x * 2), + dy * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.y * 2 / ratio), 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 * M_PI / 180.0; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + Eigen::Vector3d v_t = m * v; + camera_ortho_xy_view_shift_x += v_t.x(); + camera_ortho_xy_view_shift_y += v_t.y(); + } + } + else + { + gui_mouse_down = mouse_buttons > 0; + if (mouse_buttons & 1) + { + rotate_x += dy * 0.2f; // * mouse_sensitivity; + rotate_y += dx * 0.2f; // * mouse_sensitivity; + } + if (mouse_buttons & 4) + { + translate_x += dx * 0.05f * mouse_sensitivity; + translate_y -= dy * 0.05f * mouse_sensitivity; + } + } + + mouse_old_x = x; + mouse_old_y = y; + } + glutPostRedisplay(); +} + +void project_gui() +{ + if (ImGui::Begin("main gui window")) + { + ImGui::ColorEdit3("clear color", (float *)&clear_color); + + ImGui::InputInt("point_size", &point_size); + if (point_size < 1) + { + point_size = 1; + } + + if (ImGui::Button("load session")) + { + static std::shared_ptr open_file; + std::string input_file_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)open_file); + const auto t = [&]() + { + auto sel = pfd::open_file("Load session", "C:\\", Session_filter).result(); + for (int i = 0; i < sel.size(); i++) + { + input_file_name = sel[i]; + std::cout << "Session file: '" << input_file_name << "'" << std::endl; + } + }; + std::thread t1(t); + t1.join(); + + if (input_file_name.size() > 0) + { + session.load(fs::path(input_file_name).string(), false, 0.0, 0.0, 0.0, false); + } + } + + if (session.point_clouds_container.point_clouds.size() > 0) + { + ImGui::InputFloat("offset_intensity", &offset_intensity, 0.01, 0.1); + if (offset_intensity < 0){ + offset_intensity = 0; + } + if (offset_intensity > 1){ + offset_intensity = 1; + } + + ImGui::Checkbox("show_neighbouring_scans", &show_neighbouring_scans); + + if (show_neighbouring_scans) + { + ImGui::ColorEdit3("pc_neigbouring_color", (float *)&pc_neigbouring_color); + } + + ImGui::Text("----------- navigate with index_rendered_points_local ---------"); + + ImGui::InputInt("index_rendered_points_local", &index_rendered_points_local, 1, 10); + if (index_rendered_points_local < 0) + { + index_rendered_points_local = 0; + } + if (index_rendered_points_local >= session.point_clouds_container.point_clouds.size() - 1) + { + index_rendered_points_local = session.point_clouds_container.point_clouds.size() - 1; + } + + ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); + } + + ImGui::End(); + } + return; +} + +void display() +{ + 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) + { + + 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 * M_PI / 180.0; + 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); + } + + glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + + 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)); + viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_y / 180.f, 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(); + } + + if (ImGui::GetIO().KeyCtrl) + { + glBegin(GL_LINES); + glColor3f(1.f, 1.f, 1.f); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); + glVertex3fv(rotation_center.data()); + glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); + glEnd(); + } + + if (show_axes) + { + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(1, 0.0f, 0.0f); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 1, 0.0f); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 1); + glEnd(); + } + + if (index_rendered_points_local >= 0 && index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) + { + double max_intensity = 0.0; + for (int i = 0; i < session.point_clouds_container.point_clouds[index_rendered_points_local].intensities.size(); i++) + { + if (session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] > max_intensity) + { + max_intensity = session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i]; + } + } + + Eigen::Affine3d pose = session.point_clouds_container.point_clouds[index_rendered_points_local].m_pose; + pose(0, 3) = 0.0; + pose(1, 3) = 0.0; + pose(2, 3) = 0.0; + + glBegin(GL_POINTS); + for (int i = 0; i < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size(); i++) + { + glColor3f(session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] / max_intensity + offset_intensity, 0.0, 1.0 - session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] / max_intensity + offset_intensity); + + Eigen::Vector3d p(session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].x(), + session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].y(), + session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].z()); + p = pose * p; + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); + + if (show_neighbouring_scans){ + //pc_neigbouring_color + + glColor3f(pc_neigbouring_color.x, pc_neigbouring_color.y, pc_neigbouring_color.z); + + 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){ + if (index >= 0 && index < session.point_clouds_container.point_clouds.size()){ + Eigen::Affine3d pose = session.point_clouds_container.point_clouds[index].m_pose; + Eigen::Affine3d pose_offset = session.point_clouds_container.point_clouds[index_rendered_points_local].m_pose; + + pose(0, 3) -= pose_offset(0, 3); + pose(1, 3) -= pose_offset(1, 3); + pose(2, 3) -= pose_offset(2, 3); + + for (int i = 0; i < session.point_clouds_container.point_clouds[index].points_local.size(); i++) + { + Eigen::Vector3d p(session.point_clouds_container.point_clouds[index].points_local[i].x(), + session.point_clouds_container.point_clouds[index].points_local[i].y(), + session.point_clouds_container.point_clouds[index].points_local[i].z()); + p = pose * p; + glVertex3f(p.x(), p.y(), p.z()); + } + } + } + } + glEnd(); + } + } + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + + project_gui(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +bool initGL(int *argc, char **argv) +{ + glutInit(argc, argv); + glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); + glutInitWindowSize(window_width, window_height); + glutCreateWindow("mandeye single session viewer " HDMAPPING_VERSION_STRING); + glutDisplayFunc(display); + glutMotionFunc(motion); + + // default initialization + glClearColor(0.0, 0.0, 0.0, 1.0); + glEnable(GL_DEPTH_TEST); + + // viewport + glViewport(0, 0, window_width, window_height); + + // projection + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); + glutReshapeFunc(reshape); + ImGui::CreateContext(); + ImGuiIO &io = ImGui::GetIO(); + (void)io; + // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls + + ImGui::StyleColorsDark(); + ImGui_ImplGLUT_Init(); + ImGui_ImplGLUT_InstallFuncs(); + ImGui_ImplOpenGL2_Init(); + return true; +} + +void wheel(int button, int dir, int x, int y); + +void mouse(int glut_button, int state, int x, int y) +{ + ImGuiIO &io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + int button = -1; + if (glut_button == GLUT_LEFT_BUTTON) + button = 0; + if (glut_button == GLUT_RIGHT_BUTTON) + button = 1; + if (glut_button == GLUT_MIDDLE_BUTTON) + button = 2; + if (button != -1 && state == GLUT_DOWN) + io.MouseDown[button] = true; + if (button != -1 && state == GLUT_UP) + io.MouseDown[button] = false; + + static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && + glutMajorVersion < 3) + { + wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + } + + if (!io.WantCaptureMouse) + { + if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) + { + } + + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + } + else if (state == GLUT_UP) + { + mouse_buttons = 0; + } + mouse_old_x = x; + mouse_old_y = y; + } +} + +void wheel(int button, int dir, int x, int y) +{ + if (dir > 0) + { + if (is_ortho) + { + camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; + + if (camera_ortho_xy_view_zoom < 0.1) + { + camera_ortho_xy_view_zoom = 0.1; + } + } + else + { + translate_z -= 0.05f * translate_z; + } + } + else + { + if (is_ortho) + { + camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + } + else + { + translate_z += 0.05f * translate_z; + } + } + + return; +} + +int main(int argc, char *argv[]) +{ + initGL(&argc, argv); + glutDisplayFunc(display); + glutMouseFunc(mouse); + glutMotionFunc(motion); + glutMouseWheelFunc(wheel); + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + + ImGui::DestroyContext(); + return 0; +} \ No newline at end of file