From 7c667fce790efe905c8f825246c8e3ad417d9890 Mon Sep 17 00:00:00 2001 From: mwlasiuk Date: Thu, 15 Jan 2026 19:45:05 +0100 Subject: [PATCH] Format codebase + move external files to 3rdparty + add PCH --- CMakeLists.txt | 45 +- apps/compare_trajectories/CMakeLists.txt | 20 +- .../mandeye_compare_trajectories.cpp | 269 +- apps/concatenate_multi_livox/CMakeLists.txt | 20 +- .../concatenate_multi_livox.cpp | 58 +- apps/console_tools/matrix_mul.cpp | 6 +- ...ply_timestamps_session_point_cloud_laz.cpp | 33 +- ...iply_timestamps_session_trajectory_csv.cpp | 50 +- apps/hd_mapper/CMakeLists.txt | 14 +- apps/hd_mapper/hd_mapper.cpp | 179 +- apps/laz_to_ply/main.cpp | 26 +- apps/lidar_odometry_step_1/CMakeLists.txt | 24 +- .../enhanced_version_example.cpp | 102 +- apps/lidar_odometry_step_1/lidar_odometry.cpp | 264 +- apps/lidar_odometry_step_1/lidar_odometry.h | 46 +- .../lidar_odometry_gui.cpp | 451 +- .../lidar_odometry_utils.cpp | 202 +- .../lidar_odometry_utils.h | 262 +- .../lidar_odometry_utils_optimizers.cpp | 1435 ++-- apps/lidar_odometry_step_1/toml_io.cpp | 169 +- apps/lidar_odometry_step_1/toml_io.h | 238 +- .../lidar_odometry_step_1/version_example.cpp | 93 +- .../CMakeLists.txt | 20 +- .../livox_mid_360_intrinsic_calibration.cpp | 238 +- .../CMakeLists.txt | 20 +- .../mandeye_mission_recorder_calibration.cpp | 118 +- apps/mandeye_raw_data_viewer/CMakeLists.txt | 22 +- .../mandeye_raw_data_viewer.cpp | 378 +- .../CMakeLists.txt | 20 +- .../mandeye_single_session_viewer.cpp | 963 ++- apps/manual_color/CMakeLists.txt | 18 +- apps/manual_color/manual_color.cpp | 555 +- .../multi_session_registration/CMakeLists.txt | 22 +- .../multi_session_factor_graph.cpp | 322 +- .../multi_session_factor_graph.h | 7 +- .../multi_session_registration.cpp | 1088 ++- .../CMakeLists.txt | 23 +- .../multi_view_tls_registration.cpp | 1556 ++-- .../multi_view_tls_registration.h | 300 +- .../multi_view_tls_registration_gui.cpp | 1054 ++- .../perform_experiment.cpp | 144 +- apps/precision_forestry_tools/CMakeLists.txt | 14 +- .../precision_forestry_tools.cpp | 1064 +-- apps/quick_start_demo/CMakeLists.txt | 20 +- apps/quick_start_demo/quick_start_demo.cpp | 9 +- .../CMakeLists.txt | 18 +- .../single_session_manual_coloring.cpp | 221 +- apps/split_multi_livox/CMakeLists.txt | 18 +- apps/split_multi_livox/split_multi_livox.cpp | 41 +- cmake/imgui.cmake | 4 +- cmake/imguizmo.cmake | 4 +- cmake/implot.cmake | 4 +- core/CMakeLists.txt | 118 +- core/include/GL_assert.h | 27 +- core/include/color_las_loader.h | 24 +- core/include/control_points.h | 5 +- core/include/export_laz.h | 85 +- core/include/gnss.h | 43 +- core/include/ground_control_points.h | 51 +- core/include/hash_utils.h | 5 +- core/include/icp.h | 152 +- core/include/local_shape_features.h | 16 +- core/include/m_estimators.h | 367 +- core/include/manual_pose_graph_loop_closure.h | 36 +- core/include/ndt.h | 350 +- core/include/nmea.h | 89 +- core/include/nns.h | 10 - core/include/observation_picking.h | 218 +- .../pair_wise_iterative_closest_point.h | 29 +- core/include/pch/pch.h | 25 + core/include/pcl_wrapper.h | 10 - core/include/pfd_wrapper.hpp | 44 +- core/include/point_cloud.h | 290 +- core/include/point_clouds.h | 172 +- core/include/pose_graph_loop_closure.h | 117 +- core/include/pose_graph_slam.h | 193 +- core/include/registration_plane_feature.h | 270 +- core/include/session.h | 30 +- core/include/structures.h | 564 +- core/include/surface.h | 36 +- core/include/transformations.h | 607 +- core/include/utils.hpp | 24 +- core/src/color_las_loader.cpp | 483 +- core/src/control_points.cpp | 91 +- core/src/gnss.cpp | 191 +- core/src/ground_control_points.cpp | 71 +- core/src/hash_utils.cpp | 6 +- core/src/icp.cpp | 699 +- core/src/local_shape_features.cpp | 35 +- core/src/manual_pose_graph_loop_closure.cpp | 150 +- core/src/ndt.cpp | 7525 +++++++++-------- core/src/nmea.cpp | 242 +- core/src/nns.cpp | 1 - core/src/observation_picking.cpp | 5 +- ...zation_point_to_point_source_to_target.cpp | 5 +- ...stance_point_to_plane_source_to_target.cpp | 3 +- ...timize_plane_to_plane_source_to_target.cpp | 5 +- ...timize_point_to_plane_source_to_target.cpp | 4 +- ...projection_onto_plane_source_to_target.cpp | 5 +- .../src/pair_wise_iterative_closest_point.cpp | 177 +- core/src/pcl_wrapper.cpp | 7 - core/src/pfd_wrapper.cpp | 85 +- core/src/point_cloud.cpp | 3485 ++++---- core/src/point_clouds.cpp | 2665 +++--- core/src/pose_graph_loop_closure.cpp | 424 +- core/src/pose_graph_slam.cpp | 1856 ++-- core/src/registration_plane_feature.cpp | 32 +- core/src/session.cpp | 215 +- core/src/surface.cpp | 150 +- core/src/utils.cpp | 317 +- core_hd_mapping/CMakeLists.txt | 67 +- core_hd_mapping/include/laz_wrapper.h | 38 +- .../include/odo_with_gnss_fusion.h | 80 +- core_hd_mapping/include/pch/pch.h | 11 + core_hd_mapping/include/project_settings.h | 71 +- core_hd_mapping/include/roi_exporter.h | 39 +- .../include/single_trajectory_viewer.h | 48 +- core_hd_mapping/src/laz_wrapper.cpp | 153 +- core_hd_mapping/src/odo_with_gnss_fusion.cpp | 1759 ++-- core_hd_mapping/src/project_settings.cpp | 1452 ++-- core_hd_mapping/src/roi_exporter.cpp | 379 +- .../src/single_trajectory_viewer.cpp | 166 +- external/README.md | 1 + .../include/WGS84toCartesian.hpp | 0 {core => external}/include/plycpp.h | 2 +- {core => external}/include/wgs84_do_puwg92.h | 0 {core => external}/src/plycpp.cpp | 2 +- {core => external}/src/wgs84_do_puwg92.cc | 0 pybind/CMakeLists.txt | 31 +- pybind/core_binding.cpp | 34 +- pybind/lidar_odometry_binding.cpp | 91 +- pybind/tls_registration_binding.cpp | 75 +- 132 files changed, 21726 insertions(+), 18005 deletions(-) delete mode 100644 core/include/nns.h create mode 100644 core/include/pch/pch.h delete mode 100644 core/include/pcl_wrapper.h delete mode 100644 core/src/nns.cpp delete mode 100644 core/src/pcl_wrapper.cpp create mode 100644 core_hd_mapping/include/pch/pch.h create mode 100644 external/README.md rename {core => external}/include/WGS84toCartesian.hpp (100%) rename {core => external}/include/plycpp.h (99%) rename {core => external}/include/wgs84_do_puwg92.h (100%) rename {core => external}/src/plycpp.cpp (99%) rename {core => external}/src/wgs84_do_puwg92.cc (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 738a1c52..d413617c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,9 @@ cmake_minimum_required(VERSION 4.0.0) # Set minimum policy version to avoid compatibility issues with subprojects set(CMAKE_POLICY_VERSION_MINIMUM 3.5) +# Export compile commands for static analysis and tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + project(hd-mapping) set (HDMAPPING_VERSION_MAJOR 0) @@ -13,6 +16,16 @@ add_definitions(-DHDMAPPING_VERSION_MAJOR=${HDMAPPING_VERSION_MAJOR}) add_definitions(-DHDMAPPING_VERSION_MINOR=${HDMAPPING_VERSION_MINOR}) add_definitions(-DHDMAPPING_VERSION_PATCH=${HDMAPPING_VERSION_PATCH}) +if (MSVC) + add_definitions(-DNOMINMAX) + add_definitions(-DWIN32_LEAN_AND_MEAN) +endif() + +get_filename_component(REPOSITORY_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} ABSOLUTE) +set(THIRDPARTY_DIRECTORY ${REPOSITORY_DIRECTORY}/3rdparty) +set(THIRDPARTY_DIRECTORY_BINARY ${REPOSITORY_DIRECTORY}/3rdpartyBinary) +set(EXTERNAL_LIBRARIES_DIRECTORY ${REPOSITORY_DIRECTORY}/external) + # CPU Architecture optimization options set(HD_CPU_OPTIMIZATION "AUTO" CACHE STRING "CPU optimization target (AUTO, INTEL, AMD, ARM, GENERIC)") @@ -248,15 +261,13 @@ else() endif() endif() +set(THREADS_PREFER_PTHREAD_FLAG ON) +find_package(Threads REQUIRED) + find_package(OpenGL REQUIRED) add_definitions(-D_USE_MATH_DEFINES) -get_filename_component(REPOSITORY_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} ABSOLUTE) -set(EXTERNAL_LIBRARIES_DIRECTORY ${REPOSITORY_DIRECTORY}/3rdparty) -set(EXTERNAL_LIBRARIES_DIRECTORY_BINARY ${REPOSITORY_DIRECTORY}/3rdpartyBinary) - - set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) @@ -266,8 +277,8 @@ include(cmake/imgui.cmake) include(cmake/imguizmo.cmake) include(cmake/implot.cmake) -add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/eigen) -set(EIGEN3_INCLUDE_DIR ${EXTERNAL_LIBRARIES_DIRECTORY}/eigen) +add_subdirectory(${THIRDPARTY_DIRECTORY}/eigen) +set(EIGEN3_INCLUDE_DIR ${THIRDPARTY_DIRECTORY}/eigen) MESSAGE(STATUS "Using bundled Eigen3 : ${EIGEN3_INCLUDE_DIR}") @@ -276,8 +287,8 @@ if (BUILD_WITH_BUNDLED_FREEGLUT) set(FREEGLUT_BUILD_DEMOS OFF CACHE BOOL "" FORCE) - add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut) - set(FREEGLUT_INCLUDE_DIR ${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut/include) + add_subdirectory(${THIRDPARTY_DIRECTORY}/freeglut) + set(FREEGLUT_INCLUDE_DIR ${THIRDPARTY_DIRECTORY}/freeglut/include) set(FREEGLUT_LIBRARY freeglut) # Suppress unused variable warnings from FreeGlut if (CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") @@ -293,8 +304,8 @@ endif () option (BUILD_WITH_BUNDLED_GLEW "Build with bundled GLEW" ON) if (BUILD_WITH_BUNDLED_GLEW) - add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake) - set(GLEW_INCLUDE_DIR ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include) + add_subdirectory(${THIRDPARTY_DIRECTORY}/glew-cmake) + set(GLEW_INCLUDE_DIR ${THIRDPARTY_DIRECTORY}/glew-cmake/include) set(GLEW_LIBRARY libglew_static) MESSAGE(STATUS "Using bundled GLEW") @@ -307,8 +318,8 @@ endif() option(BUILD_WITH_BUNDLED_LIBLASZIP "Build with bundled Lib LASZIP" ON) if (BUILD_WITH_BUNDLED_LIBLASZIP) - add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/LASzip) - set(LASZIP_INCLUDE_DIR ${EXTERNAL_LIBRARIES_DIRECTORY}) + add_subdirectory(${THIRDPARTY_DIRECTORY}/LASzip) + set(LASZIP_INCLUDE_DIR ${THIRDPARTY_DIRECTORY}) set(LASZIP_LIBRARY laszip) MESSAGE(STATUS "Using bundled LASzip") #suppress warnings from LASzip @@ -329,7 +340,7 @@ else() endif () # Download or Search for OpenCV -include(${EXTERNAL_LIBRARIES_DIRECTORY_BINARY}/OpenCV/CMakeLists.txt) +include(${THIRDPARTY_DIRECTORY_BINARY}/OpenCV/CMakeLists.txt) find_package(OpenCV REQUIRED) MESSAGE(STATUS "OpenCV include dir: ${OpenCV_INCLUDE_DIRS}, OpenCV librarys: ${OpenCV_LIBS}") @@ -343,8 +354,8 @@ if (BUILD_WITH_BUNDLED_ONETBB) set(TBBMALLOC_BUILD OFF CACHE BOOL "" FORCE) set(TBB_CPF OFF CACHE BOOL "" FORCE) - add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/oneTBB) - MESSAGE(STATUS "Using bundled oneTBB from: ${EXTERNAL_LIBRARIES_DIRECTORY}/oneTBB") + add_subdirectory(${THIRDPARTY_DIRECTORY}/oneTBB) + MESSAGE(STATUS "Using bundled oneTBB from: ${THIRDPARTY_DIRECTORY}/oneTBB") else() # Find TBB from system find_package(TBB REQUIRED) @@ -374,7 +385,7 @@ else() endif() -add_subdirectory(${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) +add_subdirectory(${THIRDPARTY_DIRECTORY}/Fusion/Fusion) add_subdirectory(core) add_subdirectory(core_hd_mapping) diff --git a/apps/compare_trajectories/CMakeLists.txt b/apps/compare_trajectories/CMakeLists.txt index 96671bcc..33344f8d 100644 --- a/apps/compare_trajectories/CMakeLists.txt +++ b/apps/compare_trajectories/CMakeLists.txt @@ -13,19 +13,19 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( mandeye_compare_trajectories diff --git a/apps/compare_trajectories/mandeye_compare_trajectories.cpp b/apps/compare_trajectories/mandeye_compare_trajectories.cpp index d1190b0f..c1e76ebc 100644 --- a/apps/compare_trajectories/mandeye_compare_trajectories.cpp +++ b/apps/compare_trajectories/mandeye_compare_trajectories.cpp @@ -1,11 +1,15 @@ #include #include #include -#include #include +#include + +// clang-format off #include #include +// clang-format on + #include #include @@ -15,8 +19,8 @@ #include -#include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include #include @@ -39,34 +43,25 @@ float translate_z = -50.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}; +bool gui_mouse_down{ false }; float mouse_sensitivity = 1.0; bool move_source_trajectory_with_gizmo = false; bool show_correspondences = false; -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_gizmo[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; namespace Data { std::map trajectory_gt; std::map trajectory_est; Eigen::Matrix4d trajectory_offset = Eigen::Matrix4d::Identity(); -} +} // namespace Data // Function to split a CSV line into values -std::vector parseCSVLine(const std::string &line) +std::vector parseCSVLine(const std::string& line) { std::vector values; std::stringstream ss(line); @@ -79,7 +74,7 @@ std::vector parseCSVLine(const std::string &line) return values; } -std::vector parseCSVLineSpace(const std::string &line) +std::vector parseCSVLineSpace(const std::string& line) { std::vector values; std::stringstream ss(line); @@ -92,7 +87,7 @@ std::vector parseCSVLineSpace(const std::string &line) return values; } -std::map load_trajectory_from_CSV(const std::string &file_path) +std::map load_trajectory_from_CSV(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -124,7 +119,7 @@ std::map load_trajectory_from_CSV(const std::string &fi return trajectory; } -std::map load_trajectory_from_CSV_LIDARROT(const std::string &file_path) +std::map load_trajectory_from_CSV_LIDARROT(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -154,9 +149,7 @@ std::map load_trajectory_from_CSV_LIDARROT(const std::s double r10 = values[7], r11 = values[8], r12 = values[9]; double r20 = values[10], r21 = values[11], r22 = values[12]; Eigen::Matrix3d rotationMatrix; - rotationMatrix << r00, r01, r02, - r10, r11, r12, - r20, r21, r22; + rotationMatrix << r00, r01, r02, r10, r11, r12, r20, r21, r22; pose.block<3, 3>(0, 0) = rotationMatrix; trajectory[timestamp] = pose; @@ -164,7 +157,7 @@ std::map load_trajectory_from_CSV_LIDARROT(const std::s return trajectory; } -std::map load_trajectory_from_CSV_UNIXROT(const std::string &file_path) +std::map load_trajectory_from_CSV_UNIXROT(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -194,9 +187,7 @@ std::map load_trajectory_from_CSV_UNIXROT(const std::st double r10 = values[7], r11 = values[8], r12 = values[9]; double r20 = values[10], r21 = values[11], r22 = values[12]; Eigen::Matrix3d rotationMatrix; - rotationMatrix << r00, r01, r02, - r10, r11, r12, - r20, r21, r22; + rotationMatrix << r00, r01, r02, r10, r11, r12, r20, r21, r22; pose.block<3, 3>(0, 0) = rotationMatrix; trajectory[timestamp] = pose; @@ -204,7 +195,7 @@ std::map load_trajectory_from_CSV_UNIXROT(const std::st return trajectory; } -std::map load_trajectory_from_CSV_LIDARUNIXROT(const std::string &file_path) +std::map load_trajectory_from_CSV_LIDARUNIXROT(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -235,9 +226,7 @@ std::map load_trajectory_from_CSV_LIDARUNIXROT(const st double r10 = values[8], r11 = values[9], r12 = values[10]; double r20 = values[11], r21 = values[12], r22 = values[13]; Eigen::Matrix3d rotationMatrix; - rotationMatrix << r00, r01, r02, - r10, r11, r12, - r20, r21, r22; + rotationMatrix << r00, r01, r02, r10, r11, r12, r20, r21, r22; pose.block<3, 3>(0, 0) = rotationMatrix; trajectory[timestamplidar] = pose; @@ -245,7 +234,7 @@ std::map load_trajectory_from_CSV_LIDARUNIXROT(const st return trajectory; } -std::map load_trajectory_from_CSV_LIDARQ(const std::string &file_path) +std::map load_trajectory_from_CSV_LIDARQ(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -278,7 +267,7 @@ std::map load_trajectory_from_CSV_LIDARQ(const std::str return trajectory; } -std::map load_trajectory_from_CSV_UNIXQ(const std::string &file_path) +std::map load_trajectory_from_CSV_UNIXQ(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -311,7 +300,7 @@ std::map load_trajectory_from_CSV_UNIXQ(const std::stri return trajectory; } -std::map load_trajectory_from_CSV_LIDARUNIXQ(const std::string &file_path) +std::map load_trajectory_from_CSV_LIDARUNIXQ(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -345,7 +334,7 @@ std::map load_trajectory_from_CSV_LIDARUNIXQ(const std: return trajectory; } -std::map load_trajectory_from_CSV_STEP1(const std::string &file_path) +std::map load_trajectory_from_CSV_STEP1(const std::string& file_path) { std::map trajectory; std::ifstream file(file_path); @@ -375,9 +364,7 @@ std::map load_trajectory_from_CSV_STEP1(const std::stri double r10 = values[7], r11 = values[8], r12 = values[9]; double r20 = values[10], r21 = values[11], r22 = values[12]; Eigen::Matrix3d rotationMatrix; - rotationMatrix << r00, r01, r02, - r10, r11, r12, - r20, r21, r22; + rotationMatrix << r00, r01, r02, r10, r11, r12, r20, r21, r22; pose.block<3, 3>(0, 0) = rotationMatrix; trajectory[timestamp] = pose; @@ -396,12 +383,16 @@ void reshape(int w, int h) } else { - ImGuiIO &io = ImGui::GetIO(); + 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( + -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); @@ -410,7 +401,7 @@ void reshape(int w, int h) void motion(int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); if (!io.WantCaptureMouse) @@ -424,8 +415,10 @@ void motion(int x, int y) 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); + 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; @@ -464,7 +457,7 @@ void project_gui() { if (ImGui::Begin("main gui window")) { - ImGui::ColorEdit3("clear color", (float *)&clear_color); + ImGui::ColorEdit3("clear color", (float*)&clear_color); ImGui::InputInt("point_size", &point_size); point_size = std::max(1, point_size); @@ -475,8 +468,8 @@ void project_gui() ImGui::Checkbox("move_source_trajectory_with_gizmo", &move_source_trajectory_with_gizmo); // if (ImGui::Button("Load target trajectory (e.g. ground truth trajectory)) // { - // auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); - // if (!file_path.empty()) + // auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", + // "*.csv"}).result(); if (!file_path.empty()) // { // Data::trajectory_gt = load_trajectory_from_CSV(file_path[0]); // } @@ -485,7 +478,8 @@ void project_gui() ImGui::Text("--------------------------------"); if (ImGui::Button("(Step 1) Load target trajectory (timestampLidar,x,y,z,qx,qy,qz,qw(ground truth trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_LIDARQ(file_path[0]); @@ -493,7 +487,8 @@ void project_gui() } if (ImGui::Button("(Step 2) Load source trajectory (timestampLidar,x,y,z,qx,qy,qz,qw(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_LIDARQ(file_path[0]); @@ -516,7 +511,7 @@ void project_gui() double max_ATE = -1000000.0; double min_ATE = 1000000.0; int count = 0; - for (const auto &p : Data::trajectory_est) + for (const auto& p : Data::trajectory_est) { // std::cout << p.second.matrix() << std::endl; @@ -529,7 +524,8 @@ void project_gui() // std::cout << "ATE " << ATE << std::endl; double ate = (tp - it).norm(); - if (ate > max_ATE){ + if (ate > max_ATE) + { max_ATE = ate; } if (ate < min_ATE) @@ -560,7 +556,8 @@ void project_gui() { if (ImGui::Button("Load source trajectory (timestampLidar,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_LIDARROT(file_path[0]); @@ -568,15 +565,18 @@ void project_gui() } if (ImGui::Button("Load source trajectory (timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_UNIXROT(file_path[0]); } } - if (ImGui::Button("Load source trajectory (timestampLidar,timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(estimated trajectory)")) + if (ImGui::Button( + "Load source trajectory (timestampLidar,timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_LIDARUNIXROT(file_path[0]); @@ -585,7 +585,8 @@ void project_gui() if (ImGui::Button("Load source trajectory (timestampUnix,x,y,z,qx,qy,qz,qw(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_UNIXQ(file_path[0]); @@ -593,15 +594,19 @@ void project_gui() } if (ImGui::Button("Load source trajectory (timestampLidar,timestampUnix,x,y,z,qx,qy,qz,qw(estimated trajectory)")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_LIDARUNIXQ(file_path[0]); } } - if (ImGui::Button("Load source trajectory timestamp_nanoseconds,pose00,pose01,pose02,pose03,pose10,pose11,pose12,pose13,pose20,pose21,pose22,pose23,timestampUnix_nanoseconds,imuom,imufi,imuka ground truth trajectory")) + if (ImGui::Button("Load source trajectory " + "timestamp_nanoseconds,pose00,pose01,pose02,pose03,pose10,pose11,pose12,pose13,pose20,pose21,pose22,pose23," + "timestampUnix_nanoseconds,imuom,imufi,imuka ground truth trajectory")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_est = load_trajectory_from_CSV_STEP1(file_path[0]); @@ -609,7 +614,8 @@ void project_gui() } if (ImGui::Button("Load target trajectory (timestampLidar,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(ground trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_LIDARROT(file_path[0]); @@ -617,15 +623,18 @@ void project_gui() } if (ImGui::Button("Load target trajectory (timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(ground truth trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_UNIXROT(file_path[0]); } } - if (ImGui::Button("Load target trajectory (timestampLidar,timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(ground truth trajectory)")) + if (ImGui::Button("Load target trajectory (timestampLidar,timestampUnix,x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22(ground truth " + "trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_LIDARUNIXROT(file_path[0]); @@ -634,7 +643,8 @@ void project_gui() if (ImGui::Button("Load target trajectory (timestampUnix,x,y,z,qx,qy,qz,qw(ground truth trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_UNIXQ(file_path[0]); @@ -642,15 +652,19 @@ void project_gui() } if (ImGui::Button("Load target trajectory (timestampLidar,timestampUnix,x,y,z,qx,qy,qz,qw(ground truth trajectory)")) { - auto file_path = pfd::open_file("Select target trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select target trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_LIDARUNIXQ(file_path[0]); } } - if (ImGui::Button("Load target trajectory timestamp_nanoseconds,pose00,pose01,pose02,pose03,pose10,pose11,pose12,pose13,pose20,pose21,pose22,pose23,timestampUnix_nanoseconds,imuom,imufi,imuka ground truth trajectory")) + if (ImGui::Button("Load target trajectory " + "timestamp_nanoseconds,pose00,pose01,pose02,pose03,pose10,pose11,pose12,pose13,pose20,pose21,pose22,pose23," + "timestampUnix_nanoseconds,imuom,imufi,imuka ground truth trajectory")) { - auto file_path = pfd::open_file("Select source trajectory file", fs::current_path().string(), {"CSV Files", "*.csv"}).result(); + auto file_path = + pfd::open_file("Select source trajectory file", fs::current_path().string(), { "CSV Files", "*.csv" }).result(); if (!file_path.empty()) { Data::trajectory_gt = load_trajectory_from_CSV_STEP1(file_path[0]); @@ -665,7 +679,7 @@ void project_gui() const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(Eigen::Affine3d(Data::trajectory_offset.matrix())); - for (const auto &p : Data::trajectory_est) + for (const auto& p : Data::trajectory_est) { auto it = getInterpolatedPose(Data::trajectory_gt, p.first); if (!it.isZero()) @@ -676,17 +690,49 @@ void project_gui() Eigen::Matrix AtPA_; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - 1, 0, 0, 0, 1, 0, 0, 0, 1); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + 1, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 1); Eigen::Matrix AtPB_; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - 1, 0, 0, 0, 1, 0, 0, 0, 1, - p_t.x(), p_t.y(), p_t.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + 1, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 1, + p_t.x(), + p_t.y(), + p_t.z()); AtPA.block<6, 6>(0, 0) += AtPA_; AtPB.block<6, 1>(0, 0) -= AtPB_; @@ -746,7 +792,7 @@ void project_gui() void display() { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -754,14 +800,21 @@ void display() 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); + 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); @@ -780,12 +833,11 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - 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())); + 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); } @@ -859,12 +911,12 @@ void display() glPointSize(point_size); glBegin(GL_POINTS); glColor3f(1, 0, 0); - for (const auto &p : Data::trajectory_gt) + for (const auto& p : Data::trajectory_gt) { glVertex3f(p.second(0, 3), p.second(1, 3), p.second(2, 3)); } glColor3f(0, 1, 0); - for (const auto &p : Data::trajectory_est) + for (const auto& p : Data::trajectory_est) { auto tp = Data::trajectory_offset * p.second; glVertex3f(tp(0, 3), tp(1, 3), tp(2, 3)); @@ -876,7 +928,7 @@ void display() { glBegin(GL_LINES); glColor3f(1, 1, 1); - for (const auto &p : Data::trajectory_est) + for (const auto& p : Data::trajectory_est) { auto it = getInterpolatedPose(Data::trajectory_gt, p.first); if (!it.isZero()) @@ -891,7 +943,7 @@ void display() ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); ImGuizmo::BeginFrame(); ImGuizmo::Enable(true); @@ -913,11 +965,23 @@ void display() 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); + 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); + ImGuizmo::Manipulate( + m_ortho_gizmo_view, + m_ortho_projection, + ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, + ImGuizmo::WORLD, + m_gizmo, + NULL); } Data::trajectory_offset = Eigen::Map(m_gizmo).cast(); } @@ -931,7 +995,7 @@ void display() glutPostRedisplay(); } -bool initGL(int *argc, char **argv) +bool initGL(int* argc, char** argv) { glutInit(argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); @@ -953,7 +1017,7 @@ bool initGL(int *argc, char **argv) gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); glutReshapeFunc(reshape); ImGui::CreateContext(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); (void)io; // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls @@ -968,7 +1032,7 @@ 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(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -983,8 +1047,7 @@ void mouse(int glut_button, int state, int x, int y) io.MouseDown[button] = false; static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && - glutMajorVersion < 3) + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) { wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); } @@ -1041,7 +1104,7 @@ void wheel(int button, int dir, int x, int y) return; } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { if (argc == 3) { diff --git a/apps/concatenate_multi_livox/CMakeLists.txt b/apps/concatenate_multi_livox/CMakeLists.txt index 9ca0b3b7..91c0fd35 100644 --- a/apps/concatenate_multi_livox/CMakeLists.txt +++ b/apps/concatenate_multi_livox/CMakeLists.txt @@ -9,18 +9,18 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion ${FREEGLUT_INCLUDE_DIR}) target_link_libraries(concatenate_multi_livox PRIVATE core ${PLATFORM_LASZIP_LIB} diff --git a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp index 24bd5f30..2e094887 100644 --- a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp +++ b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp @@ -1,8 +1,9 @@ -#include -#include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" #include "export_laz.h" -int main(int argc, char *argv[]) +#include +#include + +int main(int argc, char* argv[]) { if (argc < 3) { @@ -21,7 +22,7 @@ int main(int argc, char *argv[]) std::vector csv_files; std::vector sn_files; std::vector all_file_names; - for (const auto &entry : std::filesystem::directory_iterator(input_dir)) + for (const auto& entry : std::filesystem::directory_iterator(input_dir)) { if (entry.is_regular_file()) { @@ -43,7 +44,8 @@ int main(int argc, char *argv[]) } // check if number of laz files is equal to number of csv files and sn files const auto scan_number = laz_files.size(); - if (scan_number != csv_files.size() || scan_number != sn_files.size()) { + if (scan_number != csv_files.size() || scan_number != sn_files.size()) + { std::cerr << "Number of laz files, csv files and sn files is not equal!" << std::endl; std::cerr << "laz files: " << laz_files.size() << std::endl; std::cerr << "csv files: " << csv_files.size() << std::endl; @@ -55,18 +57,21 @@ int main(int argc, char *argv[]) const auto calibrationFile = (std::filesystem::path(input_dir) / "calibration.json").string(); std::cout << "Loading calibration file: " << calibrationFile << std::endl; auto preloadedCalibration = MLvxCalib::GetCalibrationFromFile(calibrationFile); - if (preloadedCalibration.empty()) { + if (preloadedCalibration.empty()) + { std::cerr << "No calibration data found in file: " << calibrationFile << std::endl; std::cerr << "Please check the file format and content." << std::endl; return 1; } std::cout << "Loaded calibration for " << preloadedCalibration.size() << " sensors." << std::endl; - for (const auto &[sn, _] : preloadedCalibration) { + for (const auto& [sn, _] : preloadedCalibration) + { std::cout << " -> " << sn << std::endl; } // Get Id of Imu to use const std::string imuSnToUse = MLvxCalib::GetImuSnToUse(calibrationFile); - if (imuSnToUse.empty()) { + if (imuSnToUse.empty()) + { std::cerr << "No IMU serial number found in calibration file: " << calibrationFile << std::endl; std::cerr << "Please check the file format and content."; return 1; @@ -74,9 +79,9 @@ int main(int argc, char *argv[]) std::cout << "IMU to use: " << imuSnToUse << std::endl; // get id to serial number mapping - - if (!std::filesystem::exists(output_dir)) { - //create output directory if it does not exist + if (!std::filesystem::exists(output_dir)) + { + // create output directory if it does not exist std::filesystem::create_directory(output_dir); } @@ -85,12 +90,12 @@ int main(int argc, char *argv[]) std::sort(std::begin(csv_files), std::end(csv_files)); std::sort(std::begin(sn_files), std::end(sn_files)); - // process laz - for (int i = 0; i < scan_number; i++){ - const std::string &laz_file = laz_files[i]; - const std::string &csv_file = csv_files[i]; - const std::string &fnSn = sn_files[i]; + for (int i = 0; i < scan_number; i++) + { + const std::string& laz_file = laz_files[i]; + const std::string& csv_file = csv_files[i]; + const std::string& fnSn = sn_files[i]; const auto imuBaseName = std::filesystem::path(csv_file).filename().string(); @@ -112,10 +117,11 @@ int main(int argc, char *argv[]) intensity.reserve(data.size()); timestamps.reserve(data.size()); int counter = 0; - for (const auto &point : data) { + for (const auto& point : data) + { pointcloud.push_back(point.point); intensity.push_back(point.intensity); - timestamps.push_back(double(point.timestamp)/1e9); + timestamps.push_back(double(point.timestamp) / 1e9); } exportLaz(output_path.string(), pointcloud, intensity, timestamps, 0, 0, 0); std::cout << "Saved processed points to: " << output_path.string() << std::endl; @@ -124,22 +130,18 @@ int main(int argc, char *argv[]) auto imu = load_imu(csv_file.c_str(), imuNumberToUse); std::ofstream out(output_path_csv.string()); - if (!out.is_open()) { + if (!out.is_open()) + { std::cerr << "Failed to open output file: " << output_path_csv.string() << std::endl; continue; } out << "timestamp timestampUnix accX accY accZ gyroX gyroY gyroZ\n"; - for (const auto &[timestamp, gyro, accel] : imu) { - out << static_cast(1e9 * timestamp.first) << " " - << static_cast(1e9 * timestamp.second) << " " - << accel.axis.x << " " - << accel.axis.y << " " - << accel.axis.z << " " - << gyro.axis.x << " " - << gyro.axis.y << " " + for (const auto& [timestamp, gyro, accel] : imu) + { + out << static_cast(1e9 * timestamp.first) << " " << static_cast(1e9 * timestamp.second) << " " + << accel.axis.x << " " << accel.axis.y << " " << accel.axis.z << " " << gyro.axis.x << " " << gyro.axis.y << " " << gyro.axis.z << "\n"; } std::cout << "Saved IMU data to: " << output_path_csv.string() << std::endl; } - } \ No newline at end of file diff --git a/apps/console_tools/matrix_mul.cpp b/apps/console_tools/matrix_mul.cpp index 4d03511a..02c7109c 100644 --- a/apps/console_tools/matrix_mul.cpp +++ b/apps/console_tools/matrix_mul.cpp @@ -1,10 +1,10 @@ -#include -#include #include +#include +#include #include #include -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { TaitBryanPose pose; pose.px = 0; diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp index 7a6b205d..c01219eb 100644 --- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp +++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp @@ -1,6 +1,6 @@ -#include -#include #include +#include +#include #include #include @@ -10,7 +10,7 @@ #include -bool load_pc(PointCloud &pc, std::string input_file_name) +bool load_pc(PointCloud& pc, std::string input_file_name) { laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) @@ -49,7 +49,7 @@ bool load_pc(PointCloud &pc, std::string input_file_name) point_clouds.push_back(pc);*/ return false; } - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -71,7 +71,7 @@ bool load_pc(PointCloud &pc, std::string input_file_name) // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.c_str(), header->number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { fprintf(stderr, ":DLL ERROR: getting point pointer from laszip reader\n"); @@ -112,7 +112,8 @@ bool load_pc(PointCloud &pc, std::string input_file_name) laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) << "' contains " << npoints << " points" << std::endl; + std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) + << "' contains " << npoints << " points" << std::endl; laszip_I64 p_count = 0; @@ -137,9 +138,10 @@ bool load_pc(PointCloud &pc, std::string input_file_name) pc.timestamps.push_back(p.timestamp); // Eigen::Vector3d color( - // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / static_cast(0xFFFFU) : 1.0f)) / 256.0); + // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) + /// 256.0, static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) + //: 1.0f)) / 256.0, static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / + // static_cast(0xFFFFU) : 1.0f)) / 256.0); Eigen::Vector3d color( static_cast(point->rgb[0]) / 256.0, @@ -160,21 +162,24 @@ bool load_pc(PointCloud &pc, std::string input_file_name) return true; } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - if(argc != 4){ + if (argc != 4) + { std::cout << "USAGE: " << argv[0] << " input_point_cloud output_point_cloud multiplier(e.g. 1000)" << std::endl; return 1; } PointCloud pc; - - if(!load_pc(pc, argv[1])){ + + if (!load_pc(pc, argv[1])) + { std::cout << "Problem with loading '" << argv[1] << "'" << std::endl; return 2; } - for (auto &t:pc.timestamps){ + for (auto& t : pc.timestamps) + { t *= atof(argv[2]); } diff --git a/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp b/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp index 63f86481..e5b938d6 100644 --- a/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp +++ b/apps/console_tools/multiply_timestamps_session_trajectory_csv.cpp @@ -1,12 +1,12 @@ -#include -#include #include +#include +#include #include #include #include -inline void split(std::string &str, char delim, std::vector &out) +inline void split(std::string& str, char delim, std::vector& out) { size_t start; size_t end = 0; @@ -18,7 +18,7 @@ inline void split(std::string &str, char delim, std::vector &out) } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { if (argc != 4) { @@ -28,7 +28,7 @@ int main(int argc, char *argv[]) std::filesystem::path trj_path = argv[1]; - if(std::filesystem::exists(trj_path)) + if (std::filesystem::exists(trj_path)) { std::cout << " loading.. "; @@ -67,7 +67,7 @@ int main(int argc, char *argv[]) std::istringstream(strs[12]) >> ltn.m_pose(2, 3); ltn.timestamps.second = 0.0; - ltn.imu_om_fi_ka = {0, 0, 0}; + ltn.imu_om_fi_ka = { 0, 0, 0 }; local_trajectory.push_back(ltn); } @@ -88,7 +88,7 @@ int main(int argc, char *argv[]) std::istringstream(strs[11]) >> ltn.m_pose(2, 2); std::istringstream(strs[12]) >> ltn.m_pose(2, 3); std::istringstream(strs[13]) >> ltn.timestamps.second; - ltn.imu_om_fi_ka = {0, 0, 0}; + ltn.imu_om_fi_ka = { 0, 0, 0 }; ltn.timestamps.first *= atof(argv[3]); @@ -117,7 +117,7 @@ int main(int argc, char *argv[]) std::istringstream(strs[16]) >> ltn.imu_om_fi_ka.z(); ltn.timestamps.first *= atof(argv[3]); - + local_trajectory.push_back(ltn); } } @@ -125,7 +125,7 @@ int main(int argc, char *argv[]) std::cout << local_trajectory.size() << " local nodes" << std::endl; infile.close(); - //save trajectory + // save trajectory std::ofstream outfile; outfile.open(argv[2]); if (!outfile.good()) @@ -134,28 +134,20 @@ int main(int argc, char *argv[]) return 3; } - outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 timestampUnix_nanoseconds om_rad fi_rad ka_rad" << std::endl; + outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 " + "timestampUnix_nanoseconds om_rad fi_rad ka_rad" + << std::endl; for (int j = 0; j < local_trajectory.size(); j++) { - outfile - << std::setprecision(20) << local_trajectory[j].timestamps.first << " " << std::setprecision(10) - << local_trajectory[j].m_pose(0, 0) << " " - << local_trajectory[j].m_pose(0, 1) << " " - << local_trajectory[j].m_pose(0, 2) << " " - << local_trajectory[j].m_pose(0, 3) << " " - << local_trajectory[j].m_pose(1, 0) << " " - << local_trajectory[j].m_pose(1, 1) << " " - << local_trajectory[j].m_pose(1, 2) << " " - << local_trajectory[j].m_pose(1, 3) << " " - << local_trajectory[j].m_pose(2, 0) << " " - << local_trajectory[j].m_pose(2, 1) << " " - << local_trajectory[j].m_pose(2, 2) << " " - << local_trajectory[j].m_pose(2, 3) << " " - << std::setprecision(20) << local_trajectory[j].timestamps.second << " " - << local_trajectory[j].imu_om_fi_ka.x() << " " - << local_trajectory[j].imu_om_fi_ka.y() << " " - << local_trajectory[j].imu_om_fi_ka.z() << " " - << std::endl; + outfile << std::setprecision(20) << local_trajectory[j].timestamps.first << " " << std::setprecision(10) + << local_trajectory[j].m_pose(0, 0) << " " << local_trajectory[j].m_pose(0, 1) << " " + << local_trajectory[j].m_pose(0, 2) << " " << local_trajectory[j].m_pose(0, 3) << " " + << local_trajectory[j].m_pose(1, 0) << " " << local_trajectory[j].m_pose(1, 1) << " " + << local_trajectory[j].m_pose(1, 2) << " " << local_trajectory[j].m_pose(1, 3) << " " + << local_trajectory[j].m_pose(2, 0) << " " << local_trajectory[j].m_pose(2, 1) << " " + << local_trajectory[j].m_pose(2, 2) << " " << local_trajectory[j].m_pose(2, 3) << " " << std::setprecision(20) + << local_trajectory[j].timestamps.second << " " << local_trajectory[j].imu_om_fi_ka.x() << " " + << local_trajectory[j].imu_om_fi_ka.y() << " " << local_trajectory[j].imu_om_fi_ka.z() << " " << std::endl; } outfile.close(); } diff --git a/apps/hd_mapper/CMakeLists.txt b/apps/hd_mapper/CMakeLists.txt index 9fd3a95e..2ee5fee5 100644 --- a/apps/hd_mapper/CMakeLists.txt +++ b/apps/hd_mapper/CMakeLists.txt @@ -19,15 +19,15 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include) if(WIN32) diff --git a/apps/hd_mapper/hd_mapper.cpp b/apps/hd_mapper/hd_mapper.cpp index 3a4eb36b..f93bea8b 100644 --- a/apps/hd_mapper/hd_mapper.cpp +++ b/apps/hd_mapper/hd_mapper.cpp @@ -1,11 +1,14 @@ #include #include #include -#include #include +#include + +// clang-format off #include #include +// clang-format on #include @@ -22,11 +25,11 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, float picking_plane_height); static bool show_demo_window = true; static bool show_another_window = false; -//static ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); +// static ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); const unsigned int window_width = 800; const unsigned int window_height = 600; int mouse_old_x, mouse_old_y; -int mouse_buttons = 0; +int mouse_buttons = 0; bool gui_mouse_down{ false }; float mouse_sensitivity = 1.0; bool show_axes = false; @@ -45,7 +48,8 @@ void reshape(int w, int h); void my_display_code() { - // 1. Show the big demo window (Most of the sample code is in ImGui::ShowDemoWindow()! You can browse its code to learn more about Dear ImGui!). + // 1. Show the big demo window (Most of the sample code is in ImGui::ShowDemoWindow()! You can browse its code to learn more about Dear + // ImGui!). if (show_demo_window) ImGui::ShowDemoWindow(&show_demo_window); @@ -54,16 +58,16 @@ void my_display_code() static float f = 0.0f; static int counter = 0; - ImGui::Begin("Hello, world!"); // Create a window called "Hello, world!" and append into it. + ImGui::Begin("Hello, world!"); // Create a window called "Hello, world!" and append into it. - ImGui::Text("This is some useful text."); // Display some text (you can use a format strings too) - ImGui::Checkbox("Demo Window", &show_demo_window); // Edit bools storing our window open/close state + ImGui::Text("This is some useful text."); // Display some text (you can use a format strings too) + ImGui::Checkbox("Demo Window", &show_demo_window); // Edit bools storing our window open/close state ImGui::Checkbox("Another Window", &show_another_window); - ImGui::SliderFloat("float", &f, 0.0f, 1.0f); // Edit 1 float using a slider from 0.0f to 1.0f + ImGui::SliderFloat("float", &f, 0.0f, 1.0f); // Edit 1 float using a slider from 0.0f to 1.0f ImGui::ColorEdit3("clear color", (float*)&project_setings.clear_color); // Edit 3 floats representing a color - if (ImGui::Button("Button")) // Buttons return true when clicked (most widgets return true when edited/activated) + if (ImGui::Button("Button")) // Buttons return true when clicked (most widgets return true when edited/activated) counter++; ImGui::SameLine(); ImGui::Text("counter = %d", counter); @@ -75,7 +79,8 @@ void my_display_code() // 3. Show another simple window. if (show_another_window) { - ImGui::Begin("Another Window", &show_another_window); // Pass a pointer to our bool variable (the window will have a closing button that will clear the bool when clicked) + ImGui::Begin("Another Window", &show_another_window); // Pass a pointer to our bool variable (the window will have a closing button + // that will clear the bool when clicked) ImGui::Text("Hello from another window!"); if (ImGui::Button("Close Me")) show_another_window = false; @@ -83,13 +88,15 @@ void my_display_code() } } -void display() { +void display() +{ ImGuiIO& io = ImGui::GetIO(); reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glClearColor(project_setings.clear_color.x * project_setings.clear_color.w, - project_setings.clear_color.y * project_setings.clear_color.w, - project_setings.clear_color.z * project_setings.clear_color.w, + glClearColor( + project_setings.clear_color.x * project_setings.clear_color.w, + project_setings.clear_color.y * project_setings.clear_color.w, + project_setings.clear_color.z * project_setings.clear_color.w, project_setings.clear_color.w); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); @@ -98,42 +105,40 @@ void display() { glMatrixMode(GL_MODELVIEW); glLoadIdentity(); - if (common_data.is_ortho) { + if (common_data.is_ortho) + { Eigen::Vector3d v_eye_t(common_data.translate_x * mouse_sensitivity, common_data.translate_y * mouse_sensitivity, 10); Eigen::Vector3d v_center_t(common_data.translate_x * mouse_sensitivity, common_data.translate_y * mouse_sensitivity, 0); Eigen::Vector3d v_t(0, 1, 0); - 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()); + 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()); } - else { + else + { Eigen::Vector3d v(-zoom, 0, 0); TaitBryanPose pose_tb; pose_tb.ka = common_data.rotate_y; - //pose_tb.fi = common_data.rotate_y; - //pose_tb.ka = common_data.rotate_y; + // pose_tb.fi = common_data.rotate_y; + // pose_tb.ka = common_data.rotate_y; Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose_tb); Eigen::Vector3d vt = m * v; - Eigen::Vector3d v_eye_t(common_data.roi(0, 3)+ vt.x(), common_data.roi(1, 3) + vt.y(), common_data.roi(2, 3) + camera_eye_height); - - - Eigen::Vector3d v_center_t(common_data.roi(0,3), common_data.roi(1,3), common_data.roi(2,3) + camera_eye_height); + Eigen::Vector3d v_eye_t(common_data.roi(0, 3) + vt.x(), common_data.roi(1, 3) + vt.y(), common_data.roi(2, 3) + camera_eye_height); + + Eigen::Vector3d v_center_t(common_data.roi(0, 3), common_data.roi(1, 3), common_data.roi(2, 3) + camera_eye_height); Eigen::Vector3d v_t(0, 0, 1); - 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()); - //glTranslatef(common_data.translate_x, common_data.translate_y, common_data.translate_z); - //glRotatef(common_data.rotate_x, 1.0, 0.0, 0.0); - //glRotatef(common_data.rotate_y, 0.0, 0.0, 1.0); + 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()); + // glTranslatef(common_data.translate_x, common_data.translate_y, common_data.translate_z); + // glRotatef(common_data.rotate_x, 1.0, 0.0, 0.0); + // glRotatef(common_data.rotate_y, 0.0, 0.0, 1.0); - //common_data.roi. + // common_data.roi. } - if (show_axes) { + if (show_axes) + { glBegin(GL_LINES); glColor3f(1.0f, 0.0f, 0.0f); glVertex3f(0.0f, 0.0f, 0.0f); @@ -151,32 +156,41 @@ void display() { ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); - //my_display_code(); + // my_display_code(); project_setings.imgui(odo_with_gnss_fusion, laz_wrapper.sectors, roi_exporter.rois_with_constraints, common_data); - if (common_data.odo_with_gnss_fusion) { + if (common_data.odo_with_gnss_fusion) + { odo_with_gnss_fusion.imgui(common_data); } - else if (common_data.single_trajectory_viewer) { + else if (common_data.single_trajectory_viewer) + { single_trajectory_viewer.imgui(common_data); } - else { - if (common_data.roi_exorter)roi_exporter.imgui(common_data, project_setings, laz_wrapper.sectors); - if (common_data.laz_wrapper)laz_wrapper.imgui(common_data, project_setings); + else + { + if (common_data.roi_exorter) + roi_exporter.imgui(common_data, project_setings, laz_wrapper.sectors); + if (common_data.laz_wrapper) + laz_wrapper.imgui(common_data, project_setings); } - - - if (common_data.odo_with_gnss_fusion) { - odo_with_gnss_fusion.render(); + + if (common_data.odo_with_gnss_fusion) + { + odo_with_gnss_fusion.render(); } - else if (common_data.single_trajectory_viewer) { + else if (common_data.single_trajectory_viewer) + { single_trajectory_viewer.render(); } - else { + else + { project_setings.render(roi_exporter.rois_with_constraints); - if (common_data.roi_exorter)roi_exporter.render(common_data); - if (common_data.laz_wrapper)laz_wrapper.render(common_data); + if (common_data.roi_exorter) + roi_exporter.render(common_data); + if (common_data.laz_wrapper) + laz_wrapper.render(common_data); } ImGui::Render(); @@ -186,7 +200,8 @@ void display() { glutPostRedisplay(); } -void motion(int x, int y) { +void motion(int x, int y) +{ ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); @@ -196,20 +211,24 @@ void motion(int x, int y) { dx = (float)(x - mouse_old_x); dy = (float)(y - mouse_old_y); gui_mouse_down = mouse_buttons > 0; - if (mouse_buttons & 1 && !common_data.is_ortho) { + if (mouse_buttons & 1 && !common_data.is_ortho) + { common_data.rotate_x += dy * 0.001f * mouse_sensitivity; common_data.rotate_y += dx * 0.001f * mouse_sensitivity; camera_eye_height += dy * 0.1f * mouse_sensitivity; } - else if (mouse_buttons & 4) { + else if (mouse_buttons & 4) + { common_data.translate_z += dy * 1.0f * mouse_sensitivity; zoom += dy * 0.1f * mouse_sensitivity; - if (zoom < 0.1) { + if (zoom < 0.1) + { zoom = 0.1; } } - else if (mouse_buttons & 3) { + else if (mouse_buttons & 3) + { common_data.translate_x += dx * 0.5f * mouse_sensitivity; common_data.translate_y -= dy * 0.5f * mouse_sensitivity; } @@ -219,13 +238,17 @@ void motion(int x, int y) { glutPostRedisplay(); } -void mouse(int glut_button, int state, 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 (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) @@ -233,13 +256,16 @@ void mouse(int glut_button, int state, int x, int y) { if (!io.WantCaptureMouse) { - if (state == GLUT_DOWN) { + if (state == GLUT_DOWN) + { mouse_buttons |= 1 << glut_button; - if (common_data.is_ortho && button == 0) { + if (common_data.is_ortho && button == 0) + { common_data.roi = GLWidgetGetOGLPos(x, y, 0.0); } } - else if (state == GLUT_UP) { + else if (state == GLUT_UP) + { mouse_buttons = 0; } mouse_old_x = x; @@ -247,21 +273,31 @@ void mouse(int glut_button, int state, int x, int y) { } } -void reshape(int w, int h) { +void reshape(int w, int h) +{ glViewport(0, 0, (GLsizei)w, (GLsizei)h); glMatrixMode(GL_PROJECTION); glLoadIdentity(); - if (!common_data.is_ortho) { + if (!common_data.is_ortho) + { gluPerspective(60.0, (GLfloat)w / (GLfloat)h, 0.01, 100000.0); } - else { - glOrtho(-common_data.translate_z, common_data.translate_z, -common_data.translate_z * (float)h / float(w), common_data.translate_z * float(h) / float(w), -100000, 100000); + else + { + glOrtho( + -common_data.translate_z, + common_data.translate_z, + -common_data.translate_z * (float)h / float(w), + common_data.translate_z* float(h) / float(w), + -100000, + 100000); } glMatrixMode(GL_MODELVIEW); glLoadIdentity(); } -bool initGL(int* argc, char** argv) { +bool initGL(int* argc, char** argv) +{ glutInit(argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); glutInitWindowSize(window_width, window_height); @@ -282,8 +318,9 @@ bool initGL(int* argc, char** argv) { 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 + ImGuiIO& io = ImGui::GetIO(); + (void)io; + // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls ImGui::StyleColorsDark(); ImGui_ImplGLUT_Init(); @@ -292,7 +329,8 @@ bool initGL(int* argc, char** argv) { return true; } -int main(int argc, char **argv){ +int main(int argc, char** argv) +{ initGL(&argc, argv); glutDisplayFunc(display); glutMouseFunc(mouse); @@ -321,7 +359,8 @@ Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const Plane& plane) float a = plane.a * laser_beam.direction.x() + plane.b * laser_beam.direction.y() + plane.c * laser_beam.direction.z(); - if (a > -TOLERANCE && a < TOLERANCE) { + if (a > -TOLERANCE && a < TOLERANCE) + { return out_point; } @@ -334,7 +373,7 @@ Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const Plane& plane) return out_point; } -Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, float picking_plane_height/*, const ObservationPicking& observation_picking*/) +Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, float picking_plane_height /*, const ObservationPicking& observation_picking*/) { GLint viewport[4]; GLdouble modelview[16]; diff --git a/apps/laz_to_ply/main.cpp b/apps/laz_to_ply/main.cpp index 00a41a5a..7945cf05 100644 --- a/apps/laz_to_ply/main.cpp +++ b/apps/laz_to_ply/main.cpp @@ -1,17 +1,17 @@ #include +#include #include -#include -#include #include -#include +#include +#include -bool check_path_ext(const char *path, const char *ext) +bool check_path_ext(const char* path, const char* ext) { return std::filesystem::path(path).extension() == ext; } -bool convert_and_save(const char *from, const char *to) +bool convert_and_save(const char* from, const char* to) { laszip_POINTER laszip_reader = nullptr; if (laszip_create(&laszip_reader)) @@ -25,13 +25,13 @@ bool convert_and_save(const char *from, const char *to) return false; } - laszip_header *header = nullptr; + laszip_header* header = nullptr; if (laszip_get_header_pointer(laszip_reader, &header)) { return false; } - laszip_point *point = nullptr; + laszip_point* point = nullptr; if (laszip_get_point_pointer(header, &point)) { return false; @@ -66,7 +66,7 @@ bool convert_and_save(const char *from, const char *to) to_file << "property float z\n"; to_file << "end_header\n"; - to_file.write((const char *)points.data(), points.size() * sizeof(float)); + to_file.write((const char*)points.data(), points.size() * sizeof(float)); } else { @@ -76,12 +76,12 @@ bool convert_and_save(const char *from, const char *to) return true; } -int main(const int argc, const char **argv) +int main(const int argc, const char** argv) { const int expected_argc = 3; - const char *expected_laz_extension = ".laz"; - const char *expected_ply_extension = ".ply"; + const char* expected_laz_extension = ".laz"; + const char* expected_ply_extension = ".ply"; if (argc != expected_argc) { @@ -91,8 +91,8 @@ int main(const int argc, const char **argv) return EXIT_FAILURE; } - const char *from = argv[1]; - const char *to = argv[2]; + const char* from = argv[1]; + const char* to = argv[2]; if (!check_path_ext(from, expected_laz_extension)) { diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 7b901c2d..b49b7801 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -27,25 +27,25 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/tomlplusplus/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( lidar_odometry_step_1 PRIVATE Fusion - # PRIVATE ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib + # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/lidar_odometry_step_1/enhanced_version_example.cpp b/apps/lidar_odometry_step_1/enhanced_version_example.cpp index 62fd7120..f0decf0f 100644 --- a/apps/lidar_odometry_step_1/enhanced_version_example.cpp +++ b/apps/lidar_odometry_step_1/enhanced_version_example.cpp @@ -1,107 +1,129 @@ // Enhanced example: Robust version handling when loading TOML configurations // This demonstrates the new version validation and missing version handling -#include "toml_io.h" #include "lidar_odometry_utils.h" +#include "toml_io.h" #include -void example_robust_version_handling() { +void example_robust_version_handling() +{ std::cout << "=== Robust Version Handling Example ===" << std::endl; - + TomlIO toml_io; - + // Test scenarios: std::vector test_configs = { - "modern_config.toml", // Has version info - "legacy_config.toml", // No version info (older config) + "modern_config.toml", // Has version info + "legacy_config.toml", // No version info (older config) "incompatible_config.toml" // Different version }; - - for (const auto& config_file : test_configs) { + + for (const auto& config_file : test_configs) + { std::cout << "\n--- Testing: " << config_file << " ---" << std::endl; - + // 1. Quick version check before loading auto version_info = toml_io.CheckConfigVersion(config_file); - - if (!version_info.found) { + + if (!version_info.found) + { std::cout << "[LEGACY] Legacy config detected (no version info)" << std::endl; std::cout << " -> Will auto-update during load" << std::endl; - } else { + } + else + { std::cout << "[CONFIG] Config version: " << version_info.software_version << std::endl; std::cout << " Created on: " << version_info.build_date << std::endl; - + std::string current = get_software_version(); - if (toml_io.IsVersionCompatible(version_info.software_version, current)) { + if (toml_io.IsVersionCompatible(version_info.software_version, current)) + { std::cout << " Status: [COMPATIBLE] Compatible" << std::endl; - } else { + } + else + { std::cout << " Status: [WARNING] May need update" << std::endl; } } - + // 2. Load configuration (with automatic version handling) LidarOdometryParams params; - if (toml_io.LoadParametersFromTomlFile(config_file, params)) { + if (toml_io.LoadParametersFromTomlFile(config_file, params)) + { std::cout << "[SUCCESS] Configuration loaded successfully!" << std::endl; std::cout << " Final version: " << params.software_version << std::endl; - } else { + } + else + { std::cout << "[ERROR] Failed to load configuration" << std::endl; } } } // Example of handling different version scenarios in application code -void application_version_handling_example() { +void application_version_handling_example() +{ std::cout << "\n=== Application Version Handling ===" << std::endl; - + TomlIO toml_io; std::string config_file = "user_config.toml"; - + // Check version before deciding how to proceed auto version_info = toml_io.CheckConfigVersion(config_file); - - if (!version_info.found) { + + if (!version_info.found) + { std::cout << "[LEGACY] Detected legacy configuration without version info" << std::endl; std::cout << " Options:" << std::endl; std::cout << " 1. Load and auto-update (recommended)" << std::endl; std::cout << " 2. Create backup before loading" << std::endl; std::cout << " 3. Ask user for confirmation" << std::endl; - + // Example: Create backup for safety std::string backup_file = config_file + ".backup"; std::cout << " -> Creating backup: " << backup_file << std::endl; // std::filesystem::copy_file(config_file, backup_file); // Uncomment in real code - - } else { + } + else + { std::string current_version = get_software_version(); - if (!toml_io.IsVersionCompatible(version_info.software_version, current_version)) { + if (!toml_io.IsVersionCompatible(version_info.software_version, current_version)) + { std::cout << "[VERSION] Version mismatch detected" << std::endl; std::cout << " File version: " << version_info.software_version << std::endl; std::cout << " Current version: " << current_version << std::endl; - + // Determine action based on version difference - if (version_info.software_version < current_version) { + if (version_info.software_version < current_version) + { std::cout << " -> Config is older, will upgrade during load" << std::endl; - } else { + } + else + { std::cout << " -> Config is newer, potential compatibility issues" << std::endl; std::cout << " -> Recommend updating HDMapping software" << std::endl; } } } - + // Load with automatic handling LidarOdometryParams params; bool loaded = toml_io.LoadParametersFromTomlFile(config_file, params); - - if (loaded) { + + if (loaded) + { // Check if version was updated and offer to save - if (!version_info.found || version_info.software_version != params.software_version) { + if (!version_info.found || version_info.software_version != params.software_version) + { std::cout << "[SAVE] Configuration was updated with current version" << std::endl; std::cout << " Save updated config? (y/n): "; - + // In real application, get user input char response = 'y'; // Simulated user response - if (response == 'y' || response == 'Y') { - if (toml_io.SaveParametersToTomlFile(config_file, params)) { + if (response == 'y' || response == 'Y') + { + if (toml_io.SaveParametersToTomlFile(config_file, params)) + { std::cout << " [SUCCESS] Updated configuration saved" << std::endl; } } @@ -129,20 +151,20 @@ What happens when version info is missing from TOML: 4. USER FEEDBACK: Console output examples: - + For missing version: WARNING: No version information found in config file: legacy_config.toml This config was created with an older version of HDMapping. -> Updated config with current version: 0.84.0 -> Consider saving the config to persist version information. - + For version mismatch: WARNING: Version mismatch detected: Config version: 0.83.0 Current version: 0.84.0 Config created on: Jul 15 2025 Consider updating the configuration file. - + For compatible version: SUCCESS: Version compatible: 0.84.0 diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index e32b5af1..66c58b22 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -6,7 +6,8 @@ namespace fs = std::filesystem; const float RAD_TO_DEG = 180.0f / static_cast(M_PI); -bool load_data(std::vector& input_file_names, +bool load_data( + std::vector& input_file_names, LidarOdometryParams& params, std::vector>& pointsPerFile, Imu& imu_data, @@ -55,10 +56,11 @@ bool load_data(std::vector& input_file_names, } } - auto get4index = [](const std::string& path) -> std::string { + auto get4index = [](const std::string& path) -> std::string + { auto s = fs::path(path).stem().string(); return s.size() > 4 ? s.substr(s.size() - 4) : s; - }; + }; for (size_t i = 0; i < laz_files.size(); i++) { @@ -197,8 +199,7 @@ bool load_data(std::vector& input_file_names, const int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); // --- Load IMU data (still sequential, could parallelize later) - std::cout << "loading IMU data from 'imu****.csv' using ID " << imuNumberToUse - << " from reference '" << sn_file << "' ..\n"; + std::cout << "loading IMU data from 'imu****.csv' using ID " << imuNumberToUse << " from reference '" << sn_file << "' ..\n"; std::size_t minSize = std::min(laz_files.size(), csv_files.size()); // Each thread loads into its own local vector @@ -206,7 +207,8 @@ bool load_data(std::vector& input_file_names, tbb::parallel_for( tbb::blocked_range(0, minSize), - [&](const tbb::blocked_range& range) { + [&](const tbb::blocked_range& range) + { for (size_t i = range.begin(); i < range.end(); ++i) { imuChunks[i] = load_imu(csv_files[i].c_str(), imuNumberToUse); @@ -218,8 +220,11 @@ bool load_data(std::vector& input_file_names, imu_data.insert(imu_data.end(), std::make_move_iterator(chunk.begin()), std::make_move_iterator(chunk.end())); // Sort once after merging - std::sort(imu_data.begin(), imu_data.end(), - [](const auto& a, const auto& b) { + std::sort( + imu_data.begin(), + imu_data.end(), + [](const auto& a, const auto& b) + { return std::get<0>(a).first < std::get<0>(b).first; }); @@ -229,7 +234,7 @@ bool load_data(std::vector& input_file_names, std::cout << "Loading point clouds (" << minSize << " files)..\n"; std::cout << "total - filter = kept\n-----------------------\n"; - if (debugMsg) //if true this will break parallelism + if (debugMsg) // if true this will break parallelism { for (const auto& [id, calib] : combinedCalibration) std::cout << " id : " << id << "\n" << calib.matrix() << "\n"; @@ -245,14 +250,23 @@ bool load_data(std::vector& input_file_names, params.working_directory_cache = wdp_cache.string(); // --- Parallel load of LAZ files - tbb::parallel_for(size_t(0), minSize, [&](size_t i) + tbb::parallel_for( + size_t(0), + minSize, + [&](size_t i) { const std::string& fn = laz_files[i]; - auto data = load_point_cloud(fn.c_str(), true, params.filter_threshold_xy_inner, params.filter_threshold_xy_outer, combinedCalibration); + auto data = load_point_cloud( + fn.c_str(), true, params.filter_threshold_xy_inner, params.filter_threshold_xy_outer, combinedCalibration); - std::sort(data.begin(), data.end(), [](const Point3Di& a, const Point3Di& b) - { return a.timestamp < b.timestamp; }); + std::sort( + data.begin(), + data.end(), + [](const Point3Di& a, const Point3Di& b) + { + return a.timestamp < b.timestamp; + }); // Optional calibration validation (first file only) if ((i == 0) && params.save_calibration_validation) @@ -271,7 +285,8 @@ bool load_data(std::vector& input_file_names, for (size_t j = 0; j < std::min(data.size(), (size_t)params.calibration_validation_points); ++j) { const auto& p = data[j]; - testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" << (int)p.lidarid << "\n"; + testPointcloud << p.point.x() << "\t" << p.point.y() << "\t" << p.point.z() << "\t" << p.intensity << "\t" + << (int)p.lidarid << "\n"; } } @@ -298,7 +313,14 @@ bool load_data(std::vector& input_file_names, } void calculate_trajectory( - Trajectory& trajectory, Imu& imu_data, bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, double ahrs_gain, bool debugMsg, bool use_remove_imu_bias_from_first_stationary_scan) + Trajectory& trajectory, + Imu& imu_data, + bool fusionConventionNwu, + bool fusionConventionEnu, + bool fusionConventionNed, + double ahrs_gain, + bool debugMsg, + bool use_remove_imu_bias_from_first_stationary_scan) { FusionAhrs ahrs; FusionAhrsInitialise(&ahrs); @@ -360,10 +382,9 @@ void calculate_trajectory( for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { - static_cast(gyr.axis.x * RAD_TO_DEG) - static_cast(bias_gyr_x), - static_cast(gyr.axis.y * RAD_TO_DEG) - static_cast(bias_gyr_y), - static_cast(gyr.axis.z * RAD_TO_DEG) - static_cast(bias_gyr_z) }; + const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG) - static_cast(bias_gyr_x), + static_cast(gyr.axis.y * RAD_TO_DEG) - static_cast(bias_gyr_y), + static_cast(gyr.axis.z * RAD_TO_DEG) - static_cast(bias_gyr_z) }; const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; /*if (provious_time_stamp == 0.0) @@ -432,7 +453,13 @@ void calculate_trajectory( counter++; if (counter % 100 == 0) { - printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, (int)imu_data.size()); + printf( + "Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", + euler.angle.roll, + euler.angle.pitch, + euler.angle.yaw, + counter++, + (int)imu_data.size()); } } @@ -441,7 +468,11 @@ void calculate_trajectory( } bool compute_step_1( - std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, const std::atomic& pause) + std::vector>& pointsPerFile, + LidarOdometryParams& params, + Trajectory& trajectory, + std::vector& worker_data, + const std::atomic& pause) { int number_of_initial_points = 0; double timestamp_begin = 0.0; @@ -512,7 +543,7 @@ bool compute_step_1( if (i % 50 == 0) std::cout << "\rrunning iterations: " << i + 1 << "/" << n_iter << std::flush; - WorkerData& wd = worker_data.emplace_back(); // construct in-place + WorkerData& wd = worker_data.emplace_back(); // construct in-place wd.intermediate_trajectory.reserve(threshold); wd.intermediate_trajectory_motion_model.reserve(threshold); wd.intermediate_trajectory_timestamps.reserve(threshold); @@ -537,14 +568,16 @@ bool compute_step_1( for (size_t index = 0; index < pointsPerFile.size(); index++) { auto lower = std::lower_bound( - pointsPerFile[index].begin(), pointsPerFile[index].end(), + pointsPerFile[index].begin(), + pointsPerFile[index].end(), wd.intermediate_trajectory_timestamps[0].first, [](const Point3Di& point, double timestamp) { return point.timestamp < timestamp; }); auto upper = std::lower_bound( - pointsPerFile[index].begin(), pointsPerFile[index].end(), + pointsPerFile[index].begin(), + pointsPerFile[index].end(), wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first, [](const Point3Di& point, double timestamp) { @@ -560,15 +593,15 @@ bool compute_step_1( // correct points timestamps if (wd.intermediate_trajectory_timestamps.size() > 2) { - double ts_begin = wd.intermediate_trajectory_timestamps[0].first; double ts_step = (wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first - - wd.intermediate_trajectory_timestamps[0].first) / + wd.intermediate_trajectory_timestamps[0].first) / points.size(); // std::cout << "ts_begin " << ts_begin << std::endl; // std::cout << "ts_step " << ts_step << std::endl; - // std::cout << "ts_end " << wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first << std::endl; + // std::cout << "ts_end " << wd.intermediate_trajectory_timestamps[wd.intermediate_trajectory_timestamps.size() - 1].first << + // std::endl; for (size_t pp = 0; pp < points.size(); pp++) points[pp].timestamp = ts_begin + pp * ts_step; @@ -578,9 +611,14 @@ bool compute_step_1( for (size_t k = 0; k < points.size(); k++) { Point3Di& p = points[k]; - auto lower = std::lower_bound(wd.intermediate_trajectory_timestamps.begin(), wd.intermediate_trajectory_timestamps.end(), p.timestamp, + auto lower = std::lower_bound( + wd.intermediate_trajectory_timestamps.begin(), + wd.intermediate_trajectory_timestamps.end(), + p.timestamp, [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + { + return lhs.first < rhs; + }); // p.index_pose = std::distance(wd.intermediate_trajectory_timestamps.begin(), lower); int index_pose = std::distance(wd.intermediate_trajectory_timestamps.begin(), lower) - 1; @@ -592,8 +630,13 @@ bool compute_step_1( p.index_pose = -1; } - auto it = std::remove_if(points.begin(), points.end(), - [](const Point3Di& p) { return p.index_pose == -1; }); + auto it = std::remove_if( + points.begin(), + points.end(), + [](const Point3Di& p) + { + return p.index_pose == -1; + }); points.erase(it, points.end()); if (points.size() <= 1000) @@ -620,12 +663,14 @@ bool compute_step_1( if (intermediate_points.size() > 0) { if (!save_vector_data(wd.intermediate_points_cache_file_name.string(), intermediate_points)) - std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" + << std::endl; } - else //ensure we always have intermediate points + else // ensure we always have intermediate points { if (!save_vector_data(wd.intermediate_points_cache_file_name.string(), points)) - std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with save_vector_data for file '" << wd.intermediate_points_cache_file_name.string() << "'" + << std::endl; } } @@ -641,7 +686,7 @@ bool compute_step_1( return true; } -void run_consistency(std::vector &worker_data, LidarOdometryParams ¶ms) +void run_consistency(std::vector& worker_data, LidarOdometryParams& params) { std::cout << "Point cloud consistency and trajectory smoothness START" << std::endl; for (int i = 0; i < params.num_constistency_iter; i++) @@ -663,7 +708,7 @@ void run_consistency(std::vector &worker_data, LidarOdometryParams & std::cout << "Point cloud consistency and trajectory smoothness FINISHED" << std::endl; } -void save_result(std::vector &worker_data, LidarOdometryParams ¶ms, fs::path outwd, double elapsed_time_s) +void save_result(std::vector& worker_data, LidarOdometryParams& params, fs::path outwd, double elapsed_time_s) { std::filesystem::create_directory(outwd); // concatenate data @@ -680,7 +725,8 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ std::vector original_points; // = worker_data[i].load_points(worker_data[i].original_points_cache_file_name); if (!load_vector_data(worker_data[i].original_points_cache_file_name.string(), original_points)) { - std::cout << "problem with load_vector_data file '" << worker_data[i].original_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with load_vector_data file '" << worker_data[i].original_points_cache_file_name.string() << "'" + << std::endl; } if (i % 1000 == 0) @@ -699,19 +745,22 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ } } tmp_data = filtered_local_point_cloud;*/ - for (auto &t : tmp_data) + for (auto& t : tmp_data) { t.index_pose += pose_offset; } - wd.intermediate_trajectory.insert(std::end(wd.intermediate_trajectory), - std::begin(worker_data[i].intermediate_trajectory), std::end(worker_data[i].intermediate_trajectory)); + wd.intermediate_trajectory.insert( + std::end(wd.intermediate_trajectory), + std::begin(worker_data[i].intermediate_trajectory), + std::end(worker_data[i].intermediate_trajectory)); - wd.intermediate_trajectory_timestamps.insert(std::end(wd.intermediate_trajectory_timestamps), - std::begin(worker_data[i].intermediate_trajectory_timestamps), std::end(worker_data[i].intermediate_trajectory_timestamps)); + wd.intermediate_trajectory_timestamps.insert( + std::end(wd.intermediate_trajectory_timestamps), + std::begin(worker_data[i].intermediate_trajectory_timestamps), + std::end(worker_data[i].intermediate_trajectory_timestamps)); - original_points_to_save.insert(std::end(original_points_to_save), - std::begin(tmp_data), std::end(tmp_data)); + original_points_to_save.insert(std::end(original_points_to_save), std::begin(tmp_data), std::end(tmp_data)); wd.imu_om_fi_ka.insert(std::end(wd.imu_om_fi_ka), std::begin(worker_data[i].imu_om_fi_ka), std::end(worker_data[i].imu_om_fi_ka)); pose_offset += worker_data[i].intermediate_trajectory.size(); @@ -729,7 +778,8 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ if (!save_vector_data(wd.original_points_to_save_cache_file_name.string(), original_points_to_save)) { - std::cout << "problem with save_vector_data for file '" << wd.original_points_to_save_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with save_vector_data for file '" << wd.original_points_to_save_cache_file_name.string() << "'" + << std::endl; std::cout << __FILE__ << " " << __LINE__ << std::endl; } @@ -758,7 +808,8 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ if (!save_vector_data(wd.original_points_to_save_cache_file_name.string(), original_points_to_save)) { - std::cout << "problem with save_vector_data for file '" << wd.original_points_to_save_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with save_vector_data for file '" << wd.original_points_to_save_cache_file_name.string() << "'" + << std::endl; std::cout << __FILE__ << " " << __LINE__ << std::endl; } @@ -799,19 +850,28 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ std::vector original_points_to_save; // = worker_data[i].load_points(worker_data[i].original_points_cache_file_name); if (!load_vector_data(worker_data_concatenated[i].original_points_to_save_cache_file_name.string(), original_points_to_save)) { - std::cout << "problem with load_vector_data file '" << worker_data_concatenated[i].original_points_to_save_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with load_vector_data file '" + << worker_data_concatenated[i].original_points_to_save_cache_file_name.string() << "'" << std::endl; std::cout << __FILE__ << " " << __LINE__ << std::endl; } points_to_vector( - original_points_to_save, worker_data_concatenated[i].intermediate_trajectory, - params.threshould_output_filter, &index_poses_i, global_pointcloud, intensity, timestamps, true, params.save_index_pose); + original_points_to_save, + worker_data_concatenated[i].intermediate_trajectory, + params.threshould_output_filter, + &index_poses_i, + global_pointcloud, + intensity, + timestamps, + true, + params.save_index_pose); exportLaz(path.string(), global_pointcloud, intensity, timestamps); - - if(params.save_index_pose){ + + if (params.save_index_pose) + { index_poses.push_back(index_poses_i); } - + m_poses.push_back(worker_data_concatenated[i].intermediate_trajectory[0]); file_names.push_back(filename); @@ -830,30 +890,21 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ return; } - outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 timestampUnix_nanoseconds om_rad fi_rad ka_rad" << std::endl; + outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 " + "timestampUnix_nanoseconds om_rad fi_rad ka_rad" + << std::endl; for (int j = 0; j < worker_data_concatenated[i].intermediate_trajectory.size(); j++) { - auto pose = worker_data_concatenated[i].intermediate_trajectory[0].inverse() * worker_data_concatenated[i].intermediate_trajectory[j]; - - outfile - << std::setprecision(20) << worker_data_concatenated[i].intermediate_trajectory_timestamps[j].first * 1e9 << " " << std::setprecision(10) - << pose(0, 0) << " " - << pose(0, 1) << " " - << pose(0, 2) << " " - << pose(0, 3) << " " - << pose(1, 0) << " " - << pose(1, 1) << " " - << pose(1, 2) << " " - << pose(1, 3) << " " - << pose(2, 0) << " " - << pose(2, 1) << " " - << pose(2, 2) << " " - << pose(2, 3) << " " - << std::setprecision(20) << worker_data_concatenated[i].intermediate_trajectory_timestamps[j].second * 1e9 << " " - << worker_data_concatenated[i].imu_om_fi_ka[j].x() << " " - << worker_data_concatenated[i].imu_om_fi_ka[j].y() << " " - << worker_data_concatenated[i].imu_om_fi_ka[j].z() << " " - << std::endl; + auto pose = + worker_data_concatenated[i].intermediate_trajectory[0].inverse() * worker_data_concatenated[i].intermediate_trajectory[j]; + + outfile << std::setprecision(20) << worker_data_concatenated[i].intermediate_trajectory_timestamps[j].first * 1e9 << " " + << std::setprecision(10) << pose(0, 0) << " " << pose(0, 1) << " " << pose(0, 2) << " " << pose(0, 3) << " " + << pose(1, 0) << " " << pose(1, 1) << " " << pose(1, 2) << " " << pose(1, 3) << " " << pose(2, 0) << " " << pose(2, 1) + << " " << pose(2, 2) << " " << pose(2, 3) << " " << std::setprecision(20) + << worker_data_concatenated[i].intermediate_trajectory_timestamps[j].second * 1e9 << " " + << worker_data_concatenated[i].imu_om_fi_ka[j].x() << " " << worker_data_concatenated[i].imu_om_fi_ka[j].y() << " " + << worker_data_concatenated[i].imu_om_fi_ka[j].z() << " " << std::endl; } outfile.close(); // @@ -866,7 +917,8 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ save_poses(path2.string(), m_poses, file_names); fs::path index_poses_path = outwd / "index_poses.json"; - if (params.save_index_pose){ + if (params.save_index_pose) + { nlohmann::json j_index_poses = index_poses; std::ofstream out_index(index_poses_path); if (!out_index) @@ -900,7 +952,8 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ j["lidar_odometry_version"] = HDMAPPING_VERSION_STRING; j["length of trajectory[m]"] = params.total_length_of_calculated_trajectory; j["elapsed time seconds"] = elapsed_time_s; - if (params.save_index_pose){ + if (params.save_index_pose) + { j["index_poses_path"] = index_poses_path.string(); } j["point_sizes_path"] = point_sizes_path.string(); @@ -917,8 +970,7 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ path /= filename; std::cout << "adding file: " << path << std::endl; - nlohmann::json jfn{ - {"file_name", path.string()}}; + nlohmann::json jfn{ { "file_name", path.string() } }; jlaz_file_names.push_back(jfn); } jj["laz_file_names"] = jlaz_file_names; @@ -933,17 +985,16 @@ void save_result(std::vector &worker_data, LidarOdometryParams ¶ // Save processing results and complex data to JSON file save_processing_results_json(params, outwd, elapsed_time_s); - - //remove cache - //std::cout << "remove cache: '" << params.working_directory_cache << "' START" << std::endl; - //std::filesystem::remove_all(params.working_directory_cache); - //std::cout << "remove cache: '" << params.working_directory_cache << "' FINISHED" << std::endl; + // remove cache + // std::cout << "remove cache: '" << params.working_directory_cache << "' START" << std::endl; + // std::filesystem::remove_all(params.working_directory_cache); + // std::cout << "remove cache: '" << params.working_directory_cache << "' FINISHED" << std::endl; } -void filter_reference_buckets(LidarOdometryParams ¶ms) +void filter_reference_buckets(LidarOdometryParams& params) { NDTBucketMapType reference_buckets_out; - for (const auto &b : params.reference_buckets) + for (const auto& b : params.reference_buckets) { if (b.second.number_of_points > 10) { @@ -965,13 +1016,13 @@ void filter_reference_buckets(LidarOdometryParams ¶ms) params.reference_buckets = reference_buckets_out; } -void save_trajectory_to_ascii(std::vector &worker_data, std::string output_file_name) +void save_trajectory_to_ascii(std::vector& worker_data, std::string output_file_name) { ofstream file; file.open(output_file_name); - for (const auto &wd : worker_data) + for (const auto& wd : worker_data) { - for (const auto &it : wd.intermediate_trajectory) + for (const auto& it : wd.intermediate_trajectory) { file << it(0, 3) << " " << it(1, 3) << " " << it(2, 3) << std::endl; } @@ -979,7 +1030,7 @@ void save_trajectory_to_ascii(std::vector &worker_data, std::string file.close(); } -void load_reference_point_clouds(std::vector input_file_names, LidarOdometryParams ¶ms) +void load_reference_point_clouds(std::vector input_file_names, LidarOdometryParams& params) { params.reference_buckets.clear(); params.reference_points.clear(); @@ -996,19 +1047,20 @@ void load_reference_point_clouds(std::vector input_file_names, Lida params.buckets_indoor = params.reference_buckets; } -std::string save_results_automatic(LidarOdometryParams ¶ms, std::vector &worker_data, std::string working_directory, double elapsed_seconds) +std::string save_results_automatic( + LidarOdometryParams& params, std::vector& worker_data, std::string working_directory, double elapsed_seconds) { fs::path outwd = get_next_result_path(working_directory); save_result(worker_data, params, outwd, elapsed_seconds); return outwd.string(); } -std::vector run_lidar_odometry(std::string input_dir, LidarOdometryParams ¶ms) +std::vector run_lidar_odometry(std::string input_dir, LidarOdometryParams& params) { Session session; std::vector worker_data; std::vector input_file_names; - for (const auto &entry : std::filesystem::directory_iterator(input_dir)) + for (const auto& entry : std::filesystem::directory_iterator(input_dir)) { if (fs::is_regular_file(entry)) { @@ -1023,9 +1075,17 @@ std::vector run_lidar_odometry(std::string input_dir, LidarOdometryP return worker_data; } Trajectory trajectory; - calculate_trajectory(trajectory, imu_data, params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, params.ahrs_gain, true, params.use_removie_imu_bias_from_first_stationary_scan); - - std::atomic pause{false}; + calculate_trajectory( + trajectory, + imu_data, + params.fusionConventionNwu, + params.fusionConventionEnu, + params.fusionConventionNed, + params.ahrs_gain, + true, + params.use_removie_imu_bias_from_first_stationary_scan); + + std::atomic pause{ false }; if (!compute_step_1(pointsPerFile, params, trajectory, worker_data, pause)) { @@ -1043,12 +1103,10 @@ std::vector run_lidar_odometry(std::string input_dir, LidarOdometryP return worker_data; } - - return worker_data; } -void save_parameters_toml(const LidarOdometryParams ¶ms, const fs::path &outwd, double elapsed_seconds) +void save_parameters_toml(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds) { // Get current date and time auto now = std::chrono::system_clock::now(); @@ -1082,14 +1140,13 @@ void save_parameters_toml(const LidarOdometryParams ¶ms, const fs::path &out { std::cerr << "Failed to save parameters to TOML file: " << toml_path << std::endl; } - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error saving parameters to TOML file: " << e.what() << std::endl; } } -void save_processing_results_json(const LidarOdometryParams ¶ms, const fs::path &outwd, double elapsed_seconds) +void save_processing_results_json(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds) { // Get current date and time auto now = std::chrono::system_clock::now(); @@ -1153,7 +1210,7 @@ void save_processing_results_json(const LidarOdometryParams ¶ms, const fs::p results["motion_model_correction"]["ka"] = params.motion_model_correction.ka; // Transformation matrix (if needed for debugging) - auto &m_g = params.m_g; + auto& m_g = params.m_g; for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) @@ -1175,8 +1232,7 @@ void save_processing_results_json(const LidarOdometryParams ¶ms, const fs::p { std::cerr << "Failed to create results JSON file: " << json_path << std::endl; } - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error saving processing results to JSON file: " << e.what() << std::endl; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 289091f0..667dab9a 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -1,37 +1,51 @@ -#ifndef _LIDAR_ODOMETRY_H_ -#define _LIDAR_ODOMETRY_H_ +#pragma once + +#include +#include #include "lidar_odometry_utils.h" -#include +#include "toml_io.h" #include -#include #include +#include #include -#include -#include -#include "toml_io.h" +#include //#define SAMPLE_PERIOD (1.0 / 200.0) using Trajectory = std::map>; using Imu = std::vector, FusionVector, FusionVector>>; -bool load_data(std::vector& input_file_names, LidarOdometryParams& params, std::vector>& pointsPerFile, Imu& imu_data, bool debugMsg); +bool load_data( + std::vector& input_file_names, + LidarOdometryParams& params, + std::vector>& pointsPerFile, + Imu& imu_data, + bool debugMsg); void calculate_trajectory( - Trajectory &trajectory, Imu &imu_data, bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, double ahrs_gain, bool debugMsg, bool use_removie_imu_bias_from_first_stationary_scan); + Trajectory& trajectory, + Imu& imu_data, + bool fusionConventionNwu, + bool fusionConventionEnu, + bool fusionConventionNed, + double ahrs_gain, + bool debugMsg, + bool use_removie_imu_bias_from_first_stationary_scan); bool compute_step_1( - std::vector> &pointsPerFile, LidarOdometryParams ¶ms, - Trajectory &trajectory, std::vector &worker_data, const std::atomic &pause); + std::vector>& pointsPerFile, + LidarOdometryParams& params, + Trajectory& trajectory, + std::vector& worker_data, + const std::atomic& pause); void run_consistency(std::vector& worker_data, LidarOdometryParams& params); void filter_reference_buckets(LidarOdometryParams& params); void load_reference_point_clouds(std::vector input_file_names, LidarOdometryParams& params); -void save_result(std::vector &worker_data, LidarOdometryParams ¶ms, fs::path outwd, double elapsed_seconds); -void save_parameters_toml(const LidarOdometryParams ¶ms, const fs::path &outwd, double elapsed_seconds); -void save_processing_results_json(const LidarOdometryParams ¶ms, const fs::path &outwd, double elapsed_seconds); +void save_result(std::vector& worker_data, LidarOdometryParams& params, fs::path outwd, double elapsed_seconds); +void save_parameters_toml(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds); +void save_processing_results_json(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds); void save_trajectory_to_ascii(std::vector& worker_data, std::string output_file_name); std::string save_results_automatic( - LidarOdometryParams ¶ms, std::vector &worker_data, std::string working_directory, double elapsed_seconds); + LidarOdometryParams& params, std::vector& worker_data, std::string working_directory, double elapsed_seconds); std::vector run_lidar_odometry(std::string input_dir, LidarOdometryParams& params); // bool SaveParametersToTomlFile(const std::string &filepath, const LidarOdometryParams ¶ms); // bool LoadParametersFromTomlFile(const std::string &filepath, LidarOdometryParams ¶ms); -#endif \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 5a7ad649..2ecf9ca8 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -1,40 +1,43 @@ #include #include #include -#include #include +#include + //#define GLEW_STATIC //#include #include #include -#include "lidar_odometry_utils.h" #include "lidar_odometry.h" -#include +#include "lidar_odometry_utils.h" #include +#include #include -#include +#include "toml_io.h" #include -#include -#include -#include -#include #include -#include "toml_io.h" +#include +#include +#include +#include +#include #ifdef _WIN32 -#include #include "resource.h" +#include + #endif /////////////////////////////////////////////////////////////////////////////////// // This is LiDAR odometry (step 1) -// This program calculates trajectory based on IMU and LiDAR data provided by MANDEYE mobile mapping system https://github.com/JanuszBedkowski/mandeye_controller -// The output is a session proving trajekctory and point clouds that can be further processed by "multi_view_tls_registration" program. +// This program calculates trajectory based on IMU and LiDAR data provided by MANDEYE mobile mapping system +// https://github.com/JanuszBedkowski/mandeye_controller The output is a session proving trajekctory and point clouds that can be further +// processed by "multi_view_tls_registration" program. // #define SAMPLE_PERIOD (1.0 / 200.0) @@ -45,90 +48,90 @@ std::vector infoLines = { "", "It results trajectory and point clouds as single session for 'multi_view_tls_registration_step_2' program.", "", - "Next step will be to load session.json file with 'multi_view_tls_registration_step_2' program."}; + "Next step will be to load session.json file with 'multi_view_tls_registration_step_2' program." +}; // App specific shortcuts (Type and Shortcut are just for easy reference) -static const std::vector appShortcuts = { - {"Normal keys", "A", ""}, - {"", "Ctrl+A", ""}, - {"", "B", ""}, - {"", "Ctrl+B", ""}, - {"", "C", ""}, - {"", "Ctrl+C", ""}, - {"", "D", ""}, - {"", "Ctrl+D", ""}, - {"", "E", ""}, - {"", "Ctrl+E", ""}, - {"", "F", ""}, - {"", "Ctrl+F", ""}, - {"", "G", ""}, - {"", "Ctrl+G", ""}, - {"", "H", ""}, - {"", "Ctrl+H", ""}, - {"", "I", ""}, - {"", "Ctrl+I", ""}, - {"", "J", ""}, - {"", "Ctrl+K", ""}, - {"", "K", ""}, - {"", "Ctrl+K", ""}, - {"", "L", ""}, - {"", "Ctrl+L", ""}, - {"", "M", ""}, - {"", "Ctrl+M", ""}, - {"", "N", ""}, - {"", "Ctrl+N", ""}, - {"", "O", ""}, - {"", "Ctrl+O", "Open data"}, - {"", "P", ""}, - {"", "Ctrl+P", ""}, - {"", "Q", ""}, - {"", "Ctrl+Q", ""}, - {"", "R", ""}, - {"", "Ctrl+R", ""}, - {"", "Shift+R", ""}, - {"", "S", ""}, - {"", "Ctrl+S", ""}, - {"", "Ctrl+Shift+S", ""}, - {"", "T", ""}, - {"", "Ctrl+T", ""}, - {"", "U", ""}, - {"", "Ctrl+U", ""}, - {"", "V", ""}, - {"", "Ctrl+V", ""}, - {"", "W", ""}, - {"", "Ctrl+W", ""}, - {"", "X", ""}, - {"", "Ctrl+X", ""}, - {"", "Y", ""}, - {"", "Ctrl+Y", ""}, - {"", "Z", ""}, - {"", "Ctrl+Z", ""}, - {"", "Shift+Z", ""}, - {"", "1-9", ""}, - {"Special keys", "Up arrow", ""}, - {"", "Shift + up arrow", ""}, - {"", "Ctrl + up arrow", ""}, - {"", "Down arrow", ""}, - {"", "Shift + down arrow", ""}, - {"", "Ctrl + down arrow", ""}, - {"", "Left arrow", ""}, - {"", "Shift + left arrow", ""}, - {"", "Ctrl + left arrow", ""}, - {"", "Right arrow", ""}, - {"", "Shift + right arrow", ""}, - {"", "Ctrl + right arrow", ""}, - {"", "Pg down", ""}, - {"", "Pg up", ""}, - {"", "- key", ""}, - {"", "+ key", ""}, - {"Mouse related", "Left click + drag", ""}, - {"", "Right click + drag", "n"}, - {"", "Scroll", ""}, - {"", "Shift + scroll", ""}, - {"", "Shift + drag", ""}, - {"", "Ctrl + left click", ""}, - {"", "Ctrl + right click", ""}, - {"", "Ctrl + middle click", ""} }; +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open data" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; namespace fs = std::filesystem; @@ -156,10 +159,7 @@ std::vector worker_data; std::string working_directory = ""; bool initial_transformation_gizmo = false; -float m_gizmo[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; float x_displacement = 0.01; @@ -177,12 +177,12 @@ Session session; std::vector> pointsPerFile; Imu imu_data; Trajectory trajectory; -std::atomic loRunning{false}; -std::atomic loProgress{0.0}; -std::atomic loPause{false}; +std::atomic loRunning{ false }; +std::atomic loProgress{ 0.0 }; +std::atomic loPause{ false }; std::chrono::time_point loStartTime; -std::atomic loElapsedSeconds{0.0}; -std::atomic loEstimatedTimeRemaining{0.0}; +std::atomic loElapsedSeconds{ 0.0 }; +std::atomic loEstimatedTimeRemaining{ 0.0 }; #if _WIN32 #define DEFAULT_PATH "C:\\" @@ -192,7 +192,7 @@ std::atomic loEstimatedTimeRemaining{0.0}; /////////////////////////////////////////////////////////////////////////////////// -void set_lidar_odometry_default_params(LidarOdometryParams ¶ms) +void set_lidar_odometry_default_params(LidarOdometryParams& params) { params.decimation = 0.01; params.in_out_params_indoor.resolution_X = 0.1; @@ -309,7 +309,8 @@ int get_index(set s, int k) return -1; } -void find_best_stretch(std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) +void find_best_stretch( + std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) { for (size_t i = 0; i < points.size(); i++) { @@ -595,7 +596,7 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observ return pos; } -void step1(const std::atomic &loPause) +void step1(const std::atomic& loPause) { std::string input_folder_name; std::vector input_file_names; @@ -608,26 +609,31 @@ void step1(const std::atomic &loPause) std::string newTitle = winTitle + " - ..\\" + std::filesystem::path(input_folder_name).filename().string(); glutSetWindowTitle(newTitle.c_str()); - for (const auto &entry : fs::directory_iterator(input_folder_name)) + for (const auto& entry : fs::directory_iterator(input_folder_name)) if (entry.is_regular_file()) input_file_names.push_back(entry.path().string()); if (load_data(input_file_names, params, pointsPerFile, imu_data, full_debug_messages)) { working_directory = fs::path(input_file_names[0]).parent_path().string(); - calculate_trajectory(trajectory, imu_data, params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, params.ahrs_gain, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); + calculate_trajectory( + trajectory, + imu_data, + params.fusionConventionNwu, + params.fusionConventionEnu, + params.fusionConventionNed, + params.ahrs_gain, + full_debug_messages, + params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); step_1_done = true; } else { - std::string message_info = "Problem with loading data from folder '" + input_folder_name + "' (Pease check if folder exists). Program will close once You click OK!!!"; + std::string message_info = "Problem with loading data from folder '" + input_folder_name + + "' (Pease check if folder exists). Program will close once You click OK!!!"; std::cout << message_info << std::endl; - [[maybe_unused]] - pfd::message message( - "Information", - message_info.c_str(), - pfd::choice::ok, pfd::icon::info); + [[maybe_unused]] pfd::message message("Information", message_info.c_str(), pfd::choice::ok, pfd::icon::info); message.result(); exit(1); @@ -635,7 +641,7 @@ void step1(const std::atomic &loPause) } } -void step2(const std::atomic &loPause) +void step2(const std::atomic& loPause) { double ts_failure = 0.0; if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages)) @@ -644,7 +650,7 @@ void step2(const std::atomic &loPause) { for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) { - const std::string &imufn = csv_files.at(fileNo); + const std::string& imufn = csv_files.at(fileNo); const std::string snFn = (fileNo >= sn_files.size()) ? ("") : (sn_files.at(fileNo)); const auto idToSn = MLvxCalib::GetIdToSnMapping(snFn); // GetId of Imu to use @@ -670,11 +676,7 @@ void save_results(bool info, double elapsed_seconds) { std::string message_info = "Results saved to folder: '" + outwd.string() + "'"; std::cout << message_info << std::endl; - [[maybe_unused]] - pfd::message message( - "Information", - message_info.c_str(), - pfd::choice::ok, pfd::icon::info); + [[maybe_unused]] pfd::message message("Information", message_info.c_str(), pfd::choice::ok, pfd::icon::info); message.result(); } } @@ -781,7 +783,7 @@ void settings_gui() if (!simple_gui) { - ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputInt("Threshold nr poses", ¶ms.threshold_nr_poses); if (params.threshold_nr_poses < 1) { @@ -890,7 +892,8 @@ void settings_gui() ImGui::SetTooltip("Local LiDAR coordinates"); ImGui::InputInt("Number of iterations", ¶ms.nr_iter); - ImGui::InputDouble("Sliding window trajectory length threshold [m]", ¶ms.sliding_window_trajectory_length_threshold, 0.0, 0.0, "%.2f"); + ImGui::InputDouble( + "Sliding window trajectory length threshold [m]", ¶ms.sliding_window_trajectory_length_threshold, 0.0, 0.0, "%.2f"); ImGui::InputInt("Threshold initial points", ¶ms.threshold_initial_points); ImGui::NewLine(); @@ -902,7 +905,8 @@ void settings_gui() ImGui::Text("Fusion convention: "); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame"); + ImGui::SetTooltip( + "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame"); ImGui::SameLine(); ImGui::RadioButton("NWU", &fusionConvention, 0); @@ -930,7 +934,8 @@ void settings_gui() { ImGui::BeginTooltip(); ImGui::Text("Attitude and Heading Reference System gain:"); - ImGui::Text("How strongly the accelerometer/magnetometer corrections influence the orientation estimate versus gyroscope integration"); + ImGui::Text("How strongly the accelerometer/magnetometer corrections influence the orientation estimate versus gyroscope " + "integration"); ImGui::Text("Larger value means faster response to changes in orientation, but more noise"); ImGui::EndTooltip(); } @@ -943,7 +948,7 @@ void settings_gui() if (ImGui::Button("Load data")) { loStartTime = std::chrono::system_clock::now(); - + step1(loPause); std::cout << "Load data done please click 'Compute all' to continue calculations" << std::endl; @@ -951,10 +956,10 @@ void settings_gui() double elapsedSeconds = elapsed.count(); std::cout << "Elapsed time: " << formatTime(elapsedSeconds).c_str() << std::endl; - } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Select folder containing IMU *.csv and LiDAR *.laz files produced by MANDEYE (e.g.: 'continousScanning_*')"); + ImGui::SetTooltip( + "Select folder containing IMU *.csv and LiDAR *.laz files produced by MANDEYE (e.g.: 'continousScanning_*')"); } if (step_1_done && !step_2_done) { @@ -972,11 +977,11 @@ void settings_gui() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("This process makes trajectory smooth, point cloud will be more consistent"); + ImGui::Text("This process makes trajectory smooth, point cloud will be more consistent"); ImGui::SetTooltip("Press optionally before pressing 'Save result'"); ImGui::EndTooltip(); } - + ImGui::SameLine(); ImGui::Checkbox("Use multiple Gaussians for each bucket", ¶ms.use_mutliple_gaussian); if (ImGui::IsItemHovered()) @@ -986,11 +991,11 @@ void settings_gui() save_results(true, 0.0); ImGui::SameLine(); - ImGui::Text("Press this button for saving resulting trajectory and point clouds as single session for 'multi_view_tls_registration_step_2' program"); + ImGui::Text("Press this button for saving resulting trajectory and point clouds as single session for " + "'multi_view_tls_registration_step_2' program"); } if (step_1_done && step_2_done) { - if (ImGui::Button("Save all point clouds to single las/laz file")) { const auto output_file_name = mandeye::fd::SaveFileDialog("Save las/laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); @@ -1275,7 +1280,8 @@ void settings_gui() << "scan_0.laz" << std::endl << "0.999775 0.000552479 -0.0212158 -0.0251188" << std::endl << "0.000834612 0.997864 0.0653156 -0.0381429" << std::endl - << "0.0212066 - 0.0653186 0.997639 -0.000757752" << "0 0 0 1" << std::endl + << "0.0212066 - 0.0653186 0.997639 -0.000757752" + << "0 0 0 1" << std::endl << "scan_1.laz" << std::endl << "0.999783 0.00178963 -0.0207603 -0.0309683" << std::endl << "-0.000467341 0.99798 0.0635239 -0.0517512" << std::endl @@ -1389,9 +1395,12 @@ void settings_gui() { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(worker_data[i].intermediate_trajectory[j]); - pose.px = first_pose.translation().x() + translation.x() * (counter / number_all_nodes_inside_interval); - pose.py = first_pose.translation().y() + translation.y() * (counter / number_all_nodes_inside_interval); - pose.pz = first_pose.translation().z() + translation.z() * (counter / number_all_nodes_inside_interval); + pose.px = + first_pose.translation().x() + translation.x() * (counter / number_all_nodes_inside_interval); + pose.py = + first_pose.translation().y() + translation.y() * (counter / number_all_nodes_inside_interval); + pose.pz = + first_pose.translation().z() + translation.z() * (counter / number_all_nodes_inside_interval); counter += 1.0f; @@ -1455,8 +1464,8 @@ void progress_window() { ImGui::Begin("Progress", nullptr, ImGuiWindowFlags_AlwaysAutoResize); - ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); - + ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); + // Calculate elapsed time and ETA auto currentTime = std::chrono::system_clock::now(); std::chrono::duration elapsed = currentTime - loStartTime; @@ -1481,18 +1490,18 @@ void progress_window() { std::string completionTime = formatCompletionTime(estimatedTimeRemaining); snprintf(progressText, sizeof(progressText), "Processing: %.1f%%", progress * 100.0f); - snprintf(timeInfo, sizeof(timeInfo), - "Elapsed: %s | Remaining: %s | Estimated finish: ~%s", - formatTime(elapsedSeconds).c_str(), - formatTime(estimatedTimeRemaining).c_str(), - completionTime.c_str()); + snprintf( + timeInfo, + sizeof(timeInfo), + "Elapsed: %s | Remaining: %s | Estimated finish: ~%s", + formatTime(elapsedSeconds).c_str(), + formatTime(estimatedTimeRemaining).c_str(), + completionTime.c_str()); } else { snprintf(progressText, sizeof(progressText), "Processing: %.1f%%", progress * 100.0f); - snprintf(timeInfo, sizeof(timeInfo), - "Elapsed: %s | Calculating completion time...", - formatTime(elapsedSeconds).c_str()); + snprintf(timeInfo, sizeof(timeInfo), "Elapsed: %s | Calculating completion time...", formatTime(elapsedSeconds).c_str()); } ImGui::ProgressBar(progress, ImVec2(-1.0f, 0.0f), progressText); @@ -1556,37 +1565,33 @@ void openData() std::ostringstream oss; oss << "Data saved to folder:\n'" << working_directory << "\\lio_result_*'\n" - << "Calculated trajectory length: " - << std::fixed << std::setprecision(1) << params.total_length_of_calculated_trajectory << "[m]\n" + << "Calculated trajectory length: " << std::fixed << std::setprecision(1) + << params.total_length_of_calculated_trajectory << "[m]\n" << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str(); - [[maybe_unused]] - pfd::message message( - "Information", - oss.str(), - pfd::choice::ok, pfd::icon::info); + [[maybe_unused]] pfd::message message("Information", oss.str(), pfd::choice::ok, pfd::icon::info); message.result(); - }); loThread.detach(); } - else //no data loaded + else // no data loaded loRunning = false; } -void step1(const std::string &folder, - LidarOdometryParams ¶ms, - std::vector> &pointsPerFile, - Imu &imu_data, - std::string &working_directory, - Trajectory &trajectory, - std::vector &worker_data, - const std::atomic &loPause) +void step1( + const std::string& folder, + LidarOdometryParams& params, + std::vector>& pointsPerFile, + Imu& imu_data, + std::string& working_directory, + Trajectory& trajectory, + std::vector& worker_data, + const std::atomic& loPause) { std::vector input_file_names; - for (const auto &entry : fs::directory_iterator(folder)) + for (const auto& entry : fs::directory_iterator(folder)) { if (!entry.is_directory()) { @@ -1598,27 +1603,40 @@ void step1(const std::string &folder, if (load_data(input_file_names, params, pointsPerFile, imu_data, full_debug_messages)) { working_directory = fs::path(input_file_names[0]).parent_path().string(); - calculate_trajectory(trajectory, imu_data, params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, params.ahrs_gain, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); + calculate_trajectory( + trajectory, + imu_data, + params.fusionConventionNwu, + params.fusionConventionEnu, + params.fusionConventionNed, + params.ahrs_gain, + full_debug_messages, + params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); std::cout << "step_1_done" << std::endl; } } -void step2(std::vector &worker_data, LidarOdometryParams ¶ms, const std::atomic &loPause) +void step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) { double ts_failure = 0.0; std::atomic loProgress; compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); } -void save_results(bool info, double elapsed_seconds, std::string &working_directory, - std::vector &worker_data, LidarOdometryParams ¶ms, fs::path outwd) +void save_results( + bool info, + double elapsed_seconds, + std::string& working_directory, + std::vector& worker_data, + LidarOdometryParams& params, + fs::path outwd) { save_result(worker_data, params, outwd, elapsed_seconds); } void display() -{ +{ ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); @@ -1657,7 +1675,7 @@ void display() glColor3d(0.0, 1.0, 0.0); glPointSize(point_size); glBegin(GL_POINTS); - for (const auto &p : params.initial_points) + for (const auto& p : params.initial_points) { auto pp = params.m_g * p.point; glVertex3d(pp.x(), pp.y(), pp.z()); @@ -1667,7 +1685,7 @@ void display() if (show_covs) { - for (const auto &b : params.buckets_indoor) + for (const auto& b : params.buckets_indoor) draw_ellipse(b.second.cov, b.second.mean, Eigen::Vector3f(0.0f, 0.0f, 1.0f), 3); } @@ -1718,9 +1736,9 @@ void display() { // glBegin(GL_LINE_STRIP); glBegin(GL_LINES); - for (const auto &wd : worker_data) + for (const auto& wd : worker_data) { - for (const auto &it : wd.intermediate_trajectory) + for (const auto& it : wd.intermediate_trajectory) { glColor3f(1, 0, 0); glVertex3f(it(0, 3), it(1, 3), it(2, 3)); @@ -1743,9 +1761,9 @@ void display() glPointSize(3); glColor3f(0, 1, 1); glBegin(GL_POINTS); - for (const auto &wd : worker_data) + for (const auto& wd : worker_data) { - for (const auto &it : wd.intermediate_trajectory) + for (const auto& it : wd.intermediate_trajectory) glVertex3f(it(0, 3), it(1, 3), it(2, 3)); } glEnd(); @@ -1755,10 +1773,10 @@ void display() glBegin(GL_LINES); if (worker_data.size() > 0) { - const auto &wd = worker_data[worker_data.size() - 1]; + const auto& wd = worker_data[worker_data.size() - 1]; if (wd.intermediate_trajectory.size() > 0) { - const auto &it = wd.intermediate_trajectory[wd.intermediate_trajectory.size() - 1]; + const auto& it = wd.intermediate_trajectory[wd.intermediate_trajectory.size() - 1]; glVertex3f(it(0, 3) - 1, it(1, 3), it(2, 3)); glVertex3f(it(0, 3) + 1, it(1, 3), it(2, 3)); @@ -1777,7 +1795,7 @@ void display() { glColor3f(1, 0, 0); glBegin(GL_POINTS); - for (const auto &b : params.reference_buckets) + for (const auto& b : params.reference_buckets) glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); glEnd(); } @@ -1826,7 +1844,7 @@ void display() ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); ShowMainDockSpace(); @@ -1836,7 +1854,7 @@ void display() { openData(); - //workaround + // workaround io.AddKeyEvent(ImGuiKey_O, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1953,8 +1971,8 @@ void display() if (ImGui::BeginMenu("Parameters")) { - - ImGui::MenuItem("Remove IMU bias from first stationary scan", nullptr, ¶ms.use_removie_imu_bias_from_first_stationary_scan); + ImGui::MenuItem( + "Remove IMU bias from first stationary scan", nullptr, ¶ms.use_removie_imu_bias_from_first_stationary_scan); if (ImGui::IsItemHovered()) ImGui::SetTooltip("IMU bias will be removed given there's a stationary period at the start of the recording"); @@ -1970,8 +1988,8 @@ void display() ImGui::Text("0.3 [s] will make processing time aprox equal to 3x scan time"); ImGui::EndTooltip(); } - - if(ImGui::MenuItem("Set real time performance")) + + if (ImGui::MenuItem("Set real time performance")) params.real_time_threshold_seconds = 0.1; if (ImGui::IsItemHovered()) ImGui::SetTooltip("For IMU 200Hz, 20 nodes in optimization window"); @@ -2018,8 +2036,7 @@ void display() TomlIO toml_io; toml_io.LoadParametersFromTomlFile(input_file_names[0], params); std::cout << "Parameters loaded from: " << input_file_names[0] << std::endl; - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cerr << "Error loading TOML file: " << e.what() << std::endl; } @@ -2042,7 +2059,7 @@ void display() std::cerr << "Failed to save parameters." << std::endl; } } - + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -2096,7 +2113,7 @@ void display() ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background", (float *)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); bg_color = params.clear_color; @@ -2113,7 +2130,9 @@ void display() camMenu(); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -2132,7 +2151,7 @@ void display() if (initial_transformation_gizmo) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); // ImGuizmo ----------------------------------------------- ImGuizmo::BeginFrame(); ImGuizmo::Enable(true); @@ -2144,7 +2163,13 @@ void display() 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); + ImGuizmo::Manipulate( + &modelview[0], + &projection[0], + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); params.m_g(0, 0) = m_gizmo[0]; params.m_g(1, 0) = m_gizmo[1]; @@ -2166,7 +2191,7 @@ void display() if (gizmo_stretch_interval) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); // ImGuizmo ----------------------------------------------- ImGuizmo::BeginFrame(); ImGuizmo::Enable(true); @@ -2178,7 +2203,13 @@ void display() 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); + ImGuizmo::Manipulate( + &modelview[0], + &projection[0], + ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | ImGuizmo::ROTATE_Y, + ImGuizmo::WORLD, + m_gizmo, + NULL); stretch_gizmo_m(0, 0) = m_gizmo[0]; stretch_gizmo_m(1, 0) = m_gizmo[1]; @@ -2226,7 +2257,8 @@ void display() glutPostRedisplay(); } -void on_exit(){ +void on_exit() +{ // remove cache std::cout << "remove cache: '" << params.working_directory_cache << "' START" << std::endl; std::filesystem::remove_all(params.working_directory_cache); @@ -2263,19 +2295,19 @@ void mouse(int glut_button, int state, int x, int y) mouse_buttons |= 1 << glut_button; else if (state == GLUT_UP) mouse_buttons = 0; - + mouse_old_x = x; mouse_old_y = y; } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { set_lidar_odometry_default_params(params); try { - if (argc == 4) //runnning from command line + if (argc == 4) // runnning from command line { // Load parameters from file using original TomlIO class TomlIO toml_io; @@ -2288,28 +2320,20 @@ int main(int argc, char *argv[]) std::chrono::time_point start, end; start = std::chrono::system_clock::now(); - std::atomic loPause{false}; - step1(argv[1], - params, - pointsPerFile, - imu_data, - working_directory, - trajectory, - worker_data, - loPause); + std::atomic loPause{ false }; + step1(argv[1], params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause); step2(worker_data, params, loPause); end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; std::time_t end_time = std::chrono::system_clock::to_time_t(end); - std::cout << "calculations finished computation at " - << std::ctime(&end_time) + std::cout << "calculations finished computation at " << std::ctime(&end_time) << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n"; save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, argv[3]); } - else //full GUI mode + else // full GUI mode { std::cout << argv[0] << " input_folder parameters(*.toml) output_folder" << std::endl; @@ -2322,17 +2346,14 @@ int main(int argc, char *argv[]) ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); } - } - catch (const std::bad_alloc& e) + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index e5d47939..f5ed58c5 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -1,19 +1,21 @@ #include "lidar_odometry_utils.h" -#include #include "csv.hpp" #include -#include #include +#include + // #include namespace fs = std::filesystem; +// TODO(m.wlasiuk) : these 2 functions - core/utils + // this function provides unique index unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z) { return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); + ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + ((static_cast(z) << 0) & (0x000000000000FFFFull)); } // this function provides unique index for input point p and 3D space decomposition into buckets b @@ -40,11 +42,16 @@ std::vector decimate(const std::vector& points, double bucke ip[i].index_of_point = i; ip[i].index_of_bucket = get_rgd_index(points[i].point, b); } - std::sort(ip.begin(), ip.end(), [](const PointCloud::PointBucketIndexPair& a, const PointCloud::PointBucketIndexPair& b) - { return a.index_of_bucket < b.index_of_bucket; }); + std::sort( + ip.begin(), + ip.end(), + [](const PointCloud::PointBucketIndexPair& a, const PointCloud::PointBucketIndexPair& b) + { + return a.index_of_bucket < b.index_of_bucket; + }); if (ip.size() != 0) - out.emplace_back(points[ip[0].index_of_point]); + out.emplace_back(points[ip[0].index_of_point]); for (int i = 1; i < ip.size(); i++) if (ip[i - 1].index_of_bucket != ip[i].index_of_bucket) @@ -54,9 +61,8 @@ std::vector decimate(const std::vector& points, double bucke return out; } -Eigen::Matrix4d getInterpolatedPose(const std::map &trajectory, double query_time) +Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time) { - Eigen::Matrix4d ret(Eigen::Matrix4d::Zero()); auto it_lower = trajectory.lower_bound(query_time); auto it_next = it_lower; @@ -120,7 +126,7 @@ Eigen::Matrix4d getInterpolatedPose(const std::map &tra return ret; } -void limit_covariance(Eigen::Matrix3d &io_cov) +void limit_covariance(Eigen::Matrix3d& io_cov) { return; // std::cout << "------io_cov in ------------" << std::endl; @@ -147,8 +153,7 @@ void limit_covariance(Eigen::Matrix3d &io_cov) // std::cout << io_cov << std::endl; } -void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, - std::vector &points_global, Eigen::Vector3d viewport) +void update_rgd(NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, std::vector& points_global, Eigen::Vector3d viewport) { Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); @@ -160,10 +165,10 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, if (bucket_it != buckets.end()) { - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; this_bucket.number_of_points++; - const auto &curr_mean = points_global[i].point; - const auto &mean = this_bucket.mean; + const auto& curr_mean = points_global[i].point; + const auto& mean = this_bucket.mean; // buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points; auto mean_diff = mean - curr_mean; @@ -173,12 +178,13 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, cov_update.row(2) = mean_diff.z() * mean_diff; // this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * + // this_bucket.number_of_points); if (this_bucket.number_of_points == 2) { this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); // limit_covariance(this_bucket.cov); } @@ -186,7 +192,7 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, if (this_bucket.number_of_points == 3) { this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); // limit_covariance(this_bucket.cov); // calculate normal vector Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); @@ -205,12 +211,12 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, if (this_bucket.number_of_points > 3) { - Eigen::Vector3d &nv = this_bucket.normal_vector; + Eigen::Vector3d& nv = this_bucket.normal_vector; if (nv.dot(viewport - this_bucket.mean) >= 0.0) { this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); // limit_covariance(this_bucket.cov); } } @@ -219,15 +225,18 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, { NDT::Bucket bucket_to_add; bucket_to_add.mean = points_global[i].point; - bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03; //ToDo move to params + bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03; // ToDo move to params bucket_to_add.number_of_points = 1; buckets.emplace(index_of_bucket, bucket_to_add); } } } -void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, - std::vector &points_global, std::vector &points_global_spherical) +void update_rgd_spherical_coordinates( + NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + std::vector& points_global, + std::vector& points_global_spherical) { Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); @@ -239,10 +248,10 @@ void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucket if (bucket_it != buckets.end()) { - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; this_bucket.number_of_points++; - const auto &curr_mean = points_global[i].point; - const auto &mean = this_bucket.mean; + const auto& curr_mean = points_global[i].point; + const auto& mean = this_bucket.mean; // buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points; auto mean_diff = mean - curr_mean; @@ -252,19 +261,20 @@ void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucket cov_update.row(2) = mean_diff.z() * mean_diff; // this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * + // this_bucket.number_of_points); if (this_bucket.number_of_points == 2) { this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); // limit_covariance(this_bucket.cov); } if (this_bucket.number_of_points >= 3) { this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); // limit_covariance(this_bucket.cov); // calculate normal vector // Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); @@ -288,7 +298,8 @@ void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucket // if (nv.dot(viewport - this_bucket.mean) >= 0.0) //{ // this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points + - // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points); + // cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * + // this_bucket.number_of_points); // limit_covariance(this_bucket.cov); // } // } @@ -311,7 +322,9 @@ bool save_poses(const std::string file_name, std::vector m_pose if (!outfile.good()) { std::cout << "can not save file: '" << file_name << "'" << std::endl; - std::cout << "if You can see only '' it means there is no filename assigned to poses, please read manual or contact me januszbedkowski@gmail.com" << std::endl; + std::cout << "if You can see only '' it means there is no filename assigned to poses, please read manual or contact me " + "januszbedkowski@gmail.com" + << std::endl; std::cout << "To assign filename to poses please use following two buttons in multi_view_tls_registration_step_2" << std::endl; std::cout << "1: update initial poses from RESSO file" << std::endl; std::cout << "2: update poses from RESSO file" << std::endl; @@ -332,12 +345,12 @@ bool save_poses(const std::string file_name, std::vector m_pose return true; } -std::vector, FusionVector, FusionVector>> load_imu(const std::string &imu_file, int imuToUse) +std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse) { std::vector, FusionVector, FusionVector>> all_data; csv::CSVFormat format; - format.delimiter({' ', ',', '\t'}); + format.delimiter({ ' ', ',', '\t' }); try { @@ -391,7 +404,8 @@ std::vector, FusionVector, FusionVector>> l acc.axis.y = row["accY"].get(); acc.axis.z = row["accZ"].get(); all_data.emplace_back(std::pair(timestamp / 1e9, timestampUnix / 1e9), gyr, acc); - // std::cout << "acc.axis.x: " << acc.axis.x << " acc.axis.y: " << acc.axis.y << " acc.axis.z " << acc.axis.z << " imu_id: " << imu_id << std::endl; + // std::cout << "acc.axis.x: " << acc.axis.x << " acc.axis.y: " << acc.axis.y << " acc.axis.z " << acc.axis.z << " + // imu_id: " << imu_id << std::endl; } } } @@ -417,7 +431,8 @@ std::vector, FusionVector, FusionVector>> l { iss >> timestampUnix; } - // std::cout << data[0] << " " << data[1] << " " << data[2] << " " << data[3] << " " << data[4] << " " << data[5] << " " << data[6] << std::endl; + // std::cout << data[0] << " " << data[1] << " " << data[2] << " " << data[3] << " " << data[4] << " " << data[5] << " " + // << data[6] << std::endl; if (data[0] > 0 && imuId == imuToUse) { FusionVector gyr; @@ -436,17 +451,21 @@ std::vector, FusionVector, FusionVector>> l myfile.close(); } } - } - catch (...) + } catch (...) { std::cout << "load_imu error for file: '" << imu_file << "'" << std::endl; - //return all_data; + // return all_data; } return all_data; } -std::vector load_point_cloud(const std::string &lazFile, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy_inner, double filter_threshold_xy_outer, const std::unordered_map &calibrations) +std::vector load_point_cloud( + const std::string& lazFile, + bool ommit_points_with_timestamp_equals_zero, + double filter_threshold_xy_inner, + double filter_threshold_xy_outer, + const std::unordered_map& calibrations) { std::vector points; laszip_POINTER laszip_reader; @@ -463,7 +482,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po std::abort(); } // std::cout << "compressed : " << is_compressed << std::endl; - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -471,7 +490,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po std::abort(); } // fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); @@ -483,7 +502,6 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po for (laszip_U32 j = 0; j < header->number_of_point_records; j++) { - if (laszip_read_point(laszip_reader)) { fprintf(stderr, "DLL ERROR: reading point %u\n", j); @@ -504,7 +522,10 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po } Eigen::Affine3d calibration = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(id); - const Eigen::Vector3d pf(header->x_offset + header->x_scale_factor * static_cast(point->X), header->y_offset + header->y_scale_factor * static_cast(point->Y), header->z_offset + header->z_scale_factor * static_cast(point->Z)); + const Eigen::Vector3d pf( + header->x_offset + header->x_scale_factor * static_cast(point->X), + header->y_offset + header->y_scale_factor * static_cast(point->Y), + header->z_offset + header->z_scale_factor * static_cast(point->Z)); p.point = calibration * (pf); p.lidarid = id; @@ -549,7 +570,8 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po } */ - if (sqrt(pf.x() * pf.x() + pf.y() * pf.y()) > filter_threshold_xy_inner && sqrt(pf.x() * pf.x() + pf.y() * pf.y()) < filter_threshold_xy_outer) + if (sqrt(pf.x() * pf.x() + pf.y() * pf.y()) > filter_threshold_xy_inner && + sqrt(pf.x() * pf.x() + pf.y() * pf.y()) < filter_threshold_xy_outer) { points.emplace_back(p); } @@ -571,7 +593,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_po return points; } -std::unordered_map MLvxCalib::GetIdToSnMapping(const std::string &filename) +std::unordered_map MLvxCalib::GetIdToSnMapping(const std::string& filename) { if (!std::filesystem::exists(filename)) { @@ -599,10 +621,7 @@ std::unordered_map MLvxCalib::GetIdToSnMapping(const std::stri return dataMap; } -inline bool JsonGetBool( - const nlohmann::json& obj, - const char* key, - bool defaultValue = false) +inline bool JsonGetBool(const nlohmann::json& obj, const char* key, bool defaultValue = false) { if (!obj.contains(key)) return defaultValue; @@ -622,7 +641,7 @@ inline bool JsonGetBool( return defaultValue; } -std::unordered_map MLvxCalib::GetCalibrationFromFile(const std::string &filename) +std::unordered_map MLvxCalib::GetCalibrationFromFile(const std::string& filename) { std::unordered_map dataMap; std::ifstream file; @@ -639,10 +658,9 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi try { jsonData = json::parse(file); - } - catch (const json::exception& e) + } catch (const json::exception& e) { - std::cerr << "JSON parsing error in file '" << filename << "': " << e.what() << std::endl; + std::cerr << "JSON parsing error in file '" << filename << "': " << e.what() << std::endl; return {}; } @@ -650,13 +668,13 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi return {}; // Iterate through the JSON object and parse each value into Eigen::Affine3d - for (auto &calibrationEntry : jsonData["calibration"].items()) + for (auto& calibrationEntry : jsonData["calibration"].items()) { - const std::string &lidarSn = calibrationEntry.key(); + const std::string& lidarSn = calibrationEntry.key(); Eigen::Matrix4d value; // std::cout << "lidarSn : " << lidarSn << std::endl; - bool identity = JsonGetBool(calibrationEntry.value(),"identity", false); + bool identity = JsonGetBool(calibrationEntry.value(), "identity", false); if (identity) { dataMap[lidarSn] = Eigen::Matrix4d::Identity(); @@ -684,7 +702,7 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi value = value.transpose(); } - bool inverted = JsonGetBool(calibrationEntry.value(),"inverted", false); + bool inverted = JsonGetBool(calibrationEntry.value(), "inverted", false); if (inverted) value = value.inverse(); @@ -700,18 +718,18 @@ std::unordered_map MLvxCalib::GetCalibrationFromFi if (jsonData.contains("blacklist")) { json blacklist = jsonData["blacklist"]; - for (const auto &item : blacklist) + for (const auto& item : blacklist) { std::string blacklistedSn = item.get(); - //dataMap[blacklistedSn] = Eigen::Matrix4d::Zero(); - //avoid dealing with zero matrices later on + // dataMap[blacklistedSn] = Eigen::Matrix4d::Zero(); + // avoid dealing with zero matrices later on dataMap.erase(blacklistedSn); } } return dataMap; } -std::string MLvxCalib::GetImuSnToUse(const std::string &filename) +std::string MLvxCalib::GetImuSnToUse(const std::string& filename) { std::ifstream file; using json = nlohmann::json; @@ -727,8 +745,7 @@ std::string MLvxCalib::GetImuSnToUse(const std::string &filename) try { jsonData = json::parse(file); - } - catch (const json::exception& e) + } catch (const json::exception& e) { std::cerr << "JSON parsing error in file '" << filename << "': " << e.what() << std::endl; return {}; @@ -742,23 +759,24 @@ std::string MLvxCalib::GetImuSnToUse(const std::string &filename) return jsonData["imuToUse"].get(); } -std::unordered_map MLvxCalib::CombineIntoCalibration(const std::unordered_map &idToSn, const std::unordered_map &calibration) +std::unordered_map MLvxCalib::CombineIntoCalibration( + const std::unordered_map& idToSn, const std::unordered_map& calibration) { if (calibration.empty()) { return std::unordered_map(); } std::unordered_map dataMap; - for (const auto &[id, sn] : idToSn) + for (const auto& [id, sn] : idToSn) { // std::cout << "XXX: "<< id << " " << sn << std::endl; - const auto &affine = calibration.at(sn); + const auto& affine = calibration.at(sn); dataMap[id] = affine; } return dataMap; } -int MLvxCalib::GetImuIdToUse(const std::unordered_map &idToSn, const std::string &snToUse) +int MLvxCalib::GetImuIdToUse(const std::unordered_map& idToSn, const std::string& snToUse) { if (snToUse.empty() || idToSn.empty()) { @@ -768,7 +786,7 @@ int MLvxCalib::GetImuIdToUse(const std::unordered_map &idToSn, std::cout << __FILE__ << " " << __LINE__ << std::endl; return 0; } - for (const auto &[id, sn] : idToSn) + for (const auto& [id, sn] : idToSn) { // std::cout << "snToUse " << snToUse << " sn " << sn << std::endl; if (snToUse == sn) @@ -783,7 +801,7 @@ fs::path get_next_result_path(const std::string working_directory) { std::regex pattern(R"(lio_result_(\d+))"); int max_number = -1; - for (const auto &entry : fs::directory_iterator(working_directory)) + for (const auto& entry : fs::directory_iterator(working_directory)) { if (entry.is_directory()) { @@ -800,7 +818,12 @@ fs::path get_next_result_path(const std::string working_directory) return (working_directory / fs::path("lio_result_" + std::to_string(max_number + 1))); } -bool loadLaz(const std::string &filename, std::vector &points_out, std::vector index_poses_i, std::vector &intermediate_trajectory, const Eigen::Affine3d &m_pose) +bool loadLaz( + const std::string& filename, + std::vector& points_out, + std::vector index_poses_i, + std::vector& intermediate_trajectory, + const Eigen::Affine3d& m_pose) { if (!std::filesystem::exists(filename)) { @@ -822,7 +845,7 @@ bool loadLaz(const std::string &filename, std::vector &points_out, std return false; } - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { std::cerr << "DLL ERROR: getting header pointer from laszip reader\n"; @@ -831,7 +854,7 @@ bool loadLaz(const std::string &filename, std::vector &points_out, std laszip_I64 num_points = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { std::cerr << "DLL ERROR: getting point pointer from laszip reader\n"; @@ -877,7 +900,7 @@ bool loadLaz(const std::string &filename, std::vector &points_out, std return true; } -bool load_poses(const fs::path &poses_file, std::vector &out_poses) +bool load_poses(const fs::path& poses_file, std::vector& out_poses) { std::ifstream infile(poses_file); if (!infile.is_open()) @@ -901,10 +924,12 @@ bool load_poses(const fs::path &poses_file, std::vector &out_po return true; } -bool load_trajectory_csv(const std::string &filename, const Eigen::Affine3d &m_pose, - std::vector> &intermediate_trajectory_timestamps, - std::vector &intermediate_trajectory, - std::vector &imu_om_fi_ka) +bool load_trajectory_csv( + const std::string& filename, + const Eigen::Affine3d& m_pose, + std::vector>& intermediate_trajectory_timestamps, + std::vector& intermediate_trajectory, + std::vector& imu_om_fi_ka) { std::ifstream file(filename); if (!file.is_open()) @@ -927,10 +952,7 @@ bool load_trajectory_csv(const std::string &filename, const Eigen::Affine3d &m_p ss >> ts1 >> p00 >> p01 >> p02 >> p03 >> p10 >> p11 >> p12 >> p13 >> p20 >> p21 >> p22 >> p23 >> ts2 >> imu_x >> imu_y >> imu_z; Eigen::Matrix4d rel_mat; - rel_mat << p00, p01, p02, p03, - p10, p11, p12, p13, - p20, p21, p22, p23, - 0, 0, 0, 1; + rel_mat << p00, p01, p02, p03, p10, p11, p12, p13, p20, p21, p22, p23, 0, 0, 0, 1; Eigen::Affine3d relative_pose(rel_mat); Eigen::Affine3d global_pose = m_pose * relative_pose; @@ -943,7 +965,7 @@ bool load_trajectory_csv(const std::string &filename, const Eigen::Affine3d &m_p return true; } -bool load_point_sizes(const std::filesystem::path &path, std::vector &vector) +bool load_point_sizes(const std::filesystem::path& path, std::vector& vector) { std::ifstream in_file(path); if (!in_file) @@ -961,7 +983,7 @@ bool load_point_sizes(const std::filesystem::path &path, std::vector &vecto return false; } vector.clear(); - for (const auto &val : j) + for (const auto& val : j) { if (!val.is_number_integer()) { @@ -970,8 +992,7 @@ bool load_point_sizes(const std::filesystem::path &path, std::vector &vecto } vector.push_back(val.get()); } - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error parsing index poses JSON: " << e.what() << std::endl; return false; @@ -979,7 +1000,7 @@ bool load_point_sizes(const std::filesystem::path &path, std::vector &vecto return true; } -bool load_index_poses(const std::filesystem::path &path, std::vector> &index_poses_out) +bool load_index_poses(const std::filesystem::path& path, std::vector>& index_poses_out) { std::ifstream in_file(path); if (!in_file) @@ -998,7 +1019,7 @@ bool load_index_poses(const std::filesystem::path &path, std::vector chunk_indices; - for (const auto &val : chunk) + for (const auto& val : chunk) { if (!val.is_number_integer()) { @@ -1017,8 +1038,7 @@ bool load_index_poses(const std::filesystem::path &path, std::vector &worker_data_out) +bool load_worker_data_from_results(const fs::path& session_file, std::vector& worker_data_out) { #if 0 std::ifstream f(session_file); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 6550a617..e953f155 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -1,45 +1,46 @@ -#ifndef _LIDAR_ODOMETRY_UTILS_H_ -#define _LIDAR_ODOMETRY_UTILS_H_ +#pragma once -#include +#include +#include +#include #include -#include +#include #include + +#include #include -#include -#include -#include -#include +#include +#include #include #include +#include #include -#include + +#include #include -#include #include -#include -#include -#include #include +#include +#include + #if WITH_GUI == 1 #include #endif namespace fs = std::filesystem; - using NDTBucketMapType = std::unordered_map; using NDTBucketMapType2 = std::unordered_map; // Helper function for getting software version from CMake macros -inline std::string get_software_version() { - #ifdef HDMAPPING_VERSION_MAJOR - return std::to_string(HDMAPPING_VERSION_MAJOR) + "." + - std::to_string(HDMAPPING_VERSION_MINOR) + "." + - std::to_string(HDMAPPING_VERSION_PATCH); - #else - return "0.84.0"; // fallback if CMake macros not available - #endif +inline std::string get_software_version() +{ +#ifdef HDMAPPING_VERSION_MAJOR + return std::to_string(HDMAPPING_VERSION_MAJOR) + "." + std::to_string(HDMAPPING_VERSION_MINOR) + "." + + std::to_string(HDMAPPING_VERSION_PATCH); +#else + return "0.84.0"; // fallback if CMake macros not available +#endif } struct LidarOdometryParams @@ -48,25 +49,25 @@ struct LidarOdometryParams std::string software_version = get_software_version(); std::string config_version = "1.0"; std::string build_date = __DATE__; - - //performance + + // performance bool useMultithread = true; double real_time_threshold_seconds = 10.0; // for realtime use: threshold_nr_poses * 0.005, where 0.005 is related with IMU frequency - //filter points - double filter_threshold_xy_inner = 0.3; //filtering points during load - double filter_threshold_xy_outer = 70.0; //filtering points during load - double decimation = 0.01; // filtering points during load - double threshould_output_filter = 0.5; //for export --> all points xyz.norm() < threshould_output_filter will be removed - int min_counter_concatenated_trajectory_nodes = 10; //for export + // filter points + double filter_threshold_xy_inner = 0.3; // filtering points during load + double filter_threshold_xy_outer = 70.0; // filtering points during load + double decimation = 0.01; // filtering points during load + double threshould_output_filter = 0.5; // for export --> all points xyz.norm() < threshould_output_filter will be removed + int min_counter_concatenated_trajectory_nodes = 10; // for export - //Madgwick filter + // Madgwick filter bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; double ahrs_gain = 0.5; - //lidar odometry control + // lidar odometry control bool use_motion_from_previous_step = true; int nr_iter = 100; NDT::GridParameters in_out_params_indoor; @@ -76,11 +77,11 @@ struct LidarOdometryParams int threshold_initial_points = 10000; int threshold_nr_poses = 20; - //lidar odometry debug info + // lidar odometry debug info bool save_calibration_validation = false; int calibration_validation_points = 1000000; - //consistency + // consistency int num_constistency_iter = 10; bool use_mutliple_gaussian = false; @@ -122,12 +123,12 @@ struct LidarOdometryParams double rgd_sf_sigma_fi_deg = 0.01; double rgd_sf_sigma_ka_deg = 0.01; - //paths + // paths std::string current_output_dir = ""; std::string working_directory_preview = ""; std::string working_directory_cache = ""; - - //other + + // other Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); std::vector initial_points; double consecutive_distance = 0.0; @@ -142,7 +143,7 @@ struct LidarOdometryParams bool use_removie_imu_bias_from_first_stationary_scan = false; - //ablation study + // ablation study bool ablation_study_use_planarity = false; bool ablation_study_use_norm = false; bool ablation_study_use_hierarchical_rgd = true; @@ -154,26 +155,33 @@ unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); // this function finds interpolated pose between two poses according to query_time -Eigen::Matrix4d getInterpolatedPose(const std::map &trajectory, double query_time); +Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); // this function reduces number of points by preserving only first point for each bucket {bucket_x, bucket_y, bucket_z} -std::vector decimate(const std::vector &points, double bucket_x, double bucket_y, double bucket_z); +std::vector decimate(const std::vector& points, double bucket_x, double bucket_y, double bucket_z); // this function updates each bucket (mean value, covariance) in regular grid decomposition -void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, - std::vector &points_global, Eigen::Vector3d viewport = Eigen::Vector3d(0, 0, 0)); - -void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, - std::vector &points_global, std::vector &points_global_spherical); +void update_rgd( + NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + std::vector& points_global, + Eigen::Vector3d viewport = Eigen::Vector3d(0, 0, 0)); + +void update_rgd_spherical_coordinates( + NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + std::vector& points_global, + std::vector& points_global_spherical); //! This function load inertial measurement unit data. //! This function expects a file with the following format: -//! timestamp angular_velocity_x angular_velocity_y angular_velocity_z linear_acceleration_x linear_acceleration_y linear_acceleration_z imu_id +//! timestamp angular_velocity_x angular_velocity_y angular_velocity_z linear_acceleration_x linear_acceleration_y linear_acceleration_z +//! imu_id //! @note imu_id is an optional column, if not present, it is assumed that all data comes from the same IMU. //! @param imu_file - path to file with IMU data //! @param imuToUse - id number of IMU to use, the same index as in pointcloud return by @ref load_point_cloud //! @return vector of tuples (std::pair, angular_velocity, linear_acceleration) -std::vector, FusionVector, FusionVector>> load_imu(const std::string &imu_file, int imuToUse); +std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse); //! This function load point cloud from LAS/LAZ file. //! Optionally it can apply extrinsic calibration to each point. @@ -184,69 +192,113 @@ std::vector, FusionVector, FusionVector>> l //! @param filter_threshold_xy - threshold for filtering points in xy plane //! @param calibrations - map of calibrations for each scanner key is scanner id. //! @return vector of points of @ref Point3Di type -std::vector load_point_cloud(const std::string &lazFile, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy_inner, double filter_threshold_xy_outer, - const std::unordered_map &calibrations); +std::vector load_point_cloud( + const std::string& lazFile, + bool ommit_points_with_timestamp_equals_zero, + double filter_threshold_xy_inner, + double filter_threshold_xy_outer, + const std::unordered_map& calibrations); bool save_poses(const std::string file_name, std::vector m_poses, std::vector filenames); fs::path get_next_result_path(const std::string working_directory); // this function performs main LiDAR odometry calculations -void optimize_lidar_odometry(std::vector &intermediate_points, std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params_indoor, NDTBucketMapType &buckets_indoor, - NDT::GridParameters &rgd_params_outdoor, NDTBucketMapType &buckets_outdoor, - bool useMultithread, double max_distance, double &delta, double lm_factor, TaitBryanPose motion_model_correction, - double lidar_odometry_motion_model_x_1_sigma_m, - double lidar_odometry_motion_model_y_1_sigma_m, - double lidar_odometry_motion_model_z_1_sigma_m, - double lidar_odometry_motion_model_om_1_sigma_deg, - double lidar_odometry_motion_model_fi_1_sigma_deg, - double lidar_odometry_motion_model_ka_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_x_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_y_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_z_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, - bool ablation_study_use_planarity, - bool ablation_study_use_norm, - bool ablation_study_use_hierarchical_rgd, - bool ablation_study_use_view_point_and_normal_vectors); - -void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, bool useMultithread); - -void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points_sf, std::vector &intermediate_trajectory, - const std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, bool useMultithread, double wx, double wy, double wz, double wom, double wfi, double wka); - -void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, /*NDTBucketMapType &buckets*/ std::vector points_global, bool useMultithread /*, - bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ +void optimize_lidar_odometry( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params_indoor, + NDTBucketMapType& buckets_indoor, + NDT::GridParameters& rgd_params_outdoor, + NDTBucketMapType& buckets_outdoor, + bool useMultithread, + double max_distance, + double& delta, + double lm_factor, + TaitBryanPose motion_model_correction, + double lidar_odometry_motion_model_x_1_sigma_m, + double lidar_odometry_motion_model_y_1_sigma_m, + double lidar_odometry_motion_model_z_1_sigma_m, + double lidar_odometry_motion_model_om_1_sigma_deg, + double lidar_odometry_motion_model_fi_1_sigma_deg, + double lidar_odometry_motion_model_ka_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_x_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_y_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_z_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, + bool ablation_study_use_planarity, + bool ablation_study_use_norm, + bool ablation_study_use_hierarchical_rgd, + bool ablation_study_use_view_point_and_normal_vectors); + +void optimize_sf( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + bool useMultithread); + +void optimize_sf2( + std::vector& intermediate_points, + std::vector& intermediate_points_sf, + std::vector& intermediate_trajectory, + const std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + bool useMultithread, + double wx, + double wy, + double wz, + double wom, + double wfi, + double wka); + +void optimize_icp( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + /*NDTBucketMapType &buckets*/ std::vector points_global, + bool useMultithread /*, +bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ ); // this function registers initial point cloud to geoferenced point cloud -void align_to_reference(NDT::GridParameters &rgd_params, std::vector &initial_points, Eigen::Affine3d &m_g, NDTBucketMapType &buckets); +void align_to_reference( + NDT::GridParameters& rgd_params, std::vector& initial_points, Eigen::Affine3d& m_g, NDTBucketMapType& buckets); // this function apply correction to pitch and roll // void fix_ptch_roll(std::vector &worker_data); -bool compute_step_2(std::vector &worker_data, LidarOdometryParams ¶ms, double &ts_failure, std::atomic &loProgress, const std::atomic &pause, bool debugMsg); -void compute_step_2_fast_forward_motion(std::vector &worker_data, LidarOdometryParams ¶ms); +bool compute_step_2( + std::vector& worker_data, + LidarOdometryParams& params, + double& ts_failure, + std::atomic& loProgress, + const std::atomic& pause, + bool debugMsg); +void compute_step_2_fast_forward_motion(std::vector& worker_data, LidarOdometryParams& params); // for reconstructing worker data from step 1 output -bool loadLaz(const std::string &filename, std::vector &points_out, std::vector index_poses_i, std::vector &intermediate_trajectory, const Eigen::Affine3d &inverse_pose); -bool load_poses(const fs::path &poses_file, std::vector &out_poses); -bool load_trajectory_csv(const std::string &filename, const Eigen::Affine3d &m_pose, - std::vector> &intermediate_trajectory_timestamps, - std::vector &intermediate_trajectory, - std::vector &imu_om_fi_ka); -bool load_point_sizes(const std::filesystem::path &path, std::vector &vector); -bool load_index_poses(const std::filesystem::path &path, std::vector> &index_poses_out); -bool load_worker_data_from_results(const fs::path &session_file, std::vector &worker_data_out); +bool loadLaz( + const std::string& filename, + std::vector& points_out, + std::vector index_poses_i, + std::vector& intermediate_trajectory, + const Eigen::Affine3d& inverse_pose); +bool load_poses(const fs::path& poses_file, std::vector& out_poses); +bool load_trajectory_csv( + const std::string& filename, + const Eigen::Affine3d& m_pose, + std::vector>& intermediate_trajectory_timestamps, + std::vector& intermediate_trajectory, + std::vector& imu_om_fi_ka); +bool load_point_sizes(const std::filesystem::path& path, std::vector& vector); +bool load_index_poses(const std::filesystem::path& path, std::vector>& index_poses_out); +bool load_worker_data_from_results(const fs::path& session_file, std::vector& worker_data_out); //! This namespace contains functions for loading calibration file (.json/.mjc and .sn). //! @@ -293,38 +345,36 @@ bool load_worker_data_from_results(const fs::path &session_file, std::vector GetIdToSnMapping(const std::string &filename); + std::unordered_map GetIdToSnMapping(const std::string& filename); //! Parse the calibration file and return a map from serial number to calibration. //! @param filename calibration file //! @return map of extrinsic calibration, where key is serial number of the lidar. - std::unordered_map GetCalibrationFromFile(const std::string &filename); + std::unordered_map GetCalibrationFromFile(const std::string& filename); //! Parse the calibration file and return a serial number of the Livox to use for IMU. //! @param filename calibration file //! @return serial number of the Livox to use for IMU - std::string GetImuSnToUse(const std::string &filename); + std::string GetImuSnToUse(const std::string& filename); //! Combine the id to serial number mapping and the calibration into a single map. //! The single map is from sensor id to calibration. //! @param idToSn mapping from serial number to Id number in pointcloud or IMU CSV //! @param calibration map of extrinsic calibration, where key is serial number of the lidar. //! @return map from sensor id to extrinsic calibration - std::unordered_map CombineIntoCalibration(const std::unordered_map &idToSn, - const std::unordered_map &calibration); + std::unordered_map CombineIntoCalibration( + const std::unordered_map& idToSn, const std::unordered_map& calibration); //! Get the id of the IMU to use. //! @param idToSn mapping from serial number to Id number in pointcloud or IMU CSV //! @param snToUse serial number of the Livox to use for IMU //! @return id of the IMU to use - int GetImuIdToUse(const std::unordered_map &idToSn, const std::string &snToUse); -} + int GetImuIdToUse(const std::unordered_map& idToSn, const std::string& snToUse); +} // namespace MLvxCalib -void Consistency(std::vector &worker_data, LidarOdometryParams ¶ms); -void Consistency2(std::vector &worker_data, LidarOdometryParams ¶ms); -#endif +void Consistency(std::vector& worker_data, LidarOdometryParams& params); +void Consistency2(std::vector& worker_data, LidarOdometryParams& params); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index e91a4159..1244017f 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -19,19 +19,19 @@ namespace }; //! Sumation of blocks - void SumBlocks(const std::vector &blocks, Eigen::MatrixXd &AtPAndt, Eigen::MatrixXd &AtPBndt) + void SumBlocks(const std::vector& blocks, Eigen::MatrixXd& AtPAndt, Eigen::MatrixXd& AtPBndt) { - for (auto &block : blocks) + for (auto& block : blocks) { const int c = block.c; AtPAndt.block<6, 6>(c, c) += block.AtPAndtBlocksToSum; AtPBndt.block<6, 1>(c, 0) -= block.AtPBndtBlocksToSum; } }; -} -std::vector> nns(std::vector points_global, const std::vector &indexes_for_nn) +} // namespace +std::vector> nns(std::vector points_global, const std::vector& indexes_for_nn) { - Eigen::Vector3d search_radious = {0.1, 0.1, 0.1}; + Eigen::Vector3d search_radious = { 0.1, 0.1, 0.1 }; std::vector> nn; @@ -43,9 +43,13 @@ std::vector> nns(std::vector points_global, const indexes.emplace_back(index, i); } - std::sort(indexes.begin(), indexes.end(), - [](const std::pair &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -118,13 +122,16 @@ std::vector> nns(std::vector points_global, const return nn; } -void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, /*NDTBucketMapType &buckets*/ std::vector points_global, bool useMultithread /*, - bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ +void optimize_icp( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + /*NDTBucketMapType &buckets*/ std::vector points_global, + bool useMultithread /*, +bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ ) { - std::vector all_points_global = points_global; for (int i = 0; i < all_points_global.size(); i++) @@ -169,8 +176,8 @@ void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_points, std::vector AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - target.x(), target.y(), target.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + target.x(), + target.y(), + target.z()); int c = intermediate_points_i.index_pose * 6; @@ -212,70 +251,73 @@ void optimize_icp(std::vector &intermediate_points, std::vector relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1_simplified_1(relative_pose_measurement_odo, - poses_desired[odo_edges[i].first].px, - poses_desired[odo_edges[i].first].py, - poses_desired[odo_edges[i].first].pz, - poses_desired[odo_edges[i].first].om, - poses_desired[odo_edges[i].first].fi, - poses_desired[odo_edges[i].first].ka, - poses_desired[odo_edges[i].second].px, - poses_desired[odo_edges[i].second].py, - poses_desired[odo_edges[i].second].pz, - poses_desired[odo_edges[i].second].om, - poses_desired[odo_edges[i].second].fi, - poses_desired[odo_edges[i].second].ka); + relative_pose_tait_bryan_wc_case1_simplified_1( + relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); Eigen::Matrix AtPAodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified(AtPAodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - 1000000, - 1000000, - 1000000, - 100000000, - 100000000, - // 100000000, underground mining - // 1000000, underground mining - 1000000); + relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified( + AtPAodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 1000000, + 1000000, + 1000000, + 100000000, + 100000000, + // 100000000, underground mining + // 1000000, underground mining + 1000000); Eigen::Matrix AtPBodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified(AtPBodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - relative_pose_measurement_odo(0, 0), - relative_pose_measurement_odo(1, 0), - relative_pose_measurement_odo(2, 0), - relative_pose_measurement_odo(3, 0), - relative_pose_measurement_odo(4, 0), - relative_pose_measurement_odo(5, 0), - 1000000, - 1000000, - 1000000, - 100000000, - 100000000, - // 100000000, underground mining - // 1000000, underground mining - 1000000); + relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified( + AtPBodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + relative_pose_measurement_odo(3, 0), + relative_pose_measurement_odo(4, 0), + relative_pose_measurement_odo(5, 0), + 1000000, + 1000000, + 1000000, + 100000000, + 100000000, + // 100000000, underground mining + // 1000000, underground mining + 1000000); int ic_1 = odo_edges[i].first * 6; int ic_2 = odo_edges[i].second * 6; @@ -316,17 +358,15 @@ void optimize_icp(std::vector &intermediate_points, std::vector delta; point_to_line_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - point_source_local.x(), point_source_local.y(), point_source_local.z(), - point_on_target_line.x(), point_on_target_line.y(), point_on_target_line.z(), - vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); + current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, + current_pose.ka, point_source_local.x(), point_source_local.y(), point_source_local.z(), point_on_target_line.x(), + point_on_target_line.y(), point_on_target_line.z(), vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); Eigen::Matrix delta_jacobian; point_to_line_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - point_source_local.x(), point_source_local.y(), point_source_local.z(), - point_on_target_line.x(), point_on_target_line.y(), point_on_target_line.z(), - vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); + current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, + current_pose.ka, point_source_local.x(), point_source_local.y(), point_source_local.z(), point_on_target_line.x(), + point_on_target_line.y(), point_on_target_line.z(), vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); int ir = tripletListB.size(); @@ -403,11 +443,14 @@ void optimize_icp(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, NDTBucketMapType &buckets_, bool multithread) +void optimize_sf( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets_, + bool multithread) { - auto int_tr = intermediate_trajectory; auto int_tr_tmp = intermediate_trajectory; auto int_tr_mm = intermediate_trajectory_motion_model; @@ -469,7 +512,7 @@ void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_points, std::vectorsecond; + auto& this_bucket = bucket_it->second; Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); Eigen::Matrix jacobian; TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); - std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, point_local.x(), point_local.y(), point_local.z()); + std::mutex& m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; std::unique_lock lck(m); int c = intermediate_points_i.index_pose * 6; @@ -559,7 +614,6 @@ void optimize_sf(std::vector &intermediate_points, std::vector delta; relative_pose_obs_eq_tait_bryan_wc_case1( delta, @@ -575,22 +629,28 @@ void optimize_sf(std::vector &intermediate_points, std::vector jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); int ir = tripletListB.size(); @@ -681,8 +741,7 @@ void optimize_sf(std::vector &intermediate_points, std::vector> - solver(AtPA); + Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); @@ -716,16 +775,20 @@ void optimize_sf(std::vector &intermediate_points, std::vector &intermediate_points, std::vector &intermediate_points_sf, std::vector &intermediate_trajectory, - const std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params, bool useMultithread, double wx, - double wy, - double wz, - double wom, - double wfi, - double wka) +void optimize_sf2( + std::vector& intermediate_points, + std::vector& intermediate_points_sf, + std::vector& intermediate_trajectory, + const std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params, + bool useMultithread, + double wx, + double wy, + double wz, + double wom, + double wfi, + double wka) { - auto trj = intermediate_trajectory; std::vector point_cloud_global; std::vector point_cloud_global_sc; @@ -746,7 +809,7 @@ void optimize_sf2(std::vector &intermediate_points, std::vector Blocks + const auto hessian_fun = [&](const int& indexes_i) -> Blocks { int c = intermediate_points[indexes_i].index_pose * 6; auto index_of_bucket = get_rgd_index(point_cloud_global_sc[indexes_i], b); @@ -754,38 +817,72 @@ void optimize_sf2(std::vector &intermediate_points, std::vector::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; } - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; if ((infm.array() < -threshold).any()) - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; - const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points[indexes_i].index_pose]; // intermediate_trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d &p_s = intermediate_points[indexes_i].point; + const Eigen::Affine3d& m_pose = + intermediate_trajectory[intermediate_points[indexes_i] + .index_pose]; // intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points[indexes_i].point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); // Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); // planarity Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); @@ -802,7 +899,7 @@ void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points, std::vector AtPAodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified(AtPAodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - wx, - wy, - wz, - wom, - wfi, - wka); + relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified( + AtPAodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + wx, + wy, + wz, + wom, + wfi, + wka); Eigen::Matrix AtPBodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified(AtPBodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - 0, 0, 0, 0, 0, 0, - wx, - wy, - wz, - wom, - wfi, - wka); + relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified( + AtPBodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + 0, + 0, + 0, + 0, + 0, + 0, + wx, + wy, + wz, + wom, + wfi, + wka); int ic_1 = odo_edges[i].first * 6; int ic_2 = odo_edges[i].second * 6; @@ -952,8 +1056,7 @@ void optimize_sf2(std::vector &intermediate_points, std::vector> - solver(AtPA); + Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; for (int k = 0; k < x.outerSize(); ++k) @@ -999,10 +1102,10 @@ void optimize_sf2(std::vector &intermediate_points, std::vector &intermediate_points, - std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - const NDTBucketMapType &buckets, + const std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + const NDTBucketMapType& buckets, double distance_bucket, double polar_angle_deg, double azimutal_angle_deg, @@ -1016,7 +1119,6 @@ void optimize_rigid_sf( double rgd_sf_sigma_fi_deg, double rgd_sf_sigma_ka_deg) { - auto shift = intermediate_trajectory[0].translation(); auto _intermediate_trajectory = intermediate_trajectory; @@ -1029,13 +1131,13 @@ void optimize_rigid_sf( rgd_params_sc.resolution_Y = polar_angle_deg; rgd_params_sc.resolution_Z = azimutal_angle_deg; - for (auto &t : _intermediate_trajectory) + for (auto& t : _intermediate_trajectory) t.translation() -= shift; - for (auto &t : _intermediate_trajectory_motion_model) + for (auto& t : _intermediate_trajectory_motion_model) t.translation() -= shift; - for (auto &b : _buckets) + for (auto& b : _buckets) b.second.mean -= shift; std::vector points_rgd; @@ -1044,7 +1146,7 @@ void optimize_rigid_sf( auto minv = _intermediate_trajectory[0].inverse(); - for (const auto &b : _buckets) + for (const auto& b : _buckets) { auto pinv = minv * b.second.mean; @@ -1087,57 +1189,91 @@ void optimize_rigid_sf( { [[maybe_unused]] std::atomic number_of_observations = 0; - const auto hessian_fun = [&](const int &indexes_i) -> Blocks + const auto hessian_fun = [&](const int& indexes_i) -> Blocks { int c = 0; - const auto &point_local = points_local[indexes_i]; + const auto& point_local = points_local[indexes_i]; auto point_global = first_pose * point_local; double r_l = point_global.norm(); double polar_angle_deg_l = atan2(point_global.y(), point_global.x()) * RAD_TO_DEG; double azimutal_angle_deg_l = acos(point_global.z() / r_l) * RAD_TO_DEG; - auto index_of_bucket = get_rgd_index({r_l, polar_angle_deg_l, azimutal_angle_deg_l}, {rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z}); + auto index_of_bucket = get_rgd_index( + { r_l, polar_angle_deg_l, azimutal_angle_deg_l }, + { rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z }); auto bucket_it = buckets_sf.find(index_of_bucket); // no bucket found if (bucket_it == buckets_sf.end()) - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) { // std::cout << "infm.array() " << infm.array() << std::endl; - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; } if ((infm.array() < -threshold).any()) { // std::cout << "infm.array() " << infm.array() << std::endl; - return {Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c}; + return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; } - const Eigen::Affine3d &m_pose = first_pose; - const Eigen::Vector3d &p_s = points_local[indexes_i]; + const Eigen::Affine3d& m_pose = first_pose; + const Eigen::Vector3d& p_s = points_local[indexes_i]; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); // int c = 0; @@ -1156,7 +1292,7 @@ void optimize_rigid_sf( AtPB *= w; number_of_observations++; - return {AtPA, AtPB, c}; + return { AtPA, AtPB, c }; }; std::vector blocks(indexes.size()); @@ -1285,32 +1421,36 @@ void optimize_rigid_sf( intermediate_trajectory_motion_model = _intermediate_trajectory; } -void optimize_lidar_odometry(std::vector &intermediate_points, - std::vector &intermediate_trajectory, - std::vector &intermediate_trajectory_motion_model, - NDT::GridParameters &rgd_params_indoor, NDTBucketMapType &buckets_indoor, - NDT::GridParameters &rgd_params_outdoor, NDTBucketMapType &buckets_outdoor, bool multithread, - double max_distance, double &delta, - double lm_factor, - TaitBryanPose motion_model_correction, - double lidar_odometry_motion_model_x_1_sigma_m, - double lidar_odometry_motion_model_y_1_sigma_m, - double lidar_odometry_motion_model_z_1_sigma_m, - double lidar_odometry_motion_model_om_1_sigma_deg, - double lidar_odometry_motion_model_fi_1_sigma_deg, - double lidar_odometry_motion_model_ka_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_x_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_y_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_z_1_sigma_m, - double lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, - double lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, - bool ablation_study_use_planarity, - bool ablation_study_use_norm, - bool ablation_study_use_hierarchical_rgd, - bool ablation_study_use_view_point_and_normal_vectors) +void optimize_lidar_odometry( + std::vector& intermediate_points, + std::vector& intermediate_trajectory, + std::vector& intermediate_trajectory_motion_model, + NDT::GridParameters& rgd_params_indoor, + NDTBucketMapType& buckets_indoor, + NDT::GridParameters& rgd_params_outdoor, + NDTBucketMapType& buckets_outdoor, + bool multithread, + double max_distance, + double& delta, + double lm_factor, + TaitBryanPose motion_model_correction, + double lidar_odometry_motion_model_x_1_sigma_m, + double lidar_odometry_motion_model_y_1_sigma_m, + double lidar_odometry_motion_model_z_1_sigma_m, + double lidar_odometry_motion_model_om_1_sigma_deg, + double lidar_odometry_motion_model_fi_1_sigma_deg, + double lidar_odometry_motion_model_ka_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_x_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_y_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_z_1_sigma_m, + double lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, + double lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, + bool ablation_study_use_planarity, + bool ablation_study_use_norm, + bool ablation_study_use_hierarchical_rgd, + bool ablation_study_use_view_point_and_normal_vectors) { - double sigma_motion_model_om = lidar_odometry_motion_model_om_1_sigma_deg * DEG_TO_RAD; double sigma_motion_model_fi = lidar_odometry_motion_model_fi_1_sigma_deg * DEG_TO_RAD; double sigma_motion_model_ka = lidar_odometry_motion_model_ka_1_sigma_deg * DEG_TO_RAD; @@ -1335,7 +1475,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, std::vector mutexes(intermediate_trajectory.size()); - const auto hessian_fun_indoor = [&](const Point3Di &intermediate_points_i) + const auto hessian_fun_indoor = [&](const Point3Di& intermediate_points_i) { if (intermediate_points_i.point.norm() < 0.1 || intermediate_points_i.point.norm() > max_distance) // ToDo return; @@ -1348,10 +1488,10 @@ void optimize_lidar_odometry(std::vector &intermediate_points, if (bucket_it == buckets_indoor.end()) return; - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; // if(buckets[index_of_bucket].number_of_points >= 5){ - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) @@ -1363,31 +1503,63 @@ void optimize_lidar_odometry(std::vector &intermediate_points, if (ablation_study_use_view_point_and_normal_vectors) { // check nv - Eigen::Vector3d &nv = this_bucket.normal_vector; + Eigen::Vector3d& nv = this_bucket.normal_vector; Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); if (nv.dot(viewport - this_bucket.mean) < 0) return; } - const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d &p_s = intermediate_points_i.point; + const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points_i.point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); // Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); int c = intermediate_points_i.index_pose * 6; @@ -1420,7 +1592,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, AtPB *= w; } - std::mutex &m = mutexes[intermediate_points_i.index_pose]; + std::mutex& m = mutexes[intermediate_points_i.index_pose]; std::unique_lock lck(m); AtPAndt.block<6, 6>(c, c) += AtPA; AtPBndt.block<6, 1>(c, 0) -= AtPB; @@ -1435,7 +1607,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, { Eigen::Vector3d b_outdoor(rgd_params_outdoor.resolution_X, rgd_params_outdoor.resolution_Y, rgd_params_outdoor.resolution_Z); - const auto hessian_fun_outdoor = [&](const Point3Di &intermediate_points_i) + const auto hessian_fun_outdoor = [&](const Point3Di& intermediate_points_i) { if (intermediate_points_i.point.norm() < 5.0 || intermediate_points_i.point.norm() > max_distance) // ToDo return; @@ -1448,9 +1620,9 @@ void optimize_lidar_odometry(std::vector &intermediate_points, if (bucket_it == buckets_outdoor.end()) return; - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) @@ -1462,31 +1634,63 @@ void optimize_lidar_odometry(std::vector &intermediate_points, if (ablation_study_use_view_point_and_normal_vectors) { // check nv - Eigen::Vector3d &nv = this_bucket.normal_vector; + Eigen::Vector3d& nv = this_bucket.normal_vector; Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); if (nv.dot(viewport - this_bucket.mean) < 0) return; } - const Eigen::Affine3d &m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d &p_s = intermediate_points_i.point; + const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points_i.point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); // Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); int c = intermediate_points_i.index_pose * 6; @@ -1515,7 +1719,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, AtPB *= planarity; } - std::mutex &m = mutexes[intermediate_points_i.index_pose]; + std::mutex& m = mutexes[intermediate_points_i.index_pose]; std::unique_lock lck(m); AtPAndt.block<6, 6>(c, c) += AtPA; AtPBndt.block<6, 1>(c, 0) -= AtPB; @@ -1543,23 +1747,25 @@ void optimize_lidar_odometry(std::vector &intermediate_points, for (size_t i = 0; i < odo_edges.size(); i++) { Eigen::Matrix relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1_simplified_1(relative_pose_measurement_odo, - poses_desired[odo_edges[i].first].px, - poses_desired[odo_edges[i].first].py, - poses_desired[odo_edges[i].first].pz, - poses_desired[odo_edges[i].first].om, - poses_desired[odo_edges[i].first].fi, - poses_desired[odo_edges[i].first].ka, - poses_desired[odo_edges[i].second].px, - poses_desired[odo_edges[i].second].py, - poses_desired[odo_edges[i].second].pz, - poses_desired[odo_edges[i].second].om, - poses_desired[odo_edges[i].second].fi, - poses_desired[odo_edges[i].second].ka); + relative_pose_tait_bryan_wc_case1_simplified_1( + relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); // relative_pose_measurement_odo(2, 0) += 0.01; - // Eigen::Vector3d relative(relative_pose_measurement_odo(0, 0), relative_pose_measurement_odo(1, 0), relative_pose_measurement_odo(2, 0)); + // Eigen::Vector3d relative(relative_pose_measurement_odo(0, 0), relative_pose_measurement_odo(1, 0), + // relative_pose_measurement_odo(2, 0)); // relative_pose_measurement_odo(2, 0) += relative.norm() * sin(poses_desired[odo_edges[i].first].fi); // TaitBryanPose relative_pose_measurement_odo @@ -1568,59 +1774,60 @@ void optimize_lidar_odometry(std::vector &intermediate_points, relative_pose_measurement_odo(4, 0) += motion_model_correction.fi * DEG_TO_RAD; relative_pose_measurement_odo(5, 0) += motion_model_correction.ka * DEG_TO_RAD; - Eigen::Matrix - AtPAodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified(AtPAodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - // 1000000, - // 10000000000, - // 10000000000, - w_motion_model_x, - w_motion_model_y, - w_motion_model_z, - w_motion_model_om * cauchy(relative_pose_measurement_odo(3, 0), 1), // 100000000, // - w_motion_model_fi * cauchy(relative_pose_measurement_odo(4, 0), 1), // 100000000, // - w_motion_model_ka * cauchy(relative_pose_measurement_odo(5, 0), 1)); // 100000000); + Eigen::Matrix AtPAodo; + relative_pose_obs_eq_tait_bryan_wc_case1_AtPA_simplified( + AtPAodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + // 1000000, + // 10000000000, + // 10000000000, + w_motion_model_x, + w_motion_model_y, + w_motion_model_z, + w_motion_model_om * cauchy(relative_pose_measurement_odo(3, 0), 1), // 100000000, // + w_motion_model_fi * cauchy(relative_pose_measurement_odo(4, 0), 1), // 100000000, // + w_motion_model_ka * cauchy(relative_pose_measurement_odo(5, 0), 1)); // 100000000); Eigen::Matrix AtPBodo; - relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified(AtPBodo, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - relative_pose_measurement_odo(0, 0), - relative_pose_measurement_odo(1, 0), - relative_pose_measurement_odo(2, 0), - relative_pose_measurement_odo(3, 0), - relative_pose_measurement_odo(4, 0), - relative_pose_measurement_odo(5, 0), - // 1000000, - // 10000000000, - // 10000000000, - w_motion_model_x, - w_motion_model_y, - w_motion_model_z, - w_motion_model_om * cauchy(relative_pose_measurement_odo(3, 0), 1), // 100000000, // - w_motion_model_fi * cauchy(relative_pose_measurement_odo(4, 0), 1), // 100000000, // - w_motion_model_ka * cauchy(relative_pose_measurement_odo(5, 0), 1)); + relative_pose_obs_eq_tait_bryan_wc_case1_AtPB_simplified( + AtPBodo, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + relative_pose_measurement_odo(3, 0), + relative_pose_measurement_odo(4, 0), + relative_pose_measurement_odo(5, 0), + // 1000000, + // 10000000000, + // 10000000000, + w_motion_model_x, + w_motion_model_y, + w_motion_model_z, + w_motion_model_om * cauchy(relative_pose_measurement_odo(3, 0), 1), // 100000000, // + w_motion_model_fi * cauchy(relative_pose_measurement_odo(4, 0), 1), // 100000000, // + w_motion_model_ka * cauchy(relative_pose_measurement_odo(5, 0), 1)); int ic_1 = odo_edges[i].first * 6; int ic_2 = odo_edges[i].second * 6; @@ -1652,12 +1859,24 @@ void optimize_lidar_odometry(std::vector &intermediate_points, tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); - tripletListP.emplace_back(ir, ir, 1.0 / (lidar_odometry_motion_model_fix_origin_x_1_sigma_m * lidar_odometry_motion_model_fix_origin_x_1_sigma_m)); - tripletListP.emplace_back(ir + 1, ir + 1, 1.0 / (lidar_odometry_motion_model_fix_origin_y_1_sigma_m * lidar_odometry_motion_model_fix_origin_y_1_sigma_m)); - tripletListP.emplace_back(ir + 2, ir + 2, 1.0 / (lidar_odometry_motion_model_fix_origin_z_1_sigma_m * lidar_odometry_motion_model_fix_origin_z_1_sigma_m)); - tripletListP.emplace_back(ir + 3, ir + 3, 1.0 / (lidar_odometry_motion_model_fix_origin_om_1_sigma_deg * lidar_odometry_motion_model_fix_origin_om_1_sigma_deg)); - tripletListP.emplace_back(ir + 4, ir + 4, 1.0 / (lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg * lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg)); - tripletListP.emplace_back(ir + 5, ir + 5, 1.0 / (lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg * lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg)); + tripletListP.emplace_back( + ir, ir, 1.0 / (lidar_odometry_motion_model_fix_origin_x_1_sigma_m * lidar_odometry_motion_model_fix_origin_x_1_sigma_m)); + tripletListP.emplace_back( + ir + 1, ir + 1, 1.0 / (lidar_odometry_motion_model_fix_origin_y_1_sigma_m * lidar_odometry_motion_model_fix_origin_y_1_sigma_m)); + tripletListP.emplace_back( + ir + 2, ir + 2, 1.0 / (lidar_odometry_motion_model_fix_origin_z_1_sigma_m * lidar_odometry_motion_model_fix_origin_z_1_sigma_m)); + tripletListP.emplace_back( + ir + 3, + ir + 3, + 1.0 / (lidar_odometry_motion_model_fix_origin_om_1_sigma_deg * lidar_odometry_motion_model_fix_origin_om_1_sigma_deg)); + tripletListP.emplace_back( + ir + 4, + ir + 4, + 1.0 / (lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg * lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg)); + tripletListP.emplace_back( + ir + 5, + ir + 5, + 1.0 / (lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg * lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg)); tripletListB.emplace_back(ir, 0, 0); tripletListB.emplace_back(ir + 1, 0, 0); @@ -1695,8 +1914,7 @@ void optimize_lidar_odometry(std::vector &intermediate_points, AtPA_I *= lm_factor; AtPA += (AtPA_I); - Eigen::SimplicialCholesky> - solver(AtPA); + Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); std::vector h_x; for (int k = 0; k < x.outerSize(); ++k) @@ -1736,7 +1954,8 @@ void optimize_lidar_odometry(std::vector &intermediate_points, return; } -void align_to_reference(NDT::GridParameters &rgd_params, std::vector &initial_points, Eigen::Affine3d &m_g, NDTBucketMapType &reference_buckets) +void align_to_reference( + NDT::GridParameters& rgd_params, std::vector& initial_points, Eigen::Affine3d& m_g, NDTBucketMapType& reference_buckets) { Eigen::SparseMatrix AtPAndt(6, 6); Eigen::SparseMatrix AtPBndt(6, 1); @@ -1761,24 +1980,56 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & if ((infm.array() < -threshold).any()) continue; - const Eigen::Affine3d &m_pose = m_g; - const Eigen::Vector3d &p_s = initial_points[i].point; + const Eigen::Affine3d& m_pose = m_g; + const Eigen::Vector3d& p_s = initial_points[i].point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); // Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2)); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); Eigen::Matrix AtPB; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - infm(0, 0), infm(0, 1), infm(0, 2), infm(1, 0), infm(1, 1), infm(1, 2), infm(2, 0), infm(2, 1), infm(2, 2), - reference_buckets[index_of_bucket].mean.x(), reference_buckets[index_of_bucket].mean.y(), reference_buckets[index_of_bucket].mean.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + reference_buckets[index_of_bucket].mean.x(), + reference_buckets[index_of_bucket].mean.y(), + reference_buckets[index_of_bucket].mean.z()); int c = 0; @@ -1820,7 +2071,13 @@ void align_to_reference(NDT::GridParameters &rgd_params, std::vector & std::cout << "align_to_reference FAILED" << std::endl; } -bool compute_step_2(std::vector &worker_data, LidarOdometryParams ¶ms, double &ts_failure, std::atomic &loProgress, const std::atomic &pause, bool debugMsg) +bool compute_step_2( + std::vector& worker_data, + LidarOdometryParams& params, + double& ts_failure, + std::atomic& loProgress, + const std::atomic& pause, + bool debugMsg) { // exit(1); bool debug = false; @@ -1851,7 +2108,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p update_rgd(params.in_out_params_indoor, params.buckets_indoor, pp, params.m_g.translation()); - if (params.ablation_study_use_hierarchical_rgd){ + if (params.ablation_study_use_hierarchical_rgd) + { update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, pp, params.m_g.translation()); } @@ -1879,11 +2137,13 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { - // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); - // mean_shift /= ((worker_data[i - 2].intermediate_trajectory.size()) - 2); + // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - worker_data[i - + // 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); mean_shift /= + // ((worker_data[i - 2].intermediate_trajectory.size()) - 2); - mean_shift = worker_data[i - 1].intermediate_trajectory[worker_data[i - 1].intermediate_trajectory.size() - 1].translation() - - worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); + mean_shift = + worker_data[i - 1].intermediate_trajectory[worker_data[i - 1].intermediate_trajectory.size() - 1].translation() - + worker_data[i - 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - // worker_data[i - 2].intermediate_trajectory[0].translation(); @@ -1896,7 +2156,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p } std::vector new_trajectory; - Eigen::Affine3d current_node = worker_data[i - 1].intermediate_trajectory[worker_data[i - 1].intermediate_trajectory.size() - 1]; + Eigen::Affine3d current_node = + worker_data[i - 1].intermediate_trajectory[worker_data[i - 1].intermediate_trajectory.size() - 1]; new_trajectory.push_back(current_node); for (int tr = 1; tr < worker_data[i].intermediate_trajectory.size(); tr++) @@ -1929,10 +2190,10 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p auto firstm = tr[0]; - for (auto &t : tr) + for (auto& t : tr) t.translation() -= firstm.translation(); - for (auto &t : trmm) + for (auto& t : trmm) t.translation() -= firstm.translation(); NDT::GridParameters rgd_params_sc; @@ -2017,31 +2278,32 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::cout << "problem with saving file: " << output_file_name << std::endl; } - for (auto &t : tr) + for (auto& t : tr) t.translation() += firstm.translation(); - for (auto &t : trmm) + for (auto& t : trmm) t.translation() += firstm.translation(); worker_data[i].intermediate_trajectory = tr; worker_data[i].intermediate_trajectory_motion_model = tr; - optimize_rigid_sf(intermediate_points, - worker_data[i].intermediate_trajectory, - worker_data[i].intermediate_trajectory_motion_model, - params.buckets_indoor, - params.distance_bucket_rigid_sf, - params.polar_angle_deg_rigid_sf, - params.azimutal_angle_deg_rigid_sf, - params.robust_and_accurate_lidar_odometry_rigid_sf_iterations, - params.max_distance_lidar_rigid_sf, - params.useMultithread, - params.rgd_sf_sigma_x_m, - params.rgd_sf_sigma_y_m, - params.rgd_sf_sigma_z_m, - params.rgd_sf_sigma_om_deg, - params.rgd_sf_sigma_fi_deg, - params.rgd_sf_sigma_ka_deg); + optimize_rigid_sf( + intermediate_points, + worker_data[i].intermediate_trajectory, + worker_data[i].intermediate_trajectory_motion_model, + params.buckets_indoor, + params.distance_bucket_rigid_sf, + params.polar_angle_deg_rigid_sf, + params.azimutal_angle_deg_rigid_sf, + params.robust_and_accurate_lidar_odometry_rigid_sf_iterations, + params.max_distance_lidar_rigid_sf, + params.useMultithread, + params.rgd_sf_sigma_x_m, + params.rgd_sf_sigma_y_m, + params.rgd_sf_sigma_z_m, + params.rgd_sf_sigma_om_deg, + params.rgd_sf_sigma_fi_deg, + params.rgd_sf_sigma_ka_deg); } worker_data[i].intermediate_trajectory_motion_model = worker_data[i].intermediate_trajectory; @@ -2058,28 +2320,35 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p { iter_end = iter; delta = 100000.0; - optimize_lidar_odometry(intermediate_points, worker_data[i].intermediate_trajectory, worker_data[i].intermediate_trajectory_motion_model, - params.in_out_params_indoor, params.buckets_indoor, - params.in_out_params_outdoor, params.buckets_outdoor, - params.useMultithread, params.max_distance_lidar, delta, /*add_pitch_roll_constraint, worker_data[i].imu_roll_pitch,*/ - lm_factor, - params.motion_model_correction, - params.lidar_odometry_motion_model_x_1_sigma_m, - params.lidar_odometry_motion_model_y_1_sigma_m, - params.lidar_odometry_motion_model_z_1_sigma_m, - params.lidar_odometry_motion_model_om_1_sigma_deg, - params.lidar_odometry_motion_model_fi_1_sigma_deg, - params.lidar_odometry_motion_model_ka_1_sigma_deg, - params.lidar_odometry_motion_model_fix_origin_x_1_sigma_m, - params.lidar_odometry_motion_model_fix_origin_y_1_sigma_m, - params.lidar_odometry_motion_model_fix_origin_z_1_sigma_m, - params.lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, - params.lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, - params.lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, - params.ablation_study_use_planarity, - params.ablation_study_use_norm, - params.ablation_study_use_hierarchical_rgd, - params.ablation_study_use_view_point_and_normal_vectors); + optimize_lidar_odometry( + intermediate_points, + worker_data[i].intermediate_trajectory, + worker_data[i].intermediate_trajectory_motion_model, + params.in_out_params_indoor, + params.buckets_indoor, + params.in_out_params_outdoor, + params.buckets_outdoor, + params.useMultithread, + params.max_distance_lidar, + delta, /*add_pitch_roll_constraint, worker_data[i].imu_roll_pitch,*/ + lm_factor, + params.motion_model_correction, + params.lidar_odometry_motion_model_x_1_sigma_m, + params.lidar_odometry_motion_model_y_1_sigma_m, + params.lidar_odometry_motion_model_z_1_sigma_m, + params.lidar_odometry_motion_model_om_1_sigma_deg, + params.lidar_odometry_motion_model_fi_1_sigma_deg, + params.lidar_odometry_motion_model_ka_1_sigma_deg, + params.lidar_odometry_motion_model_fix_origin_x_1_sigma_m, + params.lidar_odometry_motion_model_fix_origin_y_1_sigma_m, + params.lidar_odometry_motion_model_fix_origin_z_1_sigma_m, + params.lidar_odometry_motion_model_fix_origin_om_1_sigma_deg, + params.lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg, + params.lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg, + params.ablation_study_use_planarity, + params.ablation_study_use_norm, + params.ablation_study_use_hierarchical_rgd, + params.ablation_study_use_view_point_and_normal_vectors); if (delta < 1e-12) { // std::cout << "finished at iteration " << iter + 1 << "/" << params.nr_iter; @@ -2107,9 +2376,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::cout << "finished at iteration " << iter_end + 1 << "/" << params.nr_iter; - std::cout << " optimizing worker_data " << i + 1 << "/" << worker_data.size() - << " with acc_distance " << fixed << std::setprecision(2) << acc_distance << "[m] in " - << fixed << std::setprecision(2) << elapsed_seconds1.count() + std::cout << " optimizing worker_data " << i + 1 << "/" << worker_data.size() << " with acc_distance " << fixed + << std::setprecision(2) << acc_distance << "[m] in " << fixed << std::setprecision(2) << elapsed_seconds1.count() << "[s], delta "; if (delta > 1e-12) std::cout << std::setprecision(10) << delta << "!!!"; @@ -2127,8 +2395,15 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p std::vector intensity; std::vector timestamps; points_to_vector( - intermediate_points, worker_data[i].intermediate_trajectory, - 0, nullptr, global_pointcloud, intensity, timestamps, false, params.save_index_pose); + intermediate_points, + worker_data[i].intermediate_trajectory, + 0, + nullptr, + global_pointcloud, + intensity, + timestamps, + false, + params.save_index_pose); std::string fn = params.working_directory_preview + "/temp_point_cloud_" + std::to_string(i) + ".laz"; exportLaz(fn.c_str(), global_pointcloud, intensity, timestamps); } @@ -2145,7 +2420,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p acc_distance = acc_distance_tmp; std::cout << "please split data set into subsets" << std::endl; ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - std::cout << "calculations canceled for TIMESTAMP: " << (long long int)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; + std::cout << "calculations canceled for TIMESTAMP: " + << (long long int)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; return false; } @@ -2192,14 +2468,23 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p points_global = points_global_new; // decimate - if (params.decimation > 0){ + if (params.decimation > 0) + { decimate(points_global, params.decimation, params.decimation, params.decimation); } - update_rgd(params.in_out_params_indoor, params.buckets_indoor, points_global, worker_data[i].intermediate_trajectory[0].translation()); + update_rgd( + params.in_out_params_indoor, + params.buckets_indoor, + points_global, + worker_data[i].intermediate_trajectory[0].translation()); if (params.ablation_study_use_hierarchical_rgd) { - update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, points_global, worker_data[i].intermediate_trajectory[0].translation()); + update_rgd( + params.in_out_params_outdoor, + params.buckets_outdoor, + points_global, + worker_data[i].intermediate_trajectory[0].translation()); } // endu = std::chrono::system_clock::now(); @@ -2222,7 +2507,8 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p update_rgd(params.in_out_params_indoor, params.buckets_indoor, pg, worker_data[i].intermediate_trajectory[0].translation()); if (params.ablation_study_use_hierarchical_rgd) { - update_rgd(params.in_out_params_outdoor, params.buckets_outdoor, pg, worker_data[i].intermediate_trajectory[0].translation()); + update_rgd( + params.in_out_params_outdoor, params.buckets_outdoor, pg, worker_data[i].intermediate_trajectory[0].translation()); } } @@ -2249,7 +2535,9 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p params.total_length_of_calculated_trajectory = 0; for (int i = 1; i < worker_data.size(); i++) - params.total_length_of_calculated_trajectory += (worker_data[i].intermediate_trajectory[0].translation() - worker_data[i - 1].intermediate_trajectory[0].translation()).norm(); + params.total_length_of_calculated_trajectory += + (worker_data[i].intermediate_trajectory[0].translation() - worker_data[i - 1].intermediate_trajectory[0].translation()) + .norm(); std::cout << "total_length_of_calculated_trajectory: " << params.total_length_of_calculated_trajectory << " [m]" << std::endl; } @@ -2257,11 +2545,11 @@ bool compute_step_2(std::vector &worker_data, LidarOdometryParams &p return true; } -void compute_step_2_fast_forward_motion(std::vector &worker_data, LidarOdometryParams ¶ms) +void compute_step_2_fast_forward_motion(std::vector& worker_data, LidarOdometryParams& params) { } -void Consistency(std::vector &worker_data, LidarOdometryParams ¶ms) +void Consistency(std::vector& worker_data, LidarOdometryParams& params) { std::vector trajectory; std::vector trajectory_motion_model; @@ -2288,7 +2576,8 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ { std::vector intermediate_points; if (!load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points)) - std::cout << "problem with load_vector_data file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with load_vector_data file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" + << std::endl; for (int j = 0; j < intermediate_points.size(); j++) all_points_local.push_back(intermediate_points[j]); @@ -2298,7 +2587,8 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ std::vector> tripletListP; std::vector> tripletListB; - Eigen::Vector3d b(params.in_out_params_indoor.resolution_X, params.in_out_params_indoor.resolution_Y, params.in_out_params_indoor.resolution_Z); + Eigen::Vector3d b( + params.in_out_params_indoor.resolution_X, params.in_out_params_indoor.resolution_Y, params.in_out_params_indoor.resolution_Z); std::vector my_mutex(1); @@ -2319,46 +2609,48 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ for (size_t i = 1; i < poses.size() - 1; i++) { Eigen::Matrix delta; - smoothness_obs_eq_tait_bryan_wc(delta, - poses[i - 1].px, - poses[i - 1].py, - poses[i - 1].pz, - poses[i - 1].om, - poses[i - 1].fi, - poses[i - 1].ka, - poses[i].px, - poses[i].py, - poses[i].pz, - poses[i].om, - poses[i].fi, - poses[i].ka, - poses[i + 1].px, - poses[i + 1].py, - poses[i + 1].pz, - poses[i + 1].om, - poses[i + 1].fi, - poses[i + 1].ka); + smoothness_obs_eq_tait_bryan_wc( + delta, + poses[i - 1].px, + poses[i - 1].py, + poses[i - 1].pz, + poses[i - 1].om, + poses[i - 1].fi, + poses[i - 1].ka, + poses[i].px, + poses[i].py, + poses[i].pz, + poses[i].om, + poses[i].fi, + poses[i].ka, + poses[i + 1].px, + poses[i + 1].py, + poses[i + 1].pz, + poses[i + 1].om, + poses[i + 1].fi, + poses[i + 1].ka); Eigen::Matrix jacobian; - smoothness_obs_eq_tait_bryan_wc_jacobian(jacobian, - poses[i - 1].px, - poses[i - 1].py, - poses[i - 1].pz, - poses[i - 1].om, - poses[i - 1].fi, - poses[i - 1].ka, - poses[i].px, - poses[i].py, - poses[i].pz, - poses[i].om, - poses[i].fi, - poses[i].ka, - poses[i + 1].px, - poses[i + 1].py, - poses[i + 1].pz, - poses[i + 1].om, - poses[i + 1].fi, - poses[i + 1].ka); + smoothness_obs_eq_tait_bryan_wc_jacobian( + jacobian, + poses[i - 1].px, + poses[i - 1].py, + poses[i - 1].pz, + poses[i - 1].om, + poses[i - 1].fi, + poses[i - 1].ka, + poses[i].px, + poses[i].py, + poses[i].pz, + poses[i].om, + poses[i].fi, + poses[i].ka, + poses[i + 1].px, + poses[i + 1].py, + poses[i + 1].pz, + poses[i + 1].om, + poses[i + 1].fi, + poses[i + 1].ka); const auto ir = tripletListB.size(); @@ -2460,9 +2752,14 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ { if (index < all_points_local.size()) { - auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), all_points_local[index].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + timestamps.begin(), + timestamps.end(), + all_points_local[index].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(timestamps.begin(), lower) - 1; if (index_pose >= 0 && index_pose < trajectory.size()) { @@ -2480,7 +2777,7 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ { update_rgd(params.in_out_params_indoor, buckets, points_global, trajectory[points_global[0].index_pose].translation()); - const auto hessian_fun = [&](const Point3Di &intermediate_points_i) + const auto hessian_fun = [&](const Point3Di& intermediate_points_i) { if (intermediate_points_i.point.norm() < 1.0) return; @@ -2493,9 +2790,9 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ if (bucket_it == buckets.end()) return; - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) @@ -2504,8 +2801,8 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ if ((infm.array() < -threshold).any()) return; - const Eigen::Affine3d &m_pose = trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d &p_s = intermediate_points_i.point; + const Eigen::Affine3d& m_pose = trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points_i.point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); double delta_x; double delta_y; @@ -2514,17 +2811,29 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ Eigen::Matrix jacobian; // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); int c = intermediate_points_i.index_pose * 6; - std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; + std::mutex& m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; std::unique_lock lck(m); int ir = tripletListB.size(); @@ -2607,7 +2916,7 @@ void Consistency(std::vector &worker_data, LidarOdometryParams ¶ worker_data[indexes[i].first].intermediate_trajectory[indexes[i].second] = trajectory[i]; } -void Consistency2(std::vector &worker_data, LidarOdometryParams ¶ms) +void Consistency2(std::vector& worker_data, LidarOdometryParams& params) { // global_tmp.clear(); @@ -2636,7 +2945,8 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par { std::vector intermediate_points; if (!load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points)) - std::cout << "problem with load_vector_data for file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with load_vector_data for file '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" + << std::endl; for (int j = 0; j < intermediate_points.size(); j++) all_points_local.push_back(intermediate_points[j]); @@ -2646,7 +2956,8 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par std::vector> tripletListP; std::vector> tripletListB; - Eigen::Vector3d b(params.in_out_params_indoor.resolution_X, params.in_out_params_indoor.resolution_Y, params.in_out_params_indoor.resolution_Z); + Eigen::Vector3d b( + params.in_out_params_indoor.resolution_X, params.in_out_params_indoor.resolution_Y, params.in_out_params_indoor.resolution_Z); std::vector my_mutex(1); @@ -2666,46 +2977,48 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par for (size_t i = 1; i < poses.size() - 1; i++) { Eigen::Matrix delta; - smoothness_obs_eq_tait_bryan_wc(delta, - poses[i - 1].px, - poses[i - 1].py, - poses[i - 1].pz, - poses[i - 1].om, - poses[i - 1].fi, - poses[i - 1].ka, - poses[i].px, - poses[i].py, - poses[i].pz, - poses[i].om, - poses[i].fi, - poses[i].ka, - poses[i + 1].px, - poses[i + 1].py, - poses[i + 1].pz, - poses[i + 1].om, - poses[i + 1].fi, - poses[i + 1].ka); + smoothness_obs_eq_tait_bryan_wc( + delta, + poses[i - 1].px, + poses[i - 1].py, + poses[i - 1].pz, + poses[i - 1].om, + poses[i - 1].fi, + poses[i - 1].ka, + poses[i].px, + poses[i].py, + poses[i].pz, + poses[i].om, + poses[i].fi, + poses[i].ka, + poses[i + 1].px, + poses[i + 1].py, + poses[i + 1].pz, + poses[i + 1].om, + poses[i + 1].fi, + poses[i + 1].ka); Eigen::Matrix jacobian; - smoothness_obs_eq_tait_bryan_wc_jacobian(jacobian, - poses[i - 1].px, - poses[i - 1].py, - poses[i - 1].pz, - poses[i - 1].om, - poses[i - 1].fi, - poses[i - 1].ka, - poses[i].px, - poses[i].py, - poses[i].pz, - poses[i].om, - poses[i].fi, - poses[i].ka, - poses[i + 1].px, - poses[i + 1].py, - poses[i + 1].pz, - poses[i + 1].om, - poses[i + 1].fi, - poses[i + 1].ka); + smoothness_obs_eq_tait_bryan_wc_jacobian( + jacobian, + poses[i - 1].px, + poses[i - 1].py, + poses[i - 1].pz, + poses[i - 1].om, + poses[i - 1].fi, + poses[i - 1].ka, + poses[i].px, + poses[i].py, + poses[i].pz, + poses[i].om, + poses[i].fi, + poses[i].ka, + poses[i + 1].px, + poses[i + 1].py, + poses[i + 1].pz, + poses[i + 1].om, + poses[i + 1].fi, + poses[i + 1].ka); int ir = tripletListB.size(); @@ -2806,9 +3119,14 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par { if (index < all_points_local.size()) { - auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), all_points_local[index].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + timestamps.begin(), + timestamps.end(), + all_points_local[index].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(timestamps.begin(), lower) - 1; if (index_pose >= 0 && index_pose < trajectory.size()) { @@ -2848,9 +3166,13 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par auto index_of_bucket = get_rgd_index(points_global[i].point, b); index_pairs.emplace_back(index_of_bucket, i); } - std::sort(index_pairs.begin(), index_pairs.end(), - [](const std::pair &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + index_pairs.begin(), + index_pairs.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); NDTBucketMapType2 buckets; @@ -2866,7 +3188,7 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par } } - for (auto &b : buckets) + for (auto& b : buckets) { for (int i = b.second.index_begin_inclusive; i < b.second.index_end_exclusive; i++) { @@ -2882,30 +3204,39 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par } } - for (auto &bb : b.second.buckets) + for (auto& bb : b.second.buckets) { if (bb.second.point_indexes.size() >= 5) { bb.second.valid = true; bb.second.mean = Eigen::Vector3d(0, 0, 0); - for (const auto &p : bb.second.point_indexes) + for (const auto& p : bb.second.point_indexes) bb.second.mean += points_global[p].point; bb.second.mean /= bb.second.point_indexes.size(); bb.second.cov.setZero(); - for (const auto &p : bb.second.point_indexes) + for (const auto& p : bb.second.point_indexes) { - bb.second.cov(0, 0) += (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.x() - points_global[p].point.x()); - bb.second.cov(0, 1) += (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.y() - points_global[p].point.y()); - bb.second.cov(0, 2) += (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.z() - points_global[p].point.z()); - bb.second.cov(1, 0) += (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.x() - points_global[p].point.x()); - bb.second.cov(1, 1) += (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.y() - points_global[p].point.y()); - bb.second.cov(1, 2) += (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.z() - points_global[p].point.z()); - bb.second.cov(2, 0) += (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.x() - points_global[p].point.x()); - bb.second.cov(2, 1) += (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.y() - points_global[p].point.y()); - bb.second.cov(2, 2) += (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.z() - points_global[p].point.z()); + bb.second.cov(0, 0) += + (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.x() - points_global[p].point.x()); + bb.second.cov(0, 1) += + (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.y() - points_global[p].point.y()); + bb.second.cov(0, 2) += + (bb.second.mean.x() - points_global[p].point.x()) * (bb.second.mean.z() - points_global[p].point.z()); + bb.second.cov(1, 0) += + (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.x() - points_global[p].point.x()); + bb.second.cov(1, 1) += + (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.y() - points_global[p].point.y()); + bb.second.cov(1, 2) += + (bb.second.mean.y() - points_global[p].point.y()) * (bb.second.mean.z() - points_global[p].point.z()); + bb.second.cov(2, 0) += + (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.x() - points_global[p].point.x()); + bb.second.cov(2, 1) += + (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.y() - points_global[p].point.y()); + bb.second.cov(2, 2) += + (bb.second.mean.z() - points_global[p].point.z()) * (bb.second.mean.z() - points_global[p].point.z()); } bb.second.cov /= bb.second.point_indexes.size(); @@ -2917,7 +3248,7 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par nv.normalize(); // flip towards viewport - const auto &m = trajectory[points_global[bb.second.point_indexes[0]].index_pose]; + const auto& m = trajectory[points_global[bb.second.point_indexes[0]].index_pose]; if (nv.dot(m.translation() - bb.second.mean) < 0.0) nv *= -1.0; @@ -2950,13 +3281,13 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par if (bucket_it == buckets.end()) continue; - for (auto &bb : bucket_it->second.buckets) + for (auto& bb : bucket_it->second.buckets) { if (bb.second.valid) { - auto &this_bucket = bb.second; + auto& this_bucket = bb.second; - const Eigen::Matrix3d &infm = this_bucket.cov.inverse(); + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); const double threshold = 100000.0; if ((infm.array() > threshold).any()) @@ -2966,7 +3297,7 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par continue; // check nv - Eigen::Vector3d &nv = this_bucket.normal_vector; + Eigen::Vector3d& nv = this_bucket.normal_vector; Eigen::Vector3d viewport = trajectory[points_global[i].index_pose].translation(); if (nv.dot(viewport - this_bucket.mean) < 0) { @@ -2974,8 +3305,8 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par continue; } - const Eigen::Affine3d &m_pose = trajectory[points_global[i].index_pose]; - const Eigen::Vector3d &p_s = points_local[i].point; + const Eigen::Affine3d& m_pose = trajectory[points_global[i].index_pose]; + const Eigen::Vector3d& p_s = points_local[i].point; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); double delta_x; double delta_y; @@ -2984,17 +3315,29 @@ void Consistency2(std::vector &worker_data, LidarOdometryParams &par Eigen::Matrix jacobian; // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); int c = points_global[i].index_pose * 6; - std::mutex &m = my_mutex[0]; + std::mutex& m = my_mutex[0]; std::unique_lock lck(m); int ir = tripletListB.size(); diff --git a/apps/lidar_odometry_step_1/toml_io.cpp b/apps/lidar_odometry_step_1/toml_io.cpp index 50be097c..808fa5bb 100644 --- a/apps/lidar_odometry_step_1/toml_io.cpp +++ b/apps/lidar_odometry_step_1/toml_io.cpp @@ -2,13 +2,13 @@ #include #include -bool TomlIO::SaveParametersToTomlFile(const std::string &filepath, LidarOdometryParams ¶ms) +bool TomlIO::SaveParametersToTomlFile(const std::string& filepath, LidarOdometryParams& params) { toml::table tbl; - for (const auto &[category, attributes] : CATEGORIES) + for (const auto& [category, attributes] : CATEGORIES) { toml::table cat_tbl; - for (const auto &attribute : attributes) + for (const auto& attribute : attributes) { if (attribute == "in_out_params_indoor" || attribute == "in_out_params_outdoor") continue; @@ -24,7 +24,7 @@ bool TomlIO::SaveParametersToTomlFile(const std::string &filepath, LidarOdometry if (attribute == "clear_color") continue; // Skip GUI-only parameter when building without GUI #endif - + // Check if it's a motion_model_correction parameter auto motion_it = MOTION_MODEL_CORRECTION_POINTERS.find(attribute); if (motion_it != MOTION_MODEL_CORRECTION_POINTERS.end()) @@ -32,17 +32,21 @@ bool TomlIO::SaveParametersToTomlFile(const std::string &filepath, LidarOdometry cat_tbl.insert(attribute, params.motion_model_correction.*(motion_it->second)); continue; } - + auto it = POINTERS.find(attribute); if (it == POINTERS.end()) continue; - std::visit([&](auto ptr) - { cat_tbl.insert(attribute, params.*ptr); }, it->second); + std::visit( + [&](auto ptr) + { + cat_tbl.insert(attribute, params.*ptr); + }, + it->second); } tbl.insert(category, std::move(cat_tbl)); } // separate grid params - auto insert_grid_params = [&](const std::string &name, const NDT::GridParameters &grid) + auto insert_grid_params = [&](const std::string& name, const NDT::GridParameters& grid) { toml::table nested_tbl; nested_tbl.insert("bounding_box_min_X", grid.bounding_box_min_X); @@ -63,9 +67,9 @@ bool TomlIO::SaveParametersToTomlFile(const std::string &filepath, LidarOdometry }; insert_grid_params("in_out_params_indoor", params.in_out_params_indoor); insert_grid_params("in_out_params_outdoor", params.in_out_params_outdoor); - + std::ofstream out(filepath); - + // Add header comment with version information out << "# HDMapping Configuration File" << std::endl; out << "# Generated by HDMapping v" << params.software_version << std::endl; @@ -75,23 +79,23 @@ bool TomlIO::SaveParametersToTomlFile(const std::string &filepath, LidarOdometry out << "# This file contains parameters organized in categories." << std::endl; out << "# Version information is stored in [version_info] section for compatibility checking." << std::endl; out << std::endl; - + // Write version_info section first for quick identification out << "[version_info]" << std::endl; out << "software_version = \"" << params.software_version << "\"" << std::endl; out << "config_version = \"" << params.config_version << "\"" << std::endl; out << "build_date = \"" << params.build_date << "\"" << std::endl; out << std::endl; - + // Remove version_info from tbl to avoid duplication since we write it manually tbl.erase("version_info"); - + out << tbl; return true; } -template -void TomlIO::set_if_exists(NDT::GridParameters &grid, const toml::table *tbl, const std::string &key, T NDT::GridParameters::*member) +template +void TomlIO::set_if_exists(NDT::GridParameters& grid, const toml::table* tbl, const std::string& key, T NDT::GridParameters::*member) { if (!tbl) return; @@ -104,7 +108,7 @@ void TomlIO::set_if_exists(NDT::GridParameters &grid, const toml::table *tbl, co } } -void TomlIO::read_grid_params(NDT::GridParameters &grid, const toml::table *grid_tbl) +void TomlIO::read_grid_params(NDT::GridParameters& grid, const toml::table* grid_tbl) { set_if_exists(grid, grid_tbl, "bounding_box_min_X", &NDT::GridParameters::bounding_box_min_X); set_if_exists(grid, grid_tbl, "bounding_box_min_Y", &NDT::GridParameters::bounding_box_min_Y); @@ -131,33 +135,31 @@ void TomlIO::read_grid_params(NDT::GridParameters &grid, const toml::table *grid set_if_exists(grid, grid_tbl, "resolution_Z", &NDT::GridParameters::resolution_Z); } -bool TomlIO::LoadParametersFromTomlFile(const std::string &filepath, LidarOdometryParams ¶ms) +bool TomlIO::LoadParametersFromTomlFile(const std::string& filepath, LidarOdometryParams& params) { toml::table tbl; try { tbl = toml::parse_file(filepath); - } - catch (const toml::parse_error &err) + } catch (const toml::parse_error& err) { - std::cerr << "Parsing failed:\n" - << err << "\n"; + std::cerr << "Parsing failed:\n" << err << "\n"; return false; } - for (const auto &[category, attributes] : CATEGORIES) + for (const auto& [category, attributes] : CATEGORIES) { auto cat_it = tbl.find(category); if (cat_it == tbl.end()) continue; - const auto &cat_tbl = cat_it->second.as_table(); + const auto& cat_tbl = cat_it->second.as_table(); if (!cat_tbl) continue; - for (const auto &attribute : attributes) + for (const auto& attribute : attributes) { // grid params will be processed separately if (attribute == "in_out_params_indoor" || attribute == "in_out_params_outdoor") continue; - + // Check if it's a motion_model_correction parameter auto motion_it = MOTION_MODEL_CORRECTION_POINTERS.find(attribute); if (motion_it != MOTION_MODEL_CORRECTION_POINTERS.end()) @@ -169,28 +171,35 @@ bool TomlIO::LoadParametersFromTomlFile(const std::string &filepath, LidarOdomet } continue; } - + auto it = POINTERS.find(attribute); if (it == POINTERS.end()) continue; auto attr_it = cat_tbl->find(attribute); if (attr_it == cat_tbl->end()) continue; - std::visit([&](auto ptr) - { - using MemberType = std::remove_reference_t; - if constexpr (std::is_same_v) { - params.*ptr = attr_it->second.value_or(false); - } - else if constexpr (std::is_same_v) { - params.*ptr = static_cast(attr_it->second.value_or(0)); - } - else if constexpr (std::is_same_v) { - params.*ptr = attr_it->second.value_or(0.0); - } - else if constexpr (std::is_same_v) { - params.*ptr = attr_it->second.value_or(std::string{}); - } }, it->second); + std::visit( + [&](auto ptr) + { + using MemberType = std::remove_reference_t; + if constexpr (std::is_same_v) + { + params.*ptr = attr_it->second.value_or(false); + } + else if constexpr (std::is_same_v) + { + params.*ptr = static_cast(attr_it->second.value_or(0)); + } + else if constexpr (std::is_same_v) + { + params.*ptr = attr_it->second.value_or(0.0); + } + else if constexpr (std::is_same_v) + { + params.*ptr = attr_it->second.value_or(std::string{}); + } + }, + it->second); } #if WITH_GUI == 1 @@ -209,93 +218,113 @@ bool TomlIO::LoadParametersFromTomlFile(const std::string &filepath, LidarOdomet } read_grid_params(params.in_out_params_indoor, tbl["in_out_params_indoor"].as_table()); read_grid_params(params.in_out_params_outdoor, tbl["in_out_params_outdoor"].as_table()); - + // Check and handle version information auto version_info = CheckConfigVersion(filepath); - if (!version_info.found) { + if (!version_info.found) + { std::cout << "WARNING: No version information found in config file: " << filepath << std::endl; std::cout << " This config was created with an older version of HDMapping." << std::endl; HandleMissingVersion(params); - } else { + } + else + { std::string current_version = get_software_version(); - if (!IsVersionCompatible(version_info.software_version, current_version)) { + if (!IsVersionCompatible(version_info.software_version, current_version)) + { std::cout << "WARNING: Version mismatch detected:" << std::endl; std::cout << " Config version: " << version_info.software_version << std::endl; std::cout << " Current version: " << current_version << std::endl; std::cout << " Config created on: " << version_info.build_date << std::endl; std::cout << " Consider updating the configuration file." << std::endl; - } else { + } + else + { std::cout << "SUCCESS: Version compatible: " << current_version << std::endl; } } - + return true; } // Version validation and handling implementations -TomlIO::VersionInfo TomlIO::CheckConfigVersion(const std::string &filepath) { +TomlIO::VersionInfo TomlIO::CheckConfigVersion(const std::string& filepath) +{ VersionInfo info; - - try { + + try + { auto config = toml::parse_file(filepath); - - if (config.contains("version_info")) { + + if (config.contains("version_info")) + { auto version_section = config["version_info"].as_table(); - if (version_section) { + if (version_section) + { auto sw_ver = version_section->get("software_version"); auto cfg_ver = version_section->get("config_version"); auto build_dt = version_section->get("build_date"); - + info.software_version = sw_ver ? sw_ver->as_string()->get() : ""; info.config_version = cfg_ver ? cfg_ver->as_string()->get() : ""; info.build_date = build_dt ? build_dt->as_string()->get() : ""; info.found = true; } } - } catch (const std::exception& e) { + } catch (const std::exception& e) + { std::cerr << "Error checking version in config: " << e.what() << std::endl; } - + return info; } -bool TomlIO::IsVersionCompatible(const std::string &file_version, const std::string ¤t_version) { - if (file_version.empty() || current_version.empty()) { +bool TomlIO::IsVersionCompatible(const std::string& file_version, const std::string& current_version) +{ + if (file_version.empty() || current_version.empty()) + { return false; } - + // Parse version numbers (simple implementation) - auto parse_version = [](const std::string& version) { + auto parse_version = [](const std::string& version) + { std::vector parts; std::stringstream ss(version); std::string item; - while (std::getline(ss, item, '.')) { - try { + while (std::getline(ss, item, '.')) + { + try + { parts.push_back(std::stoi(item)); - } catch (...) { + } catch (...) + { parts.push_back(0); } } return parts; }; - + auto file_parts = parse_version(file_version); auto current_parts = parse_version(current_version); - + // Ensure both have at least 3 parts (major.minor.patch) - while (file_parts.size() < 3) file_parts.push_back(0); - while (current_parts.size() < 3) current_parts.push_back(0); - + while (file_parts.size() < 3) + file_parts.push_back(0); + while (current_parts.size() < 3) + current_parts.push_back(0); + // Compatible if major and minor versions match return (file_parts[0] == current_parts[0]) && (file_parts[1] == current_parts[1]); } -void TomlIO::HandleMissingVersion(LidarOdometryParams ¶ms) { +void TomlIO::HandleMissingVersion(LidarOdometryParams& params) +{ // Set current version information for missing version configs params.software_version = get_software_version(); params.config_version = "1.0"; params.build_date = __DATE__; - + std::cout << " -> Updated config with current version: " << params.software_version << std::endl; std::cout << " -> Consider saving the config to persist version information." << std::endl; } \ No newline at end of file diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index 1b4df5b3..982f803c 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -1,121 +1,169 @@ -#ifndef _TOML_IO_H_ -#define _TOML_IO_H_ +#pragma once -#include #include +#include #include + #include "lidar_odometry_utils.h" class TomlIO { public: - TomlIO() { ; }; - ~TomlIO() { ; }; + TomlIO() + { + ; + }; + ~TomlIO() + { + ; + }; - using ParamValue = std::variant< - bool LidarOdometryParams::*, - double LidarOdometryParams::*, - int LidarOdometryParams::*, - std::string LidarOdometryParams::*>; + using ParamValue = std:: + variant; const std::map POINTERS = { // version information - {"software_version", &LidarOdometryParams::software_version}, - {"config_version", &LidarOdometryParams::config_version}, - {"build_date", &LidarOdometryParams::build_date}, - - {"useMultithread", &LidarOdometryParams::useMultithread}, - {"real_time_threshold_seconds", &LidarOdometryParams::real_time_threshold_seconds}, - {"filter_threshold_xy_inner", &LidarOdometryParams::filter_threshold_xy_inner}, - {"filter_threshold_xy_outer", &LidarOdometryParams::filter_threshold_xy_outer}, - {"decimation", &LidarOdometryParams::decimation}, - {"threshould_output_filter", &LidarOdometryParams::threshould_output_filter}, - {"min_counter_concatenated_trajectory_nodes", &LidarOdometryParams::min_counter_concatenated_trajectory_nodes}, - {"fusionConventionNwu", &LidarOdometryParams::fusionConventionNwu}, - {"fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu}, - {"fusionConventionNed", &LidarOdometryParams::fusionConventionNed}, - {"ahrs_gain", &LidarOdometryParams::ahrs_gain}, - {"use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step}, - {"nr_iter", &LidarOdometryParams::nr_iter}, - {"sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold}, - {"max_distance_lidar", &LidarOdometryParams::max_distance_lidar}, - {"threshold_initial_points", &LidarOdometryParams::threshold_initial_points}, - {"threshold_nr_poses", &LidarOdometryParams::threshold_nr_poses}, - {"save_calibration_validation", &LidarOdometryParams::save_calibration_validation}, - {"calibration_validation_points", &LidarOdometryParams::calibration_validation_points}, - {"num_constistency_iter", &LidarOdometryParams::num_constistency_iter}, - {"use_mutliple_gaussian", &LidarOdometryParams::use_mutliple_gaussian}, - {"lidar_odometry_motion_model_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_x_1_sigma_m}, - {"lidar_odometry_motion_model_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_y_1_sigma_m}, - {"lidar_odometry_motion_model_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_z_1_sigma_m}, - {"lidar_odometry_motion_model_om_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_om_1_sigma_deg}, - {"lidar_odometry_motion_model_fi_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fi_1_sigma_deg}, - {"lidar_odometry_motion_model_ka_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_ka_1_sigma_deg}, - {"lidar_odometry_motion_model_fix_origin_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_x_1_sigma_m}, - {"lidar_odometry_motion_model_fix_origin_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_y_1_sigma_m}, - {"lidar_odometry_motion_model_fix_origin_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_z_1_sigma_m}, - {"lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_om_1_sigma_deg}, - {"lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg}, - {"lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg}, - {"use_robust_and_accurate_lidar_odometry", &LidarOdometryParams::use_robust_and_accurate_lidar_odometry}, - {"distance_bucket", &LidarOdometryParams::distance_bucket}, - {"polar_angle_deg", &LidarOdometryParams::polar_angle_deg}, - {"azimutal_angle_deg", &LidarOdometryParams::azimutal_angle_deg}, - {"robust_and_accurate_lidar_odometry_iterations", &LidarOdometryParams::robust_and_accurate_lidar_odometry_iterations}, - {"distance_bucket_rigid_sf", &LidarOdometryParams::distance_bucket_rigid_sf}, - {"polar_angle_deg_rigid_sf", &LidarOdometryParams::polar_angle_deg_rigid_sf}, - {"azimutal_angle_deg_rigid_sf", &LidarOdometryParams::azimutal_angle_deg_rigid_sf}, - {"robust_and_accurate_lidar_odometry_rigid_sf_iterations", &LidarOdometryParams::robust_and_accurate_lidar_odometry_rigid_sf_iterations}, - {"rgd_sf_sigma_x_m", &LidarOdometryParams::rgd_sf_sigma_x_m}, - {"rgd_sf_sigma_y_m", &LidarOdometryParams::rgd_sf_sigma_y_m}, - {"rgd_sf_sigma_z_m", &LidarOdometryParams::rgd_sf_sigma_z_m}, - {"rgd_sf_sigma_om_deg", &LidarOdometryParams::rgd_sf_sigma_om_deg}, - {"rgd_sf_sigma_fi_deg", &LidarOdometryParams::rgd_sf_sigma_fi_deg}, - {"rgd_sf_sigma_ka_deg", &LidarOdometryParams::rgd_sf_sigma_ka_deg}, - {"max_distance_lidar_rigid_sf", &LidarOdometryParams::max_distance_lidar_rigid_sf}, - {"current_output_dir", &LidarOdometryParams::current_output_dir}, - {"working_directory_preview", &LidarOdometryParams::working_directory_preview}}; + { "software_version", &LidarOdometryParams::software_version }, + { "config_version", &LidarOdometryParams::config_version }, + { "build_date", &LidarOdometryParams::build_date }, + + { "useMultithread", &LidarOdometryParams::useMultithread }, + { "real_time_threshold_seconds", &LidarOdometryParams::real_time_threshold_seconds }, + { "filter_threshold_xy_inner", &LidarOdometryParams::filter_threshold_xy_inner }, + { "filter_threshold_xy_outer", &LidarOdometryParams::filter_threshold_xy_outer }, + { "decimation", &LidarOdometryParams::decimation }, + { "threshould_output_filter", &LidarOdometryParams::threshould_output_filter }, + { "min_counter_concatenated_trajectory_nodes", &LidarOdometryParams::min_counter_concatenated_trajectory_nodes }, + { "fusionConventionNwu", &LidarOdometryParams::fusionConventionNwu }, + { "fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu }, + { "fusionConventionNed", &LidarOdometryParams::fusionConventionNed }, + { "ahrs_gain", &LidarOdometryParams::ahrs_gain }, + { "use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step }, + { "nr_iter", &LidarOdometryParams::nr_iter }, + { "sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold }, + { "max_distance_lidar", &LidarOdometryParams::max_distance_lidar }, + { "threshold_initial_points", &LidarOdometryParams::threshold_initial_points }, + { "threshold_nr_poses", &LidarOdometryParams::threshold_nr_poses }, + { "save_calibration_validation", &LidarOdometryParams::save_calibration_validation }, + { "calibration_validation_points", &LidarOdometryParams::calibration_validation_points }, + { "num_constistency_iter", &LidarOdometryParams::num_constistency_iter }, + { "use_mutliple_gaussian", &LidarOdometryParams::use_mutliple_gaussian }, + { "lidar_odometry_motion_model_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_x_1_sigma_m }, + { "lidar_odometry_motion_model_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_y_1_sigma_m }, + { "lidar_odometry_motion_model_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_z_1_sigma_m }, + { "lidar_odometry_motion_model_om_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_om_1_sigma_deg }, + { "lidar_odometry_motion_model_fi_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fi_1_sigma_deg }, + { "lidar_odometry_motion_model_ka_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_ka_1_sigma_deg }, + { "lidar_odometry_motion_model_fix_origin_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_x_1_sigma_m }, + { "lidar_odometry_motion_model_fix_origin_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_y_1_sigma_m }, + { "lidar_odometry_motion_model_fix_origin_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_z_1_sigma_m }, + { "lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_om_1_sigma_deg }, + { "lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg }, + { "lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg }, + { "use_robust_and_accurate_lidar_odometry", &LidarOdometryParams::use_robust_and_accurate_lidar_odometry }, + { "distance_bucket", &LidarOdometryParams::distance_bucket }, + { "polar_angle_deg", &LidarOdometryParams::polar_angle_deg }, + { "azimutal_angle_deg", &LidarOdometryParams::azimutal_angle_deg }, + { "robust_and_accurate_lidar_odometry_iterations", &LidarOdometryParams::robust_and_accurate_lidar_odometry_iterations }, + { "distance_bucket_rigid_sf", &LidarOdometryParams::distance_bucket_rigid_sf }, + { "polar_angle_deg_rigid_sf", &LidarOdometryParams::polar_angle_deg_rigid_sf }, + { "azimutal_angle_deg_rigid_sf", &LidarOdometryParams::azimutal_angle_deg_rigid_sf }, + { "robust_and_accurate_lidar_odometry_rigid_sf_iterations", + &LidarOdometryParams::robust_and_accurate_lidar_odometry_rigid_sf_iterations }, + { "rgd_sf_sigma_x_m", &LidarOdometryParams::rgd_sf_sigma_x_m }, + { "rgd_sf_sigma_y_m", &LidarOdometryParams::rgd_sf_sigma_y_m }, + { "rgd_sf_sigma_z_m", &LidarOdometryParams::rgd_sf_sigma_z_m }, + { "rgd_sf_sigma_om_deg", &LidarOdometryParams::rgd_sf_sigma_om_deg }, + { "rgd_sf_sigma_fi_deg", &LidarOdometryParams::rgd_sf_sigma_fi_deg }, + { "rgd_sf_sigma_ka_deg", &LidarOdometryParams::rgd_sf_sigma_ka_deg }, + { "max_distance_lidar_rigid_sf", &LidarOdometryParams::max_distance_lidar_rigid_sf }, + { "current_output_dir", &LidarOdometryParams::current_output_dir }, + { "working_directory_preview", &LidarOdometryParams::working_directory_preview } + }; // Special handling for TaitBryanPose members const std::map MOTION_MODEL_CORRECTION_POINTERS = { - {"motion_model_correction_om", &TaitBryanPose::om}, - {"motion_model_correction_fi", &TaitBryanPose::fi}, - {"motion_model_correction_ka", &TaitBryanPose::ka}}; + { "motion_model_correction_om", &TaitBryanPose::om }, + { "motion_model_correction_fi", &TaitBryanPose::fi }, + { "motion_model_correction_ka", &TaitBryanPose::ka } + }; std::map> CATEGORIES = { - {"version_info", {"software_version", "config_version", "build_date"}}, - {"performance", {"useMultithread", "real_time_threshold_seconds"}}, - {"filter_points", {"filter_threshold_xy_inner", "filter_threshold_xy_outer", "decimation", "threshould_output_filter", "min_counter_concatenated_trajectory_nodes"}}, - {"madgwick_filter", {"fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "ahrs_gain"}}, - {"lidar_odometry_control", {"use_motion_from_previous_step", "nr_iter", "sliding_window_trajectory_length_threshold", "max_distance_lidar", "threshold_initial_points", "threshold_nr_poses"}}, - {"lidar_odometry_debug_info", {"save_calibration_validation", "calibration_validation_points"}}, - {"consistency", {"num_constistency_iter", "use_mutliple_gaussian"}}, - {"motion_model_uncertainty", {"lidar_odometry_motion_model_x_1_sigma_m", "lidar_odometry_motion_model_y_1_sigma_m", "lidar_odometry_motion_model_z_1_sigma_m", "lidar_odometry_motion_model_om_1_sigma_deg", "lidar_odometry_motion_model_fi_1_sigma_deg", "lidar_odometry_motion_model_ka_1_sigma_deg"}}, - {"motion_model_correction", {"motion_model_correction_om", "motion_model_correction_fi", "motion_model_correction_ka"}}, - {"motion_model_first_node_prior_uncertainty", {"lidar_odometry_motion_model_fix_origin_x_1_sigma_m", "lidar_odometry_motion_model_fix_origin_y_1_sigma_m", "lidar_odometry_motion_model_fix_origin_z_1_sigma_m", "lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", "lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", "lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg"}}, - {"robust_lidar_odometry", {"use_robust_and_accurate_lidar_odometry", "distance_bucket", "polar_angle_deg", "azimutal_angle_deg", "robust_and_accurate_lidar_odometry_iterations", "distance_bucket_rigid_sf", "polar_angle_deg_rigid_sf", "azimutal_angle_deg_rigid_sf", "robust_and_accurate_lidar_odometry_rigid_sf_iterations", "max_distance_lidar_rigid_sf", "rgd_sf_sigma_x_m", "rgd_sf_sigma_y_m", "rgd_sf_sigma_z_m", "rgd_sf_sigma_om_deg", "rgd_sf_sigma_fi_deg", "rgd_sf_sigma_ka_deg"}}, - {"paths", {"current_output_dir", "working_directory_preview"}}, - { "misc", {"clear_color"} } }; + { "version_info", { "software_version", "config_version", "build_date" } }, + { "performance", { "useMultithread", "real_time_threshold_seconds" } }, + { "filter_points", + { "filter_threshold_xy_inner", + "filter_threshold_xy_outer", + "decimation", + "threshould_output_filter", + "min_counter_concatenated_trajectory_nodes" } }, + { "madgwick_filter", { "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "ahrs_gain" } }, + { "lidar_odometry_control", + { "use_motion_from_previous_step", + "nr_iter", + "sliding_window_trajectory_length_threshold", + "max_distance_lidar", + "threshold_initial_points", + "threshold_nr_poses" } }, + { "lidar_odometry_debug_info", { "save_calibration_validation", "calibration_validation_points" } }, + { "consistency", { "num_constistency_iter", "use_mutliple_gaussian" } }, + { "motion_model_uncertainty", + { "lidar_odometry_motion_model_x_1_sigma_m", + "lidar_odometry_motion_model_y_1_sigma_m", + "lidar_odometry_motion_model_z_1_sigma_m", + "lidar_odometry_motion_model_om_1_sigma_deg", + "lidar_odometry_motion_model_fi_1_sigma_deg", + "lidar_odometry_motion_model_ka_1_sigma_deg" } }, + { "motion_model_correction", { "motion_model_correction_om", "motion_model_correction_fi", "motion_model_correction_ka" } }, + { "motion_model_first_node_prior_uncertainty", + { "lidar_odometry_motion_model_fix_origin_x_1_sigma_m", + "lidar_odometry_motion_model_fix_origin_y_1_sigma_m", + "lidar_odometry_motion_model_fix_origin_z_1_sigma_m", + "lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", + "lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", + "lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg" } }, + { "robust_lidar_odometry", + { "use_robust_and_accurate_lidar_odometry", + "distance_bucket", + "polar_angle_deg", + "azimutal_angle_deg", + "robust_and_accurate_lidar_odometry_iterations", + "distance_bucket_rigid_sf", + "polar_angle_deg_rigid_sf", + "azimutal_angle_deg_rigid_sf", + "robust_and_accurate_lidar_odometry_rigid_sf_iterations", + "max_distance_lidar_rigid_sf", + "rgd_sf_sigma_x_m", + "rgd_sf_sigma_y_m", + "rgd_sf_sigma_z_m", + "rgd_sf_sigma_om_deg", + "rgd_sf_sigma_fi_deg", + "rgd_sf_sigma_ka_deg" } }, + { "paths", { "current_output_dir", "working_directory_preview" } }, + { "misc", { "clear_color" } } + }; + + bool SaveParametersToTomlFile(const std::string& filepath, LidarOdometryParams& params); - bool SaveParametersToTomlFile(const std::string &filepath, LidarOdometryParams ¶ms); - // Version validation and handling functions - struct VersionInfo { + struct VersionInfo + { std::string software_version; std::string config_version; std::string build_date; bool found = false; }; - - VersionInfo CheckConfigVersion(const std::string &filepath); - bool IsVersionCompatible(const std::string &file_version, const std::string ¤t_version); - void HandleMissingVersion(LidarOdometryParams ¶ms); - - template - void set_if_exists(NDT::GridParameters &grid, const toml::table *tbl, const std::string &key, T NDT::GridParameters::*member); - void read_grid_params(NDT::GridParameters &grid, const toml::table *grid_tbl); - bool LoadParametersFromTomlFile(const std::string &filepath, LidarOdometryParams ¶ms); -}; -#endif \ No newline at end of file + VersionInfo CheckConfigVersion(const std::string& filepath); + bool IsVersionCompatible(const std::string& file_version, const std::string& current_version); + void HandleMissingVersion(LidarOdometryParams& params); + + template + void set_if_exists(NDT::GridParameters& grid, const toml::table* tbl, const std::string& key, T NDT::GridParameters::*member); + void read_grid_params(NDT::GridParameters& grid, const toml::table* grid_tbl); + bool LoadParametersFromTomlFile(const std::string& filepath, LidarOdometryParams& params); +}; diff --git a/apps/lidar_odometry_step_1/version_example.cpp b/apps/lidar_odometry_step_1/version_example.cpp index c677f4d0..3aad08df 100644 --- a/apps/lidar_odometry_step_1/version_example.cpp +++ b/apps/lidar_odometry_step_1/version_example.cpp @@ -1,107 +1,128 @@ // Example: How to use version information from TOML configuration files // This demonstrates reading and validating version information -#include "toml_io.h" #include "lidar_odometry_utils.h" +#include "toml_io.h" #include -void example_version_usage() { +void example_version_usage() +{ std::cout << "=== Version Information Usage Example ===" << std::endl; - + // 1. Create parameters with current version LidarOdometryParams params; std::cout << "Current software version: " << params.software_version << std::endl; std::cout << "Config version: " << params.config_version << std::endl; std::cout << "Build date: " << params.build_date << std::endl; - + // 2. Save configuration to TOML file (with version header) TomlIO toml_io; std::string config_file = "example_config.toml"; - - if (toml_io.SaveParametersToTomlFile(config_file, params)) { + + if (toml_io.SaveParametersToTomlFile(config_file, params)) + { std::cout << "Configuration saved to: " << config_file << std::endl; std::cout << "File includes version header for easy identification!" << std::endl; } - + // 3. Load and validate version compatibility LidarOdometryParams loaded_params; - if (toml_io.LoadParametersFromTomlFile(config_file, loaded_params)) { + if (toml_io.LoadParametersFromTomlFile(config_file, loaded_params)) + { std::cout << "Configuration loaded successfully!" << std::endl; - + // Version compatibility check std::string current_version = get_software_version(); std::string loaded_version = loaded_params.software_version; - - if (current_version == loaded_version) { + + if (current_version == loaded_version) + { std::cout << "SUCCESS: Version match: " << current_version << std::endl; - } else { + } + else + { std::cout << "WARNING: Version mismatch:" << std::endl; std::cout << " Current: " << current_version << std::endl; std::cout << " Config: " << loaded_version << std::endl; - + // You can implement migration logic here - if (should_migrate_config(loaded_version, current_version)) { + if (should_migrate_config(loaded_version, current_version)) + { std::cout << " Migration required!" << std::endl; } } - + // Build date information std::cout << "Config was created on: " << loaded_params.build_date << std::endl; } } // Example version comparison function -bool should_migrate_config(const std::string& old_version, const std::string& new_version) { +bool should_migrate_config(const std::string& old_version, const std::string& new_version) +{ // Parse version numbers (simplified example) - auto parse_version = [](const std::string& version) { + auto parse_version = [](const std::string& version) + { std::vector parts; std::stringstream ss(version); std::string item; - while (std::getline(ss, item, '.')) { + while (std::getline(ss, item, '.')) + { parts.push_back(std::stoi(item)); } return parts; }; - + auto old_parts = parse_version(old_version); auto new_parts = parse_version(new_version); - + // Migration needed if major or minor version changed - if (old_parts.size() >= 2 && new_parts.size() >= 2) { + if (old_parts.size() >= 2 && new_parts.size() >= 2) + { return (old_parts[0] != new_parts[0]) || (old_parts[1] != new_parts[1]); } - + return false; } // Example of reading version from existing TOML without loading full config -void check_config_version(const std::string& config_file) { - try { +void check_config_version(const std::string& config_file) +{ + try + { auto config = toml::parse_file(config_file); - - if (config.contains("version_info")) { + + if (config.contains("version_info")) + { auto version_info = config["version_info"]; - - if (version_info.contains("software_version")) { + + if (version_info.contains("software_version")) + { std::string file_version = version_info["software_version"].value_or(""); std::string current_version = get_software_version(); - + std::cout << "Quick version check:" << std::endl; std::cout << " File version: " << file_version << std::endl; std::cout << " Current version: " << current_version << std::endl; - - if (file_version == current_version) { + + if (file_version == current_version) + { std::cout << " Status: [COMPATIBLE] Compatible" << std::endl; - } else { + } + else + { std::cout << " Status: [WARNING] May need update" << std::endl; } } - } else { + } + else + { std::cout << "WARNING: No version information found in config file!" << std::endl; std::cout << " This config was created with an older version." << std::endl; } - - } catch (const std::exception& e) { + + } catch (const std::exception& e) + { std::cerr << "Error reading config: " << e.what() << std::endl; } } @@ -122,7 +143,7 @@ Usage in your applications: # Generated by HDMapping v0.84.0 # Config version: 1.0 # Created on: Aug 3 2025 - # + # # This file contains 74 parameters organized in categories. # Version information is stored in [version_info] section for compatibility checking. diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index ab552d2a..cfe0e42e 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -14,19 +14,19 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( livox_mid_360_intrinsic_calibration diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp index 22cb3443..16e258fa 100644 --- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -1,11 +1,15 @@ +// clang-format off +#include +#include +// clang-format on + #include #include #include -#include #include -#include -#include +#include + #include #include @@ -15,8 +19,8 @@ #include "pfd_wrapper.hpp" -#include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include #include @@ -55,18 +59,12 @@ float translate_z = -50.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}; +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_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; bool show_pc = true; bool show_single_point_and_ray = false; @@ -115,12 +113,16 @@ void reshape(int w, int h) } else { - ImGuiIO &io = ImGui::GetIO(); + 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( + -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); @@ -129,7 +131,7 @@ void reshape(int w, int h) void motion(int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); if (!io.WantCaptureMouse) @@ -143,8 +145,10 @@ void motion(int x, int y) 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); + 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; @@ -179,7 +183,7 @@ void motion(int x, int y) glutPostRedisplay(); } -std::vector load_pc(const std::string &lazFile) +std::vector load_pc(const std::string& lazFile) { std::vector points; laszip_POINTER laszip_reader; @@ -196,7 +200,7 @@ std::vector load_pc(const std::string &lazFile) std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -204,7 +208,7 @@ std::vector load_pc(const std::string &lazFile) std::abort(); } fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); @@ -218,7 +222,6 @@ std::vector load_pc(const std::string &lazFile) for (laszip_U32 j = 0; j < header->number_of_point_records; j++) { - if (laszip_read_point(laszip_reader)) { fprintf(stderr, "DLL ERROR: reading point %u\n", j); @@ -231,7 +234,10 @@ std::vector load_pc(const std::string &lazFile) int id = point->user_data; // Eigen::Affine3d calibration = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(id); - const Eigen::Vector3d pf(header->x_offset + header->x_scale_factor * static_cast(point->X), header->y_offset + header->y_scale_factor * static_cast(point->Y), header->z_offset + header->z_scale_factor * static_cast(point->Z)); + const Eigen::Vector3d pf( + header->x_offset + header->x_scale_factor * static_cast(point->X), + header->y_offset + header->y_scale_factor * static_cast(point->Y), + header->z_offset + header->z_scale_factor * static_cast(point->Z)); p.point = pf; p.lidarid = id; @@ -250,11 +256,11 @@ void project_gui() { if (ImGui::Begin("main gui window")) { - ImGui::ColorEdit3("clear color", (float *)&clear_color); + ImGui::ColorEdit3("clear color", (float*)&clear_color); if (show_pc) { - ImGui::ColorEdit3("pc_color", (float *)&pc_color); + ImGui::ColorEdit3("pc_color", (float*)&pc_color); } if (ImGui::Button("Load pointcloud (lidar****.laz) (step 1)")) @@ -268,7 +274,7 @@ void project_gui() { auto pc = load_pc(input_file_names[i].c_str()); - for (const auto &p : pc) + for (const auto& p : pc) { point_cloud.emplace_back(p); } @@ -287,8 +293,8 @@ void project_gui() if (show_single_point_and_ray) { - ImGui::ColorEdit3("pc_color_point", (float *)&pc_color_point); - ImGui::ColorEdit3("pc_color_ray", (float *)&pc_color_ray); + ImGui::ColorEdit3("pc_color_point", (float*)&pc_color_point); + ImGui::ColorEdit3("pc_color_ray", (float*)&pc_color_ray); ImGui::InputInt("single_point_size", &single_point_size); if (single_point_size < 1) @@ -312,8 +318,8 @@ void project_gui() intrinsic_path.push_back(intrinsics[index_rendered_point_and_ray].translation()); } - ImGui::ColorEdit3("pc_color_point_cloud_path", (float *)&pc_color_point_cloud_path); - ImGui::ColorEdit3("intrinsic_path_color", (float *)&intrinsic_path_color); + ImGui::ColorEdit3("pc_color_point_cloud_path", (float*)&pc_color_point_cloud_path); + ImGui::ColorEdit3("intrinsic_path_color", (float*)&intrinsic_path_color); if (ImGui::Button("clear point_cloud_path and intrinsic path")) { @@ -329,11 +335,11 @@ void project_gui() } if (ImGui::Button("calibrate_intrinsics x 10")) { - for(int i = 0; i < 10; i++){ - std::cout << "iteration: " << i + 1 << " of 10" << std::endl; + for (int i = 0; i < 10; i++) + { + std::cout << "iteration: " << i + 1 << " of 10" << std::endl; calibrate_intrinsics(); } - } if (ImGui::Button("calibrate_intrinsics x 100")) { @@ -344,7 +350,7 @@ void project_gui() } } - ImGui::ColorEdit3("intrinsics_color", (float *)&intrinsics_color); + ImGui::ColorEdit3("intrinsics_color", (float*)&intrinsics_color); ImGui::Checkbox("show_intrinsics", &show_intrinsics); ImGui::Checkbox("apply_intrinsics_in_render", &apply_intrinsics_in_render); @@ -381,7 +387,8 @@ void project_gui() { point_intrinsic_correspondances.clear(); - Eigen::Vector3d current_point = intrinsics[point_cloud[index_rendered_point_and_ray].index_pose] * point_cloud[index_rendered_point_and_ray].point; + Eigen::Vector3d current_point = + intrinsics[point_cloud[index_rendered_point_and_ray].index_pose] * point_cloud[index_rendered_point_and_ray].point; for (int i = 0; i < point_cloud.size(); i++) { @@ -894,7 +901,7 @@ void project_gui() void display() { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -902,13 +909,21 @@ void display() 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); + 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); @@ -927,12 +942,11 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - 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())); + 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); } @@ -1031,14 +1045,23 @@ void display() glPointSize(single_point_size); glColor3f(pc_color_point.x, pc_color_point.y, pc_color_point.z); glBegin(GL_POINTS); - glVertex3f(point_cloud[index_rendered_point_and_ray].point.x(), point_cloud[index_rendered_point_and_ray].point.y(), point_cloud[index_rendered_point_and_ray].point.z()); + glVertex3f( + point_cloud[index_rendered_point_and_ray].point.x(), + point_cloud[index_rendered_point_and_ray].point.y(), + point_cloud[index_rendered_point_and_ray].point.z()); glEnd(); glPointSize(1); glColor3f(pc_color_ray.x, pc_color_ray.y, pc_color_ray.z); glBegin(GL_LINES); - glVertex3f(intrinsics[index_rendered_point_and_ray](0, 3), intrinsics[index_rendered_point_and_ray](1, 3), intrinsics[index_rendered_point_and_ray](2, 3)); - glVertex3f(point_cloud[index_rendered_point_and_ray].point.x(), point_cloud[index_rendered_point_and_ray].point.y(), point_cloud[index_rendered_point_and_ray].point.z()); + glVertex3f( + intrinsics[index_rendered_point_and_ray](0, 3), + intrinsics[index_rendered_point_and_ray](1, 3), + intrinsics[index_rendered_point_and_ray](2, 3)); + glVertex3f( + point_cloud[index_rendered_point_and_ray].point.x(), + point_cloud[index_rendered_point_and_ray].point.y(), + point_cloud[index_rendered_point_and_ray].point.z()); glEnd(); } @@ -1110,7 +1133,7 @@ void display() glColor3f(0, 0, 0); glBegin(GL_LINES); - for (const auto &p : point_intrinsic_correspondances) + for (const auto& p : point_intrinsic_correspondances) { glVertex3f(p.first.x(), p.first.y(), p.first.z()); glVertex3f(p.second.x(), p.second.y(), p.second.z()); @@ -1238,7 +1261,7 @@ void display() #endif ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); project_gui(); @@ -1249,7 +1272,7 @@ void display() glutPostRedisplay(); } -bool initGL(int *argc, char **argv) +bool initGL(int* argc, char** argv) { glutInit(argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); @@ -1271,7 +1294,7 @@ bool initGL(int *argc, char **argv) gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); glutReshapeFunc(reshape); ImGui::CreateContext(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); (void)io; // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls @@ -1286,7 +1309,7 @@ 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(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -1301,8 +1324,7 @@ void mouse(int glut_button, int state, int x, int y) io.MouseDown[button] = false; static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && - glutMajorVersion < 3) + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) { wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); } @@ -1359,7 +1381,7 @@ void wheel(int button, int dir, int x, int y) return; } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { // params.decimation = 0.03; @@ -1382,8 +1404,7 @@ void calibrate_intrinsics() { bool multithread = false; - std::cout - << "calibrate_intrinsics" << std::endl; + std::cout << "calibrate_intrinsics" << std::endl; // step 1 build rgd std::cout << "build rgd" << std::endl; @@ -1410,7 +1431,7 @@ void calibrate_intrinsics() NDTBucketMapType buckets; - update_rgd(rgd_params, buckets, point_cloud_global, {0, 0, 0}); + update_rgd(rgd_params, buckets, point_cloud_global, { 0, 0, 0 }); std::cout << "buckets.size(): " << buckets.size() << std::endl; @@ -1428,7 +1449,7 @@ void calibrate_intrinsics() // std::vector mutexes(intrinsics.size()); // std::cout << "jojo" << std::endl; - const auto hessian_fun = [&](const Point3Di &intermediate_points_i) + const auto hessian_fun = [&](const Point3Di& intermediate_points_i) { int ir = tripletListB.size(); double delta_x; @@ -1447,20 +1468,32 @@ void calibrate_intrinsics() { return; } - auto &this_bucket = bucket_it->second; + auto& this_bucket = bucket_it->second; Eigen::Vector3d mean(this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); Eigen::Matrix jacobian; TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, point_local.x(), point_local.y(), point_local.z()); int c = intermediate_points_i.index_pose * 6; for (int row = 0; row < 3; row++) @@ -1545,19 +1578,20 @@ void calibrate_intrinsics() 0); Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); int ir = tripletListB.size(); @@ -1627,19 +1661,20 @@ void calibrate_intrinsics() 0); Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges_to_0[i].first].px, - poses[odo_edges_to_0[i].first].py, - poses[odo_edges_to_0[i].first].pz, - poses[odo_edges_to_0[i].first].om, - poses[odo_edges_to_0[i].first].fi, - poses[odo_edges_to_0[i].first].ka, - poses[odo_edges_to_0[i].second].px, - poses[odo_edges_to_0[i].second].py, - poses[odo_edges_to_0[i].second].pz, - poses[odo_edges_to_0[i].second].om, - poses[odo_edges_to_0[i].second].fi, - poses[odo_edges_to_0[i].second].ka); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges_to_0[i].first].px, + poses[odo_edges_to_0[i].first].py, + poses[odo_edges_to_0[i].first].pz, + poses[odo_edges_to_0[i].first].om, + poses[odo_edges_to_0[i].first].fi, + poses[odo_edges_to_0[i].first].ka, + poses[odo_edges_to_0[i].second].px, + poses[odo_edges_to_0[i].second].py, + poses[odo_edges_to_0[i].second].pz, + poses[odo_edges_to_0[i].second].om, + poses[odo_edges_to_0[i].second].fi, + poses[odo_edges_to_0[i].second].ka); int ir = tripletListB.size(); @@ -1730,8 +1765,7 @@ void calibrate_intrinsics() // AtPA_I *= 1; // AtPA += AtPA_I; - Eigen::SimplicialCholesky> - solver(AtPA); + Eigen::SimplicialCholesky> solver(AtPA); std::cout << "start solving" << std::endl; Eigen::SparseMatrix x = solver.solve(AtPB); std::cout << "start finished" << std::endl; diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index 3b31c4cf..2e027255 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -24,19 +24,19 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( mandeye_mission_recorder_calibration 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 47fd8f23..f068dab3 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -1,12 +1,14 @@ +#include + #include #include #include -#include #include +#include + //#define GLEW_STATIC //#include -#include #include #include @@ -18,8 +20,8 @@ #include "pfd_wrapper.hpp" -#include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include #include @@ -28,22 +30,21 @@ #include #ifdef _WIN32 - #include - #include // <-- Required for ShellExecuteA - #include "resource.h" +#include "resource.h" +#include // <-- Required for ShellExecuteA +#include + #endif /////////////////////////////////////////////////////////////////////////////////// std::string winTitle = std::string("MR calibration ") + HDMAPPING_VERSION_STRING; -std::vector infoLines = { - "This program is optional step in MANDEYE process", - "", - "Used for MR (Mission Recorder) hardware using two LiDAR units" -}; +std::vector infoLines = { "This program is optional step in MANDEYE process", + "", + "Used for MR (Mission Recorder) hardware using two LiDAR units" }; -//App specific shortcuts (using empty dummy until needed) +// App specific shortcuts (using empty dummy until needed) std::vector appShortcuts(80, { "", "", "" }); #define SAMPLE_PERIOD (1.0 / 200.0) @@ -52,15 +53,9 @@ namespace fs = std::filesystem; ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.00f); ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.00f); -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; std::vector laz_files; std::vector csv_files; @@ -107,7 +102,8 @@ bool manual_calibration = false; /////////////////////////////////////////////////////////////////////////////////// -void load_pc(const std::string &lazFile, std::vector& points, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy) +void load_pc( + const std::string& lazFile, std::vector& points, bool ommit_points_with_timestamp_equals_zero, double filter_threshold_xy) { laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) @@ -123,7 +119,7 @@ void load_pc(const std::string &lazFile, std::vector& points, bool omm std::abort(); } std::cout << "Compressed : " << is_compressed << std::endl; - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -131,7 +127,7 @@ void load_pc(const std::string &lazFile, std::vector& points, bool omm std::abort(); } fprintf(stderr, "File '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); @@ -145,7 +141,6 @@ void load_pc(const std::string &lazFile, std::vector& points, bool omm for (laszip_U32 j = 0; j < header->number_of_point_records; j++) { - if (laszip_read_point(laszip_reader)) { fprintf(stderr, "DLL ERROR: reading point %u\n", j); @@ -164,7 +159,10 @@ void load_pc(const std::string &lazFile, std::vector& points, bool omm // } // Eigen::Affine3d calibration = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(id); - const Eigen::Vector3d pf(header->x_offset + header->x_scale_factor * static_cast(point->X), header->y_offset + header->y_scale_factor * static_cast(point->Y), header->z_offset + header->z_scale_factor * static_cast(point->Z)); + const Eigen::Vector3d pf( + header->x_offset + header->x_scale_factor * static_cast(point->X), + header->y_offset + header->y_scale_factor * static_cast(point->Y), + header->z_offset + header->z_scale_factor * static_cast(point->Z)); p.point = /*calibration **/ (pf); p.lidarid = id; @@ -250,22 +248,21 @@ void settings_gui() ImGui::Separator(); } - ImGui::BeginDisabled(!(idToSn.size() > 0 && calibrations.size() == 0)); { if (calibrations.size() == 0) { if (ImGui::Button("Create new calibration (optional before step 2)")) { - const auto input_file_name = mandeye::fd::SaveFileDialog("Save calibration file", mandeye::fd::Calibration_filter, ".mjc", "calibration.mjc"); + const auto input_file_name = mandeye::fd::SaveFileDialog( + "Save calibration file", mandeye::fd::Calibration_filter, ".mjc", "calibration.mjc"); if (input_file_name.size() > 0) { std::string l1 = idToSn.at(0); std::string l2 = idToSn.at(1); - std::cout - << "output_file_name: " << input_file_name << std::endl; + std::cout << "output_file_name: " << input_file_name << std::endl; nlohmann::json j; j["calibration"][l1.c_str()]["identity"] = "true"; @@ -373,8 +370,8 @@ void settings_gui() if (calibrated_lidar.size() == 1) { - manual_calibration = true; //if only one LiDAR, force manual calibration - chosen_lidar = 0; + manual_calibration = true; // if only one LiDAR, force manual calibration + chosen_lidar = 0; } } } @@ -514,8 +511,8 @@ void settings_gui() ImGui::SetTooltip(kaText); ImGui::PopItemWidth(); - if (tmp.px != tb_pose.px || tmp.py != tb_pose.py || tmp.pz != tb_pose.pz || - tmp.om != tb_pose.om || tmp.fi != tb_pose.fi || tmp.ka != tb_pose.ka) + if (tmp.px != tb_pose.px || tmp.py != tb_pose.py || tmp.pz != tb_pose.pz || tmp.om != tb_pose.om || + tmp.fi != tb_pose.fi || tmp.ka != tb_pose.ka) { tb_pose.om = tb_pose.om * DEG_TO_RAD; tb_pose.fi = tb_pose.fi * DEG_TO_RAD; @@ -532,7 +529,7 @@ void settings_gui() // calibrations.at(0) = m0; } ImGui::EndDisabled(); - + if (imu_lidar.size() > 1) { const auto hintText = "Choose LiDAR in horizontal orientation!"; @@ -559,7 +556,8 @@ void settings_gui() if (ImGui::Button("Save resulted calibration file (step 5)")) { - const auto new_calibration_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::Calibration_filter, ".mjc", calibration_file_name); + const auto new_calibration_file_name = + mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::Calibration_filter, ".mjc", calibration_file_name); if (new_calibration_file_name.size() > 0) { @@ -662,13 +660,21 @@ void display() } else { - glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100000, 100000); + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); - glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100, 100); + camera_ortho_xy_view_zoom / ratio, + -100, + 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -687,10 +693,9 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); @@ -703,7 +708,7 @@ void display() if (calibration.size() > 0) { - for (const auto &c : calibration) + for (const auto& c : calibration) { Eigen::Affine3d m = c.second; glBegin(GL_LINES); @@ -735,7 +740,7 @@ void display() glPointSize(point_size); glBegin(GL_POINTS); - for (const auto &p : point_cloud) + for (const auto& p : point_cloud) { Eigen::Affine3d cal = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(p.lidarid); @@ -764,7 +769,7 @@ void display() { glPointSize(point_size); glBegin(GL_POINTS); - for (const auto &p : point_cloud) + for (const auto& p : point_cloud) { Eigen::Affine3d cal = calibrations.empty() ? Eigen::Affine3d::Identity() : calibrations.at(p.lidarid); @@ -800,7 +805,7 @@ void display() ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); ShowMainDockSpace(); @@ -811,7 +816,6 @@ void display() if (ImGui::BeginMainMenuBar()) { - if (ImGui::BeginMenu("View")) { ImGui::BeginDisabled(!(point_cloud.size() > 0)); @@ -872,7 +876,9 @@ void display() camMenu(); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -885,7 +891,6 @@ void display() ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); - ImGui::EndMainMenuBar(); } @@ -907,7 +912,7 @@ void display() void mouse(int glut_button, int state, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -940,7 +945,7 @@ void mouse(int glut_button, int state, int x, int y) } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { params.decimation = 0.03; @@ -953,17 +958,14 @@ int main(int argc, char *argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - } - catch (const std::bad_alloc& e) + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index 90674af9..c40e584e 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -26,20 +26,20 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/tomlplusplus/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( mandeye_raw_data_viewer diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index b46cd712..28c85166 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1,11 +1,14 @@ +// clang-format off +#include +#include +// clang-format on + #include #include #include #include #include -#include -#include #include #include @@ -31,41 +34,32 @@ #include #ifdef _WIN32 - #include - #include // <-- Required for ShellExecuteA - #include "resource.h" +#include +#include // <-- Required for ShellExecuteA +#include "resource.h" #endif /////////////////////////////////////////////////////////////////////////////////// std::string winTitle = std::string("Raw data viewer ") + HDMAPPING_VERSION_STRING; -std::vector infoLines = { - "This program is optional step in MANDEYE process", - "", - "It analyzes LiDAR data created by Mission Recorder", - "Next step will be to load data with 'lidar_odometry_step_1' app" -}; +std::vector infoLines = { "This program is optional step in MANDEYE process", + "", + "It analyzes LiDAR data created by Mission Recorder", + "Next step will be to load data with 'lidar_odometry_step_1' app" }; -//App specific shortcuts (using empty dummy until needed) +// App specific shortcuts (using empty dummy until needed) std::vector appShortcuts(80, { "", "", "" }); - #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.00f); ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.00f); -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; struct AllData { @@ -95,8 +89,8 @@ std::vector laz_files; std::vector photos_files; std::map photo_files_ts; -double filter_threshold_xy_inner = 0.0; //no filtering for raw viewing -double filter_threshold_xy_outer = 300.0; //no filtering for raw viewing +double filter_threshold_xy_inner = 0.0; // no filtering for raw viewing +double filter_threshold_xy_outer = 300.0; // no filtering for raw viewing bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; @@ -139,7 +133,7 @@ namespace photos double nearestTs = 0; int photo_width_cam0 = 0; int photo_height_cam0 = 0; -} +} // namespace photos /////////////////////////////////////////////////////////////////////////////////// @@ -165,35 +159,41 @@ static ImVec2 DisplayImageFit(ImTextureID tex, int tex_w, int tex_h, bool allow_ return disp; } -void render_nearest_photo(double ts) { +void render_nearest_photo(double ts) +{ using namespace photos; // find closest photo - if (photo_files_ts.empty()) { + if (photo_files_ts.empty()) + { return; } uint64_t ts_uint64 = static_cast(ts * 1e9); auto it = photo_files_ts.lower_bound(ts_uint64); - if (it == photo_files_ts.end()) { + if (it == photo_files_ts.end()) + { return; } - const std::string &photo_file = it->second; - if (photo_file == imgToShowFn) { + const std::string& photo_file = it->second; + if (photo_file == imgToShowFn) + { return; } std::cout << "render_nearest_photo: " << photo_file << std::endl; cv::Mat img = cv::imread(photo_file); - if (img.empty()) { + if (img.empty()) + { return; } cv::Mat img_rgb; cv::cvtColor(img, img_rgb, cv::COLOR_BGR2RGB); imgToShow = img_rgb; imgToShowFn = photo_file; - photos::nearestTs = double(it->first)/1e9; + photos::nearestTs = double(it->first) / 1e9; // upload to OpenGL texture photo_width_cam0 = imgToShow.cols; photo_height_cam0 = imgToShow.rows; - if (photo_texture_cam0 == 0) { + if (photo_texture_cam0 == 0) + { glGenTextures(1, &photo_texture_cam0); } glBindTexture(GL_TEXTURE_2D, photo_texture_cam0); @@ -206,7 +206,6 @@ void render_nearest_photo(double ts) { void optimize() { - #if 0 rgd_nn.clear(); @@ -591,17 +590,23 @@ void optimize() if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - std::vector tr = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory; - std::vector trmm = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory_motion_model; + std::vector tr = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory; + std::vector trmm = + all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory_motion_model; std::vector points_local_sf; std::vector points_local; for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + { + return lhs.first < rhs; + }); int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; @@ -613,8 +618,12 @@ void optimize() double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) { - double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; - double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; + double polar_angle_deg_l = atan2( + all_data[index_rendered_points_local].points_local[i].point.y(), + all_data[index_rendered_points_local].points_local[i].point.x()) / + M_PI * 180.0; + double azimutal_angle_deg_l = + acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; points_local.push_back(all_data[index_rendered_points_local].points_local[i]); @@ -656,9 +665,14 @@ std::vector> get_nn() // index data for (size_t i = 0; i < worker_data.points_local.size(); i++) { - auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + auto lower = std::lower_bound( + worker_data.timestamps.begin(), + worker_data.timestamps.end(), + worker_data.points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + { + return lhs.first < rhs; + }); int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; @@ -677,8 +691,8 @@ std::vector> get_nn() // rgd_params.resolution_Y = 0.3; // polar angle deg // rgd_params.resolution_Z = 0.3; // azimutal angle deg - rgd_params.resolution_X = distance_bucket; // distance bucket - rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg std::vector point_cloud_global; @@ -692,7 +706,8 @@ std::vector> get_nn() double r_l = worker_data.points_local[i].point.norm(); if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) { - double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double polar_angle_deg_l = + atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; Eigen::Vector3d pp = worker_data.points_local[i].point; @@ -775,9 +790,14 @@ std::vector> get_mean_cov() // index data for (size_t i = 0; i < worker_data.points_local.size(); i++) { - auto lower = std::lower_bound(worker_data.timestamps.begin(), worker_data.timestamps.end(), worker_data.points_local[i].timestamp, + auto lower = std::lower_bound( + worker_data.timestamps.begin(), + worker_data.timestamps.end(), + worker_data.points_local[i].timestamp, [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + { + return lhs.first < rhs; + }); int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; @@ -792,8 +812,8 @@ std::vector> get_mean_cov() // rgd_params.resolution_Y = 0.3; // polar angle deg // rgd_params.resolution_Z = 0.3; // azimutal angle deg - rgd_params.resolution_X = distance_bucket; // distance bucket - rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg std::vector point_cloud_global; @@ -807,7 +827,8 @@ std::vector> get_mean_cov() double r_l = worker_data.points_local[i].point.norm(); if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) { - double polar_angle_deg_l = atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double polar_angle_deg_l = + atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; Eigen::Vector3d pp = worker_data.points_local[i].point; @@ -868,13 +889,13 @@ void loadData() { std::vector input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::All_Filter, true); - //no files selected, quit loading + // no files selected, quit loading if (input_file_names.empty()) - return; + return; - LidarOdometryParams params; //dummy for load_data function - params.save_calibration_validation = false; - params.filter_threshold_xy_inner = filter_threshold_xy_inner; + LidarOdometryParams params; // dummy for load_data function + params.save_calibration_validation = false; + params.filter_threshold_xy_inner = filter_threshold_xy_inner; params.filter_threshold_xy_outer = filter_threshold_xy_outer; std::vector> pointsPerFile; @@ -882,7 +903,7 @@ void loadData() if (load_data(input_file_names, params, pointsPerFile, imu_data, false)) { - //clear possible previous data + // clear possible previous data all_data.clear(); all_data.shrink_to_fit(); @@ -913,7 +934,7 @@ void loadData() laz_files.clear(); - //specific processing for RAW data viewer + // specific processing for RAW data viewer std::sort(input_file_names.begin(), input_file_names.end()); for (const auto& fileName : input_file_names) @@ -923,7 +944,8 @@ void loadData() if (ext == ".laz" || ext == ".las") laz_files.push_back(fileName); - else if (ext == ".jpg" || ext == ".jpeg") { + else if (ext == ".jpg" || ext == ".jpeg") + { photos_files.push_back(fileName); // decode filename e.g.: ` cam0_1761264773592270949.jpg` const std::string filename = fs::path(fileName).stem().string(); @@ -936,18 +958,19 @@ void loadData() if (cam_id == "cam0" && !timestamp.empty()) { - try { + try + { uint64_t ts = std::stoull(timestamp); photo_files_ts[ts] = fileName; - } - catch (const std::exception& e) { + } catch (const std::exception& e) + { std::cerr << "Error parsing timestamp from filename: " << filename << " - " << e.what() << std::endl; } } } } - //rest of RAW data viewer processing + // rest of RAW data viewer processing FusionAhrs ahrs; FusionAhrsInitialise(&ahrs); @@ -972,8 +995,11 @@ void loadData() for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI) }; - // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), static_cast(gyr.axis.z)}; + const FusionVector gyroscope = { static_cast(gyr.axis.x * 180.0 / M_PI), + static_cast(gyr.axis.y * 180.0 / M_PI), + static_cast(gyr.axis.z * 180.0 / M_PI) }; + // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), + // static_cast(gyr.axis.z)}; const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; if (first) @@ -1023,7 +1049,13 @@ void loadData() const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; if (counter % 100 == 0) - printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %zu]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, imu_data.size()); + printf( + "Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %zu]\n", + euler.angle.roll, + euler.angle.pitch, + euler.angle.yaw, + counter++, + imu_data.size()); // log it for implot imu_data_plot.timestampLidar.push_back(timestamp_pair.first); @@ -1078,9 +1110,14 @@ void loadData() std::cout << "Indexed: " << i + 1 << " of " << pointsPerFile.size() << " files\r"; for (const auto& pp : pointsPerFile[i]) { - auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), pp.timestamp, + auto lower = std::lower_bound( + timestamps.begin(), + timestamps.end(), + pp.timestamp, [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + { + return lhs.first < rhs; + }); int index_pose = std::distance(timestamps.begin(), lower) - 1; @@ -1130,7 +1167,8 @@ void loadData() for (size_t i = 0; i < timestamps.size(); i++) { - if (timestamps[i].first >= points_local[0].timestamp && timestamps[i].first <= points_local[points_local.size() - 1].timestamp) + if (timestamps[i].first >= points_local[0].timestamp && + timestamps[i].first <= points_local[points_local.size() - 1].timestamp) { data.timestamps.push_back(timestamps[i]); data.poses.push_back(poses[i]); @@ -1141,11 +1179,12 @@ void loadData() if (data.timestamps.size() > 2) { double ts_begin = data.timestamps[0].first; - double ts_step = (data.timestamps[data.timestamps.size() - 1].first - data.timestamps[0].first) / data.points_local.size(); + double ts_step = + (data.timestamps[data.timestamps.size() - 1].first - data.timestamps[0].first) / data.points_local.size(); - //std::cout << "ts_begin " << ts_begin << std::endl; - //std::cout << "ts_step " << ts_step << std::endl; - //std::cout << "ts_end " << data.timestamps[data.timestamps.size() - 1].first << std::endl; + // std::cout << "ts_begin " << ts_begin << std::endl; + // std::cout << "ts_step " << ts_step << std::endl; + // std::cout << "ts_end " << data.timestamps[data.timestamps.size() - 1].first << std::endl; for (size_t pp = 0; pp < data.points_local.size(); pp++) data.points_local[pp].timestamp = ts_begin + pp * ts_step; @@ -1188,9 +1227,12 @@ void imu_data_gui() { ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - ImPlot::PlotLine("accX", imu_data_plot.timestampLidar.data(), imu_data_plot.accX.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("accY", imu_data_plot.timestampLidar.data(), imu_data_plot.accY.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("accZ", imu_data_plot.timestampLidar.data(), imu_data_plot.accZ.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "accX", imu_data_plot.timestampLidar.data(), imu_data_plot.accX.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "accY", imu_data_plot.timestampLidar.data(), imu_data_plot.accY.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "accZ", imu_data_plot.timestampLidar.data(), imu_data_plot.accZ.data(), (int)imu_data_plot.timestampLidar.size()); ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); ImPlot::EndPlot(); } @@ -1199,9 +1241,12 @@ void imu_data_gui() { ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - ImPlot::PlotLine("angX", imu_data_plot.timestampLidar.data(), imu_data_plot.angX.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("angY", imu_data_plot.timestampLidar.data(), imu_data_plot.angY.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("angZ", imu_data_plot.timestampLidar.data(), imu_data_plot.angZ.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "angX", imu_data_plot.timestampLidar.data(), imu_data_plot.angX.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "angY", imu_data_plot.timestampLidar.data(), imu_data_plot.angY.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "angZ", imu_data_plot.timestampLidar.data(), imu_data_plot.angZ.data(), (int)imu_data_plot.timestampLidar.size()); ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); ImPlot::EndPlot(); } @@ -1210,9 +1255,12 @@ void imu_data_gui() { ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); - ImPlot::PlotLine("yaw", imu_data_plot.timestampLidar.data(), imu_data_plot.yaw.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("pitch", imu_data_plot.timestampLidar.data(), imu_data_plot.pitch.data(), (int)imu_data_plot.timestampLidar.size()); - ImPlot::PlotLine("roll", imu_data_plot.timestampLidar.data(), imu_data_plot.roll.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "yaw", imu_data_plot.timestampLidar.data(), imu_data_plot.yaw.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "pitch", imu_data_plot.timestampLidar.data(), imu_data_plot.pitch.data(), (int)imu_data_plot.timestampLidar.size()); + ImPlot::PlotLine( + "roll", imu_data_plot.timestampLidar.data(), imu_data_plot.roll.data(), (int)imu_data_plot.timestampLidar.size()); ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); ImPlot::EndPlot(); } @@ -1246,16 +1294,10 @@ void imu_data_gui() file << std::setprecision(20); for (size_t i = 0; i < imu_data_plot.timestampLidar.size(); i++) { - file << imu_data_plot.timestampLidar[i] << " " << - imu_data_plot.angX[i] << " " << - imu_data_plot.angY[i] << " " << - imu_data_plot.angZ[i] << " " << - imu_data_plot.accX[i] << " " << - imu_data_plot.accY[i] << " " << - imu_data_plot.accZ[i] << " " << - imu_data_plot.yaw[i] << " " << - imu_data_plot.pitch[i] << " " << - imu_data_plot.roll[i] << std::endl; + file << imu_data_plot.timestampLidar[i] << " " << imu_data_plot.angX[i] << " " << imu_data_plot.angY[i] << " " + << imu_data_plot.angZ[i] << " " << imu_data_plot.accX[i] << " " << imu_data_plot.accY[i] << " " + << imu_data_plot.accZ[i] << " " << imu_data_plot.yaw[i] << " " << imu_data_plot.pitch[i] << " " + << imu_data_plot.roll[i] << std::endl; } file.close(); @@ -1306,12 +1348,12 @@ void settings_gui() } } - ImGui::BeginDisabled(!is_init); + ImGui::BeginDisabled(!is_init); if (ImGui::Button("Save point cloud")) { std::string output_file_name = ""; output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter); - + if (output_file_name.size() > 0) { std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; @@ -1324,7 +1366,8 @@ void settings_gui() { for (size_t i = 0; i < all_points_local[index_rendered_points_local].size(); i++) { - pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), + all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); timestamps.push_back(all_points_local[index_rendered_points_local][i].timestamp); } @@ -1334,7 +1377,7 @@ void settings_gui() std::cout << "problem with saving file: " << output_file_name << std::endl; } } - ImGui::EndDisabled(); + ImGui::EndDisabled(); ImGui::Separator(); @@ -1368,20 +1411,33 @@ void settings_gui() { for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) { - std::cout << index_pose << " total nr of poses: [" << all_data[index_rendered_points_local].poses.size() << "] " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << " ts point: " << all_data[index_rendered_points_local].points_local[i].timestamp << " pose ts: " << all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; - - if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + std::cout << index_pose << " total nr of poses: [" << all_data[index_rendered_points_local].poses.size() << "] " + << all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first + << " ts point: " << all_data[index_rendered_points_local].points_local[i].timestamp + << " pose ts: " << all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; + + if (fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) { // std::cout << all_data[index_rendered_points_local].points_local[i].timestamp << std::endl; - max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); + max_diff = fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); } } } @@ -1432,13 +1488,21 @@ void display() } else { - glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100000, 100000); + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); - glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100, 100); + camera_ortho_xy_view_zoom / ratio, + -100, + 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -1457,10 +1521,9 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); @@ -1484,7 +1547,8 @@ void display() { glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); } - glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), + all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); } } glEnd(); @@ -1494,29 +1558,43 @@ void display() glBegin(GL_POINTS); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { - double max_diff = 0.0; for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) { - if (fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + if (fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) { - // std::cout << index_pose << " " << all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; - max_diff = fabs(all_data[index_rendered_points_local].points_local[i].timestamp - all_data[index_rendered_points_local].timestamps[index_pose].first); + // std::cout << index_pose << " " << all_data[index_rendered_points_local].points_local[i].timestamp - + // all_data[index_rendered_points_local].timestamps[index_pose].first << std::endl; + max_diff = fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); } } } for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; @@ -1526,7 +1604,8 @@ void display() { if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) { - // if (fabs(all_data[index_rendered_points_local].timestamps[index_pose].first - all_data[index_rendered_points_local].points_local[i].timestamp) < 0.001) + // if (fabs(all_data[index_rendered_points_local].timestamps[index_pose].first - + // all_data[index_rendered_points_local].points_local[i].timestamp) < 0.001) //{ Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; @@ -1549,9 +1628,14 @@ void display() { for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { - auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, - [](std::pair lhs, double rhs) -> bool - { return lhs.first < rhs; }); + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; @@ -1595,7 +1679,7 @@ void display() { glColor3f(0, 0, 0); glBegin(GL_LINES); - for (const auto &nn : rgd_nn) + for (const auto& nn : rgd_nn) { glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); @@ -1605,7 +1689,7 @@ void display() if (show_mean_cov) { - for (const auto &mc : mean_cov) + for (const auto& mc : mean_cov) draw_ellipse(mc.second, mc.first, Eigen::Vector3f(1, 0, 0), 1); } @@ -1613,7 +1697,7 @@ void display() ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); ShowMainDockSpace(); @@ -1621,15 +1705,11 @@ void display() if (all_data.size() > 0) { - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) - || ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true) - ) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) || + ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true)) index_rendered_points_local += 1; - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - || ImGui::IsKeyPressed(ImGuiKey_PageDown, true) - || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true) - ) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) || + ImGui::IsKeyPressed(ImGuiKey_PageDown, true) || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true)) index_rendered_points_local -= 1; if (index_rendered_points_local < 0) @@ -1640,7 +1720,7 @@ void display() if (ImGui::BeginMainMenuBar()) { - //if (!is_init) + // if (!is_init) { if (ImGui::Button("Load data")) loadData(); @@ -1686,7 +1766,6 @@ void display() ImGui::Separator(); - if (ImGui::MenuItem("Show rgd_nn", nullptr, &show_rgd_nn)) { if (show_rgd_nn) @@ -1779,7 +1858,9 @@ void display() ImGui::SameLine(); ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -1802,7 +1883,8 @@ void display() if (photos::photo_texture_cam0) { - if (ImGui::Begin("CAM0")) { + if (ImGui::Begin("CAM0")) + { using namespace photos; ImGui::Text("fn name: %s ", photos::imgToShowFn.c_str()); ImGui::Text("timestamp: %s", std::to_string(photos::nearestTs).c_str()); @@ -1816,7 +1898,7 @@ void display() if (show_imu_data) imu_data_gui(); - if (is_settings_gui) + if (is_settings_gui) settings_gui(); ImGui::Render(); @@ -1828,7 +1910,7 @@ void display() void mouse(int glut_button, int state, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -1843,8 +1925,7 @@ void mouse(int glut_button, int state, int x, int y) io.MouseDown[button] = false; static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && - glutMajorVersion < 3) + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) { wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); } @@ -1879,18 +1960,15 @@ int main(int argc, char* argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - ImPlot::DestroyContext(); - } - catch (const std::bad_alloc& e) + ImPlot::DestroyContext(); + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/mandeye_single_session_viewer/CMakeLists.txt b/apps/mandeye_single_session_viewer/CMakeLists.txt index edecaa8d..1bfd69a5 100644 --- a/apps/mandeye_single_session_viewer/CMakeLists.txt +++ b/apps/mandeye_single_session_viewer/CMakeLists.txt @@ -23,19 +23,19 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( mandeye_single_session_viewer 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 3f6513fc..2d803351 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -1,13 +1,15 @@ -#include +// clang-format off +#include +#include +// clang-format on + +#include #include #include -#include #include -//#define GLEW_STATIC -#include +#include -#include #include #include @@ -27,9 +29,9 @@ #include #ifdef _WIN32 - #include - #include // <-- Required for ShellExecuteA - #include "resource.h" +#include +#include // <-- Required for ShellExecuteA +#include "resource.h" #endif #include @@ -37,96 +39,92 @@ std::string winTitle = std::string("Single session viewer ") + HDMAPPING_VERSION_STRING; -std::vector infoLines = { - "This program is optional step in MANDEYE process", - "", - "It analyzes session created in step_1 for problems that need to be addressed further", - "Next step will be to load session file with 'multi_view_tls_registration_step_2' app" -}; - -//App specific shortcuts (Type and Shortcut are just for easy reference) -static const std::vector appShortcuts = { - {"Normal keys", "A", ""}, - {"", "Ctrl+A", ""}, - {"", "B", ""}, - {"", "Ctrl+B", ""}, - {"", "C", ""}, - {"", "Ctrl+C", ""}, - {"", "D", ""}, - {"", "Ctrl+D", ""}, - {"", "E", ""}, - {"", "Ctrl+E", ""}, - {"", "F", ""}, - {"", "Ctrl+F", ""}, - {"", "G", ""}, - {"", "Ctrl+G", ""}, - {"", "H", ""}, - {"", "Ctrl+H", ""}, - {"", "I", ""}, - {"", "Ctrl+I", ""}, - {"", "J", ""}, - {"", "Ctrl+K", ""}, - {"", "K", ""}, - {"", "Ctrl+K", ""}, - {"", "L", ""}, - {"", "Ctrl+L", ""}, - {"", "M", ""}, - {"", "Ctrl+M", ""}, - {"", "N", ""}, - {"", "Ctrl+N", "show Neightbouring scans"}, - {"", "O", ""}, - {"", "Ctrl+O", "Open session"}, - {"", "P", ""}, - {"", "Ctrl+P", "Properties"}, - {"", "Q", ""}, - {"", "Ctrl+Q", ""}, - {"", "R", ""}, - {"", "Ctrl+R", ""}, - {"", "Shift+R", ""}, - {"", "S", ""}, - {"", "Ctrl+S", ""}, - {"", "Ctrl+Shift+S", ""}, - {"", "T", ""}, - {"", "Ctrl+T", ""}, - {"", "U", ""}, - {"", "Ctrl+U", ""}, - {"", "V", ""}, - {"", "Ctrl+V", ""}, - {"", "W", ""}, - {"", "Ctrl+W", ""}, - {"", "X", ""}, - {"", "Ctrl+X", ""}, - {"", "Y", ""}, - {"", "Ctrl+Y", ""}, - {"", "Z", ""}, - {"", "Ctrl+Z", ""}, - {"", "Shift+Z", ""}, - {"", "1-9", ""}, - {"Special keys", "Up arrow", "Intensity offset +"}, - {"", "Shift + up arrow", ""}, - {"", "Ctrl + up arrow", ""}, - {"", "Down arrow", "Intensity offset -"}, - {"", "Shift + down arrow", ""}, - {"", "Ctrl + down arrow", ""}, - {"", "Left arrow", "Point cloud index -"}, - {"", "Shift + left arrow", ""}, - {"", "Ctrl + left arrow", ""}, - {"", "Right arrow", "Point cloud index +"}, - {"", "Shift + right arrow", ""}, - {"", "Ctrl + right arrow", ""}, - {"", "Pg down", "Point cloud index -"}, - {"", "Pg up", "Point cloud index +"}, - {"", "- key", "Point cloud index -"}, - {"", "+ key", "Point cloud index +"}, - {"Mouse related", "Left click + drag", ""}, - {"", "Right click + drag", "n"}, - {"", "Scroll", ""}, - {"", "Shift + scroll", ""}, - {"", "Shift + drag", ""}, - {"", "Ctrl + left click", ""}, - {"", "Ctrl + right click", ""}, - {"", "Ctrl + middle click", ""} -}; +std::vector infoLines = { "This program is optional step in MANDEYE process", + "", + "It analyzes session created in step_1 for problems that need to be addressed further", + "Next step will be to load session file with 'multi_view_tls_registration_step_2' app" }; + +// App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "show Neightbouring scans" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open session" }, + { "", "P", "" }, + { "", "Ctrl+P", "Properties" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "Intensity offset +" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "Intensity offset -" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "Point cloud index -" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "Point cloud index +" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "Point cloud index -" }, + { "", "Pg up", "Point cloud index +" }, + { "", "- key", "Point cloud index -" }, + { "", "+ key", "Point cloud index +" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; @@ -134,21 +132,15 @@ namespace fs = std::filesystem; ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.0f); -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; int index_rendered_points_local = -1; float offset_intensity = 0.0; bool show_neighbouring_scans = false; -int colorScheme = 0; //0=solid color; gradients:1=intensity, 2=cloud height +int colorScheme = 0; // 0=solid color; gradients:1=intensity, 2=cloud height int oldcolorScheme = 0; bool usePose = false; @@ -162,7 +154,7 @@ bool session_loaded = false; int session_total_number_of_points = 0; PointClouds::PointCloudDimensions session_dims; -//built in console output redirection to imgui window +// built in console output redirection to imgui window /////////////////////////////////////////////////////////////////////////////////// #ifdef _WIN32 @@ -171,10 +163,14 @@ bool consWin = true; bool consImGui = false; bool consHooked = false; -class DualStreamBuf : public std::streambuf { +class DualStreamBuf : public std::streambuf +{ public: DualStreamBuf(std::streambuf* sb1, std::string* imguiBuffer, bool isCerr) - : consoleBuf(sb1), imguiBuffer(imguiBuffer), isCerr(isCerr) { + : consoleBuf(sb1) + , imguiBuffer(imguiBuffer) + , isCerr(isCerr) + { } protected: @@ -183,7 +179,7 @@ class DualStreamBuf : public std::streambuf { int overflow(int c) override { - //handle EOF + // handle EOF if (c == traits_type::eof()) return traits_type::not_eof(c); @@ -225,11 +221,7 @@ class DualStreamBuf : public std::streambuf { std::string* imguiBuffer; }; - - - - -static std::string g_ImGuiLog; // store text for ImGui +static std::string g_ImGuiLog; // store text for ImGui static DualStreamBuf* g_coutBuf = nullptr; static DualStreamBuf* g_cerrBuf = nullptr; static std::streambuf* g_origCoutBuf = nullptr; @@ -237,22 +229,24 @@ static std::streambuf* g_origCerrBuf = nullptr; void ConsoleHook() { - g_origCoutBuf = std::cout.rdbuf(); // save original buffer - g_origCerrBuf = std::cerr.rdbuf(); // save original buffer + g_origCoutBuf = std::cout.rdbuf(); // save original buffer + g_origCerrBuf = std::cerr.rdbuf(); // save original buffer g_coutBuf = new DualStreamBuf(g_origCoutBuf, &g_ImGuiLog, false); g_cerrBuf = new DualStreamBuf(g_origCerrBuf, &g_ImGuiLog, true); - std::cout.rdbuf(g_coutBuf); // replace - std::cerr.rdbuf(g_cerrBuf); // replace - - consHooked = true; + std::cout.rdbuf(g_coutBuf); // replace + std::cerr.rdbuf(g_cerrBuf); // replace + + consHooked = true; } void ConsoleUnhook() { - if (g_origCoutBuf) std::cout.rdbuf(g_origCoutBuf); //restore original buffer - if (g_origCerrBuf) std::cerr.rdbuf(g_origCerrBuf); //restore original buffer - delete g_coutBuf; //delete custom buffers - delete g_cerrBuf; //delete custom buffers + if (g_origCoutBuf) + std::cout.rdbuf(g_origCoutBuf); // restore original buffer + if (g_origCerrBuf) + std::cerr.rdbuf(g_origCerrBuf); // restore original buffer + delete g_coutBuf; // delete custom buffers + delete g_cerrBuf; // delete custom buffers g_coutBuf = g_cerrBuf = nullptr; g_ImGuiLog.clear(); @@ -265,11 +259,11 @@ void ImGuiConsole(bool* p_open) { if (ImGui::Begin("Console", p_open)) { - if (ImGui::Button("Clear")) - g_ImGuiLog.clear(); - //ImGui::SameLine(); - //if (ImGui::Button("Copy")) - // ImGui::LogToClipboard(); + if (ImGui::Button("Clear")) + g_ImGuiLog.clear(); + // ImGui::SameLine(); + // if (ImGui::Button("Copy")) + // ImGui::LogToClipboard(); ImGui::Separator(); @@ -287,14 +281,14 @@ void ImGuiConsole(bool* p_open) ImGui::PushStyleColor(ImGuiCol_FrameBgActive, ImVec4(0, 0, 0, 0)); ImGui::InputTextMultiline( "##console", - g_ImGuiLogBuf, sizeof(g_ImGuiLogBuf), + g_ImGuiLogBuf, + sizeof(g_ImGuiLogBuf), ImVec2(-1.0f, -1.0f), - ImGuiInputTextFlags_ReadOnly | ImGuiInputTextFlags_AllowTabInput - ); + ImGuiInputTextFlags_ReadOnly | ImGuiInputTextFlags_AllowTabInput); ImGui::PopStyleColor(3); // auto-scroll - //if (ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) + // if (ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) // ImGui::SetScrollHereY(1.0f); } ImGui::EndChild(); @@ -303,17 +297,15 @@ void ImGuiConsole(bool* p_open) ImGui::End(); } - - -//VBO/VAO/SSBO proof of concept implementation through glew. openGL 4.6 required +// VBO/VAO/SSBO proof of concept implementation through glew. openGL 4.6 required /////////////////////////////////////////////////////////////////////////////////// // - //in order to test have to be enabled from View menu before loading session so buffers are created - //should be tested with smaller sessions since session is "doubled" with gl_Points vector - bool gl_useVBOs = false; +// in order to test have to be enabled from View menu before loading session so buffers are created +// should be tested with smaller sessions since session is "doubled" with gl_Points vector +bool gl_useVBOs = false; - // vertex shader - const char* pointsVertSource = R"glsl( +// vertex shader +const char* pointsVertSource = R"glsl( #version 460 core //SSBOs @@ -360,8 +352,8 @@ void ImGuiConsole(bool* p_open) } )glsl"; - // fragment shader - const char* pointsFragSource = R"glsl( +// fragment shader +const char* pointsFragSource = R"glsl( #version 460 core in float vIntensity; @@ -387,311 +379,293 @@ void ImGuiConsole(bool* p_open) } )glsl"; - /////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////// - struct gl_point { - Eigen::Vector3f pos; - float intensity; - }; +struct gl_point +{ + Eigen::Vector3f pos; + float intensity; +}; - std::vector gl_Points; - /*= { - {{0.0f, 0.0f, 1.0f}, 1.0f}, - {{0.5f, 0.0f, 1.0f}, 0.7f}, - {{0.0f, 0.5f, 1.0f}, 0.5f}, - {{0.0f, 0.0f, 2.0f}, 0.2f}, - };*/ - - struct gl_cloud { - GLint offset; - GLsizei count; - bool visible; // show/hide - }; +std::vector gl_Points; +/*= { + {{0.0f, 0.0f, 1.0f}, 1.0f}, + {{0.5f, 0.0f, 1.0f}, 0.7f}, + {{0.0f, 0.5f, 1.0f}, 0.5f}, + {{0.0f, 0.0f, 2.0f}, 0.2f}, +};*/ - // Add to clouds vector - std::vector gl_clouds; - /*= { { - 0, //offset - static_cast(gl_Points.size()), //count - true, //visible - Eigen::Matrix4f::Identity(), //pose - 1, //colorScheme - Eigen::Vector3f(1.0f, 0.0f, 0.0f) //color = red, for example - } };*/ - - // CPU-side vectors for SSBO data - struct gl_cloudSSBO { - int colorScheme; // 4 bytes - float padding0[3]; // pad to vec4 alignment (std430 requires vec3 as 16 bytes) - float fixedColor[3]; // 12 bytes for vec3 - float padding1; // pad to 16 bytes - float pose[16]; // mat4 = 16 floats (16*4 = 64 bytes) - }; +struct gl_cloud +{ + GLint offset; + GLsizei count; + bool visible; // show/hide +}; - std::vector gl_cloudIndexSSBO; // size = gl_Points.size() - std::vector gl_cloudsSSBO; // Cloud is your struct from GLSL +// Add to clouds vector +std::vector gl_clouds; +/*= { { + 0, //offset + static_cast(gl_Points.size()), //count + true, //visible + Eigen::Matrix4f::Identity(), //pose + 1, //colorScheme + Eigen::Vector3f(1.0f, 0.0f, 0.0f) //color = red, for example +} };*/ + +// CPU-side vectors for SSBO data +struct gl_cloudSSBO +{ + int colorScheme; // 4 bytes + float padding0[3]; // pad to vec4 alignment (std430 requires vec3 as 16 bytes) + float fixedColor[3]; // 12 bytes for vec3 + float padding1; // pad to 16 bytes + float pose[16]; // mat4 = 16 floats (16*4 = 64 bytes) +}; - Eigen::Matrix4f gl_mvp; //Model-View-Projection matrix +std::vector gl_cloudIndexSSBO; // size = gl_Points.size() +std::vector gl_cloudsSSBO; // Cloud is your struct from GLSL - // Uniform locations - GLuint VAO, VBO = 0; +Eigen::Matrix4f gl_mvp; // Model-View-Projection matrix - GLuint gl_uPointSize = 0; - GLuint gl_uMVP = 0; - GLuint gl_shaderProgram = 0; - GLuint gl_uIntensityScale = 0; - GLuint gl_uUsePose = 0; +// Uniform locations +GLuint VAO, VBO = 0; - GLuint gl_ssboCloudIndex, gl_ssboClouds = 0; +GLuint gl_uPointSize = 0; +GLuint gl_uMVP = 0; +GLuint gl_shaderProgram = 0; +GLuint gl_uIntensityScale = 0; +GLuint gl_uUsePose = 0; - //gl_*** functions related to glew/openGL VBO/VAO - GLuint gl_compileShader(GLenum type, const char* source) - { - GLuint shader = GL_CALL_RET(glCreateShader(type)); - GL_CALL(glShaderSource(shader, 1, &source, nullptr)); - GL_CALL(glCompileShader(shader)); - - // check compilation - GLint success; - GL_CALL(glGetShaderiv(shader, GL_COMPILE_STATUS, &success)); - if (!success) - { - char infoLog[512]; - GL_CALL(glGetShaderInfoLog(shader, 512, nullptr, infoLog)); - std::cerr << "openGL shader compilation failed: " << infoLog << std::endl; - } - return shader; - } +GLuint gl_ssboCloudIndex, gl_ssboClouds = 0; - GLuint gl_createShaderProgram(const char* vertSource, const char* fragSource) +// gl_*** functions related to glew/openGL VBO/VAO +GLuint gl_compileShader(GLenum type, const char* source) +{ + GLuint shader = GL_CALL_RET(glCreateShader(type)); + GL_CALL(glShaderSource(shader, 1, &source, nullptr)); + GL_CALL(glCompileShader(shader)); + + // check compilation + GLint success; + GL_CALL(glGetShaderiv(shader, GL_COMPILE_STATUS, &success)); + if (!success) { - GLuint vertexShader = gl_compileShader(GL_VERTEX_SHADER, vertSource); - GLuint fragmentShader = gl_compileShader(GL_FRAGMENT_SHADER, fragSource); - - GLuint program = GL_CALL_RET(glCreateProgram()); - GL_CALL(glAttachShader(program, vertexShader)); - GL_CALL(glAttachShader(program, fragmentShader)); - GL_CALL(glLinkProgram(program)); - - // check linking - GLint success; - GL_CALL(glGetProgramiv(program, GL_LINK_STATUS, &success)); - if (!success) - { - char infoLog[512]; - GL_CALL(glGetProgramInfoLog(program, 512, nullptr, infoLog)); - std::cerr << "openGL program linking failed: " << infoLog << std::endl; - } - - GL_CALL(glDeleteShader(vertexShader)); - GL_CALL(glDeleteShader(fragmentShader)); - - return program; + char infoLog[512]; + GL_CALL(glGetShaderInfoLog(shader, 512, nullptr, infoLog)); + std::cerr << "openGL shader compilation failed: " << infoLog << std::endl; } + return shader; +} - void gl_loadPointCloudBuffer(const std::vector& points, GLuint& VAO, GLuint& VBO) +GLuint gl_createShaderProgram(const char* vertSource, const char* fragSource) +{ + GLuint vertexShader = gl_compileShader(GL_VERTEX_SHADER, vertSource); + GLuint fragmentShader = gl_compileShader(GL_FRAGMENT_SHADER, fragSource); + + GLuint program = GL_CALL_RET(glCreateProgram()); + GL_CALL(glAttachShader(program, vertexShader)); + GL_CALL(glAttachShader(program, fragmentShader)); + GL_CALL(glLinkProgram(program)); + + // check linking + GLint success; + GL_CALL(glGetProgramiv(program, GL_LINK_STATUS, &success)); + if (!success) { - // Create VAO + VBO - GL_CALL(glGenVertexArrays(1, &VAO)); - GL_CALL(glBindVertexArray(VAO)); - - GL_CALL(glGenBuffers(1, &VBO)); - GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, VBO)); - GL_CALL(glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(gl_point), points.data(), GL_STATIC_DRAW)); - - // --- Attribute 0: position --- - // layout(location = 0) in vec3 aPos; - GL_CALL(glEnableVertexAttribArray(0)); - GL_CALL(glVertexAttribPointer( - 0, // attribute index - 3, // vec3 - GL_FLOAT, - GL_FALSE, - sizeof(gl_point), // stride = full struct - (void*)offsetof(gl_point, pos) // offset of position field - )); - - // --- Attribute 1: intensity --- - // layout(location = 1) in float aIntensity; - GL_CALL(glEnableVertexAttribArray(1)); - GL_CALL(glVertexAttribPointer( - 1, - 1, - GL_FLOAT, - GL_FALSE, - sizeof(gl_point), - (void*)offsetof(gl_point, intensity) - )); - - // Unbind - GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, 0)); - GL_CALL(glBindVertexArray(0)); + char infoLog[512]; + GL_CALL(glGetProgramInfoLog(program, 512, nullptr, infoLog)); + std::cerr << "openGL program linking failed: " << infoLog << std::endl; } - void gl_updateSSBOs() { - // Vertex -> Cloud index - if (gl_ssboCloudIndex == 0) - { - GL_CALL(glGenBuffers(1, &gl_ssboCloudIndex)); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); - GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, - gl_cloudIndexSSBO.size() * sizeof(int), - gl_cloudIndexSSBO.data(), - GL_DYNAMIC_DRAW)); - } - else - { - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); - GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, - 0, - gl_cloudIndexSSBO.size() * sizeof(int), - gl_cloudIndexSSBO.data())); - } + GL_CALL(glDeleteShader(vertexShader)); + GL_CALL(glDeleteShader(fragmentShader)); - GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, gl_ssboCloudIndex)); + return program; +} - // Cloud attributes - if (gl_ssboClouds == 0) - { - GL_CALL(glGenBuffers(1, &gl_ssboClouds)); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); - GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, - gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), - gl_cloudsSSBO.data(), - GL_DYNAMIC_DRAW)); - } - else - { - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); - GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, - 0, - gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), - gl_cloudsSSBO.data())); - } +void gl_loadPointCloudBuffer(const std::vector& points, GLuint& VAO, GLuint& VBO) +{ + // Create VAO + VBO + GL_CALL(glGenVertexArrays(1, &VAO)); + GL_CALL(glBindVertexArray(VAO)); + + GL_CALL(glGenBuffers(1, &VBO)); + GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, VBO)); + GL_CALL(glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(gl_point), points.data(), GL_STATIC_DRAW)); + + // --- Attribute 0: position --- + // layout(location = 0) in vec3 aPos; + GL_CALL(glEnableVertexAttribArray(0)); + GL_CALL(glVertexAttribPointer( + 0, // attribute index + 3, // vec3 + GL_FLOAT, + GL_FALSE, + sizeof(gl_point), // stride = full struct + (void*)offsetof(gl_point, pos) // offset of position field + )); - GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 1, gl_ssboClouds)); + // --- Attribute 1: intensity --- + // layout(location = 1) in float aIntensity; + GL_CALL(glEnableVertexAttribArray(1)); + GL_CALL(glVertexAttribPointer(1, 1, GL_FLOAT, GL_FALSE, sizeof(gl_point), (void*)offsetof(gl_point, intensity))); - GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, 0)); - } + // Unbind + GL_CALL(glBindBuffer(GL_ARRAY_BUFFER, 0)); + GL_CALL(glBindVertexArray(0)); +} - void gl_updateUserView() +void gl_updateSSBOs() +{ + // Vertex -> Cloud index + if (gl_ssboCloudIndex == 0) { - ImGuiIO& io = ImGui::GetIO(); - - Eigen::Affine3f viewLocal1 = Eigen::Affine3f::Identity(); - viewLocal1.translate(rotation_center); - - viewLocal1.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - viewLocal1.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal1.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - viewLocal1.translate(-rotation_center); - - //Get the projection matrix from your reshape() or manually rebuild it - float aspect = float(io.DisplaySize.x) / float(io.DisplaySize.y); - float fov = 60.0f * DEG_TO_RAD; - float nearf = 0.001f; //closest distance to camera objects are rendered [m] - float farf = 1000.0f; //furthest distance to camera objects are rendered [m] - - Eigen::Matrix4f projection = Eigen::Matrix4f::Zero(); - float f = 1.0f / tan(fov / 2.0f); - projection(0, 0) = f / aspect; - projection(1, 1) = f; - projection(2, 2) = (farf + nearf) / (nearf - farf); - projection(2, 3) = (2 * farf * nearf) / (nearf - farf); - projection(3, 2) = -1.0f; - - //Combine into a single MVP (model-view-projection) - gl_mvp = projection * viewLocal1.matrix(); + GL_CALL(glGenBuffers(1, &gl_ssboCloudIndex)); + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); + GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, gl_cloudIndexSSBO.size() * sizeof(int), gl_cloudIndexSSBO.data(), GL_DYNAMIC_DRAW)); } - - void gl_renderPointCloud() + else { - GL_CALL(glUseProgram(gl_shaderProgram)); + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboCloudIndex)); + GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, gl_cloudIndexSSBO.size() * sizeof(int), gl_cloudIndexSSBO.data())); + } - //Set point size from GUI - GL_CALL(glUniform1f(gl_uPointSize, static_cast(point_size))); - GL_CALL(glUniform1f(gl_uIntensityScale, offset_intensity)); - GL_CALL(glUniform1i(gl_uUsePose, usePose)); - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); + GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, gl_ssboCloudIndex)); - if (oldcolorScheme != colorScheme) - { - if (colorScheme == 0) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) - { - gl_cloudsSSBO[i].colorScheme = 0; - gl_cloudsSSBO[i].fixedColor[0] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - gl_cloudsSSBO[i].fixedColor[1] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - gl_cloudsSSBO[i].fixedColor[2] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; - } + // Cloud attributes + if (gl_ssboClouds == 0) + { + GL_CALL(glGenBuffers(1, &gl_ssboClouds)); + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); + GL_CALL(glBufferData(GL_SHADER_STORAGE_BUFFER, gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), gl_cloudsSSBO.data(), GL_DYNAMIC_DRAW)); + } + else + { + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, gl_ssboClouds)); + GL_CALL(glBufferSubData(GL_SHADER_STORAGE_BUFFER, 0, gl_cloudsSSBO.size() * sizeof(gl_cloudSSBO), gl_cloudsSSBO.data())); + } - else if (colorScheme == 1) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) - gl_cloudsSSBO[i].colorScheme = 1; + GL_CALL(glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 1, gl_ssboClouds)); - else if (colorScheme == 2) - for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) - { - gl_cloudsSSBO[i].colorScheme = 0; - gl_cloudsSSBO[i].fixedColor[0] = float(rand() % 255) / 255.0f; - gl_cloudsSSBO[i].fixedColor[1] = float(rand() % 255) / 255.0f; - gl_cloudsSSBO[i].fixedColor[2] = float(rand() % 255) / 255.0f; - } + GL_CALL(glBindBuffer(GL_SHADER_STORAGE_BUFFER, 0)); +} - oldcolorScheme = colorScheme; +void gl_updateUserView() +{ + ImGuiIO& io = ImGui::GetIO(); - gl_updateSSBOs(); - } + Eigen::Affine3f viewLocal1 = Eigen::Affine3f::Identity(); + viewLocal1.translate(rotation_center); + + viewLocal1.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + viewLocal1.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal1.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + viewLocal1.translate(-rotation_center); + + // Get the projection matrix from your reshape() or manually rebuild it + float aspect = float(io.DisplaySize.x) / float(io.DisplaySize.y); + float fov = 60.0f * DEG_TO_RAD; + float nearf = 0.001f; // closest distance to camera objects are rendered [m] + float farf = 1000.0f; // furthest distance to camera objects are rendered [m] + + Eigen::Matrix4f projection = Eigen::Matrix4f::Zero(); + float f = 1.0f / tan(fov / 2.0f); + projection(0, 0) = f / aspect; + projection(1, 1) = f; + projection(2, 2) = (farf + nearf) / (nearf - farf); + projection(2, 3) = (2 * farf * nearf) / (nearf - farf); + projection(3, 2) = -1.0f; + + // Combine into a single MVP (model-view-projection) + gl_mvp = projection * viewLocal1.matrix(); +} - GL_CALL(glBindVertexArray(VAO)); - /*for (size_t i = 0; i < gl_clouds.size(); i++) - { - if (!gl_clouds[i].visible) continue; +void gl_renderPointCloud() +{ + GL_CALL(glUseProgram(gl_shaderProgram)); - // Send per-cloud uniforms + // Set point size from GUI + GL_CALL(glUniform1f(gl_uPointSize, static_cast(point_size))); + GL_CALL(glUniform1f(gl_uIntensityScale, offset_intensity)); + GL_CALL(glUniform1i(gl_uUsePose, usePose)); + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); - if (usePose) + if (oldcolorScheme != colorScheme) + { + if (colorScheme == 0) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) { - Eigen::Matrix4f cloudMVP = gl_mvp * gl_cloudsSSBO[i].pose; - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, cloudMVP.data())); + gl_cloudsSSBO[i].colorScheme = 0; + gl_cloudsSSBO[i].fixedColor[0] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; + gl_cloudsSSBO[i].fixedColor[1] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; + gl_cloudsSSBO[i].fixedColor[2] = session.point_clouds_container.point_clouds[0].render_color[0] + 0.1; } - else - GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); - // Draw only this cloud’s range - //GL_CALL(glDrawArrays(GL_POINTS, gl_clouds[i].offset, gl_clouds[i].count)); - }*/ + else if (colorScheme == 1) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + gl_cloudsSSBO[i].colorScheme = 1; + + else if (colorScheme == 2) + for (size_t i = 0; i < gl_cloudsSSBO.size(); ++i) + { + gl_cloudsSSBO[i].colorScheme = 0; + gl_cloudsSSBO[i].fixedColor[0] = float(rand() % 255) / 255.0f; + gl_cloudsSSBO[i].fixedColor[1] = float(rand() % 255) / 255.0f; + gl_cloudsSSBO[i].fixedColor[2] = float(rand() % 255) / 255.0f; + } - GL_CALL(glDrawArrays(GL_POINTS, 0, gl_cloudIndexSSBO.size())); + oldcolorScheme = colorScheme; - GL_CALL(glBindVertexArray(0)); - GL_CALL(glUseProgram(0)); // back to fixed-function for legacy code + gl_updateSSBOs(); } - void gl_init() + GL_CALL(glBindVertexArray(VAO)); + /*for (size_t i = 0; i < gl_clouds.size(); i++) { - GLenum err = glewInit(); - if (err != GLEW_OK) + if (!gl_clouds[i].visible) continue; + + // Send per-cloud uniforms + + if (usePose) { - std::cerr << "GLEW init failed: " << glewGetErrorString(err) << std::endl; - return; + Eigen::Matrix4f cloudMVP = gl_mvp * gl_cloudsSSBO[i].pose; + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, cloudMVP.data())); } + else + GL_CALL(glUniformMatrix4fv(gl_uMVP, 1, GL_FALSE, gl_mvp.data())); + + // Draw only this cloud’s range + //GL_CALL(glDrawArrays(GL_POINTS, gl_clouds[i].offset, gl_clouds[i].count)); + }*/ - //Compile shaders and create shader program - gl_shaderProgram = gl_createShaderProgram(pointsVertSource, pointsFragSource); + GL_CALL(glDrawArrays(GL_POINTS, 0, gl_cloudIndexSSBO.size())); - //Get pointers - gl_uMVP = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uMVP"))); - gl_uPointSize = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uPointSize"))); - gl_uIntensityScale = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uIntensityScale"))); - gl_uUsePose = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uUsePose"))); + GL_CALL(glBindVertexArray(0)); + GL_CALL(glUseProgram(0)); // back to fixed-function for legacy code +} - GL_CALL(glEnable(GL_PROGRAM_POINT_SIZE)); +void gl_init() +{ + GLenum err = glewInit(); + if (err != GLEW_OK) + { + std::cerr << "GLEW init failed: " << glewGetErrorString(err) << std::endl; + return; } -/////////////////////////////////////////////////////////////////////////////////// + // Compile shaders and create shader program + gl_shaderProgram = gl_createShaderProgram(pointsVertSource, pointsFragSource); + // Get pointers + gl_uMVP = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uMVP"))); + gl_uPointSize = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uPointSize"))); + gl_uIntensityScale = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uIntensityScale"))); + gl_uUsePose = static_cast(GL_CALL_RET(glGetUniformLocation(gl_shaderProgram, "uUsePose"))); + GL_CALL(glEnable(GL_PROGRAM_POINT_SIZE)); +} +/////////////////////////////////////////////////////////////////////////////////// void openSession() { @@ -718,16 +692,16 @@ void openSession() if (gl_useVBOs) { - //clearing previous data + // clearing previous data GLint offset = 0; gl_clouds.clear(); gl_clouds.shrink_to_fit(); gl_cloudIndexSSBO.clear(); gl_cloudIndexSSBO.shrink_to_fit(); gl_cloudsSSBO.clear(); - gl_cloudsSSBO.shrink_to_fit(); + gl_cloudsSSBO.shrink_to_fit(); - //Convert point cloud data to gl_Points + // Convert point cloud data to gl_Points for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); ++j) { const auto& pc = session.point_clouds_container.point_clouds[j]; @@ -740,10 +714,10 @@ void openSession() gp.pos[0] = pt.x(); gp.pos[1] = pt.y(); gp.pos[2] = pt.z(); - gp.intensity = pc.intensities[i]; // same index + gp.intensity = pc.intensities[i]; // same index gl_Points.push_back(gp); - gl_cloudIndexSSBO.push_back(static_cast(j)); // cloud index + gl_cloudIndexSSBO.push_back(static_cast(j)); // cloud index } gl_cloud gc; @@ -755,26 +729,26 @@ void openSession() gl_cloudSSBO gcSSBO; Eigen::Matrix4f pose_f = pc.m_pose.matrix().cast(); std::memcpy(gcSSBO.pose, pose_f.data(), 16 * sizeof(float)); - //gcSSBO.colorScheme = colorScheme; - //gcSSBO.fixedColor[0] = pc.render_color[0]; - //gcSSBO.fixedColor[1] = pc.render_color[1]; - //gcSSBO.fixedColor[2] = pc.render_color[2]; - oldcolorScheme = -1; //force update + // gcSSBO.colorScheme = colorScheme; + // gcSSBO.fixedColor[0] = pc.render_color[0]; + // gcSSBO.fixedColor[1] = pc.render_color[1]; + // gcSSBO.fixedColor[2] = pc.render_color[2]; + oldcolorScheme = -1; // force update gl_cloudsSSBO.push_back(gcSSBO); offset += static_cast(pc.points_local.size()); } - //Load to OpenGL buffers + // Load to OpenGL buffers gl_loadPointCloudBuffer(gl_Points, VAO, VBO); - //Clear CPU-side data after load to save memory + // Clear CPU-side data after load to save memory gl_Points.clear(); gl_Points.shrink_to_fit(); - //Prepare SSBO data + // Prepare SSBO data gl_updateSSBOs(); - } + } } } @@ -794,19 +768,29 @@ void session_gui() ImGui::Separator(); ImGui::Text("Offset [m]:"); - ImGui::Text("X: %.10f; Y: %.10f; Z: %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); + ImGui::Text( + "X: %.10f; Y: %.10f; Z: %.10f", + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Click to copy to clipboard"); if (ImGui::IsItemClicked()) { char tmp[64]; - snprintf(tmp, sizeof(tmp), "%.10f %.10f %.10f", session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); + snprintf( + tmp, + sizeof(tmp), + "%.10f %.10f %.10f", + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); ImGui::SetClipboardText(tmp); } ImGui::Separator(); - ImGui::Text("Dimensions:"); + ImGui::Text("Dimensions:"); if (ImGui::BeginTable("Dimensions", 4)) { ImGui::TableSetupColumn("Coord [m]"); @@ -818,7 +802,6 @@ void session_gui() ImGui::TableNextRow(); ImGui::TableSetColumnIndex(0); - std::string text = "X"; float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; // Set cursor so text is centered @@ -859,7 +842,6 @@ void session_gui() ImGui::EndTable(); } - } void index_gui() @@ -876,7 +858,8 @@ void index_gui() ImGui::Text("index_pairs : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].index_pairs.size()); ImGui::Text("buckets : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].buckets.size()); ImGui::Text("points_local : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()); - ImGui::Text("normal_vectors_local: %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].normal_vectors_local.size()); + ImGui::Text( + "normal_vectors_local: %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].normal_vectors_local.size()); ImGui::Text("colors : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].colors.size()); ImGui::Text("points_type : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].points_type.size()); ImGui::Text("intensities : %zu", session.point_clouds_container.point_clouds[index_rendered_points_local].intensities.size()); @@ -917,7 +900,8 @@ void properties_gui() { bool justPushed = false; - if (is_session_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_session_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Session")) { if (!is_session_gui) @@ -927,13 +911,15 @@ void properties_gui() justPushed = true; } } - if (is_session_gui && !justPushed) ImGui::PopStyleColor(); + if (is_session_gui && !justPushed) + ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Properties related to whole session"); ImGui::SameLine(); - if (is_index_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_index_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Index")) { if (!is_index_gui) @@ -943,7 +929,8 @@ void properties_gui() justPushed = true; } } - if (is_index_gui && !justPushed) ImGui::PopStyleColor(); + if (is_index_gui && !justPushed) + ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Properties related to current cloud index"); @@ -959,7 +946,6 @@ void properties_gui() ImGui::End(); } - void display() { ImGuiIO& io = ImGui::GetIO(); @@ -969,10 +955,9 @@ void display() glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); - if (gl_useVBOs) { - gl_updateUserView(); //this can be optimized to be called only on change (camera movement, parameters, window resize) + gl_updateUserView(); // this can be optimized to be called only on change (camera movement, parameters, window resize) gl_renderPointCloud(); } @@ -1003,13 +988,21 @@ void display() } else { - glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100000, 100000); + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); - glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100, 100); + camera_ortho_xy_view_zoom / ratio, + -100, + 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -1028,10 +1021,9 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); @@ -1042,11 +1034,13 @@ void display() showAxes(); - if (index_rendered_points_local >= 0 && index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) + if (index_rendered_points_local >= 0 && + index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) { - const auto& cloud = session.point_clouds_container.point_clouds[index_rendered_points_local]; //avoiding multiple indexing + const auto& cloud = session.point_clouds_container.point_clouds[index_rendered_points_local]; // avoiding multiple indexing - const double inv_max_intensity = 1.0 / *std::max_element(cloud.intensities.begin(), cloud.intensities.end()); //precompute for speed + const double inv_max_intensity = 1.0 / *std::max_element(cloud.intensities.begin(), cloud.intensities.end()); // precompute for + // speed Eigen::Affine3d pose = cloud.m_pose; @@ -1057,31 +1051,32 @@ void display() 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]); } - else if (colorScheme == 1) //intensity gradient + else if (colorScheme == 1) // intensity gradient { const double norm = cloud.intensities[i] * inv_max_intensity + offset_intensity; glColor3f(norm, 0.0, 1.0 - norm); } - Eigen::Vector3d p(cloud.points_local[i].x(), - cloud.points_local[i].y(), - cloud.points_local[i].z()); + Eigen::Vector3d p(cloud.points_local[i].x(), cloud.points_local[i].y(), cloud.points_local[i].z()); p = pose * p; glVertex3f(p.x(), p.y(), p.z()); } glEnd(); - if (show_neighbouring_scans) { + if (show_neighbouring_scans) + { glColor3f(pc_neigbouring_color.x, pc_neigbouring_color.y, pc_neigbouring_color.z); glPointSize(point_size); glBegin(GL_POINTS); - for (int index = index_rendered_points_local - 20; index <= index_rendered_points_local + 20; index += 5) { - if (index != index_rendered_points_local && index >= 0 && index < session.point_clouds_container.point_clouds.size()) { - const auto& iCloud = session.point_clouds_container.point_clouds[index]; //avoiding multiple indexing + for (int index = index_rendered_points_local - 20; index <= index_rendered_points_local + 20; index += 5) + { + if (index != index_rendered_points_local && index >= 0 && index < session.point_clouds_container.point_clouds.size()) + { + const auto& iCloud = session.point_clouds_container.point_clouds[index]; // avoiding multiple indexing Eigen::Affine3d pose = iCloud.m_pose; if (usePose == false) @@ -1093,9 +1088,7 @@ void display() for (size_t i = 0; i < iCloud.points_local.size(); i++) { - Eigen::Vector3d p(iCloud.points_local[i].x(), - iCloud.points_local[i].y(), - iCloud.points_local[i].z()); + Eigen::Vector3d p(iCloud.points_local[i].x(), iCloud.points_local[i].y(), iCloud.points_local[i].z()); p = pose * p; glVertex3f(p.x(), p.y(), p.z()); } @@ -1117,7 +1110,7 @@ void display() { openSession(); - //workaround + // workaround io.AddKeyEvent(ImGuiKey_O, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1128,7 +1121,7 @@ void display() { is_properties_gui = !is_properties_gui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_P, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1140,7 +1133,7 @@ void display() { show_neighbouring_scans = !show_neighbouring_scans; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_N, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -1155,15 +1148,11 @@ void display() else if (offset_intensity > 1) offset_intensity = 1; - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) - || ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true) - ) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) || + ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true)) index_rendered_points_local += 1; - if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - || ImGui::IsKeyPressed(ImGuiKey_PageDown, true) - || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true) - ) + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) || + ImGui::IsKeyPressed(ImGuiKey_PageDown, true) || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true)) index_rendered_points_local -= 1; if (index_rendered_points_local < 0) @@ -1175,7 +1164,7 @@ void display() if (ImGui::BeginMainMenuBar()) { if (ImGui::Button("Open session")) - openSession(); + openSession(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Select session to open for analyze (Ctrl+O)"); @@ -1201,7 +1190,7 @@ void display() for (auto& point_cloud : session.point_clouds_container.point_clouds) point_cloud.point_size = point_size; - //ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + // ImGui::MenuItem("show_imu_to_lio_diff", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); ImGui::Separator(); } @@ -1229,11 +1218,9 @@ void display() ImGui::MenuItem("Use segment pose", nullptr, &usePose); ImGui::MenuItem("Show neighbouring scans", "Ctrl+N", &show_neighbouring_scans); - - //ImGui::MenuItem("show_covs", nullptr, &show_covs); + // ImGui::MenuItem("show_covs", nullptr, &show_covs); ImGui::Separator(); - ImGui::MenuItem("VBO/VAO proof of concept", nullptr, &gl_useVBOs); if (ImGui::IsItemHovered()) @@ -1248,7 +1235,7 @@ void display() } if (gl_useVBOs) - usePose = true; //forces usePose when using VBOs for now + usePose = true; // forces usePose when using VBOs for now ImGui::Separator(); @@ -1266,7 +1253,7 @@ void display() ImGui::Separator(); - ImGui::Text("Point cloud:"); + ImGui::Text("Point cloud:"); float color[3]; if (session.point_clouds_container.point_clouds.size() > 0) @@ -1286,12 +1273,12 @@ void display() pc.show_color = false; } } - ImGui::SameLine(); - if (ImGui::MenuItem("> Solid color", nullptr, (colorScheme == 0))) - colorScheme = 0; + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid color", nullptr, (colorScheme == 0))) + colorScheme = 0; if (ImGui::MenuItem("> Intensity gradient", nullptr, (colorScheme == 1))) - colorScheme = 1; + colorScheme = 1; ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputFloat("Offset intensity", &offset_intensity, 0.01, 0.1, "%.2f"); if (offset_intensity < 0) @@ -1303,7 +1290,6 @@ void display() if (ImGui::MenuItem("> Random colors per segment", nullptr, (colorScheme == 2))) colorScheme = 2; - } ImGui::EndDisabled(); @@ -1353,7 +1339,7 @@ void display() } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Scene view relevant parameters"); - + camMenu(); ImGui::SameLine(); @@ -1373,8 +1359,9 @@ void display() { ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; + double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - + session.point_clouds_container.point_clouds[0].timestamps[0]) / + 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); @@ -1386,8 +1373,9 @@ void display() { ImGui::BeginTooltip(); ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - - session.point_clouds_container.point_clouds[0].timestamps[0]) / 1e9; + double ts = (session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] - + session.point_clouds_container.point_clouds[0].timestamps[0]) / + 1e9; ImGui::Text("Delta 1st points timestamp [s]: %.6f", ts); ImGui::NewLine(); ImGui::Text("Check Properties (Ctrl+P) for more info"); @@ -1402,7 +1390,9 @@ void display() ImGui::SameLine(); ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -1419,7 +1409,7 @@ void display() } if (is_properties_gui) - properties_gui(); + properties_gui(); if (consImGui) { @@ -1449,7 +1439,7 @@ void display() void mouse(int glut_button, int state, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; @@ -1517,7 +1507,7 @@ void mouse(int glut_button, int state, int x, int y) } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { try { @@ -1528,17 +1518,14 @@ int main(int argc, char *argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - } - catch (const std::bad_alloc& e) + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/manual_color/CMakeLists.txt b/apps/manual_color/CMakeLists.txt index 93329a53..aff345ec 100644 --- a/apps/manual_color/CMakeLists.txt +++ b/apps/manual_color/CMakeLists.txt @@ -9,17 +9,17 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${FREEGLUT_INCLUDE_DIR}) target_link_libraries( diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 4dbd4111..d0020011 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -1,6 +1,6 @@ -#include #include +#include #include "imgui.h" #include "imgui_impl_glut.h" @@ -13,20 +13,20 @@ #include #include -#include #include #include +#include #include "color_las_loader.h" #include "pfd_wrapper.hpp" #include -#include -#include -#include #include #include +#include #include +#include +#include #include @@ -44,9 +44,9 @@ bool color = false; GLuint tex1; -GLUquadric *sphere; +GLUquadric* sphere; -GLuint make_tex(const std::string &fn) +GLuint make_tex(const std::string& fn) { GLuint tex; glGenTextures(1, &tex); @@ -58,7 +58,7 @@ GLuint make_tex(const std::string &fn) glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); // load and generate the texture int width, height, nrChannels; - unsigned char *data = stbi_load(fn.c_str(), &width, &height, &nrChannels, 0); + unsigned char* data = stbi_load(fn.c_str(), &width, &height, &nrChannels, 0); std::cout << "width: " << width << " height: " << height << " nrChannels: " << nrChannels << std::endl; @@ -66,7 +66,7 @@ GLuint make_tex(const std::string &fn) { if (nrChannels == 1) { - unsigned char *data3 = (unsigned char *)malloc(width * height * 3); + unsigned char* data3 = (unsigned char*)malloc(width * height * 3); int counter = 0; for (int wh = 0; wh < width * height; wh++) @@ -105,16 +105,16 @@ int mouse_buttons = 0; float rotate_x = 0.0, rotate_y = 0.0; float translate_z = -90.0; float translate_x, translate_y = 0.0; -bool gui_mouse_down{false}; +bool gui_mouse_down{ false }; void display(); void reshape(int w, int h); void mouse(int glut_button, int state, int x, int y); void motion(int x, int y); -bool initGL(int *argc, char **argv); +bool initGL(int* argc, char** argv); -float imgui_co_size{1000.0f}; -bool imgui_draw_co{true}; +float imgui_co_size{ 1000.0f }; +bool imgui_draw_co{ true }; double CameraRotationZ = 0; double CameraHeight = 0; @@ -123,19 +123,19 @@ namespace SystemData { std::vector points; std::pair clickedRay; - int closestPointIndex{-1}; + int closestPointIndex{ -1 }; std::vector pointPickedImage; std::vector pointPickedPointCloud; - unsigned char *imageData; + unsigned char* imageData; int imageWidth, imageHeight, imageNrChannels; Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); int point_size = 1; -} +} // namespace SystemData -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); // pose.om = M_PI * 0.5; @@ -160,9 +160,9 @@ int main(int argc, char *argv[]) glutMainLoop(); } -void imagePicker(const std::string &name, ImTextureID tex1, std::vector &point_picked, std::vector point_pickedInPointcloud) +void imagePicker(const std::string& name, ImTextureID tex1, std::vector& point_picked, std::vector point_pickedInPointcloud) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); static float zoom = 0.1f; const int Tex_width = 5000; const int Tex_height = 2500; @@ -189,9 +189,9 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector if (ImGui::IsKeyDown(ImGuiKey_PageDown)) zoom /= 1.00f + 0.01f * speed; - ImVec2 uv_min = ImVec2(0.0f, 0.0f); // Top-left - ImVec2 uv_max = ImVec2(1.0f, 1.0f); // Lower-right - ImVec4 tint_col = ImVec4(1.0f, 1.0f, 1.0f, 1.0f); // No tint + ImVec2 uv_min = ImVec2(0.0f, 0.0f); // Top-left + ImVec2 uv_max = ImVec2(1.0f, 1.0f); // Lower-right + ImVec4 tint_col = ImVec4(1.0f, 1.0f, 1.0f, 1.0f); // No tint ImVec4 border_col = ImVec4(1.0f, 1.0f, 1.0f, 0.5f); // 50% opaque white ImGuiWindowFlags window_flags = ImGuiWindowFlags_HorizontalScrollbar | ImGuiWindowFlags_NoScrollWithMouse; @@ -199,7 +199,7 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector ImGui::InputFloat("zoom", &zoom, 0.1f, 0.5f); float my_tex_w = Tex_width * zoom; float my_tex_h = Tex_height * zoom; - const ImVec2 child_size{ImGui::GetWindowWidth() * 1.0f, ImGui::GetWindowHeight() * 0.5f}; + const ImVec2 child_size{ ImGui::GetWindowWidth() * 1.0f, ImGui::GetWindowHeight() * 0.5f }; ImGui::Checkbox("color", &color); @@ -211,10 +211,10 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector bool visible2; }; - auto draw_zoom_pick_point = [my_tex_w, my_tex_h, tint_col, border_col](const ImTextureID &tex, std::vector &point_picked) + auto draw_zoom_pick_point = [my_tex_w, my_tex_h, tint_col, border_col](const ImTextureID& tex, std::vector& point_picked) { ImVec2 img_start = ImGui::GetItemRectMin(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); ImGui::BeginTooltip(); float region_sz = 32.0f; float region_x = io.MousePos.x - img_start.x - region_sz * 0.5f; @@ -223,7 +223,7 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector // add point if (io.MouseClicked[2] && io.KeyShift) { - ImVec2 picked_point{(io.MousePos.x - img_start.x) / my_tex_w, (io.MousePos.y - img_start.y) / my_tex_h}; + ImVec2 picked_point{ (io.MousePos.x - img_start.x) / my_tex_w, (io.MousePos.y - img_start.y) / my_tex_h }; point_picked.push_back(picked_point); } @@ -253,10 +253,12 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector ImVec2 uv1 = ImVec2((region_x + region_sz) / my_tex_w, (region_y + region_sz) / my_tex_h); ImGui::Image(tex, ImVec2(region_sz * local_zoom, region_sz * local_zoom), uv0, uv1, tint_col, border_col); ImVec2 img_start_loc = ImGui::GetItemRectMin(); - ImVec2 img_sz = {ImGui::GetItemRectMax().x - ImGui::GetItemRectMin().x, ImGui::GetItemRectMax().y - ImGui::GetItemRectMin().y}; + ImVec2 img_sz = { ImGui::GetItemRectMax().x - ImGui::GetItemRectMin().x, ImGui::GetItemRectMax().y - ImGui::GetItemRectMin().y }; ImVec2 window_center = ImVec2(img_start_loc.x + img_sz.x * 0.5f, img_start_loc.y + img_sz.y * 0.5f); - ImGui::GetForegroundDrawList()->AddLine({window_center.x - 10, window_center.y}, {window_center.x + 10, window_center.y}, IM_COL32(0, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddLine({window_center.x, window_center.y - 10}, {window_center.x, window_center.y + 10}, IM_COL32(0, 255, 0, 200), 1); + ImGui::GetForegroundDrawList()->AddLine( + { window_center.x - 10, window_center.y }, { window_center.x + 10, window_center.y }, IM_COL32(0, 255, 0, 200), 1); + ImGui::GetForegroundDrawList()->AddLine( + { window_center.x, window_center.y - 10 }, { window_center.x, window_center.y + 10 }, IM_COL32(0, 255, 0, 200), 1); ImGui::EndTooltip(); }; @@ -264,27 +266,30 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector { ImGui::Image(tex1, ImVec2(my_tex_w, my_tex_h), uv_min, uv_max, tint_col, border_col); const ImVec2 view_port_start = ImGui::GetWindowPos(); - const ImVec2 view_port_end{view_port_start.x + ImGui::GetWindowWidth(), view_port_start.y + ImGui::GetWindowHeight()}; + const ImVec2 view_port_end{ view_port_start.x + ImGui::GetWindowWidth(), view_port_start.y + ImGui::GetWindowHeight() }; ImVec2 img_start = ImGui::GetItemRectMin(); for (int i = 0; i < point_picked.size(); i++) { - const auto &p = point_picked[i]; + const auto& p = point_picked[i]; - ImVec2 center{img_start.x + p.x * my_tex_w, img_start.y + p.y * my_tex_h}; + ImVec2 center{ img_start.x + p.x * my_tex_w, img_start.y + p.y * my_tex_h }; if (center.x > view_port_start.x && center.x < view_port_end.x && center.y > view_port_start.y && center.y < view_port_end.y) { char data[16]; snprintf(data, 16, "%d", i); - ImGui::GetForegroundDrawList()->AddLine({center.x - 10, center.y}, {center.x + 10, center.y}, IM_COL32(255, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddLine({center.x, center.y - 10}, {center.x, center.y + 10}, IM_COL32(255, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddText({center.x, center.y + 10}, IM_COL32(255, 0, 0, 200), data); + ImGui::GetForegroundDrawList()->AddLine( + { center.x - 10, center.y }, { center.x + 10, center.y }, IM_COL32(255, 255, 0, 200), 1); + ImGui::GetForegroundDrawList()->AddLine( + { center.x, center.y - 10 }, { center.x, center.y + 10 }, IM_COL32(255, 255, 0, 200), 1); + ImGui::GetForegroundDrawList()->AddText({ center.x, center.y + 10 }, IM_COL32(255, 0, 0, 200), data); if (i < point_pickedInPointcloud.size()) { - ImGui::GetForegroundDrawList()->AddLine({center.x, center.y}, point_pickedInPointcloud.at(i), IM_COL32(255, 0, 0, 200), 1); + ImGui::GetForegroundDrawList()->AddLine( + { center.x, center.y }, point_pickedInPointcloud.at(i), IM_COL32(255, 0, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddText({point_pickedInPointcloud.at(i)}, IM_COL32(255, 0, 0, 200), data); + ImGui::GetForegroundDrawList()->AddText({ point_pickedInPointcloud.at(i) }, IM_COL32(255, 0, 0, 200), data); } } } @@ -298,7 +303,7 @@ void imagePicker(const std::string &name, ImTextureID tex1, std::vector ImGui::EndChild(); } -ImVec2 UnprojectPoint(const Eigen::Vector3d &point) +ImVec2 UnprojectPoint(const Eigen::Vector3d& point) { GLint viewport[4]; GLdouble modelview[16]; @@ -310,7 +315,7 @@ ImVec2 UnprojectPoint(const Eigen::Vector3d &point) GLdouble winX, winY, winZ; gluProject(point[0], point[1], point[2], modelview, projection, viewport, &winX, &winY, &winZ); - return {static_cast(winX), static_cast(viewport[3] - winY)}; + return { static_cast(winX), static_cast(viewport[3] - winY) }; } std::pair GetRay(int x, int y) @@ -344,23 +349,44 @@ std::pair GetRay(int x, int y) direction.normalize(); - return {position, direction}; + return { position, direction }; } -double GetDistanceToRay(const Eigen::Vector3d &qureyPoint, const std::pair &ray) +double GetDistanceToRay(const Eigen::Vector3d& qureyPoint, const std::pair& ray) { return ray.second.cross(qureyPoint - ray.first).norm(); } -std::vector ApplyColorToPointcloud(const std::vector &pointsRGB, const unsigned char *imageData, int imageWidth, int imageHeight, int nrChannels, const Eigen::Affine3d &transfom) +std::vector ApplyColorToPointcloud( + const std::vector& pointsRGB, + const unsigned char* imageData, + int imageWidth, + int imageHeight, + int nrChannels, + const Eigen::Affine3d& transfom) { std::vector newCloud(pointsRGB.size()); - std::transform(std::execution::par_unseq, pointsRGB.begin(), pointsRGB.end(), newCloud.begin(), [&](mandeye::PointRGB p) - { + std::transform( + std::execution::par_unseq, + pointsRGB.begin(), + pointsRGB.end(), + newCloud.begin(), + [&](mandeye::PointRGB p) + { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); double du, dv; - equrectangular_camera_colinearity_tait_bryan_wc(du,dv, imageHeight, imageWidth, - M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, + equrectangular_camera_colinearity_tait_bryan_wc( + du, + dv, + imageHeight, + imageWidth, + M_PI, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, p.point.x(), p.point.y(), p.point.z()); @@ -372,28 +398,58 @@ std::vector ApplyColorToPointcloud(const std::vector ApplyColorToPointcloudFishEye(const std::vector &pointsRGB, const unsigned char *imageData, int imageWidth, int imageHeight, int nrChannels, const Eigen::Affine3d &transfom) +std::vector ApplyColorToPointcloudFishEye( + const std::vector& pointsRGB, + const unsigned char* imageData, + int imageWidth, + int imageHeight, + int nrChannels, + const Eigen::Affine3d& transfom) { std::vector newCloud(pointsRGB.size()); - std::transform(std::execution::par_unseq, pointsRGB.begin(), pointsRGB.end(), newCloud.begin(), [&](mandeye::PointRGB p) - { + std::transform( + std::execution::par_unseq, + pointsRGB.begin(), + pointsRGB.end(), + newCloud.begin(), + [&](mandeye::PointRGB p) + { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); double du, dv; - //equrectangular_camera_colinearity_tait_bryan_wc(du,dv, imageHeight, imageWidth, + // equrectangular_camera_colinearity_tait_bryan_wc(du,dv, imageHeight, imageWidth, // M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, // p.point.x(), // p.point.y(), // p.point.z()); - projection_fisheye_camera_tait_bryan_wc(du, dv, fx, fy, cx, cy, - pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - p.point.x(), p.point.y(), p.point.z(), k1, k2, k3, k4, alpha); + projection_fisheye_camera_tait_bryan_wc( + du, + dv, + fx, + fy, + cx, + cy, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + p.point.x(), + p.point.y(), + p.point.z(), + k1, + k2, + k3, + k4, + alpha); int u = std::round(du); int v = std::round(dv); @@ -403,32 +459,38 @@ std::vector ApplyColorToPointcloudFishEye(const std::vector>= 1) { + for (; gray; gray >>= 1) + { num ^= gray; } return num; } -uint32_t packBoolsToUint32(const std::array& bools) { +uint32_t packBoolsToUint32(const std::array& bools) +{ uint32_t result = 0; - for (size_t i = 0; i < bools.size(); ++i) { - if (bools[i]) { + for (size_t i = 0; i < bools.size(); ++i) + { + if (bools[i]) + { result |= (1U << i); } } return result; } -void TimeStampCount() { +void TimeStampCount() +{ namespace SD = SystemData; static bool show_terminal = false; @@ -437,32 +499,29 @@ void TimeStampCount() { static uint64_t decodedValue = 0; static std::array diodeBits = {}; static uint64_t fullTimestamp = 0; - static const std::array newOrder = { - 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 0, 1, 2, 3, 4, 5, 6, 7 - }; + static const std::array newOrder = { 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 0, 1, 2, 3, 4, 5, 6, 7 }; static uint64_t sessionStart = 0; - static const std::array diodePositions = { - ImVec2(400, 3375), ImVec2(840, 3375), - ImVec2(1250, 3350), ImVec2(1600, 3330), ImVec2(2000, 3350), - ImVec2(2400, 3400), ImVec2(2850, 3450), ImVec2(3370, 3470), - ImVec2(3870, 3460), ImVec2(4400, 3450), ImVec2(4900, 3400), - ImVec2(5300, 3350), ImVec2(5600, 3300), ImVec2(6000, 3300), - ImVec2(6400, 3330), ImVec2(6800, 3350), ImVec2(7200, 3375), - ImVec2(7600, 3375) - }; - - if (ImGui::Button("Manual Calculate Timestamp")) { + static const std::array diodePositions = { ImVec2(400, 3375), ImVec2(840, 3375), ImVec2(1250, 3350), ImVec2(1600, 3330), + ImVec2(2000, 3350), ImVec2(2400, 3400), ImVec2(2850, 3450), ImVec2(3370, 3470), + ImVec2(3870, 3460), ImVec2(4400, 3450), ImVec2(4900, 3400), ImVec2(5300, 3350), + ImVec2(5600, 3300), ImVec2(6000, 3300), ImVec2(6400, 3330), ImVec2(6800, 3350), + ImVec2(7200, 3375), ImVec2(7600, 3375) }; + + if (ImGui::Button("Manual Calculate Timestamp")) + { show_terminal = true; } - if (show_terminal) { + if (show_terminal) + { ImGui::Begin("Timestamp Terminal", &show_terminal); float scale = 1.0f; ImVec2 imgPos = ImGui::GetCursorScreenPos(); - if (SD::imageData != nullptr) { + if (SD::imageData != nullptr) + { ImVec2 windowSize = ImGui::GetContentRegionAvail(); float aspectRatio = (float)SD::imageWidth / (float)SD::imageHeight; float scaleX = windowSize.x / SD::imageWidth; @@ -473,79 +532,100 @@ void TimeStampCount() { float scaledHeight = SD::imageHeight * scale; ImGui::Text("Loaded image:"); - imgPos = ImGui::GetCursorScreenPos(); + imgPos = ImGui::GetCursorScreenPos(); ImGui::Image(reinterpret_cast(static_cast(tex1)), ImVec2(scaledWidth, scaledHeight)); auto* drawList = ImGui::GetWindowDrawList(); ImGui::SetWindowFontScale(1.25f); - for (int i = 0; i < 18; ++i) { + for (int i = 0; i < 18; ++i) + { ImVec2 pos = diodePositions[i]; pos.x = imgPos.x + pos.x * scale; pos.y = imgPos.y + pos.y * scale; ImU32 color = diodeBits[i] ? IM_COL32(255, 0, 0, 255) : IM_COL32(0, 0, 0, 255); drawList->AddText(pos, color, std::to_string(i).c_str()); } - } else { + } + else + { ImGui::Text("No image loaded."); } ImGui::SetWindowFontScale(1.0f); ImGui::Text("Click on bits:"); - for (int i = 0; i < 18; ++i) { - if (diodeBits[i]) { - ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(255, 0, 0, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, IM_COL32(200, 0, 0, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, IM_COL32(150, 0, 0, 255)); - } else { - ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(50, 50, 50, 255)); + for (int i = 0; i < 18; ++i) + { + if (diodeBits[i]) + { + ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(255, 0, 0, 255)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, IM_COL32(200, 0, 0, 255)); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, IM_COL32(150, 0, 0, 255)); + } + else + { + ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(50, 50, 50, 255)); ImGui::PushStyleColor(ImGuiCol_ButtonHovered, IM_COL32(80, 80, 80, 255)); ImGui::PushStyleColor(ImGuiCol_ButtonActive, IM_COL32(100, 100, 100, 255)); } - + std::string label = std::to_string(i) + ": " + (diodeBits[i] ? "1" : "0"); - if (ImGui::SmallButton(label.c_str())) { + if (ImGui::SmallButton(label.c_str())) + { diodeBits[i] = !diodeBits[i]; } - - ImGui::PopStyleColor(3); - - if (i != 17) ImGui::SameLine(); + + ImGui::PopStyleColor(3); + + if (i != 17) + ImGui::SameLine(); } ImGui::NewLine(); - if (ImGui::Button("Load status.json")) { + if (ImGui::Button("Load status.json")) + { const auto input_file_names = mandeye::fd::OpenFileDialog("Choose json", mandeye::fd::Session_filter, false); - if (!input_file_names.empty()) { + if (!input_file_names.empty()) + { std::ifstream file(input_file_names.front()); - if (file) { - try { + if (file) + { + try + { nlohmann::json j; file >> j; - - if (j.contains("livox") && j["livox"].contains("LivoxLidarInfo") && j["livox"]["LivoxLidarInfo"].contains("m_sessionStart")) { + + if (j.contains("livox") && j["livox"].contains("LivoxLidarInfo") && + j["livox"]["LivoxLidarInfo"].contains("m_sessionStart")) + { sessionStart = j["livox"]["LivoxLidarInfo"]["m_sessionStart"]; std::cout << "m_sessionStart: " << sessionStart << std::endl; - + fullTimestamp = sessionStart + decodedValue; - } else { + } + else + { std::cerr << "Invalid JSON or missing 'm_sessionStart' key\n"; } - } catch (const std::exception &e) { + } catch (const std::exception& e) + { std::cerr << "JSON read error: " << e.what() << "\n"; } } } } - if (ImGui::Button("Calculate timestamp")) { + if (ImGui::Button("Calculate timestamp")) + { std::array reorderedBits = {}; - for (int i = 0; i < 18; ++i) { - reorderedBits[i] = diodeBits[newOrder[i]]; + for (int i = 0; i < 18; ++i) + { + reorderedBits[i] = diodeBits[newOrder[i]]; } packedValue = packBoolsToUint32(reorderedBits); decodedValue = static_cast(fromGrayCode(packedValue)) * 10'000'000; - if (sessionStart != 0) { + if (sessionStart != 0) + { fullTimestamp = sessionStart + decodedValue; } } @@ -560,10 +640,8 @@ void TimeStampCount() { } } - void ImGuiLoadSaveButtons() { - namespace SD = SystemData; if (ImGui::Button("Load Image")) { @@ -577,7 +655,13 @@ void ImGuiLoadSaveButtons() // std::cout << "3" << std::endl; } // std::cout << "4" << std::endl; - SystemData::points = ApplyColorToPointcloud(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); // std::cout << "5" << std::endl; } ImGui::SameLine(); @@ -588,10 +672,22 @@ void ImGuiLoadSaveButtons() { auto points = mandeye::load(input_file_names.front()); SystemData::points.resize(points.size()); - std::transform(points.begin(), points.end(), SystemData::points.begin(), [&](const mandeye::Point &p) - { return p; }); + std::transform( + points.begin(), + points.end(), + SystemData::points.begin(), + [&](const mandeye::Point& p) + { + return p; + }); } - SystemData::points = ApplyColorToPointcloud(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); } ImGui::SameLine(); if (ImGui::Button("Save Pointcloud")) @@ -616,23 +712,41 @@ void optimize() TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); for (int i = 0; i < SystemData::pointPickedImage.size(); i++) { - Eigen::Matrix delta; - observation_equation_equrectangular_camera_colinearity_tait_bryan_wc(delta, SystemData::imageHeight, SystemData::imageWidth, M_PI, - pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x * SystemData::imageWidth, - SystemData::pointPickedImage[i].y * SystemData::imageHeight); + observation_equation_equrectangular_camera_colinearity_tait_bryan_wc( + delta, + SystemData::imageHeight, + SystemData::imageWidth, + M_PI, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + SystemData::pointPickedPointCloud[i].x(), + SystemData::pointPickedPointCloud[i].y(), + SystemData::pointPickedPointCloud[i].z(), + SystemData::pointPickedImage[i].x * SystemData::imageWidth, + SystemData::pointPickedImage[i].y * SystemData::imageHeight); Eigen::Matrix jacobian; - observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian(jacobian, SystemData::imageHeight, SystemData::imageWidth, M_PI, - pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x, SystemData::pointPickedImage[i].y); + observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian( + jacobian, + SystemData::imageHeight, + SystemData::imageWidth, + M_PI, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + SystemData::pointPickedPointCloud[i].x(), + SystemData::pointPickedPointCloud[i].y(), + SystemData::pointPickedPointCloud[i].z(), + SystemData::pointPickedImage[i].x, + SystemData::pointPickedImage[i].y); int ir = tripletListB.size(); int ic_camera = 0; @@ -736,39 +850,70 @@ void optimize_fish_eye() TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); for (int i = 0; i < SystemData::pointPickedImage.size(); i++) { - Eigen::Matrix delta; - // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc(delta, SystemData::imageHeight, SystemData::imageWidth, M_PI, + // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc(delta, SystemData::imageHeight, SystemData::imageWidth, + // M_PI, // pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, // SystemData::pointPickedPointCloud[i].x(), // SystemData::pointPickedPointCloud[i].y(), // SystemData::pointPickedPointCloud[i].z(), - // SystemData::pointPickedImage[i].x * SystemData::imageWidth, - // SystemData::pointPickedImage[i].y * SystemData::imageHeight); - - observation_equation_fisheye_camera_tait_bryan_wc(delta, fx, fy, cx, cy, - pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x * SystemData::imageWidth, - SystemData::pointPickedImage[i].y * SystemData::imageHeight, - k1, k2, k3, k4, alpha); + // SystemData::pointPickedImage[i].x * + // SystemData::imageWidth, + // SystemData::pointPickedImage[i].y * + // SystemData::imageHeight); + + observation_equation_fisheye_camera_tait_bryan_wc( + delta, + fx, + fy, + cx, + cy, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + SystemData::pointPickedPointCloud[i].x(), + SystemData::pointPickedPointCloud[i].y(), + SystemData::pointPickedPointCloud[i].z(), + SystemData::pointPickedImage[i].x * SystemData::imageWidth, + SystemData::pointPickedImage[i].y * SystemData::imageHeight, + k1, + k2, + k3, + k4, + alpha); Eigen::Matrix jacobian; - // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian(jacobian, SystemData::imageHeight, SystemData::imageWidth, M_PI, - // pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, + // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian(jacobian, SystemData::imageHeight, + // SystemData::imageWidth, M_PI, + // pose.px, pose.py, pose.pz, pose.om, pose.fi, + // pose.ka, // SystemData::pointPickedPointCloud[i].x(), // SystemData::pointPickedPointCloud[i].y(), // SystemData::pointPickedPointCloud[i].z(), - // SystemData::pointPickedImage[i].x, SystemData::pointPickedImage[i].y); - - observation_equation_fisheye_camera_tait_bryan_wc_jacobian(jacobian, fx, fy, - pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - k1, k2, k3, k4, alpha); + // SystemData::pointPickedImage[i].x, + // SystemData::pointPickedImage[i].y); + + observation_equation_fisheye_camera_tait_bryan_wc_jacobian( + jacobian, + fx, + fy, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, + SystemData::pointPickedPointCloud[i].x(), + SystemData::pointPickedPointCloud[i].y(), + SystemData::pointPickedPointCloud[i].z(), + k1, + k2, + k3, + k4, + alpha); int ir = tripletListB.size(); int ic_camera = 0; @@ -865,7 +1010,7 @@ void optimize_fish_eye() void display() { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -883,7 +1028,7 @@ void display() // glColor3f(p.rgb.data()); glPointSize(SystemData::point_size); glBegin(GL_POINTS); - for (const auto &p : SystemData::points) + for (const auto& p : SystemData::points) { if (color) { @@ -901,7 +1046,7 @@ void display() ////////////////////////////////// glPointSize(10); glBegin(GL_POINTS); - for (const auto &p : SystemData::pointPickedPointCloud) + for (const auto& p : SystemData::pointPickedPointCloud) { glColor3f(1.f, 0.f, 0.f); glVertex3dv(p.data()); @@ -927,17 +1072,19 @@ void display() } ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); std::vector picked3DPoints(SystemData::pointPickedPointCloud.size()); - std::transform(SystemData::pointPickedPointCloud.begin(), SystemData::pointPickedPointCloud.end(), picked3DPoints.begin(), UnprojectPoint); + std::transform( + SystemData::pointPickedPointCloud.begin(), SystemData::pointPickedPointCloud.end(), picked3DPoints.begin(), UnprojectPoint); ImGui::Begin("Image"); ImGuiLoadSaveButtons(); TimeStampCount(); - //if (ImGui::Button("apply color to PC (fishEye)")) + // if (ImGui::Button("apply color to PC (fishEye)")) //{ - // SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + // SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, + // SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); //} if (ImGui::Button("Optimize")) @@ -952,7 +1099,13 @@ void display() std::cout << "om " << pose.om << std::endl; std::cout << "fi " << pose.fi << std::endl; std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); } ImGui::SameLine(); @@ -968,7 +1121,8 @@ void display() std::cout << "om " << pose.om << std::endl; std::cout << "fi " << pose.fi << std::endl; std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, + SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); }*/ ImGui::SameLine(); @@ -990,7 +1144,13 @@ void display() std::cout << "om " << pose.om << std::endl; std::cout << "fi " << pose.fi << std::endl; std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); } ImGui::InputInt("point_size", &SystemData::point_size); @@ -1009,17 +1169,20 @@ void display() if (!outfile.good()) { std::cout << "can not save file: " << output_file_name << std::endl; - + return; } outfile << 1 << std::endl; outfile << "camera_to_lidar_relative_pose" << std::endl; - outfile << SystemData::camera_pose(0, 0) << " " << SystemData::camera_pose(0, 1) << " " << SystemData::camera_pose(0, 2) << " " << SystemData::camera_pose(0, 3) << std::endl; - outfile << SystemData::camera_pose(1, 0) << " " << SystemData::camera_pose(1, 1) << " " << SystemData::camera_pose(1, 2) << " " << SystemData::camera_pose(1, 3) << std::endl; - outfile << SystemData::camera_pose(2, 0) << " " << SystemData::camera_pose(2, 1) << " " << SystemData::camera_pose(2, 2) << " " << SystemData::camera_pose(2, 3) << std::endl; + outfile << SystemData::camera_pose(0, 0) << " " << SystemData::camera_pose(0, 1) << " " << SystemData::camera_pose(0, 2) << " " + << SystemData::camera_pose(0, 3) << std::endl; + outfile << SystemData::camera_pose(1, 0) << " " << SystemData::camera_pose(1, 1) << " " << SystemData::camera_pose(1, 2) << " " + << SystemData::camera_pose(1, 3) << std::endl; + outfile << SystemData::camera_pose(2, 0) << " " << SystemData::camera_pose(2, 1) << " " << SystemData::camera_pose(2, 2) << " " + << SystemData::camera_pose(2, 3) << std::endl; outfile << "0 0 0 1" << std::endl; - + outfile.close(); } @@ -1072,8 +1235,8 @@ void display() std::getline(infile, line); - //PointCloud pc; - //pc.file_name = point_cloud_file_name; + // PointCloud pc; + // pc.file_name = point_cloud_file_name; SystemData::camera_pose = Eigen::Affine3d::Identity(); SystemData::camera_pose(0, 0) = r11; SystemData::camera_pose(0, 1) = r12; @@ -1098,7 +1261,13 @@ void display() std::cout << "om " << pose.om << std::endl; std::cout << "fi " << pose.fi << std::endl; std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud(SystemData::points, SystemData::imageData, SystemData::imageWidth, SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); + SystemData::points = ApplyColorToPointcloud( + SystemData::points, + SystemData::imageData, + SystemData::imageWidth, + SystemData::imageHeight, + SystemData::imageNrChannels, + SystemData::camera_pose); } imagePicker("ImagePicker", (ImTextureID)tex1, SystemData::pointPickedImage, picked3DPoints); @@ -1116,7 +1285,7 @@ void display() for (auto it = SystemData::pointPickedImage.begin(); it != SystemData::pointPickedImage.end(); it++) { auto index = std::distance(SystemData::pointPickedImage.begin(), it); - const auto &p = *it; + const auto& p = *it; ImGui::Text("%d : %.1f,%.1f", index, p.x, p.y); ImGui::SameLine(); const auto label = std::string("-##2s") + std::to_string(index); @@ -1135,7 +1304,7 @@ void display() ImGui::Text("3D:"); for (auto it = SystemData::pointPickedPointCloud.begin(); it != SystemData::pointPickedPointCloud.end(); it++) { - const auto &p = *it; + const auto& p = *it; const auto index = std::distance(SystemData::pointPickedPointCloud.begin(), it); auto prev = it != SystemData::pointPickedPointCloud.begin() ? it - 1 : SystemData::pointPickedPointCloud.end(); auto next = it + 1 != SystemData::pointPickedPointCloud.end() ? it + 1 : SystemData::pointPickedPointCloud.end(); @@ -1183,7 +1352,7 @@ void display() void mouse(int glut_button, int state, int x, int y) { ImGui_ImplGLUT_MouseFunc(glut_button, state, x, y); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) button = 0; @@ -1212,24 +1381,29 @@ void mouse(int glut_button, int state, int x, int y) SystemData::clickedRay = GetRay(x, y); std::mutex mtx; - std::pair distanceIndexPair{std::numeric_limits::max(), -1}; + std::pair distanceIndexPair{ std::numeric_limits::max(), -1 }; - std::for_each(std::execution::par_unseq, SystemData::points.begin(), SystemData::points.end(), [&](const mandeye::PointRGB &p) - { - double D = GetDistanceToRay(p.point, SystemData::clickedRay); - std::lock_guard guard(mtx); - if (D < distanceIndexPair.first) + std::for_each( + std::execution::par_unseq, + SystemData::points.begin(), + SystemData::points.end(), + [&](const mandeye::PointRGB& p) { - // Assume that SystemData::point is an array-like type implementation, naked pointer arithmetic ahead: - const int index = &p - &SystemData::points.front(); - assert(index >= 0); - assert(index < SystemData::points.size()); - distanceIndexPair = { D, index }; - } }); + double D = GetDistanceToRay(p.point, SystemData::clickedRay); + std::lock_guard guard(mtx); + if (D < distanceIndexPair.first) + { + // Assume that SystemData::point is an array-like type implementation, naked pointer arithmetic ahead: + const int index = &p - &SystemData::points.front(); + assert(index >= 0); + assert(index < SystemData::points.size()); + distanceIndexPair = { D, index }; + } + }); if (distanceIndexPair.second > 0) { - const auto &[distance, index] = distanceIndexPair; + const auto& [distance, index] = distanceIndexPair; std::cout << "Closest point found, distance " << distance << std::endl; SystemData::closestPointIndex = distanceIndexPair.second; SystemData::pointPickedPointCloud.push_back(SystemData::points.at(index).point); @@ -1249,7 +1423,7 @@ void mouse(int glut_button, int state, int x, int y) void motion(int x, int y) { ImGui_ImplGLUT_MotionFunc(x, y); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); if (!io.WantCaptureMouse) { @@ -1287,7 +1461,7 @@ void reshape(int w, int h) glLoadIdentity(); } -bool initGL(int *argc, char **argv) +bool initGL(int* argc, char** argv) { glutInit(argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE); @@ -1305,11 +1479,10 @@ bool initGL(int *argc, char **argv) // projection glMatrixMode(GL_PROJECTION); glLoadIdentity(); - gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, - 1000.0); + gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 1000.0); glutReshapeFunc(reshape); ImGui::CreateContext(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); (void)io; ImGui::StyleColorsDark(); diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt index b0f28bee..2a5fa72e 100644 --- a/apps/multi_session_registration/CMakeLists.txt +++ b/apps/multi_session_registration/CMakeLists.txt @@ -25,22 +25,22 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/freeglut/include ${LASZIP_INCLUDE_DIR}/LASzip/include) target_link_libraries( multi_session_registration_step_3 - # PRIVATE ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib + # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/multi_session_registration/multi_session_factor_graph.cpp b/apps/multi_session_registration/multi_session_factor_graph.cpp index 64ad853a..6b736c0e 100644 --- a/apps/multi_session_registration/multi_session_factor_graph.cpp +++ b/apps/multi_session_registration/multi_session_factor_graph.cpp @@ -1,19 +1,20 @@ #include "multi_session_factor_graph.h" #include -#include +#include + #include +#include #include -#include -#include #include +#include -bool optimize(std::vector &sessions, const std::vector &edges) +bool optimize(std::vector& sessions, const std::vector& edges) { - for (auto &session : sessions) + for (auto& session : sessions) { // std::cout << session.point_clouds_container.point_clouds.size() << std::endl; - for (auto &pc : session.point_clouds_container.point_clouds) + for (auto& pc : session.point_clouds_container.point_clouds) pc.m_pose_temp = pc.m_pose; } @@ -174,19 +175,20 @@ bool optimize(std::vector &sessions, const std::vector &edges) normalize_angle(all_edges[i].relative_pose_tb.om), normalize_angle(all_edges[i].relative_pose_tb.fi), normalize_angle(all_edges[i].relative_pose_tb.ka)); - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[all_edges[i].index_from].px, - poses[all_edges[i].index_from].py, - poses[all_edges[i].index_from].pz, - normalize_angle(poses[all_edges[i].index_from].om), - normalize_angle(poses[all_edges[i].index_from].fi), - normalize_angle(poses[all_edges[i].index_from].ka), - poses[all_edges[i].index_to].px, - poses[all_edges[i].index_to].py, - poses[all_edges[i].index_to].pz, - normalize_angle(poses[all_edges[i].index_to].om), - normalize_angle(poses[all_edges[i].index_to].fi), - normalize_angle(poses[all_edges[i].index_to].ka)); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); } else if (is_cw) { @@ -210,19 +212,20 @@ bool optimize(std::vector &sessions, const std::vector &edges) normalize_angle(all_edges[i].relative_pose_tb.om), normalize_angle(all_edges[i].relative_pose_tb.fi), normalize_angle(all_edges[i].relative_pose_tb.ka)); - relative_pose_obs_eq_tait_bryan_cw_case1_jacobian(jacobian, - poses[all_edges[i].index_from].px, - poses[all_edges[i].index_from].py, - poses[all_edges[i].index_from].pz, - normalize_angle(poses[all_edges[i].index_from].om), - normalize_angle(poses[all_edges[i].index_from].fi), - normalize_angle(poses[all_edges[i].index_from].ka), - poses[all_edges[i].index_to].px, - poses[all_edges[i].index_to].py, - poses[all_edges[i].index_to].pz, - normalize_angle(poses[all_edges[i].index_to].om), - normalize_angle(poses[all_edges[i].index_to].fi), - normalize_angle(poses[all_edges[i].index_to].ka)); + relative_pose_obs_eq_tait_bryan_cw_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); } int ir = tripletListB.size(); @@ -408,16 +411,15 @@ bool optimize(std::vector &sessions, const std::vector &edges) TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(m_poses[index_pose]); Eigen::Vector3d p_s = pc.local_trajectory[index].m_pose.translation(); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, + pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, gnss.gnss_poses[i].alt - gnss.offset_alt); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, + gnss.gnss_poses[i].alt - gnss.offset_alt); point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, + pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; @@ -441,7 +443,8 @@ bool optimize(std::vector &sessions, const std::vector &edges) tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); - // jacobian3x6 = get_point_to_point_jacobian_tait_bryan(pose_convention, point_clouds_container.point_clouds[i].m_pose, p_s, p_t); + // jacobian3x6 = get_point_to_point_jacobian_tait_bryan(pose_convention, + point_clouds_container.point_clouds[i].m_pose, p_s, p_t); // auto m = pc.m_pose * pc.local_trajectory[index].m_pose; // glVertex3f(m(0, 3), m(1, 3), m(2, 3)); @@ -456,20 +459,41 @@ bool optimize(std::vector &sessions, const std::vector &edges) { for (size_t jj = 0; jj < sessions[j].ground_control_points.gpcs.size(); jj++) { - Eigen::Vector3d p_s = sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].local_trajectory[sessions[j].ground_control_points.gpcs[jj].index_to_node_outer].m_pose.translation(); + Eigen::Vector3d p_s = + sessions[j] + .point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner] + .local_trajectory[sessions[j].ground_control_points.gpcs[jj].index_to_node_outer] + .m_pose.translation(); Eigen::Matrix jacobian; TaitBryanPose pose_s; - pose_s = pose_tait_bryan_from_affine_matrix(sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + pose_s = pose_tait_bryan_from_affine_matrix( + sessions[j].point_clouds_container.point_clouds[sessions[j].ground_control_points.gpcs[jj].index_to_node_inner].m_pose); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(sessions[j].ground_control_points.gpcs[jj].x, sessions[j].ground_control_points.gpcs[jj].y, sessions[j].ground_control_points.gpcs[jj].z + sessions[j].ground_control_points.gpcs[jj].lidar_height_above_ground); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t( + sessions[j].ground_control_points.gpcs[jj].x, + sessions[j].ground_control_points.gpcs[jj].y, + sessions[j].ground_control_points.gpcs[jj].z + sessions[j].ground_control_points.gpcs[jj].lidar_height_above_ground); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = sessions[j].ground_control_points.gpcs[jj].index_to_node_inner * 6 + sums[j] * 6; @@ -484,9 +508,21 @@ bool optimize(std::vector &sessions, const std::vector &edges) } } } - tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_x * sessions[j].ground_control_points.gpcs[jj].sigma_x)) * get_cauchy_w(delta_x, 1)); - tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_y * sessions[j].ground_control_points.gpcs[jj].sigma_y)) * get_cauchy_w(delta_y, 1)); - tripletListP.emplace_back(ir + 2, ir + 2, (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_z * sessions[j].ground_control_points.gpcs[jj].sigma_z)) * get_cauchy_w(delta_z, 1)); + tripletListP.emplace_back( + ir + 0, + ir + 0, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_x * sessions[j].ground_control_points.gpcs[jj].sigma_x)) * + get_cauchy_w(delta_x, 1)); + tripletListP.emplace_back( + ir + 1, + ir + 1, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_y * sessions[j].ground_control_points.gpcs[jj].sigma_y)) * + get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back( + ir + 2, + ir + 2, + (1.0 / (sessions[j].ground_control_points.gpcs[jj].sigma_z * sessions[j].ground_control_points.gpcs[jj].sigma_z)) * + get_cauchy_w(delta_z, 1)); tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); @@ -573,10 +609,9 @@ bool optimize(std::vector &sessions, const std::vector &edges) for (size_t j = 0; j < sessions.size(); j++) { - for (size_t index_pose = 0; index_pose < sessions[j].point_clouds_container.point_clouds.size(); index_pose++) { - const auto &pc = sessions[j].point_clouds_container.point_clouds[index_pose]; + const auto& pc = sessions[j].point_clouds_container.point_clouds[index_pose]; if (!pc.fuse_inclination_from_IMU) { continue; @@ -606,16 +641,42 @@ bool optimize(std::vector &sessions, const std::vector &edges) TaitBryanPose current_pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); Eigen::Matrix delta; - point_to_plane_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 1, 0, 0, // add 0,1,0 - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, // add 0,1,0 + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); Eigen::Matrix delta_jacobian; - point_to_plane_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 1, 0, 0, - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); int ir = tripletListB.size(); int ic = (index_pose + sums[j]) * 6; @@ -627,15 +688,41 @@ bool optimize(std::vector &sessions, const std::vector &edges) tripletListB.emplace_back(ir, 0, delta(0, 0)); /////////////////////////////// - point_to_plane_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 0, 1, 0, - xtg, ytg, ztg, vzx, vzy, vzz); - - point_to_plane_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 0, 1, 0, - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); ir = tripletListB.size(); // ic = index_pose * 6; @@ -654,31 +741,42 @@ bool optimize(std::vector &sessions, const std::vector &edges) for (size_t j = 0; j < sessions.size(); j++) { // CPs - auto &cps = sessions[j].control_points; - auto &point_clouds_container = sessions[j].point_clouds_container; + auto& cps = sessions[j].control_points; + auto& point_clouds_container = sessions[j].point_clouds_container; for (int i = 0; i < cps.cps.size(); i++) { if (!cps.cps[i].is_z_0) { - Eigen::Vector3d p_s(cps.cps[i].x_source_local, - cps.cps[i].y_source_local, cps.cps[i].z_source_local); + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(cps.cps[i].x_target_global, - cps.cps[i].y_target_global, cps.cps[i].z_target_global); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, cps.cps[i].z_target_global); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); @@ -707,30 +805,40 @@ bool optimize(std::vector &sessions, const std::vector &edges) } else { - - Eigen::Vector3d p_s(cps.cps[i].x_source_local, - cps.cps[i].y_source_local, cps.cps[i].z_source_local); + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(cps.cps[i].x_target_global, - cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); - //int ic = cps.cps[i].index_to_pose * 6; + // int ic = cps.cps[i].index_to_pose * 6; int ic = (cps.cps[i].index_to_pose + sums[j]) * 6; - + for (int row = 2; row < 3; row++) { for (int col = 0; col < 6; col++) @@ -741,8 +849,9 @@ bool optimize(std::vector &sessions, const std::vector &edges) } } } - // tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x, 1)); - // tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) * get_cauchy_w(delta_y, 1)); + // tripletListP.emplace_back(ir + 0, ir + 0, (1.0 / (cps.cps[i].sigma_x * cps.cps[i].sigma_x)) * get_cauchy_w(delta_x, + // 1)); tripletListP.emplace_back(ir + 1, ir + 1, (1.0 / (cps.cps[i].sigma_y * cps.cps[i].sigma_y)) * + // get_cauchy_w(delta_y, 1)); tripletListP.emplace_back(ir, ir, (1.0 / (cps.cps[i].sigma_z * cps.cps[i].sigma_z))); // tripletListB.emplace_back(ir, 0, delta_x); @@ -804,7 +913,8 @@ bool optimize(std::vector &sessions, const std::vector &edges) // std::cout << "updates:" << std::endl; // for (size_t i = 0; i < h_x.size(); i += 6) //{ - // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + // << std::endl; // } if (h_x.size() == 6 * poses.size()) @@ -813,7 +923,6 @@ bool optimize(std::vector &sessions, const std::vector &edges) for (size_t i = 0; i < poses.size(); i++) { - TaitBryanPose pose = poses[i]; pose.px += h_x[counter++] * 0.1; pose.py += h_x[counter++] * 0.1; @@ -879,13 +988,20 @@ bool optimize(std::vector &sessions, const std::vector &edges) // if (!sessions[index_trajectory[i]].is_ground_truth) //{ sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose = m_poses[i]; - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose = pose_tait_bryan_from_affine_matrix(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose); - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[0] = sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.px; - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[1] = sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.py; - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[2] = sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.pz; - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[0] = rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.om); - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[1] = rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.fi); - sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[2] = rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.ka); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose = + pose_tait_bryan_from_affine_matrix(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].m_pose); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[0] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.px; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[1] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.py; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_translation[2] = + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.pz; + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[0] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.om); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[1] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.fi); + sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].gui_rotation[2] = + rad2deg(sessions[index_trajectory[i]].point_clouds_container.point_clouds[index].pose.ka); //} index++; } diff --git a/apps/multi_session_registration/multi_session_factor_graph.h b/apps/multi_session_registration/multi_session_factor_graph.h index fdc8ea08..a911ffd1 100644 --- a/apps/multi_session_registration/multi_session_factor_graph.h +++ b/apps/multi_session_registration/multi_session_factor_graph.h @@ -1,5 +1,4 @@ -#ifndef _MULTI_SESSION_FACTOR_GRAPH_H_ -#define _MULTI_SESSION_FACTOR_GRAPH_H_ +#pragma once #include @@ -19,6 +18,4 @@ struct Edge bool is_fixed_ka = false; }; -bool optimize(std::vector &sessions, const std::vector &edges); - -#endif \ No newline at end of file +bool optimize(std::vector& sessions, const std::vector& edges); diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 5b2401b2..9aba0868 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1,18 +1,13 @@ -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif - -// #include - #include #include #include #include #include -#include #include +#include + // #define GLEW_STATIC // #include #include @@ -24,8 +19,8 @@ #include -#include #include +#include #include @@ -37,14 +32,15 @@ #include -#include #include +#include #include #ifdef _WIN32 -#include #include "resource.h" +#include + #endif #include "multi_session_factor_graph.h" @@ -56,103 +52,94 @@ std::vector infoLines = { "", "First step: create project by adding sessions (result of 'multi_view_tls_registration_step_2' program)", "Last step: save project", - "To produce map use 'multi_view_tls_registration_step_2' export functionality"}; + "To produce map use 'multi_view_tls_registration_step_2' export functionality" +}; // App specific shortcuts (Type and Shortcut are just for easy reference) -static const std::vector appShortcuts = { - {"Normal keys", "A", ""}, - {"", "Ctrl+A", "Add session(s)"}, - {"", "B", ""}, - {"", "Ctrl+B", ""}, - {"", "C", ""}, - {"", "Ctrl+C", ""}, - {"", "D", ""}, - {"", "Ctrl+D", ""}, - {"", "E", ""}, - {"", "Ctrl+E", ""}, - {"", "F", ""}, - {"", "Ctrl+F", ""}, - {"", "G", ""}, - {"", "Ctrl+G", ""}, - {"", "H", ""}, - {"", "Ctrl+H", ""}, - {"", "I", ""}, - {"", "Ctrl+I", ""}, - {"", "J", ""}, - {"", "Ctrl+K", ""}, - {"", "K", ""}, - {"", "Ctrl+K", ""}, - {"", "L", ""}, - {"", "Ctrl+L", "Load sessions"}, - {"", "M", ""}, - {"", "Ctrl+M", ""}, - {"", "N", ""}, - {"", "Ctrl+N", ""}, - {"", "O", ""}, - {"", "Ctrl+O", "Open project"}, - {"", "P", ""}, - {"", "Ctrl+P", ""}, - {"", "Q", ""}, - {"", "Ctrl+Q", ""}, - {"", "R", ""}, - {"", "Ctrl+R", "Remove session(s)"}, - {"", "Shift+R", ""}, - {"", "S", ""}, - {"", "Ctrl+S", "Save project"}, - {"", "Ctrl+Shift+S", ""}, - {"", "T", ""}, - {"", "Ctrl+T", ""}, - {"", "U", ""}, - {"", "Ctrl+U", ""}, - {"", "V", ""}, - {"", "Ctrl+V", ""}, - {"", "W", ""}, - {"", "Ctrl+W", ""}, - {"", "X", ""}, - {"", "Ctrl+X", ""}, - {"", "Y", ""}, - {"", "Ctrl+Y", ""}, - {"", "Z", ""}, - {"", "Ctrl+Z", ""}, - {"", "Shift+Z", ""}, - {"", "1-9", ""}, - {"Special keys", "Up arrow", ""}, - {"", "Shift + up arrow", ""}, - {"", "Ctrl + up arrow", ""}, - {"", "Down arrow", ""}, - {"", "Shift + down arrow", ""}, - {"", "Ctrl + down arrow", ""}, - {"", "Left arrow", ""}, - {"", "Shift + left arrow", ""}, - {"", "Ctrl + left arrow", ""}, - {"", "Right arrow", ""}, - {"", "Shift + right arrow", ""}, - {"", "Ctrl + right arrow", ""}, - {"", "Pg down", ""}, - {"", "Pg up", ""}, - {"", "- key", ""}, - {"", "+ key", ""}, - {"Mouse related", "Left click + drag", ""}, - {"", "Right click + drag", "n"}, - {"", "Scroll", ""}, - {"", "Shift + scroll", ""}, - {"", "Shift + drag", ""}, - {"", "Ctrl + left click", ""}, - {"", "Ctrl + right click", ""}, - {"", "Ctrl + middle click", ""}}; - -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; -float m_gizmo[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "Add session(s)" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "Load sessions" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open project" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "Remove session(s)" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "Save project" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; + +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; bool is_decimate = true; double bucket_x = 0.1; @@ -225,7 +212,7 @@ void ndt_gui() if (ImGui::Button("NDT optimization")) { - for (auto &s : sessions) + for (auto& s : sessions) { s.is_gizmo = false; } @@ -442,7 +429,7 @@ void loop_closure_gui() { bool is_gizmo = false; - for (const auto &s : sessions) + for (const auto& s : sessions) { if (s.is_gizmo) is_gizmo = true; @@ -487,8 +474,10 @@ void loop_closure_gui() if (prev_gizmo != edge_gizmo) { - auto m_to = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].m_pose * - affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + auto m_to = sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .m_pose * + affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); m_gizmo[0] = (float)m_to(0, 0); m_gizmo[1] = (float)m_to(1, 0); @@ -555,9 +544,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -574,9 +563,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -593,8 +582,12 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); @@ -637,7 +630,8 @@ void loop_closure_gui() icp.optimization_point_to_point_source_to_target(pcs); - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); + edges[index_active_edge].relative_pose_tb = + pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); */ } else @@ -677,14 +671,21 @@ void loop_closure_gui() icp.optimization_point_to_point_source_to_target(pcs); - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); + edges[index_active_edge].relative_pose_tb = + pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); */ int number_of_iterations = 10; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); @@ -825,7 +826,8 @@ void loop_closure_gui() icp.optimization_point_to_point_source_to_target(pcs); - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); + edges[index_active_edge].relative_pose_tb = + pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); } else { @@ -864,7 +866,8 @@ void loop_closure_gui() icp.optimization_point_to_point_source_to_target(pcs); - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); + edges[index_active_edge].relative_pose_tb = + pose_tait_bryan_from_affine_matrix(pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose); } } } @@ -923,9 +926,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -942,9 +945,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -961,21 +964,30 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); } else { - int number_of_iterations = 30; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); @@ -1031,9 +1043,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -1050,9 +1062,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -1069,21 +1081,30 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); } else { - int number_of_iterations = 30; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); @@ -1138,9 +1159,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -1157,9 +1178,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -1176,8 +1197,12 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1186,13 +1211,18 @@ void loop_closure_gui() } else { - int number_of_iterations = 30; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1249,9 +1279,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -1268,9 +1298,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -1287,8 +1317,12 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1297,13 +1331,18 @@ void loop_closure_gui() } else { - int number_of_iterations = 30; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1360,9 +1399,9 @@ void loop_closure_gui() double y_max = -1000000000000.0; double z_max = -1000000000000.0; - auto &points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - for (const auto &p : points_to.points_local) + for (const auto& p : points_to.points_local) { auto pg = points_to.m_pose * p; if (pg.x() < x_min) @@ -1379,9 +1418,9 @@ void loop_closure_gui() if (pg.z() > z_max) z_max = pg.z(); } - auto &points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; std::vector ground_truth; - for (const auto &p : points_from.points_local) + for (const auto& p : points_from.points_local) { auto pg = points_from.m_pose * p; if (pg.x() > x_min && pg.x() < x_max) @@ -1398,8 +1437,12 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1408,13 +1451,18 @@ void loop_closure_gui() } else { - int number_of_iterations = 30; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds[edges[index_active_edge].index_to].points_local; - std::vector target = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; + std::vector source = + sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + std::vector target = + sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) { @@ -1477,20 +1525,25 @@ void loop_closure_gui() } } -void save_trajectories_to_laz(const Session &session, std::string output_file_name, float curve_consecutive_distance_meters, float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling) +void save_trajectories_to_laz( + const Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling) { std::vector pointcloud; std::vector intensity; std::vector timestamps; float consecutive_distance = 0; - for (auto &p : session.point_clouds_container.point_clouds) + for (auto& p : session.point_clouds_container.point_clouds) { if (p.visible) { for (size_t i = 0; i < p.local_trajectory.size(); i++) { - const auto &pp = p.local_trajectory[i].m_pose.translation(); + const auto& pp = p.local_trajectory[i].m_pose.translation(); Eigen::Vector3d vp; vp = p.m_pose * pp; // + session.point_clouds_container.offset; @@ -1548,13 +1601,20 @@ void save_trajectories_to_laz(const Session &session, std::string output_file_na } } // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt)) - if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z())) + if (!exportLaz( + output_file_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) { std::cout << "problem with saving file: " << output_file_name << std::endl; } } -void createDXFPolyline(const std::string &filename, const std::vector &points) +void createDXFPolyline(const std::string& filename, const std::vector& points) { std::ofstream dxfFile(filename); dxfFile << std::setprecision(20); @@ -1573,21 +1633,18 @@ void createDXFPolyline(const std::string &filename, const std::vector polylinePoints; - for (auto &p : session.point_clouds_container.point_clouds) + for (auto& p : session.point_clouds_container.point_clouds) { if (p.visible) { for (size_t i = 0; i < p.local_trajectory.size(); i++) { - const auto &m = p.local_trajectory[i].m_pose; + const auto& m = p.local_trajectory[i].m_pose; Eigen::Affine3d pose = p.m_pose * m; pose.translation() += session.point_clouds_container.offset; @@ -1680,7 +1742,8 @@ void save_trajectories( outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl; } else - outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; + outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) + << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; } } } @@ -1693,47 +1756,46 @@ void save_trajectories( } } -bool save_project_settings(const std::string &file_name, const ProjectSettings &_project_settings) +bool save_project_settings(const std::string& file_name, const ProjectSettings& _project_settings) { std::cout << "saving file: '" << file_name << "'" << std::endl; nlohmann::json jj; nlohmann::json jsession_file_names; - for (const auto &pc : _project_settings.session_file_names) + for (const auto& pc : _project_settings.session_file_names) { - nlohmann::json jfn{ - {"session_file_name", pc}}; + nlohmann::json jfn{ { "session_file_name", pc } }; jsession_file_names.push_back(jfn); } jj["session_file_names"] = jsession_file_names; nlohmann::json jloop_closure_edges; - for (const auto &edge : edges) + for (const auto& edge : edges) { nlohmann::json jloop_closure_edge{ - {"px", edge.relative_pose_tb.px}, - {"py", edge.relative_pose_tb.py}, - {"pz", edge.relative_pose_tb.pz}, - {"om", edge.relative_pose_tb.om}, - {"fi", edge.relative_pose_tb.fi}, - {"ka", edge.relative_pose_tb.ka}, - {"w_px", edge.relative_pose_tb_weights.px}, - {"w_py", edge.relative_pose_tb_weights.py}, - {"w_pz", edge.relative_pose_tb_weights.pz}, - {"w_om", edge.relative_pose_tb_weights.om}, - {"w_fi", edge.relative_pose_tb_weights.fi}, - {"w_ka", edge.relative_pose_tb_weights.ka}, - {"index_from", edge.index_from}, - {"index_to", edge.index_to}, - {"is_fixed_px", edge.is_fixed_px}, - {"is_fixed_py", edge.is_fixed_py}, - {"is_fixed_pz", edge.is_fixed_pz}, - {"is_fixed_om", edge.is_fixed_om}, - {"is_fixed_fi", edge.is_fixed_fi}, - {"is_fixed_ka", edge.is_fixed_ka}, - {"index_session_from", edge.index_session_from}, - {"index_session_to", edge.index_session_to}, + { "px", edge.relative_pose_tb.px }, + { "py", edge.relative_pose_tb.py }, + { "pz", edge.relative_pose_tb.pz }, + { "om", edge.relative_pose_tb.om }, + { "fi", edge.relative_pose_tb.fi }, + { "ka", edge.relative_pose_tb.ka }, + { "w_px", edge.relative_pose_tb_weights.px }, + { "w_py", edge.relative_pose_tb_weights.py }, + { "w_pz", edge.relative_pose_tb_weights.pz }, + { "w_om", edge.relative_pose_tb_weights.om }, + { "w_fi", edge.relative_pose_tb_weights.fi }, + { "w_ka", edge.relative_pose_tb_weights.ka }, + { "index_from", edge.index_from }, + { "index_to", edge.index_to }, + { "is_fixed_px", edge.is_fixed_px }, + { "is_fixed_py", edge.is_fixed_py }, + { "is_fixed_pz", edge.is_fixed_pz }, + { "is_fixed_om", edge.is_fixed_om }, + { "is_fixed_fi", edge.is_fixed_fi }, + { "is_fixed_ka", edge.is_fixed_ka }, + { "index_session_from", edge.index_session_from }, + { "index_session_to", edge.index_session_to }, }; jloop_closure_edges.push_back(jloop_closure_edge); } @@ -1753,10 +1815,9 @@ void update_timestamp_offset() std::cout << "update_timestamp" << std::endl; time_stamp_offset = 0.0; - for (const auto &s : sessions) + for (const auto& s : sessions) { - if (!s.point_clouds_container.point_clouds.empty() && - !s.point_clouds_container.point_clouds[0].local_trajectory.empty()) + if (!s.point_clouds_container.point_clouds.empty() && !s.point_clouds_container.point_clouds[0].local_trajectory.empty()) { double ts = s.point_clouds_container.point_clouds[0].local_trajectory[0].timestamps.first; if (ts > time_stamp_offset) @@ -1767,19 +1828,19 @@ void update_timestamp_offset() std::cout << "new time_stamp_offset = " << time_stamp_offset << std::endl; } -bool revert(std::vector &sessions) +bool revert(std::vector& sessions) { - for (auto &session : sessions) + for (auto& session : sessions) { - for (auto &pc : session.point_clouds_container.point_clouds) + for (auto& pc : session.point_clouds_container.point_clouds) pc.m_pose = pc.m_pose_temp; } return true; } -bool save_results(std::vector &sessions) +bool save_results(std::vector& sessions) { - for (auto &session : sessions) + for (auto& session : sessions) { if (!session.is_ground_truth) { @@ -1790,7 +1851,7 @@ bool save_results(std::vector &sessions) return true; } -Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking &observation_picking) +Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observation_picking) { const auto laser_beam = GetLaserBeam(x, y); @@ -1808,7 +1869,7 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking &observ return pos; } -bool load_project_settings(const std::string &file_name, ProjectSettings &_project_settings) +bool load_project_settings(const std::string& file_name, ProjectSettings& _project_settings) { std::cout << "Opening project file: '" << file_name << "'\n"; @@ -1824,7 +1885,7 @@ bool load_project_settings(const std::string &file_name, ProjectSettings &_proje std::cout << "Contained sessions:\n"; - for (const auto &fn_json : data["session_file_names"]) + for (const auto& fn_json : data["session_file_names"]) { std::string fn = fn_json["session_file_name"]; _project_settings.session_file_names.push_back(fn); @@ -1835,7 +1896,7 @@ bool load_project_settings(const std::string &file_name, ProjectSettings &_proje } edges.clear(); - for (const auto &edge_json : data["loop_closure_edges"]) + for (const auto& edge_json : data["loop_closure_edges"]) { Edge edge; edge.index_from = edge_json["index_from"]; @@ -1866,8 +1927,7 @@ bool load_project_settings(const std::string &file_name, ProjectSettings &_proje std::cout << "Found " << edges.size() << "edges\nOpening done\n"; return true; - } - catch (std::exception &e) + } catch (std::exception& e) { std::cout << "can't load project settings: " << e.what() << std::endl; return false; @@ -1912,7 +1972,7 @@ void addSession() if (input_file_names.size() > 0) { - for (const auto &input_file_name : input_file_names) + for (const auto& input_file_name : input_file_names) { std::cout << "Adding session file: '" << input_file_name << "'" << std::endl; project_settings.session_file_names.push_back(input_file_name); @@ -1926,13 +1986,13 @@ void addSession() void loadSessions() { sessions.clear(); - for (const auto &ps : project_settings.session_file_names) + for (const auto& ps : project_settings.session_file_names) { Session session; session.load(fs::path(ps).string(), is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset); sessions.push_back(session); if (session.is_ground_truth) - index_gt = sessions.size() - 1; + index_gt = sessions.size() - 1; } loaded_sessions = true; @@ -1964,21 +2024,21 @@ void loadSessions() sessions = sessions_reorder; project_settings.session_file_names = session_file_names_reordered; - for (auto &e : edges) + for (auto& e : edges) { e.index_session_from = map_reorder[e.index_session_from]; e.index_session_to = map_reorder[e.index_session_to]; } std::cout << "sessions reordered, ground truth should be in front" << std::endl; - for (const auto &s : sessions) + for (const auto& s : sessions) { std::cout << "session: '" << s.session_file_name << "' ground truth [" << int(s.is_ground_truth) << "]" << std::endl; } // update time_stamp_offset std::cout << "update time_stamp_offset" << std::endl; - for (const auto &s : sessions) + for (const auto& s : sessions) { if (s.point_clouds_container.point_clouds.size() > 0) { @@ -2032,7 +2092,7 @@ void settings_gui() ImGui::SameLine(); if (ImGui::Button("Set to origin")) { - for (auto &session : sessions) + for (auto& session : sessions) { int index_point_clouds = -1; int index_local_trajectory = -1; @@ -2057,12 +2117,14 @@ void settings_gui() if (index_point_clouds != -1 && index_local_trajectory != -1) { auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; - auto m2 = session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + auto m2 = + session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; auto inv = (m1 * m2).inverse(); for (size_t index = 0; index < session.point_clouds_container.point_clouds.size(); index++) - session.point_clouds_container.point_clouds[index].m_pose = inv * session.point_clouds_container.point_clouds[index].m_pose; + session.point_clouds_container.point_clouds[index].m_pose = + inv * session.point_clouds_container.point_clouds[index].m_pose; } } } @@ -2086,7 +2148,7 @@ void settings_gui() ImGui::SameLine(); ImGui::Checkbox(("Visible##" + std::to_string(i)).c_str(), &sessions[i].visible); } - ImGui::EndDisabled(); + ImGui::EndDisabled(); ImGui::SameLine(); if (ImGui::RadioButton(("Ground truth##" + std::to_string(i)).c_str(), &index_gt, i)) @@ -2112,8 +2174,9 @@ void settings_gui() if (!sessions[i].show_rgb) { ImGui::SameLine(); - ImGui::ColorEdit3(("Color##" + std::to_string(i)).c_str(), (float *)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); - for (auto &pc : sessions[i].point_clouds_container.point_clouds) + ImGui::ColorEdit3( + ("Color##" + std::to_string(i)).c_str(), (float*)&sessions[i].render_color, ImGuiColorEditFlags_NoInputs); + for (auto& pc : sessions[i].point_clouds_container.point_clouds) { pc.render_color[0] = sessions[i].render_color[0]; pc.render_color[1] = sessions[i].render_color[1]; @@ -2123,7 +2186,7 @@ void settings_gui() } else { - for (auto &pc : sessions[i].point_clouds_container.point_clouds) + for (auto& pc : sessions[i].point_clouds_container.point_clouds) pc.show_color = sessions[i].show_rgb; } } @@ -2134,16 +2197,22 @@ void settings_gui() { if (sessions[i].point_clouds_container.point_clouds[0].local_trajectory.size() > 0) { - if (sessions[i].point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1].local_trajectory.size() > 0) + if (sessions[i] + .point_clouds_container.point_clouds[sessions[i].point_clouds_container.point_clouds.size() - 1] + .local_trajectory.size() > 0) { ImGui::SameLine(); 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; - 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); + 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); } } } @@ -2156,7 +2225,7 @@ void settings_gui() { for (size_t i = 0; i < sessions.size(); i++) { - sessions[i].is_ground_truth = (i == index_gt); + sessions[i].is_ground_truth = (i == index_gt); sessions[i].is_gizmo = (i == index_gizmo); } @@ -2265,7 +2334,7 @@ void settings_gui() void display() { - 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); @@ -2288,14 +2357,21 @@ void display() // janusz if (is_loop_closure_gui) { - // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, false, false, 100000); - // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, false, false, 100000); + // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); + // sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, + // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, + // false, false, 100000); if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size()) { - rotation_center.x() = sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); - rotation_center.y() = sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); - rotation_center.z() = sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); + rotation_center.x() = + sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); + rotation_center.y() = + sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); + rotation_center.z() = + sessions[first_session_index].point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); } if (manipulate_active_edge) @@ -2303,7 +2379,8 @@ void display() if (edges.size() > 0) { int index_src = edges[index_active_edge].index_from; - Eigen::Affine3d m_src = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; rotation_center.x() = m_src(0, 3); rotation_center.y() = m_src(1, 3); @@ -2317,9 +2394,13 @@ void display() { if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) { - rotation_center.x() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0, 3); - rotation_center.y() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1, 3); - rotation_center.z() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2, 3); + rotation_center.x() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(0, + 3); rotation_center.y() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(1, + 3); rotation_center.z() = + session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose(2, + 3); } } }*/ @@ -2340,13 +2421,21 @@ void display() } 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); + 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); @@ -2365,12 +2454,11 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - 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())); + 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); @@ -2388,24 +2476,31 @@ void display() int index_src = edges[index_active_edge].index_from; int index_trg = edges[index_active_edge].index_to; - Eigen::Affine3d m_src = sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + Eigen::Affine3d m_src = + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render(m_src, viewer_decimate_point_cloud); - sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render(m_trg, viewer_decimate_point_cloud); + sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).render( + m_src, viewer_decimate_point_cloud); + sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(index_trg).render( + m_trg, viewer_decimate_point_cloud); } } else { ObservationPicking observation_picking; - sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, observation_picking, viewer_decimate_point_cloud, false, false, false, 100000, false); - sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).render(false, observation_picking, viewer_decimate_point_cloud, false, false, false, 100000, false); + sessions[first_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_source) + .render(false, observation_picking, viewer_decimate_point_cloud, false, false, false, 100000, false); + sessions[second_session_index] + .point_clouds_container.point_clouds.at(index_loop_closure_target) + .render(false, observation_picking, viewer_decimate_point_cloud, false, false, false, 100000, false); } // sessions[first_session_index].point_clouds_container.render(); glBegin(GL_LINE_STRIP); - for (auto &pc : sessions[first_session_index].point_clouds_container.point_clouds) + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) { glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); @@ -2413,16 +2508,16 @@ void display() glEnd(); int i = 0; - for (auto &pc : sessions[first_session_index].point_clouds_container.point_clouds) + for (auto& pc : sessions[first_session_index].point_clouds_container.point_clouds) { glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(i).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); i++; } glBegin(GL_LINE_STRIP); - for (auto &pc : sessions[second_session_index].point_clouds_container.point_clouds) + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) { glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); @@ -2430,11 +2525,11 @@ void display() glEnd(); i = 0; - for (auto &pc : sessions[second_session_index].point_clouds_container.point_clouds) + for (auto& pc : sessions[second_session_index].point_clouds_container.point_clouds) { glColor3f(pc.render_color[0], pc.render_color[1], pc.render_color[2]); glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(i).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); i++; } @@ -2457,7 +2552,7 @@ void display() glEnd(); glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(j).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(j).c_str()); } } @@ -2485,16 +2580,31 @@ void display() glEnd(); glRasterPos3f((v1.x() + v2.x()) * 0.5, (v1.y() + v2.y()) * 0.5, (v1.z() + v2.z()) * 0.5 + 10 + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(i).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); } } else { - for (auto &session : sessions) + for (auto& session : sessions) { if (session.visible) { - session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, false, false, false, false, false, false, false, false, false, false, false, false, 10000); + session.point_clouds_container.render( + observation_picking, + viewer_decimate_point_cloud, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + 10000); session.ground_control_points.render(session.point_clouds_container); session.control_points.render(session.point_clouds_container, false); @@ -2523,11 +2633,15 @@ void display() { if (index_local_trajectory < session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory.size()) { - glColor3f(session.point_clouds_container.point_clouds[index_point_clouds].render_color[0], session.point_clouds_container.point_clouds[index_point_clouds].render_color[1], session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]); + glColor3f( + session.point_clouds_container.point_clouds[index_point_clouds].render_color[0], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[1], + session.point_clouds_container.point_clouds[index_point_clouds].render_color[2]); glBegin(GL_LINES); auto m1 = session.point_clouds_container.point_clouds[index_point_clouds].m_pose; - auto m2 = session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; + auto m2 = + session.point_clouds_container.point_clouds[index_point_clouds].local_trajectory[index_local_trajectory].m_pose; auto v1 = (m1 * m2).translation(); @@ -2635,7 +2749,7 @@ void display() // if (all_m_poses.size() > 1) //{ - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); // ImGuizmo ----------------------------------------------- ImGuizmo::BeginFrame(); ImGuizmo::Enable(true); @@ -2650,38 +2764,63 @@ void display() 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); + 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(); + 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); + 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 (size_t 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); + 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); } //} } @@ -2703,30 +2842,44 @@ void display() 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); + 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 (size_t 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); + 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); } } } @@ -2749,18 +2902,31 @@ void display() 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); + 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); + 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 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; + 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); } } @@ -2789,10 +2955,12 @@ void display() 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); + 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); + 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]; @@ -2810,15 +2978,18 @@ void display() 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].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_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); + 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) { @@ -2827,15 +2998,19 @@ void display() { 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.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); } } } @@ -2930,10 +3105,12 @@ else 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); + 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); + 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(); @@ -2954,10 +3131,12 @@ else 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 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); + 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); } }*/ @@ -3036,7 +3215,7 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { @@ -3044,7 +3223,7 @@ else continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); @@ -3052,11 +3231,7 @@ else std::cout << "Saving trajectory to LAZ: " << laz_path << std::endl; - save_trajectories_to_laz(session, - laz_path, - 0.0f, - 0.0f, - false); + save_trajectories_to_laz(session, laz_path, 0.0f, 0.0f, false); } std::cout << "Finished saving all trajectories to .laz files." << std::endl; @@ -3071,7 +3246,7 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { @@ -3079,7 +3254,7 @@ else continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_r.csv")).string(); @@ -3100,31 +3275,27 @@ else << "r10,r11,r12," << "r20,r21,r22\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Matrix3d rot = pose.rotation(); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.first << "," - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << rot(0, 0) << "," << rot(0, 1) << "," << rot(0, 2) << "," - << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," - << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," << std::setprecision(10) + << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) << "," << rot(0, 1) << "," + << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," << rot(2, 0) + << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3136,14 +3307,14 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { std::cerr << "No loaded session for: " << session_path << std::endl; continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_r.csv")).string(); @@ -3157,31 +3328,28 @@ else continue; } - outfile << "timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n"; + outfile << "timestampUnix,x,y,z," + << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Matrix3d rot = pose.rotation(); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.second << "," // Unix timestamp - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << rot(0, 0) << "," << rot(0, 1) << "," << rot(0, 2) << "," - << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," - << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) + << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," + << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3191,14 +3359,14 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { std::cerr << "No loaded session for: " << session_path << std::endl; continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_r.csv")).string(); @@ -3215,30 +3383,26 @@ else outfile << "timestampLidar,timestampUnix,x,y,z," << "r00,r01,r02,r10,r11,r12,r20,r21,r22\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Matrix3d rot = pose.rotation(); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.first << "," // Lidar timestamp + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp << traj.timestamps.second << "," // Unix timestamp - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << rot(0, 0) << "," << rot(0, 1) << "," << rot(0, 2) << "," - << rot(1, 0) << "," << rot(1, 1) << "," << rot(1, 2) << "," - << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << rot(0, 0) + << "," << rot(0, 1) << "," << rot(0, 2) << "," << rot(1, 0) << "," << rot(1, 1) << "," + << rot(1, 2) << "," << rot(2, 0) << "," << rot(2, 1) << "," << rot(2, 2) << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3252,7 +3416,7 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { @@ -3260,7 +3424,7 @@ else continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidar_q.csv")).string(); @@ -3278,29 +3442,26 @@ else outfile << "timestampLidar,x,y,z,qx,qy,qz,qw\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Quaterniond q(pose.rotation()); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.first << "," // Lidar timestamp - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "\n"; + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3312,7 +3473,7 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { @@ -3320,7 +3481,7 @@ else continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampUnix_q.csv")).string(); @@ -3338,29 +3499,26 @@ else outfile << "timestampUnix,x,y,z,qx,qy,qz,qw\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Quaterniond q(pose.rotation()); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.second << "," // Unix timestamp - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "\n"; + outfile << std::fixed << std::setprecision(0) << traj.timestamps.second << "," // Unix timestamp + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3372,7 +3530,7 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { @@ -3380,7 +3538,7 @@ else continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string csv_path = (dir / (folder_name + "_trajectory_timestampLidarUnix_q.csv")).string(); @@ -3398,30 +3556,27 @@ else outfile << "timestampLidar,timestampUnix,x,y,z,qx,qy,qz,qw\n"; - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); Eigen::Quaterniond q(pose.rotation()); - outfile << std::fixed << std::setprecision(0) - << traj.timestamps.first << "," // Lidar timestamp + outfile << std::fixed << std::setprecision(0) << traj.timestamps.first << "," // Lidar timestamp << traj.timestamps.second << "," // Unix timestamp - << std::setprecision(10) - << pos.x() << "," << pos.y() << "," << pos.z() << "," - << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << "\n"; + << std::setprecision(10) << pos.x() << "," << pos.y() << "," << pos.z() << "," << q.x() << "," + << q.y() << "," << q.z() << "," << q.w() << "\n"; } } outfile.close(); std::cout << "Saved: " << csv_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << csv_path << ": " << e.what() << std::endl; } @@ -3433,13 +3588,13 @@ else { for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) { - const auto &session_path = project_settings.session_file_names[i]; + const auto& session_path = project_settings.session_file_names[i]; if (i >= sessions.size()) { std::cerr << "No loaded session for: " << session_path << std::endl; continue; } - Session &session = sessions[i]; + Session& session = sessions[i]; std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); std::string folder_name = dir.filename().string(); std::string txt_path = (dir / (folder_name + "_trajectory_tum.txt")).string(); @@ -3454,11 +3609,11 @@ else continue; } - for (const auto &pc : session.point_clouds_container.point_clouds) + for (const auto& pc : session.point_clouds_container.point_clouds) { if (!pc.visible) continue; - for (const auto &traj : pc.local_trajectory) + for (const auto& traj : pc.local_trajectory) { Eigen::Affine3d pose = pc.m_pose * traj.m_pose; Eigen::Vector3d pos = pose.translation(); @@ -3466,17 +3621,15 @@ else double t_s = static_cast(traj.timestamps.first) / 1e9; - outfile << std::fixed - << std::setprecision(9) << t_s << " " - << std::setprecision(10) << pos.x() << " " << pos.y() << " " << pos.z() << " " - << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n"; + outfile << std::fixed << std::setprecision(9) << t_s << " " << std::setprecision(10) << pos.x() << " " + << pos.y() << " " << pos.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() + << "\n"; } } outfile.close(); std::cout << "Saved: " << txt_path << std::endl; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cerr << "Error creating " << txt_path << ": " << e.what() << std::endl; } @@ -3497,14 +3650,18 @@ else { ImGui::BeginTooltip(); ImGui::Text("Point cloud alignment (registration) algorithm"); - ImGui::Text("Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions rather than raw points"); - ImGui::Text("Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); - ImGui::Text("Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences with continuous probability density functions."); + ImGui::Text("Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions " + "rather than raw points"); + ImGui::Text( + "Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); + ImGui::Text("Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences " + "with continuous probability density functions."); ImGui::EndTooltip(); } // bool prev_is_loop_closure_gui - ImGui::MenuItem("Manual Loop Closure", "Ctrl+L", &is_loop_closure_gui, (number_visible_sessions == 1 || number_visible_sessions == 2)); + ImGui::MenuItem( + "Manual Loop Closure", "Ctrl+L", &is_loop_closure_gui, (number_visible_sessions == 1 || number_visible_sessions == 2)); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Manually connect overlapping scan sections"); @@ -3526,8 +3683,8 @@ else point_size = 10; if (tmp != point_size) - for (auto &session : sessions) - for (auto &point_cloud : session.point_clouds_container.point_clouds) + for (auto& session : sessions) + for (auto& point_cloud : session.point_clouds_container.point_clouds) point_cloud.point_size = point_size; ImGui::Separator(); @@ -3559,7 +3716,7 @@ else ImGui::Text("Colors:"); - ImGui::ColorEdit3("Background", (float *)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); ImGui::Separator(); @@ -3598,7 +3755,9 @@ else ImGui::Dummy(ImVec2(20, 0)); ImGui::SameLine(); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -3713,7 +3872,7 @@ else void mouse(int glut_button, int state, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -3733,12 +3892,22 @@ void mouse(int glut_button, int state, int x, int y) if (!io.WantCaptureMouse) { - if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && !manipulate_active_edge) + if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift) && + !manipulate_active_edge) { - //if (s_loop_closure_gui) + // if (s_loop_closure_gui) if ((sessions.size() > 0) && (number_visible_sessions > 0)) { - getClosestTrajectoriesPoint(sessions, x, y, first_session_index, second_session_index, number_visible_sessions, index_loop_closure_source, index_loop_closure_target, io.KeyShift); + getClosestTrajectoriesPoint( + sessions, + x, + y, + first_session_index, + second_session_index, + number_visible_sessions, + index_loop_closure_source, + index_loop_closure_target, + io.KeyShift); } else { @@ -3778,7 +3947,7 @@ void mouse(int glut_button, int state, int x, int y) } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { try { @@ -3789,17 +3958,14 @@ int main(int argc, char *argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - } - catch (const std::bad_alloc &e) + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 755f1fb4..40532e7f 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -25,23 +25,24 @@ target_include_directories( multi_view_tls_registration_step_2 PRIVATE include ${REPOSITORY_DIRECTORY}/core/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${EXTERNAL_LIBRARIES_DIRECTORY}/include) target_link_libraries( multi_view_tls_registration_step_2 - # PRIVATE ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib + # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib PRIVATE ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp index f53bf262..eaa35911 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp @@ -1,765 +1,859 @@ -#include +#include "multi_view_tls_registration.h" +#include #include #include -#include -#include "multi_view_tls_registration.h" bool has_extension(const std::string file_path, const std::string extension) { - std::string::size_type dot_pos = file_path.find_last_of('.'); - if (dot_pos == std::string::npos) - { - return false; // No extension found - } - std::string file_extension = file_path.substr(dot_pos); - return file_extension == extension; + std::string::size_type dot_pos = file_path.find_last_of('.'); + if (dot_pos == std::string::npos) + { + return false; // No extension found + } + std::string file_extension = file_path.substr(dot_pos); + return file_extension == extension; } -void initial_pose_to_identity(Session &session) +void initial_pose_to_identity(Session& session) { - if (session.point_clouds_container.point_clouds.size() > 0) - { - auto m_inv = session.point_clouds_container.point_clouds[0].m_pose.inverse(); - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - session.point_clouds_container.point_clouds[i].m_pose = m_inv * session.point_clouds_container.point_clouds[i].m_pose; - } - } + if (session.point_clouds_container.point_clouds.size() > 0) + { + auto m_inv = session.point_clouds_container.point_clouds[0].m_pose.inverse(); + for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + session.point_clouds_container.point_clouds[i].m_pose = m_inv * session.point_clouds_container.point_clouds[i].m_pose; + } + } } -void save_intersection(const Session &session, std::string output_las_name, bool xz_intersection, bool yz_intersection, bool xy_intersection, - double intersection_width) +void save_intersection( + const Session& session, + std::string output_las_name, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width) { - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; - - for (auto &p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.points_local.size(); i++) - { - const auto &pp = p.points_local[i]; - Eigen::Vector3d vp; - vp = p.m_pose * pp; // + session.point_clouds_container.offset; - - bool is_inside = false; - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width){ - is_inside = true; - } - } - - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - is_inside = true; - } - } - - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - is_inside = true; - } - } - - if (is_inside) - { - pointcloud.push_back(vp); - if (i < p.intensities.size()) - { - intensity.push_back(p.intensities[i]); - } - else - { - intensity.push_back(0); - } - if (i < p.timestamps.size()) - { - timestamps.push_back(p.timestamps[i]); - } - } - } - } - } - if (!exportLaz(output_las_name, pointcloud, intensity, timestamps, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z())) - { - std::cout << "problem with saving file: " << output_las_name << std::endl; - } + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.points_local.size(); i++) + { + const auto& pp = p.points_local[i]; + Eigen::Vector3d vp; + vp = p.m_pose * pp; // + session.point_clouds_container.offset; + + bool is_inside = false; + if (xz_intersection) + { + if (fabs(vp.y()) < intersection_width) + { + is_inside = true; + } + } + + if (yz_intersection) + { + if (fabs(vp.x()) < intersection_width) + { + is_inside = true; + } + } + + if (xy_intersection) + { + if (fabs(vp.z()) < intersection_width) + { + is_inside = true; + } + } + + if (is_inside) + { + pointcloud.push_back(vp); + if (i < p.intensities.size()) + { + intensity.push_back(p.intensities[i]); + } + else + { + intensity.push_back(0); + } + if (i < p.timestamps.size()) + { + timestamps.push_back(p.timestamps[i]); + } + } + } + } + } + if (!exportLaz( + output_las_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) + { + std::cout << "problem with saving file: " << output_las_name << std::endl; + } } -void save_separately_to_las(const Session &session, fs::path outwd, std::string extension) +void save_separately_to_las(const Session& session, fs::path outwd, std::string extension) { - for (auto &p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - fs::path file_path_in = p.file_name; - fs::path file_path_put = outwd; - file_path_put /= (file_path_in.stem().string() + "_processed" + extension); - std::cout << "file_in: " << file_path_in << std::endl; - std::cout << "file_out: " << file_path_put << std::endl; - std::cout << "start save_processed_pc" << std::endl; - bool compressed = (extension == ".laz"); - save_processed_pc(file_path_in, file_path_put, p.m_pose, session.point_clouds_container.offset, compressed); - std::cout << "save_processed_pc finished" << std::endl; - } - } + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + fs::path file_path_in = p.file_name; + fs::path file_path_put = outwd; + file_path_put /= (file_path_in.stem().string() + "_processed" + extension); + std::cout << "file_in: " << file_path_in << std::endl; + std::cout << "file_out: " << file_path_put << std::endl; + std::cout << "start save_processed_pc" << std::endl; + bool compressed = (extension == ".laz"); + save_processed_pc(file_path_in, file_path_put, p.m_pose, session.point_clouds_container.offset, compressed); + std::cout << "save_processed_pc finished" << std::endl; + } + } } -void save_trajectories_to_laz(const Session &session, std::string output_file_name, float curve_consecutive_distance_meters, float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling) +void save_trajectories_to_laz( + const Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling) { - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; - - float consecutive_distance = 0; - for (auto &p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.local_trajectory.size(); i++) - { - const auto &pp = p.local_trajectory[i].m_pose.translation(); - Eigen::Vector3d vp; - vp = p.m_pose * pp; // + session.point_clouds_container.offset; - - if (i > 0) - { - double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); - consecutive_distance += dist; - } - - bool is_curve = false; - - if (i > 100 && i < p.local_trajectory.size() - 100) - { - Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); - Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); - Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); - - Eigen::Vector3d v1 = position_curr - position_prev; - Eigen::Vector3d v2 = position_next - position_curr; - - if (v1.norm() > 0 && v2.norm() > 0) - { - double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); - - if (angle_deg > 10.0) - { - is_curve = true; - } - } - } - double tol = not_curve_consecutive_distance_meters; - - if (is_curve) - { - tol = curve_consecutive_distance_meters; - } - - if (!is_trajectory_export_downsampling) - { - pointcloud.push_back(vp); - intensity.push_back(0); - timestamps.push_back(p.local_trajectory[i].timestamps.first); - } - else - { - if (consecutive_distance >= tol) - { - consecutive_distance = 0; - pointcloud.push_back(vp); - intensity.push_back(0); - timestamps.push_back(p.local_trajectory[i].timestamps.first); - } - } - } - } - } - // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt)) - if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z())) - { - std::cout << "problem with saving file: " << output_file_name << std::endl; - } + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + float consecutive_distance = 0; + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& pp = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d vp; + vp = p.m_pose * pp; // + session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); + + if (angle_deg > 10.0) + { + is_curve = true; + } + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + { + tol = curve_consecutive_distance_meters; + } + + if (!is_trajectory_export_downsampling) + { + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + } + else + { + if (consecutive_distance >= tol) + { + consecutive_distance = 0; + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + } + } + } + } + } + // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt)) + if (!exportLaz( + output_file_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } } -void createDXFPolyline(const std::string &filename, const std::vector &points) +void createDXFPolyline(const std::string& filename, const std::vector& points) { - std::ofstream dxfFile(filename); - dxfFile << std::setprecision(20); - if (!dxfFile.is_open()) - { - std::cerr << "Failed to open file: " << filename << std::endl; - return; - } - - // DXF header - dxfFile << "0\nSECTION\n2\nHEADER\n0\nENDSEC\n"; - dxfFile << "0\nSECTION\n2\nTABLES\n0\nENDSEC\n"; - - // Start the ENTITIES section - dxfFile << "0\nSECTION\n2\nENTITIES\n"; - - // Start the POLYLINE entity - dxfFile << "0\nPOLYLINE\n"; - dxfFile << "8\n0\n"; // Layer 0 - dxfFile << "66\n1\n"; // Indicates the presence of vertices - dxfFile << "70\n8\n"; // 1 = Open polyline - - // Write the VERTEX entities - for (const auto &point : points) - { - dxfFile << "0\nVERTEX\n"; - dxfFile << "8\n0\n"; // Layer 0 - dxfFile << "10\n" - << point.x() << "\n"; // X coordinate - dxfFile << "20\n" - << point.y() << "\n"; // Y coordinate - dxfFile << "30\n" - << point.z() << "\n"; // Z coordinate - } - - // End the POLYLINE - dxfFile << "0\nSEQEND\n"; - - // End the ENTITIES section - dxfFile << "0\nENDSEC\n"; - - // End the DXF file - dxfFile << "0\nEOF\n"; - - dxfFile.close(); - std::cout << "DXF file created: " << filename << std::endl; + std::ofstream dxfFile(filename); + dxfFile << std::setprecision(20); + if (!dxfFile.is_open()) + { + std::cerr << "Failed to open file: " << filename << std::endl; + return; + } + + // DXF header + dxfFile << "0\nSECTION\n2\nHEADER\n0\nENDSEC\n"; + dxfFile << "0\nSECTION\n2\nTABLES\n0\nENDSEC\n"; + + // Start the ENTITIES section + dxfFile << "0\nSECTION\n2\nENTITIES\n"; + + // Start the POLYLINE entity + dxfFile << "0\nPOLYLINE\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "66\n1\n"; // Indicates the presence of vertices + dxfFile << "70\n8\n"; // 1 = Open polyline + + // Write the VERTEX entities + for (const auto& point : points) + { + dxfFile << "0\nVERTEX\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "10\n" << point.x() << "\n"; // X coordinate + dxfFile << "20\n" << point.y() << "\n"; // Y coordinate + dxfFile << "30\n" << point.z() << "\n"; // Z coordinate + } + + // End the POLYLINE + dxfFile << "0\nSEQEND\n"; + + // End the ENTITIES section + dxfFile << "0\nENDSEC\n"; + + // End the DXF file + dxfFile << "0\nEOF\n"; + + dxfFile.close(); + std::cout << "DXF file created: " << filename << std::endl; } void save_trajectories( - Session &session, std::string output_file_name, float curve_consecutive_distance_meters, - float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling, - bool write_lidar_timestamp, bool write_unix_timestamp, bool use_quaternions, - bool save_to_dxf) + Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling, + bool write_lidar_timestamp, + bool write_unix_timestamp, + bool use_quaternions, + bool save_to_dxf) { - std::ofstream outfile; - if (!save_to_dxf) - { - outfile.open(output_file_name); - } - if (save_to_dxf || outfile.good()) - { - float consecutive_distance = 0; - std::vector polylinePoints; - for (auto &p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.local_trajectory.size(); i++) - { - const auto &m = p.local_trajectory[i].m_pose; - Eigen::Affine3d pose = p.m_pose * m; - pose.translation() += session.point_clouds_container.offset; - - if (i > 0) - { - double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); - consecutive_distance += dist; - } - - bool is_curve = false; - - if (i > 100 && i < p.local_trajectory.size() - 100) - { - Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); - Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); - Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); - - Eigen::Vector3d v1 = position_curr - position_prev; - Eigen::Vector3d v2 = position_next - position_curr; - - if (v1.norm() > 0 && v2.norm() > 0) - { - double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); - - if (angle_deg > 10.0) - { - is_curve = true; - } - } - } - double tol = not_curve_consecutive_distance_meters; - - if (is_curve) - { - tol = curve_consecutive_distance_meters; - } - - if (!is_trajectory_export_downsampling || (is_trajectory_export_downsampling && consecutive_distance >= tol)) - { - if (is_trajectory_export_downsampling) - { - consecutive_distance = 0; - } - if (save_to_dxf) - { - polylinePoints.push_back(pose.translation()); - } - else - { - outfile << std::setprecision(20); - if (write_lidar_timestamp) - { - outfile << p.local_trajectory[i].timestamps.first << ","; - } - if (write_unix_timestamp) - { - outfile << p.local_trajectory[i].timestamps.second << ","; - } - outfile << pose(0, 3) << "," << pose(1, 3) << "," << pose(2, 3) << ","; - if (use_quaternions) - { - Eigen::Quaterniond q(pose.rotation()); - outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl; - } - else - { - outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; - } - } - } - } - } - } - if (!save_to_dxf) - { - outfile.close(); - } - else - { - createDXFPolyline(output_file_name, polylinePoints); - } - } + std::ofstream outfile; + if (!save_to_dxf) + { + outfile.open(output_file_name); + } + if (save_to_dxf || outfile.good()) + { + float consecutive_distance = 0; + std::vector polylinePoints; + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& m = p.local_trajectory[i].m_pose; + Eigen::Affine3d pose = p.m_pose * m; + pose.translation() += session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); + + if (angle_deg > 10.0) + { + is_curve = true; + } + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + { + tol = curve_consecutive_distance_meters; + } + + if (!is_trajectory_export_downsampling || (is_trajectory_export_downsampling && consecutive_distance >= tol)) + { + if (is_trajectory_export_downsampling) + { + consecutive_distance = 0; + } + if (save_to_dxf) + { + polylinePoints.push_back(pose.translation()); + } + else + { + outfile << std::setprecision(20); + if (write_lidar_timestamp) + { + outfile << p.local_trajectory[i].timestamps.first << ","; + } + if (write_unix_timestamp) + { + outfile << p.local_trajectory[i].timestamps.second << ","; + } + outfile << pose(0, 3) << "," << pose(1, 3) << "," << pose(2, 3) << ","; + if (use_quaternions) + { + Eigen::Quaterniond q(pose.rotation()); + outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl; + } + else + { + outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) + << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; + } + } + } + } + } + } + if (!save_to_dxf) + { + outfile.close(); + } + else + { + createDXFPolyline(output_file_name, polylinePoints); + } + } } -void save_scale_board_to_laz(const Session &session, std::string output_file_name, float dec, float side_len) +void save_scale_board_to_laz(const Session& session, std::string output_file_name, float dec, float side_len) { - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; - - float min_x = 1000000000.0; - float max_x = -1000000000.0; - float min_y = 1000000000.0; - float max_y = -1000000000.0; - float min_z = 1000000000.0; - float max_z = -1000000000.0; - float xy_incr = 0.001; - float z = 0.0; - if (side_len < 0.0) - { - for (auto &p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.local_trajectory.size(); i++) - { - const auto &pp = p.local_trajectory[i].m_pose.translation(); - Eigen::Vector3d vp; - vp = p.m_pose * pp; - if (vp.x() < min_x) - { - min_x = vp.x(); - } - if (vp.x() > max_x) - { - max_x = vp.x(); - } - if (vp.y() < min_y) - { - min_y = vp.y(); - } - if (vp.y() > max_y) - { - max_y = vp.y(); - } - if (vp.z() < min_z) - { - min_z = vp.z(); - } - if (vp.z() > max_z) - { - max_z = vp.z(); - } - } - } - } - min_x -= 100.0; - min_y -= 100.0; - max_x += 100.0; - max_y += 100.0; - z = (max_z + min_z) / 2.0; - } - else - { - min_x = -side_len / 2.0; - min_y = -side_len / 2.0; - max_x = side_len / 2.0; - max_y = side_len / 2.0; - xy_incr = 0.2; - z = 0.0; - } - - for (float x = min_x; x <= max_x; x += dec) - { - for (float y = min_y; y <= max_y; y += xy_incr) - { - Eigen::Vector3d vp(x, y, z); - pointcloud.push_back(vp); - intensity.push_back(0); - timestamps.push_back(0.0); - } - } - - for (float y = min_y; y <= max_y; y += dec) - { - for (float x = min_x; x <= max_x; x += xy_incr) - { - Eigen::Vector3d vp(x, y, z); - pointcloud.push_back(vp); - intensity.push_back(0); - timestamps.push_back(0.0); - } - } - - if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z())) - { - std::cout << "problem with saving file: " << output_file_name << std::endl; - } + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + float min_x = 1000000000.0; + float max_x = -1000000000.0; + float min_y = 1000000000.0; + float max_y = -1000000000.0; + float min_z = 1000000000.0; + float max_z = -1000000000.0; + float xy_incr = 0.001; + float z = 0.0; + if (side_len < 0.0) + { + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& pp = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d vp; + vp = p.m_pose * pp; + if (vp.x() < min_x) + { + min_x = vp.x(); + } + if (vp.x() > max_x) + { + max_x = vp.x(); + } + if (vp.y() < min_y) + { + min_y = vp.y(); + } + if (vp.y() > max_y) + { + max_y = vp.y(); + } + if (vp.z() < min_z) + { + min_z = vp.z(); + } + if (vp.z() > max_z) + { + max_z = vp.z(); + } + } + } + } + min_x -= 100.0; + min_y -= 100.0; + max_x += 100.0; + max_y += 100.0; + z = (max_z + min_z) / 2.0; + } + else + { + min_x = -side_len / 2.0; + min_y = -side_len / 2.0; + max_x = side_len / 2.0; + max_y = side_len / 2.0; + xy_incr = 0.2; + z = 0.0; + } + + for (float x = min_x; x <= max_x; x += dec) + { + for (float y = min_y; y <= max_y; y += xy_incr) + { + Eigen::Vector3d vp(x, y, z); + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(0.0); + } + } + + for (float y = min_y; y <= max_y; y += dec) + { + for (float x = min_x; x <= max_x; x += xy_incr) + { + Eigen::Vector3d vp(x, y, z); + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(0.0); + } + } + + if (!exportLaz( + output_file_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } } -std::vector get_matching_files(const std::string &directory, const std::string &pattern) +std::vector get_matching_files(const std::string& directory, const std::string& pattern) { - std::vector matching_files; - std::regex regex_pattern(pattern); - try - { - for (const auto &entry : fs::directory_iterator(directory)) - { - if (entry.is_regular_file()) - { // Ensure it's a regular file - const std::string filename = entry.path().filename().string(); - if (std::regex_match(filename, regex_pattern)) - { - matching_files.push_back(entry.path().string()); - } - } - } - } - catch (const std::exception &e) - { - std::cerr << "Error accessing directory: " << e.what() << std::endl; - } - - return matching_files; + std::vector matching_files; + std::regex regex_pattern(pattern); + try + { + for (const auto& entry : fs::directory_iterator(directory)) + { + if (entry.is_regular_file()) + { // Ensure it's a regular file + const std::string filename = entry.path().filename().string(); + if (std::regex_match(filename, regex_pattern)) + { + matching_files.push_back(entry.path().string()); + } + } + } + } catch (const std::exception& e) + { + std::cerr << "Error accessing directory: " << e.what() << std::endl; + } + + return matching_files; } -void run_multi_view_tls_registration( - std::string input_file_name, TLSRegistration &tls_registration, std::string output_dir) +void run_multi_view_tls_registration(std::string input_file_name, TLSRegistration& tls_registration, std::string output_dir) { - fs::path outwd = fs::path(output_dir); - Session session; - if (!fs::exists(input_file_name)) - { - std::cout << "Provided input path does not exist." << std::endl; - return; - } - if (has_extension(input_file_name, ".json") || has_extension(input_file_name, ".mjs")) - { - std::cout << "Session file: '" << input_file_name << "'" << std::endl; - session.working_directory = fs::path(input_file_name).parent_path().string(); - session.load(fs::path(input_file_name).string(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset); - } - else if (has_extension(input_file_name, ".reg") || has_extension(input_file_name, ".mjp")) - { - std::cout << "RESSO file: '" << input_file_name << "'" << std::endl; - session.working_directory = fs::path(input_file_name).parent_path().string(); - session.point_clouds_container.load(session.working_directory.c_str(), input_file_name.c_str(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, session.load_cache_mode); - } - else if (has_extension(input_file_name, ".txt")) - { - std::cout << "ETH file: '" << input_file_name << "'" << std::endl; - session.working_directory = fs::path(input_file_name).parent_path().string(); - session.point_clouds_container.load_eth(session.working_directory.c_str(), input_file_name.c_str(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z); - } - else if (!has_extension(input_file_name, "")) - { - session.point_clouds_container.point_clouds.clear(); - session.working_directory = fs::path(input_file_name).string(); - std::vector las_files; - std::vector txt_files; - for (const auto &entry : std::filesystem::directory_iterator(input_file_name)) - { - auto file_name = entry.path().string(); - if (has_extension(file_name, ".laz") || (has_extension(file_name, ".las"))) - { - las_files.push_back(file_name); - } - else if (has_extension(file_name, ".txt")) - { - txt_files.push_back(file_name); - } - } - if (las_files.size() > 0) - { - std::cout << "Las/Laz files:" << std::endl; - for (size_t i = 0; i < las_files.size(); i++) - { - std::cout << las_files[i] << std::endl; - } - session.point_clouds_container.load_whu_tls(las_files, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset, session.load_cache_mode); - } - else if (txt_files.size() > 0) - { - std::cout << "txt files:" << std::endl; - for (size_t i = 0; i < txt_files.size(); i++) - { - std::cout << txt_files[i] << std::endl; - } - session.point_clouds_container.load_3DTK_tls(txt_files, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z); - } - else - { - std::cout << "No WHU-TLS / 3DTK files available in the given directory, check path." << std::endl; - return; - } - } - else - { - std::cout << "Session file: '" << input_file_name << "'" << std::endl; - session.load(fs::path(input_file_name).string(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset); - } - std::cout << "loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; - - int number_of_point = 0; - for (const auto &pc : session.point_clouds_container.point_clouds) - { - number_of_point += pc.points_local.size(); - } - session.point_clouds_container.print_point_cloud_dimension(); - - if (tls_registration.resso_upd_init.size() > 0) - { - std::cout << "RESSO file: '" << tls_registration.resso_upd_init << "'" << std::endl; - session.working_directory = fs::path(tls_registration.resso_upd_init).parent_path().string(); - if (!session.point_clouds_container.update_initial_poses_from_RESSO(session.working_directory.c_str(), tls_registration.resso_upd_init.c_str())) - { - std::cout << "check input files" << std::endl; - return; - } - else - { - session.point_clouds_container.initial_poses_file_name = tls_registration.resso_upd_init; - std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; - } - } - if (tls_registration.resso_upd.size() > 0) - { - std::cout << "RESSO file: '" << tls_registration.resso_upd << "'" << std::endl; - session.working_directory = fs::path(tls_registration.resso_upd).parent_path().string(); - if (!session.point_clouds_container.update_poses_from_RESSO(session.working_directory.c_str(), tls_registration.resso_upd.c_str())) - { - std::cout << "check input files" << std::endl; - return; - } - else - { - std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; - session.point_clouds_container.poses_file_name = tls_registration.resso_upd; - } - } - if (tls_registration.resso_upd_inv.size() > 0) - { - std::cout << "RESSO file: '" << tls_registration.resso_upd_inv << "'" << std::endl; - session.working_directory = fs::path(tls_registration.resso_upd_inv).parent_path().string(); - if (!session.point_clouds_container.update_poses_from_RESSO_inverse(session.working_directory.c_str(), tls_registration.resso_upd_inv.c_str())) - { - std::cout << "check input files" << std::endl; - return; - } - else - { - std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; - session.point_clouds_container.poses_file_name = tls_registration.resso_upd_inv; - } - } - - if (tls_registration.initial_pose_to_identity) - { - initial_pose_to_identity(session); - } - - if (tls_registration.use_ndt) - { - if (tls_registration.compute_only_mahalanobis_distance) - { - tls_registration.ndt.optimize(session.point_clouds_container.point_clouds, true, tls_registration.compute_mean_and_cov_for_bucket); - } - else if (tls_registration.use_lie_algebra_left_jacobian_ndt) - { - tls_registration.ndt.optimize_lie_algebra_left_jacobian(session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); - } - else if (tls_registration.use_lie_algebra_right_jacobian_ndt) - { - tls_registration.ndt.optimize_lie_algebra_right_jacobian(session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); - } - else - { - std::cout << "No additional optimization option selected for NDT: using default..." << std::endl; - tls_registration.ndt.optimize(session.point_clouds_container.point_clouds, false, tls_registration.compute_mean_and_cov_for_bucket); - } - } - - if (tls_registration.use_icp) - { - if (tls_registration.point_to_point_source_to_target) - { - tls_registration.icp.optimization_point_to_point_source_to_target(session.point_clouds_container); - } - else if (tls_registration.use_lie_algebra_left_jacobian_icp) - { - tls_registration.icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); - } - else if (tls_registration.use_lie_algebra_right_jacobian_icp) - { - tls_registration.icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); - } - else if (tls_registration.point_to_point_source_to_target_compute_rms) - { - double rms = 0.0; - tls_registration.icp.optimization_point_to_point_source_to_target_compute_rms(session.point_clouds_container, rms); - std::cout << "rms(optimization_point_to_point_source_to_target): " << rms << std::endl; - } - else - { - std::cout << "No optimization option selected for ICP: skipping..." << std::endl; - } - } - - if (tls_registration.use_plane_features) - { - if (tls_registration.point_to_projection_onto_plane_source_to_target) - { - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); - } - else if (tls_registration.use_lie_algebra_left_jacobian_plane_features) - { - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); - } - else if (tls_registration.use_lie_algebra_right_jacobian_plane_features) - { - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); - } - else if (tls_registration.point_to_plane_source_to_target_dot_product) - { - tls_registration.registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); - } - else if (tls_registration.point_to_plane_source_to_target) - { - tls_registration.registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); - } - else if (tls_registration.plane_to_plane_source_to_target) - { - tls_registration.registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); - } - else - { - std::cout << "No optimization option selected for the plane features: skipping..." << std::endl; - } - } - - // TODO: add with GTSAM and with MANIF - if (tls_registration.use_pgslam) - { - tls_registration.pose_graph_slam.ndt_bucket_size[0] = tls_registration.ndt.bucket_size[0]; - tls_registration.pose_graph_slam.ndt_bucket_size[1] = tls_registration.ndt.bucket_size[1]; - tls_registration.pose_graph_slam.ndt_bucket_size[2] = tls_registration.ndt.bucket_size[2]; - tls_registration.pose_graph_slam.optimize(session.point_clouds_container); - } - - if (output_dir.length() > 0) - { - if (session.point_clouds_container.initial_poses_file_name.empty() && tls_registration.save_initial_poses) - { - std::string initial_poses_file_name = (outwd / "session_step2_ini_poses.mri").string(); - std::cout << "saving initial poses to: " << initial_poses_file_name << std::endl; - session.point_clouds_container.save_poses(initial_poses_file_name, false); - } - - if (session.point_clouds_container.poses_file_name.empty() && tls_registration.save_poses) - { - std::string poses_file_name = (outwd / "session_step2_poses.mrp").string(); - std::cout << "saving poses to: " << poses_file_name << std::endl; - session.point_clouds_container.save_poses(poses_file_name, false); - } - - session.save( - (outwd / "session_step2.mjs").string(), session.point_clouds_container.poses_file_name, - session.point_clouds_container.initial_poses_file_name, false); - std::cout << "saving result to: " << session.point_clouds_container.poses_file_name << std::endl; - session.point_clouds_container.save_poses(fs::path(session.point_clouds_container.poses_file_name).string(), false); - } - - if (tls_registration.save_laz) - { - save_all_to_las(session, (outwd / "all_step_2.laz").string(), false, true); - } - if (tls_registration.save_las) - { - save_all_to_las(session, (outwd / "all_step_2.las").string(), false, true); - } - if (tls_registration.save_as_separate_las) - { - save_separately_to_las(session, outwd, ".las"); - } - if (tls_registration.save_as_separate_laz) - { - save_separately_to_las(session, outwd, ".laz"); - } - - if (tls_registration.save_trajectories_laz) - { - save_trajectories_to_laz(session, (outwd / "trajectories.laz").string(), tls_registration.curve_consecutive_distance_meters, - tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling); - } - - if (tls_registration.save_gnss_laz) - { - tls_registration.gnss.save_to_laz((outwd / "gnss.laz").string(), session.point_clouds_container.offset.x(), - session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); - } - - if (tls_registration.save_scale_board_laz) - { - save_scale_board_to_laz(session, (outwd / "scale_board.laz").string(), tls_registration.scale_board_dec, tls_registration.scale_board_side_len); - } - - if (tls_registration.save_trajectories_csv) - { - save_trajectories( - session, (outwd / "trajectories.csv").string(), tls_registration.curve_consecutive_distance_meters, - tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, - tls_registration.write_lidar_timestamp, tls_registration.write_unix_timestamp, - tls_registration.use_quaternions, false); - } - if (tls_registration.save_trajectories_dxf) - { - save_trajectories( - session, (outwd / "trajectories.dxf").string(), tls_registration.curve_consecutive_distance_meters, - tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, - tls_registration.write_lidar_timestamp, tls_registration.write_unix_timestamp, - tls_registration.use_quaternions, true); - } + fs::path outwd = fs::path(output_dir); + Session session; + if (!fs::exists(input_file_name)) + { + std::cout << "Provided input path does not exist." << std::endl; + return; + } + if (has_extension(input_file_name, ".json") || has_extension(input_file_name, ".mjs")) + { + std::cout << "Session file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.load( + fs::path(input_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset); + } + else if (has_extension(input_file_name, ".reg") || has_extension(input_file_name, ".mjp")) + { + std::cout << "RESSO file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.point_clouds_container.load( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + session.load_cache_mode); + } + else if (has_extension(input_file_name, ".txt")) + { + std::cout << "ETH file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.point_clouds_container.load_eth( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z); + } + else if (!has_extension(input_file_name, "")) + { + session.point_clouds_container.point_clouds.clear(); + session.working_directory = fs::path(input_file_name).string(); + std::vector las_files; + std::vector txt_files; + for (const auto& entry : std::filesystem::directory_iterator(input_file_name)) + { + auto file_name = entry.path().string(); + if (has_extension(file_name, ".laz") || (has_extension(file_name, ".las"))) + { + las_files.push_back(file_name); + } + else if (has_extension(file_name, ".txt")) + { + txt_files.push_back(file_name); + } + } + if (las_files.size() > 0) + { + std::cout << "Las/Laz files:" << std::endl; + for (size_t i = 0; i < las_files.size(); i++) + { + std::cout << las_files[i] << std::endl; + } + session.point_clouds_container.load_whu_tls( + las_files, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset, + session.load_cache_mode); + } + else if (txt_files.size() > 0) + { + std::cout << "txt files:" << std::endl; + for (size_t i = 0; i < txt_files.size(); i++) + { + std::cout << txt_files[i] << std::endl; + } + session.point_clouds_container.load_3DTK_tls( + txt_files, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z); + } + else + { + std::cout << "No WHU-TLS / 3DTK files available in the given directory, check path." << std::endl; + return; + } + } + else + { + std::cout << "Session file: '" << input_file_name << "'" << std::endl; + session.load( + fs::path(input_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset); + } + std::cout << "loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + + int number_of_point = 0; + for (const auto& pc : session.point_clouds_container.point_clouds) + { + number_of_point += pc.points_local.size(); + } + session.point_clouds_container.print_point_cloud_dimension(); + + if (tls_registration.resso_upd_init.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd_init << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd_init).parent_path().string(); + if (!session.point_clouds_container.update_initial_poses_from_RESSO( + session.working_directory.c_str(), tls_registration.resso_upd_init.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + session.point_clouds_container.initial_poses_file_name = tls_registration.resso_upd_init; + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + } + } + if (tls_registration.resso_upd.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd).parent_path().string(); + if (!session.point_clouds_container.update_poses_from_RESSO(session.working_directory.c_str(), tls_registration.resso_upd.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + session.point_clouds_container.poses_file_name = tls_registration.resso_upd; + } + } + if (tls_registration.resso_upd_inv.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd_inv << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd_inv).parent_path().string(); + if (!session.point_clouds_container.update_poses_from_RESSO_inverse( + session.working_directory.c_str(), tls_registration.resso_upd_inv.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + session.point_clouds_container.poses_file_name = tls_registration.resso_upd_inv; + } + } + + if (tls_registration.initial_pose_to_identity) + { + initial_pose_to_identity(session); + } + + if (tls_registration.use_ndt) + { + if (tls_registration.compute_only_mahalanobis_distance) + { + tls_registration.ndt.optimize( + session.point_clouds_container.point_clouds, true, tls_registration.compute_mean_and_cov_for_bucket); + } + else if (tls_registration.use_lie_algebra_left_jacobian_ndt) + { + tls_registration.ndt.optimize_lie_algebra_left_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + else if (tls_registration.use_lie_algebra_right_jacobian_ndt) + { + tls_registration.ndt.optimize_lie_algebra_right_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + else + { + std::cout << "No additional optimization option selected for NDT: using default..." << std::endl; + tls_registration.ndt.optimize( + session.point_clouds_container.point_clouds, false, tls_registration.compute_mean_and_cov_for_bucket); + } + } + + if (tls_registration.use_icp) + { + if (tls_registration.point_to_point_source_to_target) + { + tls_registration.icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_left_jacobian_icp) + { + tls_registration.icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_right_jacobian_icp) + { + tls_registration.icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + } + else if (tls_registration.point_to_point_source_to_target_compute_rms) + { + double rms = 0.0; + tls_registration.icp.optimization_point_to_point_source_to_target_compute_rms(session.point_clouds_container, rms); + std::cout << "rms(optimization_point_to_point_source_to_target): " << rms << std::endl; + } + else + { + std::cout << "No optimization option selected for ICP: skipping..." << std::endl; + } + } + + if (tls_registration.use_plane_features) + { + if (tls_registration.point_to_projection_onto_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target( + session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_left_jacobian_plane_features) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_right_jacobian_plane_features) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); + } + else if (tls_registration.point_to_plane_source_to_target_dot_product) + { + tls_registration.registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + } + else if (tls_registration.point_to_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + } + else if (tls_registration.plane_to_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + } + else + { + std::cout << "No optimization option selected for the plane features: skipping..." << std::endl; + } + } + + // TODO: add with GTSAM and with MANIF + if (tls_registration.use_pgslam) + { + tls_registration.pose_graph_slam.ndt_bucket_size[0] = tls_registration.ndt.bucket_size[0]; + tls_registration.pose_graph_slam.ndt_bucket_size[1] = tls_registration.ndt.bucket_size[1]; + tls_registration.pose_graph_slam.ndt_bucket_size[2] = tls_registration.ndt.bucket_size[2]; + tls_registration.pose_graph_slam.optimize(session.point_clouds_container); + } + + if (output_dir.length() > 0) + { + if (session.point_clouds_container.initial_poses_file_name.empty() && tls_registration.save_initial_poses) + { + std::string initial_poses_file_name = (outwd / "session_step2_ini_poses.mri").string(); + std::cout << "saving initial poses to: " << initial_poses_file_name << std::endl; + session.point_clouds_container.save_poses(initial_poses_file_name, false); + } + + if (session.point_clouds_container.poses_file_name.empty() && tls_registration.save_poses) + { + std::string poses_file_name = (outwd / "session_step2_poses.mrp").string(); + std::cout << "saving poses to: " << poses_file_name << std::endl; + session.point_clouds_container.save_poses(poses_file_name, false); + } + + session.save( + (outwd / "session_step2.mjs").string(), + session.point_clouds_container.poses_file_name, + session.point_clouds_container.initial_poses_file_name, + false); + std::cout << "saving result to: " << session.point_clouds_container.poses_file_name << std::endl; + session.point_clouds_container.save_poses(fs::path(session.point_clouds_container.poses_file_name).string(), false); + } + + if (tls_registration.save_laz) + { + save_all_to_las(session, (outwd / "all_step_2.laz").string(), false, true); + } + if (tls_registration.save_las) + { + save_all_to_las(session, (outwd / "all_step_2.las").string(), false, true); + } + if (tls_registration.save_as_separate_las) + { + save_separately_to_las(session, outwd, ".las"); + } + if (tls_registration.save_as_separate_laz) + { + save_separately_to_las(session, outwd, ".laz"); + } + + if (tls_registration.save_trajectories_laz) + { + save_trajectories_to_laz( + session, + (outwd / "trajectories.laz").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling); + } + + if (tls_registration.save_gnss_laz) + { + tls_registration.gnss.save_to_laz( + (outwd / "gnss.laz").string(), + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); + } + + if (tls_registration.save_scale_board_laz) + { + save_scale_board_to_laz( + session, (outwd / "scale_board.laz").string(), tls_registration.scale_board_dec, tls_registration.scale_board_side_len); + } + + if (tls_registration.save_trajectories_csv) + { + save_trajectories( + session, + (outwd / "trajectories.csv").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + tls_registration.write_lidar_timestamp, + tls_registration.write_unix_timestamp, + tls_registration.use_quaternions, + false); + } + if (tls_registration.save_trajectories_dxf) + { + save_trajectories( + session, + (outwd / "trajectories.dxf").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + tls_registration.write_lidar_timestamp, + tls_registration.write_unix_timestamp, + tls_registration.use_quaternions, + true); + } } \ No newline at end of file diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.h b/apps/multi_view_tls_registration/multi_view_tls_registration.h index 9a31c888..2f858dfd 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.h +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.h @@ -1,166 +1,180 @@ -#ifndef _TLS_REGISTRATION_H_ -#define _TLS_REGISTRATION_H_ +#pragma once -#include +#include +#include #include -#include -#include +#include #include -#include -#include #include -#include +#include +#include +#include namespace fs = std::filesystem; struct TLSRegistration { - // NDT - bool use_ndt = false; - NDT ndt; - bool compute_only_mahalanobis_distance = false; - bool compute_mean_and_cov_for_bucket = false; - bool use_lie_algebra_left_jacobian_ndt = false; - bool use_lie_algebra_right_jacobian_ndt = false; - void set_zoller_frohlich_tls_imager_5006i_errors() - { - ndt.sigma_r = 0.0068; - ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; - ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; - } - void set_zoller_frohlich_tls_imager_5010c_errors() - { - ndt.sigma_r = 0.01; - ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; - ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; - } - void set_zoller_frohlich_tls_imager_5016_errors() - { - ndt.sigma_r = 0.00025; - ndt.sigma_polar_angle = 0.004 / 180.0 * M_PI; - ndt.sigma_azimuthal_angle = 0.004 / 180.0 * M_PI; - } - void set_faro_focus3d_errors() - { - ndt.sigma_r = 0.001; - ndt.sigma_polar_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; - ndt.sigma_azimuthal_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; - } - void set_leica_scanstation_c5_c10_errors() - { - ndt.sigma_r = 0.006; - ndt.sigma_polar_angle = 0.00006; - ndt.sigma_azimuthal_angle = 0.00006; - } - void set_riegl_vz400_errors() - { - ndt.sigma_r = 0.005; - ndt.sigma_polar_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence - ndt.sigma_azimuthal_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence - } - void set_leica_hds6100_errors() - { - ndt.sigma_r = 0.009; - ndt.sigma_polar_angle = 0.000125; - ndt.sigma_azimuthal_angle = 0.000125; - } - void set_leica_p40_errors() - { - ndt.sigma_r = 0.0012; - ndt.sigma_polar_angle = 8.0 / 3600; - ndt.sigma_azimuthal_angle = 8.0 / 3600; - } - - void set_livox_mid360_errors() - { - ndt.sigma_r = 0.02; - ndt.sigma_polar_angle = 0.15/ 180.0 * M_PI; - ndt.sigma_azimuthal_angle = 0.15 / 180.0 * M_PI; - } - - // ICP - bool use_icp = false; - ICP icp; - bool point_to_point_source_to_target = true; - bool use_lie_algebra_left_jacobian_icp = false; - bool use_lie_algebra_right_jacobian_icp = false; - bool point_to_point_source_to_target_compute_rms = false; - - // Plane features - bool use_plane_features = false; - RegistrationPlaneFeature registration_plane_feature; - bool point_to_projection_onto_plane_source_to_target = true; - bool use_lie_algebra_left_jacobian_plane_features = false; - bool use_lie_algebra_right_jacobian_plane_features = false; - bool point_to_plane_source_to_target_dot_product = false; - bool point_to_plane_source_to_target = false; - bool plane_to_plane_source_to_target = false; - - // PGSLAM - bool use_pgslam = false; - PoseGraphSLAM pose_graph_slam; - - // GNSS - GNSS gnss; - - // Loading - bool calculate_offset; // Whether to calculate offset to point cloud on loading - bool is_decimate = true; // Whether to decimate point clouds on loading - double bucket_x = 0.1; // Bucket size for decimation in x dimension - double bucket_y = 0.1; // Bucket size for decimation in y dimension - double bucket_z = 0.1; // Bucket size for decimation in z dimension - std::string resso_upd_init = ""; // Path to RESSO initial poses - std::string resso_upd = ""; // Path to RESSO poses - std::string resso_upd_inv = ""; // Path to RESSO inverse poses - - // Registration - bool initial_pose_to_identity = true; // Whether to set first pose as identity and recalculate the rest relatively to it - - // Export - bool save_las = false; // Save resulting point cloud as las - bool save_laz = false; // Save resulting point cloud as laz - bool save_as_separate_las = false; // Whether to save all scans as separate global scans in las format - bool save_as_separate_laz = false; // Whether to save all scans as separate global scans in laz format - bool save_trajectories_laz = false; // Save laz with all trajectories - bool save_gnss_laz = false; // Save laz with GNSS data - bool save_scale_board_laz = false; // Save laz with scale board - float scale_board_dec = 0.1; // Decimation for scale board laz export - float scale_board_side_len = -1.0; // Range covered in x and y dimensions - bool save_initial_poses = false; // Path where initial poses are saved - bool save_poses = false; // Path where poses are saved - bool is_trajectory_export_downsampling = false; // Whether to downsample trajectory on export - float curve_consecutive_distance_meters = 1.0f; // Meters after which trajectory point is saved (if downsampling and curve is detected) - float not_curve_consecutive_distance_meters = 0.05f; // Meters after which trajectory point is saved (if downsampling and no curve detected) - bool save_trajectories_csv = false; // Save trajectories as csv - bool save_trajectories_dxf = false; // Save trajectories as dxf - bool write_lidar_timestamp = true; // Whether lidar timestamp is wrriten to csv trajectory output - bool write_unix_timestamp = false; // Whether unix timestamp is written to csv trajectory - bool use_quaternions = true; // Whether quaternions are used when writing csv trajectory + // NDT + bool use_ndt = false; + NDT ndt; + bool compute_only_mahalanobis_distance = false; + bool compute_mean_and_cov_for_bucket = false; + bool use_lie_algebra_left_jacobian_ndt = false; + bool use_lie_algebra_right_jacobian_ndt = false; + void set_zoller_frohlich_tls_imager_5006i_errors() + { + ndt.sigma_r = 0.0068; + ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; + } + void set_zoller_frohlich_tls_imager_5010c_errors() + { + ndt.sigma_r = 0.01; + ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; + } + void set_zoller_frohlich_tls_imager_5016_errors() + { + ndt.sigma_r = 0.00025; + ndt.sigma_polar_angle = 0.004 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.004 / 180.0 * M_PI; + } + void set_faro_focus3d_errors() + { + ndt.sigma_r = 0.001; + ndt.sigma_polar_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; + } + void set_leica_scanstation_c5_c10_errors() + { + ndt.sigma_r = 0.006; + ndt.sigma_polar_angle = 0.00006; + ndt.sigma_azimuthal_angle = 0.00006; + } + void set_riegl_vz400_errors() + { + ndt.sigma_r = 0.005; + ndt.sigma_polar_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence + ndt.sigma_azimuthal_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence + } + void set_leica_hds6100_errors() + { + ndt.sigma_r = 0.009; + ndt.sigma_polar_angle = 0.000125; + ndt.sigma_azimuthal_angle = 0.000125; + } + void set_leica_p40_errors() + { + ndt.sigma_r = 0.0012; + ndt.sigma_polar_angle = 8.0 / 3600; + ndt.sigma_azimuthal_angle = 8.0 / 3600; + } + + void set_livox_mid360_errors() + { + ndt.sigma_r = 0.02; + ndt.sigma_polar_angle = 0.15 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.15 / 180.0 * M_PI; + } + + // ICP + bool use_icp = false; + ICP icp; + bool point_to_point_source_to_target = true; + bool use_lie_algebra_left_jacobian_icp = false; + bool use_lie_algebra_right_jacobian_icp = false; + bool point_to_point_source_to_target_compute_rms = false; + + // Plane features + bool use_plane_features = false; + RegistrationPlaneFeature registration_plane_feature; + bool point_to_projection_onto_plane_source_to_target = true; + bool use_lie_algebra_left_jacobian_plane_features = false; + bool use_lie_algebra_right_jacobian_plane_features = false; + bool point_to_plane_source_to_target_dot_product = false; + bool point_to_plane_source_to_target = false; + bool plane_to_plane_source_to_target = false; + + // PGSLAM + bool use_pgslam = false; + PoseGraphSLAM pose_graph_slam; + + // GNSS + GNSS gnss; + + // Loading + bool calculate_offset; // Whether to calculate offset to point cloud on loading + bool is_decimate = true; // Whether to decimate point clouds on loading + double bucket_x = 0.1; // Bucket size for decimation in x dimension + double bucket_y = 0.1; // Bucket size for decimation in y dimension + double bucket_z = 0.1; // Bucket size for decimation in z dimension + std::string resso_upd_init = ""; // Path to RESSO initial poses + std::string resso_upd = ""; // Path to RESSO poses + std::string resso_upd_inv = ""; // Path to RESSO inverse poses + + // Registration + bool initial_pose_to_identity = true; // Whether to set first pose as identity and recalculate the rest relatively to it + + // Export + bool save_las = false; // Save resulting point cloud as las + bool save_laz = false; // Save resulting point cloud as laz + bool save_as_separate_las = false; // Whether to save all scans as separate global scans in las format + bool save_as_separate_laz = false; // Whether to save all scans as separate global scans in laz format + bool save_trajectories_laz = false; // Save laz with all trajectories + bool save_gnss_laz = false; // Save laz with GNSS data + bool save_scale_board_laz = false; // Save laz with scale board + float scale_board_dec = 0.1; // Decimation for scale board laz export + float scale_board_side_len = -1.0; // Range covered in x and y dimensions + bool save_initial_poses = false; // Path where initial poses are saved + bool save_poses = false; // Path where poses are saved + bool is_trajectory_export_downsampling = false; // Whether to downsample trajectory on export + float curve_consecutive_distance_meters = 1.0f; // Meters after which trajectory point is saved (if downsampling and curve is detected) + float not_curve_consecutive_distance_meters = + 0.05f; // Meters after which trajectory point is saved (if downsampling and no curve detected) + bool save_trajectories_csv = false; // Save trajectories as csv + bool save_trajectories_dxf = false; // Save trajectories as dxf + bool write_lidar_timestamp = true; // Whether lidar timestamp is wrriten to csv trajectory output + bool write_unix_timestamp = false; // Whether unix timestamp is written to csv trajectory + bool use_quaternions = true; // Whether quaternions are used when writing csv trajectory }; bool has_extension(const std::string file_path, const std::string extension); -void initial_pose_to_identity(Session &session); +void initial_pose_to_identity(Session& session); -void save_intersection(const Session &session, std::string output_las_name, bool xz_intersection, bool yz_intersection, bool xy_intersection, double intersection_width); +void save_intersection( + const Session& session, + std::string output_las_name, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width); -void save_separately_to_las(const Session &session, fs::path outwd, std::string extension = ".las"); +void save_separately_to_las(const Session& session, fs::path outwd, std::string extension = ".las"); -void save_trajectories_to_laz(const Session &session, std::string output_file_name, float curve_consecutive_distance_meters, float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling); +void save_trajectories_to_laz( + const Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling); -void save_scale_board_to_laz(const Session &session, std::string output_file_name, float dec, float side_len = -1.0); +void save_scale_board_to_laz(const Session& session, std::string output_file_name, float dec, float side_len = -1.0); -void createDXFPolyline(const std::string &filename, const std::vector &points); +void createDXFPolyline(const std::string& filename, const std::vector& points); // void load_available_geo_points(Session& session, std::string input_file_name); void save_trajectories( - Session &session, std::string output_file_name, float curve_consecutive_distance_meters, - float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling, - bool write_lidar_timestamp = true, bool write_unix_timestamp = false, bool use_quaternions = true, - bool save_to_dxf = false); - -void run_multi_view_tls_registration( - std::string input_file_name, TLSRegistration &tls_registration, std::string output_dir = ""); -#endif \ No newline at end of file + Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling, + bool write_lidar_timestamp = true, + bool write_unix_timestamp = false, + bool use_quaternions = true, + bool save_to_dxf = false); + +void run_multi_view_tls_registration(std::string input_file_name, TLSRegistration& tls_registration, std::string output_dir = ""); 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 cfca3b5c..c010159b 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 @@ -1,30 +1,25 @@ -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif #include +#include + #include #include #include -#include #include -//#define GLEW_STATIC -//#include -#include +#include #include #include #include -#include #include +#include +#include +#include #include #include -#include -#include -#include #include @@ -32,27 +27,28 @@ #include -#include #include #include +#include -#include -#include "multi_view_tls_registration.h" #include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include "multi_view_tls_registration.h" +#include #include -#include #include +#include #include -#include -#include "wgs84_do_puwg92.h" #include "WGS84toCartesian.hpp" +#include "wgs84_do_puwg92.h" +#include #ifdef _WIN32 -#include #include "resource.h" +#include + #endif /////////////////////////////////////////////////////////////////////////////////// @@ -73,89 +69,87 @@ std::vector infoLines = { "LAZ files are the product of MANDEYE process (open them with Cloud Compare)", }; -//App specific shortcuts (Type and Shortcut are just for easy reference) -static const std::vector appShortcuts = { - {"Normal keys", "A", ""}, - {"", "Ctrl+A", "point cloud Alignment"}, - {"", "B", ""}, - {"", "Ctrl+B", ""}, - {"", "C", ""}, - {"", "Ctrl+C", "Control points"}, - {"", "D", ""}, - {"", "Ctrl+D", ""}, - {"", "E", ""}, - {"", "Ctrl+E", "lio segments Editor"}, - {"", "F", ""}, - {"", "Ctrl+F", ""}, - {"", "G", ""}, - {"", "Ctrl+G", "Ground control points"}, - {"", "H", ""}, - {"", "Ctrl+H", ""}, - {"", "I", ""}, - {"", "Ctrl+I", ""}, - {"", "J", ""}, - {"", "Ctrl+K", ""}, - {"", "K", ""}, - {"", "Ctrl+K", ""}, - {"", "L", ""}, - {"", "Ctrl+L", "manual Loop closure"}, - {"", "M", ""}, - {"", "Ctrl+M", ""}, - {"", "N", ""}, - {"", "Ctrl+N", ""}, - {"", "O", ""}, - {"", "Ctrl+O", "Open session"}, - {"", "P", ""}, - {"", "Ctrl+P", "Pose graph slam"}, - {"", "Q", ""}, - {"", "Ctrl+Q", ""}, - {"", "R", ""}, - {"", "Ctrl+R", "Random cloud colors"}, - {"", "Shift+R", ""}, - {"", "S", ""}, - {"", "Ctrl+S", "Save session"}, - {"", "Ctrl+Shift+S", "Save subsession"}, - {"", "T", ""}, - {"", "Ctrl+T", ""}, - {"", "U", ""}, - {"", "Ctrl+U", ""}, - {"", "V", ""}, - {"", "Ctrl+V", ""}, - {"", "W", ""}, - {"", "Ctrl+W", ""}, - {"", "X", ""}, - {"", "Ctrl+X", ""}, - {"", "Y", ""}, - {"", "Ctrl+Y", ""}, - {"", "Z", ""}, - {"", "Ctrl+Z", ""}, - {"", "Shift+Z", ""}, - {"", "1-9", ""}, - {"Special keys", "Up arrow", ""}, - {"", "Shift + up arrow", ""}, - {"", "Ctrl + up arrow", ""}, - {"", "Down arrow", ""}, - {"", "Shift + down arrow", ""}, - {"", "Ctrl + down arrow", ""}, - {"", "Left arrow", ""}, - {"", "Shift + left arrow", ""}, - {"", "Ctrl + left arrow", ""}, - {"", "Right arrow", ""}, - {"", "Shift + right arrow", ""}, - {"", "Ctrl + right arrow", ""}, - {"", "Pg down", ""}, - {"", "Pg up", ""}, - {"", "- key", ""}, - {"", "+ key", ""}, - {"Mouse related", "Left click + drag", ""}, - {"", "Right click + drag", "n"}, - {"", "Scroll", ""}, - {"", "Shift + scroll", ""}, - {"", "Shift + drag", ""}, - {"", "Ctrl + left click", ""}, - {"", "Ctrl + right click", ""}, - {"", "Ctrl + middle click", ""} -}; +// App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "point cloud Alignment" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "Control points" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "lio segments Editor" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "Ground control points" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "manual Loop closure" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open session" }, + { "", "P", "" }, + { "", "Ctrl+P", "Pose graph slam" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "Random cloud colors" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "Save session" }, + { "", "Ctrl+Shift+S", "Save subsession" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; namespace fs = std::filesystem; @@ -164,7 +158,7 @@ static bool show_another_window = false; bool gnssWithOffset = false; -//radio button selectors +// radio button selectors static int NDTnomSelection = 0; static int NDTpeSelection = 0; static int NDT3dSelection = 0; @@ -181,10 +175,10 @@ static int PGSpwmtSelection = 0; std::string session_file_name = ""; int session_total_number_of_points = 0; -//bool dynamicSubsampling = true; -//static double lastAdjustTime = 0.0; // last time we changed subsampling -//const double cooldownSeconds = 1; // wait between auto adjustments -//static float fps_avg = 60.0f; +// bool dynamicSubsampling = true; +// static double lastAdjustTime = 0.0; // last time we changed subsampling +// const double cooldownSeconds = 1; // wait between auto adjustments +// static float fps_avg = 60.0f; bool is_pca_gui = false; bool is_ndt_gui = true; @@ -210,20 +204,11 @@ int index_loop_closure_target = 0; int index_begin = 0; int index_end = 0; -float m_gizmo[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_projection[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; bool manipulate_only_marked_gizmo = false; @@ -242,17 +227,44 @@ bool session_loaded = false; // doi = {https://doi.org/10.1016/j.measurement.2023.113199}, // url = {https://www.sciencedirect.com/science/article/pii/S0263224123007637}, // author = {Janusz BÄ™dkowski}, -// keywords = {TLS, Point cloud, Open-source, Multi-view data registration, LiDAR data metrics, Robust loss function, Tait-bryan angles, Quaternions, Rodrigues’ formula, Lie algebra, Rotation matrix parameterization}, -// abstract = {This study addresses multi-view Terrestrial Laser Scanning Point Cloud data registration methods. Multiple rigid point cloud data registration is mandatory for aligning all scans into a common reference frame and it is still considered a challenge looking from a large-scale surveys point of view. The goal of this work is to support the development of cutting-edge registration methods in geoscience and mobile robotics domains. This work evaluates 3 data sets of total 20 scenes available in the literature. This paper provides a novel open-source framework for multi-view Terrestrial Laser Scanning Point Cloud data registration benchmarks. The goal was to verify experimentally which registration variant can improve the open-source data looking from the quantitative and qualitative points of view. In particular, the following scanners provided measurement data: Z+F TLS Imager 5006i, Z+F TLS Imager 5010C, Leica ScanStation C5, Leica ScanStation C10, Leica P40 and Riegl VZ-400. The benchmark shows an impact of the metric e.g. point to point, point to projection onto a plane, plane to plane etc..., rotation matrix parameterization (Tait-Bryan, quaternion, Rodrigues) and other implementation variations (e.g. multi-view Normal Distributions Transform, Pose Graph SLAM approach) onto the multi-view data registration accuracy and performance. An open-source project is created and it can be used for improving existing data sets reported in the literature, it is the added value of the presented research. The combination of metrics, rotation matrix parameterization and optimization algorithms creates hundreds of possible approaches. It is shown that chosen metric is a dominant factor in data registration. The rotation parameterization and other degrees of freedom of proposed variants are rather negligible compared with chosen metric. Most of the proposed approaches improve registered reference data provided by other researchers. Only for 2 from 20 scenes it was not possible to provide significant improvement. The largest improvements are evident for large-scale scenes. The project is available and maintained at https://github.com/MapsHD/HDMapping.} +// keywords = {TLS, Point cloud, Open-source, Multi-view data registration, LiDAR data metrics, Robust loss function, Tait-bryan +// angles, Quaternions, Rodrigues’ formula, Lie algebra, Rotation matrix parameterization}, abstract = {This study addresses multi-view +// Terrestrial Laser Scanning Point Cloud data registration methods. Multiple rigid point cloud data registration is mandatory for +// aligning all scans into a common reference frame and it is still considered a challenge looking from a large-scale surveys point of +// view. The goal of this work is to support the development of cutting-edge registration methods in geoscience and mobile robotics +// domains. This work evaluates 3 data sets of total 20 scenes available in the literature. This paper provides a novel open-source +// framework for multi-view Terrestrial Laser Scanning Point Cloud data registration benchmarks. The goal was to verify experimentally +// which registration variant can improve the open-source data looking from the quantitative and qualitative points of view. In +// particular, the following scanners provided measurement data: Z+F TLS Imager 5006i, Z+F TLS Imager 5010C, Leica ScanStation C5, +// Leica ScanStation C10, Leica P40 and Riegl VZ-400. The benchmark shows an impact of the metric e.g. point to point, point to +// projection onto a plane, plane to plane etc..., rotation matrix parameterization (Tait-Bryan, quaternion, Rodrigues) and other +// implementation variations (e.g. multi-view Normal Distributions Transform, Pose Graph SLAM approach) onto the multi-view data +// registration accuracy and performance. An open-source project is created and it can be used for improving existing data sets +// reported in the literature, it is the added value of the presented research. The combination of metrics, rotation matrix +// parameterization and optimization algorithms creates hundreds of possible approaches. It is shown that chosen metric is a dominant +// factor in data registration. The rotation parameterization and other degrees of freedom of proposed variants are rather negligible +// compared with chosen metric. Most of the proposed approaches improve registered reference data provided by other researchers. Only +// for 2 from 20 scenes it was not possible to provide significant improvement. The largest improvements are evident for large-scale +// scenes. The project is available and maintained at https://github.com/MapsHD/HDMapping.} // } -void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session); -void perform_experiment_on_windows(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, - RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam); -void perform_experiment_on_linux(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, - RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam); -double compute_rms(bool initial, Session &session, ObservationPicking &observation_picking); -void reset_poses(Session &session); +void export_result_to_folder(std::string output_folder_name, ObservationPicking& observation_picking, Session& session); +void perform_experiment_on_windows( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam); +void perform_experiment_on_linux( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam); +double compute_rms(bool initial, Session& session, ObservationPicking& observation_picking); +void reset_poses(Session& session); /////////////////////////////////////////////////////////////////////////////////// @@ -307,7 +319,6 @@ void ndt_gui() tls_registration.ndt.is_quaternion = (NDT3dSelection == 1); tls_registration.ndt.is_rodrigues = (NDT3dSelection == 2); - if (ImGui::Button("Optimization")) { double rms_initial = 0.0; @@ -317,7 +328,7 @@ void ndt_gui() // std::cout << "mui: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; tls_registration.ndt.optimize(session.point_clouds_container.point_clouds, false, tls_registration.compute_mean_and_cov_for_bucket); } - + if (ImGui::Button("Compute mean Mahalanobis distance")) { double rms_initial = 0.0; @@ -329,8 +340,10 @@ void ndt_gui() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Average of all Mahalanobis distances between transformed source points\nand their corresponding Gaussian cells, where:"); - ImGui::Text("Mahalanobis distance measures how far a point is from the mean of a multivariate Gaussian distribution,\ntaking into account the covariance (shape and orientation) of that distribution"); + ImGui::Text( + "Average of all Mahalanobis distances between transformed source points\nand their corresponding Gaussian cells, where:"); + ImGui::Text("Mahalanobis distance measures how far a point is from the mean of a multivariate Gaussian distribution,\ntaking into " + "account the covariance (shape and orientation) of that distribution"); ImGui::EndTooltip(); } @@ -340,14 +353,14 @@ void ndt_gui() ImGui::SameLine(); if (ImGui::Button("left Jacobian")) { - - tls_registration.ndt.optimize_lie_algebra_left_jacobian(session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + tls_registration.ndt.optimize_lie_algebra_left_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); } ImGui::SameLine(); if (ImGui::Button("right Jacobian")) { - - tls_registration.ndt.optimize_lie_algebra_right_jacobian(session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + tls_registration.ndt.optimize_lie_algebra_right_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); } ImGui::Separator(); @@ -369,26 +382,34 @@ void ndt_gui() ImGui::Text("Set error presets:"); ImGui::Text("Zoller+Fröhlich TLS Imager"); ImGui::SameLine(); - if (ImGui::Button("5006i")) tls_registration.set_zoller_frohlich_tls_imager_5006i_errors(); + if (ImGui::Button("5006i")) + tls_registration.set_zoller_frohlich_tls_imager_5006i_errors(); ImGui::SameLine(); - if (ImGui::Button("5010C")) tls_registration.set_zoller_frohlich_tls_imager_5010c_errors(); + if (ImGui::Button("5010C")) + tls_registration.set_zoller_frohlich_tls_imager_5010c_errors(); ImGui::SameLine(); - if (ImGui::Button("5016")) tls_registration.set_zoller_frohlich_tls_imager_5016_errors(); - + if (ImGui::Button("5016")) + tls_registration.set_zoller_frohlich_tls_imager_5016_errors(); ImGui::Text("Leica"); ImGui::SameLine(); - if (ImGui::Button("ScanStation C5 C10")) tls_registration.set_leica_scanstation_c5_c10_errors(); + if (ImGui::Button("ScanStation C5 C10")) + tls_registration.set_leica_scanstation_c5_c10_errors(); ImGui::SameLine(); - if (ImGui::Button("Leica HDS6100")) tls_registration.set_leica_hds6100_errors(); + if (ImGui::Button("Leica HDS6100")) + tls_registration.set_leica_hds6100_errors(); ImGui::SameLine(); - if (ImGui::Button("Leica P40")) tls_registration.set_leica_p40_errors(); + if (ImGui::Button("Leica P40")) + tls_registration.set_leica_p40_errors(); - if (ImGui::Button("Faro Focus3D")) tls_registration.set_faro_focus3d_errors(); + if (ImGui::Button("Faro Focus3D")) + tls_registration.set_faro_focus3d_errors(); ImGui::SameLine(); - if (ImGui::Button("Riegl VZ400")) tls_registration.set_riegl_vz400_errors(); + if (ImGui::Button("Riegl VZ400")) + tls_registration.set_riegl_vz400_errors(); ImGui::SameLine(); - if (ImGui::Button("Livox mid360")) tls_registration.set_livox_mid360_errors(); + if (ImGui::Button("Livox mid360")) + tls_registration.set_livox_mid360_errors(); } void icp_gui() @@ -520,13 +541,16 @@ void rpf_gui() ImGui::Separator(); ImGui::Text("Optimize point to projection onto plane source to target:"); if (ImGui::Button("Basic Jacobian")) - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target( + session.point_clouds_container); ImGui::SameLine(); if (ImGui::Button("Lie-algebra left Jacobian")) - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); ImGui::SameLine(); if (ImGui::Button("Lie-algebra right Jacobian")) - tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); ImGui::Separator(); @@ -541,7 +565,6 @@ void rpf_gui() tls_registration.registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); } - void pca_gui() { ImGui::Begin("Point cloud alignment", &is_pca_gui, ImGuiWindowFlags_MenuBar); @@ -550,7 +573,8 @@ void pca_gui() { bool justPushed = false; - if (is_ndt_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_ndt_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Normal Distributions Transform")) { if (!is_ndt_gui) @@ -561,19 +585,24 @@ void pca_gui() justPushed = true; } } - if (is_ndt_gui && !justPushed) ImGui::PopStyleColor(); + if (is_ndt_gui && !justPushed) + ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions rather than raw points"); - ImGui::Text("Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); - ImGui::Text("Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences with continuous probability density functions."); + ImGui::Text("Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions " + "rather than raw points"); + ImGui::Text( + "Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); + ImGui::Text("Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences " + "with continuous probability density functions."); ImGui::EndTooltip(); } ImGui::SameLine(); - if (is_icp_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_icp_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Iterative Closest Point")) { if (!is_icp_gui) @@ -584,18 +613,22 @@ void pca_gui() justPushed = true; } } - if (is_icp_gui && !justPushed) ImGui::PopStyleColor(); + if (is_icp_gui && !justPushed) + ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Geometric registration algorithm that aligns two point clouds\nby minimizing the Euclidean distances between corresponding points"); - ImGui::Text("Very precise at local refinement, especially point-to-plane ICP,\nbut it struggles if the starting alignment is too far off"); + ImGui::Text("Geometric registration algorithm that aligns two point clouds\nby minimizing the Euclidean distances between " + "corresponding points"); + ImGui::Text("Very precise at local refinement, especially point-to-plane ICP,\nbut it struggles if the starting alignment " + "is too far off"); ImGui::EndTooltip(); } ImGui::SameLine(); - if (is_rpf_gui) ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (is_rpf_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); if (ImGui::Button("Registration Plane Feature")) { if (!is_rpf_gui) @@ -606,12 +639,15 @@ void pca_gui() justPushed = true; } } - if (is_rpf_gui && !justPushed) ImGui::PopStyleColor(); + if (is_rpf_gui && !justPushed) + ImGui::PopStyleColor(); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Feature based registration technique that uses detected\nplanar surfaces in the environment (walls, floors, ceilings, etc.) as constraints for alignment"); - ImGui::Text("Can be much more robust to noise and partial overlap,\nrequire far fewer correspondences (just a few planes can define a full 6 DOF pose),\nhandle low texture regions better than ICP"); + ImGui::Text("Feature based registration technique that uses detected\nplanar surfaces in the environment (walls, floors, " + "ceilings, etc.) as constraints for alignment"); + ImGui::Text("Can be much more robust to noise and partial overlap,\nrequire far fewer correspondences (just a few planes " + "can define a full 6 DOF pose),\nhandle low texture regions better than ICP"); ImGui::EndTooltip(); } @@ -642,7 +678,8 @@ void pose_graph_slam_gui() if (tls_registration.pose_graph_slam.number_of_threads < 1) tls_registration.pose_graph_slam.number_of_threads = 1; - ImGui::InputInt("Number of iterations (pair wise matching)", &tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching); + ImGui::InputInt( + "Number of iterations (pair wise matching)", &tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching); if (tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching < 1) tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching = 1; @@ -714,7 +751,7 @@ void pose_graph_slam_gui() ImGui::RadioButton("Optimize with PCL (ICP based pair wise matching)", &PGSpwmtSelection, 13); #endif - //tls_registration.pose_graph_slam.set_all_to_false(); + // tls_registration.pose_graph_slam.set_all_to_false(); tls_registration.pose_graph_slam.is_ndt = (PGSpwmtSelection == 0); tls_registration.pose_graph_slam.is_optimization_point_to_point_source_to_target = (PGSpwmtSelection == 1); tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target = (PGSpwmtSelection == 2); @@ -726,8 +763,10 @@ void pose_graph_slam_gui() tls_registration.pose_graph_slam.is_ndt_lie_algebra_right_jacobian = (PGSpwmtSelection == 7); tls_registration.pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = (PGSpwmtSelection == 8); tls_registration.pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = (PGSpwmtSelection == 9); - tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = (PGSpwmtSelection == 10); - tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = (PGSpwmtSelection == 11); + tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = + (PGSpwmtSelection == 10); + tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = + (PGSpwmtSelection == 11); tls_registration.pose_graph_slam.is_optimize_pcl_ndt = (PGSpwmtSelection == 12); tls_registration.pose_graph_slam.is_optimize_pcl_icp = (PGSpwmtSelection == 13); @@ -750,7 +789,8 @@ void pose_graph_slam_gui() // double mui = 0.0; tls_registration.pose_graph_slam.optimize(session.point_clouds_container); // pose_graph_slam.optimize(point_clouds_container, rms_initial, rms_final, mui); - // std::cout << "mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; + // std::cout << "mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << + // std::endl; } #if WITH_GTSAM @@ -765,7 +805,8 @@ void pose_graph_slam_gui() double rms_final = 0.0; double mui = 0.0; tls_registration.pose_graph_slam.optimize_with_GTSAM(session.point_clouds_container); - // std::cout << "mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; + // std::cout << "mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << + // std::endl; } #endif @@ -904,14 +945,20 @@ void observation_picking_gui() ImGui::Text("Intersection %zu", i); ImGui::SetWindowFontScale(1.0f); ImGui::SameLine(); - ImGui::ColorEdit3(std::string("Color##" + std::to_string(i)).c_str(), observation_picking.intersections[i].color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3( + std::string("Color##" + std::to_string(i)).c_str(), + observation_picking.intersections[i].color, + ImGuiColorEditFlags_NoInputs); ImGui::SameLine(); if (ImGui::Button(std::string("Remove##" + std::to_string(i)).c_str())) index_intersection_to_remove = i; - ImGui::InputFloat3(std::string("Translation [m]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].translation); + ImGui::InputFloat3( + std::string("Translation [m]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].translation); ImGui::InputFloat3(std::string("Rotation [deg]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].rotation); - ImGui::InputFloat3(std::string("Width length height [m]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].width_length_height); + ImGui::InputFloat3( + std::string("Width length height [m]##" + std::to_string(i)).c_str(), + observation_picking.intersections[i].width_length_height); } if (index_intersection_to_remove != -1) @@ -989,7 +1036,8 @@ void loop_closure_gui() ImGui::PopItemWidth(); int prev_index_active_edge = session.pose_graph_loop_closure.index_active_edge; - session.pose_graph_loop_closure.Gui(session.point_clouds_container, + session.pose_graph_loop_closure.Gui( + session.point_clouds_container, index_loop_closure_source, index_loop_closure_target, m_gizmo, @@ -1093,9 +1141,10 @@ void lio_segments_gui() ImGui::SameLine(); - if(ImGui::Button("Set fuse IMU inclination for picked trajectory node")){ - - if (index_loop_closure_target >= 0 && index_loop_closure_target < session.point_clouds_container.point_clouds.size()){ + if (ImGui::Button("Set fuse IMU inclination for picked trajectory node")) + { + if (index_loop_closure_target >= 0 && index_loop_closure_target < session.point_clouds_container.point_clouds.size()) + { session.point_clouds_container.point_clouds[index_loop_closure_target].fuse_inclination_from_IMU = true; } @@ -1117,7 +1166,6 @@ void lio_segments_gui() januszjanusz*/ } - ImGui::Text("Fuse IMU inclination: "); ImGui::SameLine(); @@ -1150,18 +1198,18 @@ void lio_segments_gui() ImGui::InputDouble("acceptable angle [deg]", &angle_diff); ImGui::Separator(); - //ImGui::Text("motion model"); + // ImGui::Text("motion model"); - //session.pose_graph_loop_closure.edges. + // session.pose_graph_loop_closure.edges. - //ImGui::InputDouble("motion_model_w_px_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_px_1_sigma_m); - //ImGui::InputDouble("motion_model_w_py_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_py_1_sigma_m); - //ImGui::InputDouble("motion_model_w_pz_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_pz_1_sigma_m); - //ImGui::InputDouble("motion_model_w_om_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_om_1_sigma_deg); - //ImGui::InputDouble("motion_model_w_fi_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_fi_1_sigma_deg); - //ImGui::InputDouble("motion_model_w_ka_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_ka_1_sigma_deg); + // ImGui::InputDouble("motion_model_w_px_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_px_1_sigma_m); + // ImGui::InputDouble("motion_model_w_py_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_py_1_sigma_m); + // ImGui::InputDouble("motion_model_w_pz_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_pz_1_sigma_m); + // ImGui::InputDouble("motion_model_w_om_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_om_1_sigma_deg); + // ImGui::InputDouble("motion_model_w_fi_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_fi_1_sigma_deg); + // ImGui::InputDouble("motion_model_w_ka_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_ka_1_sigma_deg); - //ImGui::Separator(); + // ImGui::Separator(); ImGui::BeginChild("LIO segments", ImVec2(0, 0), true); { @@ -1170,7 +1218,9 @@ void lio_segments_gui() if (i > 0) ImGui::Separator(); ImGui::SetWindowFontScale(1.25f); - ImGui::Checkbox(std::filesystem::path(session.point_clouds_container.point_clouds[i].file_name).filename().string().c_str(), &session.point_clouds_container.point_clouds[i].visible); + ImGui::Checkbox( + std::filesystem::path(session.point_clouds_container.point_clouds[i].file_name).filename().string().c_str(), + &session.point_clouds_container.point_clouds[i].visible); ImGui::SetWindowFontScale(1.0f); ImGui::SameLine(); ImGui::Checkbox(("gizmo##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].gizmo); @@ -1244,7 +1294,10 @@ void lio_segments_gui() if (session.point_clouds_container.point_clouds[i].visible) { ImGui::SameLine(); - ImGui::ColorEdit3(("pc_color##" + std::to_string(i)).c_str(), session.point_clouds_container.point_clouds[i].render_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3( + ("pc_color##" + std::to_string(i)).c_str(), + session.point_clouds_container.point_clouds[i].render_color, + ImGuiColorEditFlags_NoInputs); #if 0 ImGui::SameLine(); @@ -1329,7 +1382,9 @@ void lio_segments_gui() #endif ImGui::SameLine(); - ImGui::Checkbox(("fuse IMU inclination##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU); + ImGui::Checkbox( + ("fuse IMU inclination##" + std::to_string(i)).c_str(), + &session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU); ImGui::SameLine(); ImGui::Checkbox(("show IMU##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].show_IMU); @@ -1344,7 +1399,8 @@ void lio_segments_gui() //session.point_clouds_container.point_clouds[i].m_pose //session.point_clouds_container.point_clouds[i].m_pose_temp - TaitBryanPose target_pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); + TaitBryanPose target_pose = + pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); target_pose.om = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.x(); target_pose.fi = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.y(); @@ -1356,13 +1412,20 @@ void lio_segments_gui() session.point_clouds_container.point_clouds[i].m_pose_temp = m_pose; //session.point_clouds_container.point_clouds[i].m_pose = m_pose; - //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] = session.point_clouds_container.point_clouds[i].pose.px; - //session.point_clouds_container.point_clouds[i].gui_translation[1] = session.point_clouds_container.point_clouds[i].pose.py; - //session.point_clouds_container.point_clouds[i].gui_translation[2] = session.point_clouds_container.point_clouds[i].pose.pz; - //session.point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(session.point_clouds_container.point_clouds[i].pose.om); - //session.point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); - //session.point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); + //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] = + session.point_clouds_container.point_clouds[i].pose.px; + //session.point_clouds_container.point_clouds[i].gui_translation[1] = + session.point_clouds_container.point_clouds[i].pose.py; + //session.point_clouds_container.point_clouds[i].gui_translation[2] = + session.point_clouds_container.point_clouds[i].pose.pz; + //session.point_clouds_container.point_clouds[i].gui_rotation[0] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.om); + //session.point_clouds_container.point_clouds[i].gui_rotation[1] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); + //session.point_clouds_container.point_clouds[i].gui_rotation[2] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); }*/ ImGui::Text("fixed: "); @@ -1396,7 +1459,6 @@ void lio_segments_gui() ImGui::Checkbox(("ka##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_ka); if (ImGui::IsItemHovered()) ImGui::SetTooltip(kaText); - } #if 0 ImGui::SameLine(); @@ -1406,7 +1468,6 @@ void lio_segments_gui() } #endif } - } ImGui::EndChild(); } @@ -1422,7 +1483,13 @@ void openSession() { std::cout << "Session file: '" << session_file_name << "'" << std::endl; - if (session.load(fs::path(session_file_name).string(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, tls_registration.calculate_offset)) + if (session.load( + fs::path(session_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset)) { session_loaded = true; index_begin = 0; @@ -1441,13 +1508,14 @@ void openSession() std::string saveSession() { - const std::string output_file_name = mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); + const std::string output_file_name = + mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); if (output_file_name.size() > 0) { std::cout << "Session file to save: '" << output_file_name << "'" << std::endl; - //creating filenames proposal based on current selection + // creating filenames proposal based on current selection std::filesystem::path path(output_file_name); // Extract parts const auto dir = path.parent_path(); @@ -1468,10 +1536,12 @@ std::string saveSession() "Follow guidlines available here : " "https://github.com/MapsHD/HDMapping/tree/main/doc/, " "You can do this using button 'update initial poses from RESSO file'", - pfd::choice::ok, pfd::icon::error); + pfd::choice::ok, + pfd::icon::error); message.result(); - initial_poses_file_name = mandeye::fd::SaveFileDialog("Initial poses file name", mandeye::fd::IniPoses_filter, initial_poses_file_name); + initial_poses_file_name = + mandeye::fd::SaveFileDialog("Initial poses file name", mandeye::fd::IniPoses_filter, initial_poses_file_name); std::cout << "Resso file to save: '" << initial_poses_file_name << "'" << std::endl; if (initial_poses_file_name.size() > 0) @@ -1492,7 +1562,8 @@ std::string saveSession() "Follow guidlines available here : " "https://github.com/MapsHD/HDMapping/tree/main/doc/," "You can do this using button 'update poses from RESSO file'", - pfd::choice::ok, pfd::icon::error); + pfd::choice::ok, + pfd::icon::error); message.result(); poses_file_name = mandeye::fd::SaveFileDialog("Poses file name", mandeye::fd::Poses_filter, poses_file_name); @@ -1508,16 +1579,18 @@ std::string saveSession() std::cout << "saving result to: " << poses_file_name << std::endl; session.point_clouds_container.save_poses(poses_file_name, false); - try { + try + { fs::copy_file(poses_file_name, initial_poses_file_name, fs::copy_options::overwrite_existing); - } - catch (const fs::filesystem_error& e) { + } catch (const fs::filesystem_error& e) + { std::cerr << "Error copying poses file: " << e.what() << '\n'; } - return output_file_name; + return output_file_name; } - else{ + else + { std::cout << "saving canceled" << std::endl; return ""; @@ -1537,7 +1610,14 @@ void openLaz(bool fillInSession) 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, session.load_cache_mode)) + 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, + session.load_cache_mode)) 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; @@ -1549,14 +1629,14 @@ void openLaz(bool fillInSession) 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) + 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 (fillInSession) { - for (auto &pc : session.point_clouds_container.point_clouds) + for (auto& pc : session.point_clouds_container.point_clouds) { Eigen::Affine3d m = Eigen::Affine3d::Identity(); if (pc.points_local.size() > 100) @@ -1574,14 +1654,14 @@ void openLaz(bool fillInSession) m.translation() = mean; PointCloud::LocalTrajectoryNode node; - node.imu_diff_angle_om_fi_ka_deg = {0, 0, 0}; - node.imu_om_fi_ka = {0, 0, 0}; + node.imu_diff_angle_om_fi_ka_deg = { 0, 0, 0 }; + node.imu_om_fi_ka = { 0, 0, 0 }; node.m_pose = Eigen::Affine3d::Identity(); - node.timestamps = {0, 0}; + node.timestamps = { 0, 0 }; pc.local_trajectory.push_back(node); - for (auto &p : pc.points_local) + for (auto& p : pc.points_local) { p -= mean; } @@ -1598,14 +1678,16 @@ void openLaz(bool fillInSession) std::filesystem::create_directory(session_fn); session_file_name = (fs::path(session_fn) / "newSession.mjs").string(); - session.point_clouds_container.initial_poses_file_name = "dummy"; //non empty file names signal poses present, new file names will be created on save - session.point_clouds_container.poses_file_name = "dummy"; //non empty file names signal poses present, new file names will be created on save + session.point_clouds_container.initial_poses_file_name = + "dummy"; // non empty file names signal poses present, new file names will be created on save + session.point_clouds_container.poses_file_name = + "dummy"; // non empty file names signal poses present, new file names will be created on save std::string output_file_name = saveSession(); if (output_file_name.size() > 0) { - //creating filenames proposal based on current selection + // creating filenames proposal based on current selection std::filesystem::path path(output_file_name); // Extract parts const auto dir = path.parent_path(); @@ -1620,7 +1702,7 @@ void openLaz(bool fillInSession) session.point_clouds_container.point_clouds[i].file_name = fpath.string(); exportLaz(fpath.string(), pc.points_local, pc.intensities, pc.timestamps); - //saving terajectory file + // saving terajectory file auto pathtrj = dir / ("trajectory_lio_" + std::to_string(i) + ".csv"); std::cout << "Saving trajectory: " << pathtrj << std::endl; @@ -1632,47 +1714,36 @@ void openLaz(bool fillInSession) return; } - outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 timestampUnix_nanoseconds om_rad fi_rad ka_rad" << std::endl; + outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 " + "timestampUnix_nanoseconds om_rad fi_rad ka_rad" + << std::endl; for (int j = 0; j < 1; j++) { auto pose = Eigen::Affine3d::Identity(); - outfile - << std::setprecision(20) << 0.0 << " " << std::setprecision(10) - << pose(0, 0) << " " - << pose(0, 1) << " " - << pose(0, 2) << " " - << pose(0, 3) << " " - << pose(1, 0) << " " - << pose(1, 1) << " " - << pose(1, 2) << " " - << pose(1, 3) << " " - << pose(2, 0) << " " - << pose(2, 1) << " " - << pose(2, 2) << " " - << pose(2, 3) << " " - << std::setprecision(20) << 0.0 << " " - << 0.0 << " " - << 0.0 << " " - << 0.0 << " " - << std::endl; + outfile << std::setprecision(20) << 0.0 << " " << std::setprecision(10) << pose(0, 0) << " " << pose(0, 1) << " " + << pose(0, 2) << " " << pose(0, 3) << " " << pose(1, 0) << " " << pose(1, 1) << " " << pose(1, 2) << " " + << pose(1, 3) << " " << pose(2, 0) << " " << pose(2, 1) << " " << pose(2, 2) << " " << pose(2, 3) << " " + << std::setprecision(20) << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << std::endl; } outfile.close(); } } - [[maybe_unused]] - pfd::message message( + [[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); + "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(); - //std::string mes = "Session saved to folder '" + path_ground_truth_session_folder.string() + "'"; + // std::string mes = "Session saved to folder '" + path_ground_truth_session_folder.string() + "'"; //[[maybe_unused]] pfd::message message( // "Ground truth session info", mes, // pfd::choice::ok, pfd::icon::info); - //message.result(); + // message.result(); } } } @@ -1682,8 +1753,10 @@ void saveSubsession() int inx_begin = 0; int inx_end = 0; - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++){ - if (session.point_clouds_container.point_clouds[i].visible){ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].visible) + { inx_begin = i; break; } @@ -1698,7 +1771,7 @@ void saveSubsession() // Extract parts fs::path dir = path.parent_path(); std::string stem = path.stem().string(); - const auto ext = ".mjs"; //forcing new extension even if original session was .json + const auto ext = ".mjs"; // forcing new extension even if original session was .json const std::string indexpart = " " + std::to_string(inx_begin) + "-" + std::to_string(inx_end); // Build new name @@ -1721,10 +1794,11 @@ void saveSubsession() std::cout << "Saving poses to: " << poses_file_name << std::endl; session.point_clouds_container.save_poses(fs::path(poses_file_name).string(), true); - try { + try + { fs::copy_file(poses_file_name, initial_poses_file_name, fs::copy_options::overwrite_existing); - } - catch (const fs::filesystem_error& e) { + } catch (const fs::filesystem_error& e) + { std::cerr << "Error copying poses file: " << e.what() << '\n'; } } @@ -1777,10 +1851,16 @@ void settings_gui() if (input_file_name.size() > 0) { - session.working_directory = fs::path(input_file_name).parent_path().string(); - if (!session.point_clouds_container.load(session.working_directory.c_str(), input_file_name.c_str(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z, session.load_cache_mode)) + if (!session.point_clouds_container.load( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + session.load_cache_mode)) { std::cout << "check input files" << std::endl; return; @@ -1811,7 +1891,13 @@ void settings_gui() { session.working_directory = fs::path(input_file_name).parent_path().string(); - if (!session.point_clouds_container.load_eth(session.working_directory.c_str(), input_file_name.c_str(), tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z)) + if (!session.point_clouds_container.load_eth( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z)) { std::cout << "check input files" << std::endl; return; @@ -1840,7 +1926,12 @@ void settings_gui() 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_3DTK_tls(input_file_names, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z)) + if (!session.point_clouds_container.load_3DTK_tls( + input_file_names, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z)) { std::cout << "Check input files" << std::endl; return; @@ -1861,10 +1952,10 @@ void settings_gui() input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO file", {}); if (input_file_name.size() > 0) { - session.working_directory = fs::path(input_file_name).parent_path().string(); - if (!session.point_clouds_container.update_initial_poses_from_RESSO(session.working_directory.c_str(), input_file_name.c_str())) + if (!session.point_clouds_container.update_initial_poses_from_RESSO( + session.working_directory.c_str(), input_file_name.c_str())) { std::cout << "Check input files" << std::endl; return; @@ -1911,7 +2002,8 @@ void settings_gui() { session.working_directory = fs::path(input_file_name).parent_path().string(); - if (!session.point_clouds_container.update_poses_from_RESSO_inverse(session.working_directory.c_str(), input_file_name.c_str())) + if (!session.point_clouds_container.update_poses_from_RESSO_inverse( + session.working_directory.c_str(), input_file_name.c_str())) { std::cout << "Check input files" << std::endl; return; @@ -1964,15 +2056,22 @@ void settings_gui() session.point_clouds_container.point_clouds[0].m_pose(1, 3) = y_origin; session.point_clouds_container.point_clouds[0].m_pose(2, 3) = z_origin; - session.point_clouds_container.point_clouds[0].pose = pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[0].m_pose); + session.point_clouds_container.point_clouds[0].pose = + pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[0].m_pose); - session.point_clouds_container.point_clouds[0].gui_translation[0] = (float)session.point_clouds_container.point_clouds[0].pose.px; - session.point_clouds_container.point_clouds[0].gui_translation[1] = (float)session.point_clouds_container.point_clouds[0].pose.py; - session.point_clouds_container.point_clouds[0].gui_translation[2] = (float)session.point_clouds_container.point_clouds[0].pose.pz; + session.point_clouds_container.point_clouds[0].gui_translation[0] = + (float)session.point_clouds_container.point_clouds[0].pose.px; + session.point_clouds_container.point_clouds[0].gui_translation[1] = + (float)session.point_clouds_container.point_clouds[0].pose.py; + session.point_clouds_container.point_clouds[0].gui_translation[2] = + (float)session.point_clouds_container.point_clouds[0].pose.pz; - session.point_clouds_container.point_clouds[0].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); - session.point_clouds_container.point_clouds[0].gui_rotation[1] = (float)(session.point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[0].gui_rotation[2] = (float)(session.point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + session.point_clouds_container.point_clouds[0].gui_rotation[0] = + (float)(session.point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[0].gui_rotation[1] = + (float)(session.point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[0].gui_rotation[2] = + (float)(session.point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); Eigen::Affine3d curr_m_pose2 = session.point_clouds_container.point_clouds[0].m_pose; for (size_t j = 1; j < session.point_clouds_container.point_clouds.size(); j++) @@ -1981,15 +2080,22 @@ void settings_gui() // std::cout << curr_m_pose2.matrix() << std::endl; session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose2; - 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.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); } } } @@ -2028,10 +2134,10 @@ void settings_gui() ImGui::SetTooltip("Experiments to compare methods and approaches for multi-view TLS registration"); ImGui::SameLine(); if (ImGui::Button("WINDOWS")) - perform_experiment_on_windows(session, observation_picking, tls_registration.icp, tls_registration.ndt, tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); - ImGui::SameLine(); - if (ImGui::Button("LINUX")) - perform_experiment_on_linux(session, observation_picking, tls_registration.icp, tls_registration.ndt, tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); + perform_experiment_on_windows(session, observation_picking, tls_registration.icp, tls_registration.ndt, + tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); ImGui::SameLine(); if (ImGui::Button("LINUX")) + perform_experiment_on_linux(session, observation_picking, tls_registration.icp, tls_registration.ndt, + tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); */ } } @@ -2065,25 +2171,44 @@ void display() { if (new_loop_closure_index) { - //if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) + // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) //{ - // new_rotation_center.x() = session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); - // new_rotation_center.y() = session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); - // new_rotation_center.z() = session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); + // new_rotation_center.x() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); + // new_rotation_center.y() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); + // new_rotation_center.z() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); // // new_translate_x = -new_rotation_center.x(); // new_translate_y = -new_rotation_center.y(); // camera_transition_active = true; //} - if (session.pose_graph_loop_closure.manipulate_active_edge) { + if (session.pose_graph_loop_closure.manipulate_active_edge) + { if (session.pose_graph_loop_closure.edges.size() > 0) { if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) { - new_rotation_center.x() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose.translation().x(); - new_rotation_center.y() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose.translation().y(); - new_rotation_center.z() = session.point_clouds_container.point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge].index_from].m_pose.translation().z(); + new_rotation_center.x() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .x(); + new_rotation_center.y() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .y(); + new_rotation_center.z() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .z(); } } @@ -2114,13 +2239,21 @@ void display() } else { - glOrtho(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100000, 100000); + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); - glm::mat4 proj = glm::orthoLH_ZO(-camera_ortho_xy_view_zoom, camera_ortho_xy_view_zoom, + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, -100, 100); + camera_ortho_xy_view_zoom / ratio, + -100, + 100); std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); @@ -2139,10 +2272,9 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), glm::vec3(v_t.x(), v_t.y(), v_t.z())); std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); @@ -2158,8 +2290,12 @@ void display() else { if (is_loop_closure_gui) - session.pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target, - num_edge_extended_before, num_edge_extended_after); + session.pose_graph_loop_closure.Render( + session.point_clouds_container, + index_loop_closure_source, + index_loop_closure_target, + num_edge_extended_before, + num_edge_extended_after); tls_registration.gnss.render(session.point_clouds_container); session.ground_control_points.render(session.point_clouds_container); @@ -2232,10 +2368,22 @@ void display() 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); + 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); + 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]; @@ -2253,15 +2401,22 @@ void display() 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); + 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) { @@ -2270,28 +2425,47 @@ void display() { 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.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.render( + observation_picking, + viewer_decimate_point_cloud, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, - session.point_clouds_container.xz_grid_10x10, session.point_clouds_container.xz_grid_1x1, session.point_clouds_container.xz_grid_01x01, - session.point_clouds_container.yz_grid_10x10, session.point_clouds_container.yz_grid_1x1, session.point_clouds_container.yz_grid_01x01, - session.point_clouds_container.xy_grid_10x10, session.point_clouds_container.xy_grid_1x1, session.point_clouds_container.xy_grid_01x01, - session.point_clouds_container.intersection_width, session_dims); - - // std::cout << "session.point_clouds_container.xy_grid_10x10 " << (int)session.point_clouds_container.xy_grid_10x10 << std::endl; + session.point_clouds_container.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(); @@ -2381,10 +2555,22 @@ void display() 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); + 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); + 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(); @@ -2408,7 +2594,8 @@ void display() 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); + 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); } } } @@ -2419,7 +2606,7 @@ void display() { is_pca_gui = !is_pca_gui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_A, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2428,7 +2615,7 @@ void display() { session.control_points.is_imgui = !session.control_points.is_imgui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_C, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2437,7 +2624,7 @@ void display() { is_lio_segments_gui = !is_lio_segments_gui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_E, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2446,7 +2633,7 @@ void display() { session.ground_control_points.is_imgui = !session.ground_control_points.is_imgui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_G, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2455,7 +2642,7 @@ void display() { is_loop_closure_gui = !is_loop_closure_gui; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_L, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2464,22 +2651,21 @@ void display() { openSession(); - //workaround + // workaround io.AddKeyEvent(ImGuiKey_O, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_P, false)) { is_pose_graph_slam = !is_pose_graph_slam; - //workaround + // workaround io.AddKeyEvent(ImGuiKey_P, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_R)) //random colors + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_R)) // random colors { for (auto& pc : session.point_clouds_container.point_clouds) { @@ -2489,19 +2675,19 @@ void display() pc.show_color = false; } - //workaround + // workaround io.AddKeyEvent(ImGuiKey_R, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_S, false)) { - if (io.KeyShift) + if (io.KeyShift) saveSubsession(); - else + else saveSession(); - //workaround + // workaround io.AddKeyEvent(ImGuiKey_S, false); io.AddKeyEvent(ImGuiMod_Ctrl, false); } @@ -2533,7 +2719,7 @@ void display() ImGui::SetTooltip("Create session from las/laz file(s)"); ImGui::EndPopup(); - } + } ImGui::SameLine(); ImGui::Dummy(ImVec2(20, 0)); @@ -2541,20 +2727,22 @@ void display() } else { - if (ImGui::BeginMenu("File")) { + if (ImGui::BeginMenu("File")) + { if (ImGui::MenuItem("Save session as", "Ctrl+S")) saveSession(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Save changes of full session with posibility to change filename"); - //ImGui::BeginDisabled(!((index_begin > 0) || (index_end < static_cast(session.point_clouds_container.point_clouds.size() - 1)))); + // ImGui::BeginDisabled(!((index_begin > 0) || (index_end < + // static_cast(session.point_clouds_container.point_clouds.size() - 1)))); //{ if (ImGui::MenuItem("Save subsession", "Ctrl+Shift+S")) saveSubsession(); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Save session according to selections from 'LIO segments editor' window"); //} - //ImGui::EndDisabled(); + // ImGui::EndDisabled(); ImGui::Separator(); @@ -2580,7 +2768,8 @@ void display() save_all_to_las(session, output_file_name, false, true); } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("To export in full resolution, close the program and open again and unmark 'downsample during load' before loading session"); + ImGui::SetTooltip("To export in full resolution, close the program and open again and unmark 'downsample during " + "load' before loading session"); if (ImGui::MenuItem("Separate global scans (laz)")) { @@ -2614,7 +2803,12 @@ void display() const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); std::cout << "laz file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories_to_laz(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling); + save_trajectories_to_laz( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling); } if (ImGui::IsItemHovered()) ImGui::SetTooltip("As one global scan"); @@ -2628,7 +2822,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, true, false, false, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + false, + false, + false); } if (ImGui::MenuItem("Save all as csv (timestamp Unix)##1")) { @@ -2636,7 +2839,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, false, true, false, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + true, + false, + false); } if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##1")) { @@ -2644,7 +2856,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, true, true, false, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + true, + false, + false); } ImGui::Separator(); @@ -2656,7 +2877,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, true, false, true, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + false, + true, + false); } if (ImGui::MenuItem("Save all as csv (timestamp Unix)##2")) { @@ -2664,7 +2894,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, false, true, true, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + true, + true, + false); } if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##2")) { @@ -2672,7 +2911,16 @@ void display() std::cout << "csv file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, true, true, true, false); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + true, + true, + false); } ImGui::Separator(); @@ -2683,7 +2931,16 @@ void display() std::cout << "dxf file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - save_trajectories(session, output_file_name, tls_registration.curve_consecutive_distance_meters, tls_registration.not_curve_consecutive_distance_meters, tls_registration.is_trajectory_export_downsampling, false, false, false, true); + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + false, + false, + true); } ImGui::EndMenu(); @@ -2814,11 +3071,16 @@ void display() if (ImGui::MenuItem("Save GNSS data to las/laz file")) { - const auto output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); + const auto output_file_name = + mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; if (output_file_name.size() > 0) - tls_registration.gnss.save_to_laz(output_file_name, session.point_clouds_container.offset.x(), session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); + tls_registration.gnss.save_to_laz( + output_file_name, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); } if (ImGui::MenuItem("Save metascan points in PUWG92")) @@ -2886,7 +3148,6 @@ void display() 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 @@ -2930,7 +3191,6 @@ void display() ImGui::EndTooltip(); } - if (ImGui::BeginMenu("Tools")) { @@ -2939,7 +3199,8 @@ void display() { ImGui::BeginTooltip(); ImGui::Text("Point cloud alignment (registration) algorithms:"); - ImGui::Text("(aligning two 3D point sets from LiDAR scans, by estimating\nthe relative pose(translation and rotation) between them)"); + ImGui::Text("(aligning two 3D point sets from LiDAR scans, by estimating\nthe relative pose(translation and rotation) " + "between them)"); ImGui::Text("- Normal Distributions Transform"); ImGui::Text("- Iterative Closest Point"); ImGui::Text("- Registration Plane Feature"); @@ -2949,14 +3210,14 @@ void display() ImGui::MenuItem("Pose Graph SLAM", "Ctrl+P", &is_pose_graph_slam); ImGui::MenuItem("Observations", nullptr, &is_manual_analisys); - ImGui::Separator(); ImGui::MenuItem("Control Points", "Ctrl+C", &session.control_points.is_imgui, !session.ground_control_points.is_imgui); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Known reference points used to align or verify the scan"); - ImGui::MenuItem("Ground Control Points", "Ctrl+G", &session.ground_control_points.is_imgui, !session.control_points.is_imgui); + ImGui::MenuItem( + "Ground Control Points", "Ctrl+G", &session.ground_control_points.is_imgui, !session.control_points.is_imgui); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Accurate real-world points used to georeference the scan"); @@ -2972,7 +3233,8 @@ void display() ImGui::EndMenu(); } - if (ImGui::BeginMenu("Intersections")) { + if (ImGui::BeginMenu("Intersections")) + { ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputDouble("Intersection width [m]", &session.point_clouds_container.intersection_width, 0.0, 0.0, "%.2f"); if (session.point_clouds_container.intersection_width < 0.001) @@ -2992,8 +3254,12 @@ void display() if (output_file_name.size() > 0) { - save_intersection(session, output_file_name, - session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, session.point_clouds_container.intersection_width); } } @@ -3012,8 +3278,12 @@ void display() if (output_file_name.size() > 0) { - save_intersection(session, output_file_name, - session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, session.point_clouds_container.intersection_width); } } @@ -3032,8 +3302,12 @@ void display() if (output_file_name.size() > 0) { - save_intersection(session, output_file_name, - session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, session.point_clouds_container.intersection_width); } } @@ -3101,7 +3375,6 @@ void display() ImGui::BeginDisabled(!session_loaded); { - float color[3]; if (session_loaded) { @@ -3165,8 +3438,8 @@ void display() ImGui::EndTooltip(); } #endif - //ImGui::MenuItem("Subwindow", nullptr, &consImGui); - //if (ImGui::IsItemHovered()) + // ImGui::MenuItem("Subwindow", nullptr, &consImGui); + // if (ImGui::IsItemHovered()) // ImGui::SetTooltip("Show/hide console output as GUI window"); ImGui::EndMenu(); @@ -3193,20 +3466,20 @@ void display() ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); ImGui::SameLine(); - //fps_avg = fps_avg * 0.7f + ImGui::GetIO().Framerate * 0.3f; // exponential smoothing + // fps_avg = fps_avg * 0.7f + ImGui::GetIO().Framerate * 0.3f; // exponential smoothing - //double now = ImGui::GetTime(); // ImGui’s built-in timer (in seconds) + // double now = ImGui::GetTime(); // ImGui’s built-in timer (in seconds) - //ImGui::Checkbox("dynamic", &dynamicSubsampling); - //if (ImGui::IsItemHovered()) + // ImGui::Checkbox("dynamic", &dynamicSubsampling); + // if (ImGui::IsItemHovered()) // ImGui::SetTooltip("automatically control subsampling vs FPS: increase bellow 10, decrease above 60"); - //if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) + // if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) //{ // viewer_decimate_point_cloud += 1; // lastAdjustTime = now; //} - //ImGui::SameLine(); - //ImGui::Text("(avg %.1f)", fps_avg); + // ImGui::SameLine(); + // ImGui::Text("(avg %.1f)", fps_avg); if (viewer_decimate_point_cloud < 1) viewer_decimate_point_cloud = 1; @@ -3216,7 +3489,9 @@ void display() } ImGui::EndDisabled(); - ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); @@ -3230,14 +3505,13 @@ void display() ImGui::PopStyleVar(2); ImGui::PopStyleColor(3); - ImGui::EndMainMenuBar(); } if (is_settings_gui) settings_gui(); - // my_display_code(); + // my_display_code(); if (is_pca_gui) pca_gui(); @@ -3286,10 +3560,9 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observ return pos; } - void mouse(int glut_button, int state, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; @@ -3310,7 +3583,6 @@ void mouse(int glut_button, int state, int x, int y) if (!io.WantCaptureMouse) { - if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_LEFT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift)) { if (session.ground_control_points.is_imgui) @@ -3328,11 +3600,12 @@ void mouse(int glut_button, int state, int x, int y) session.control_points.index_picked_point = -1; int i = session.control_points.index_pose; - if (session.control_points.index_pose >= 0 && session.control_points.index_pose < session.point_clouds_container.point_clouds.size()) + if (session.control_points.index_pose >= 0 && + session.control_points.index_pose < session.point_clouds_container.point_clouds.size()) { for (size_t j = 0; j < session.point_clouds_container.point_clouds[i].points_local.size(); j++) { - const auto &p = session.point_clouds_container.point_clouds[i].points_local[j]; + const auto& p = session.point_clouds_container.point_clouds[i].points_local[j]; Eigen::Vector3d vp = session.point_clouds_container.point_clouds[i].m_pose * p; double dist = distance_point_to_line(vp, laser_beam); @@ -3419,7 +3692,7 @@ void mouse(int glut_button, int state, int x, int y) } } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { try { @@ -3430,17 +3703,14 @@ int main(int argc, char *argv[]) ImGui_ImplOpenGL2_Shutdown(); ImGui_ImplGLUT_Shutdown(); ImGui::DestroyContext(); - } - catch (const std::bad_alloc& e) + } catch (const std::bad_alloc& e) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } - catch (const std::exception& e) + } catch (const std::exception& e) { std::cout << e.what(); - } - catch (...) + } catch (...) { std::cerr << "Unknown fatal error occurred." << std::endl; } diff --git a/apps/multi_view_tls_registration/perform_experiment.cpp b/apps/multi_view_tls_registration/perform_experiment.cpp index a24a2c27..6d448b03 100644 --- a/apps/multi_view_tls_registration/perform_experiment.cpp +++ b/apps/multi_view_tls_registration/perform_experiment.cpp @@ -1,16 +1,16 @@ -#ifndef _USE_MATH_DEFINES -#define _USE_MATH_DEFINES -#endif #include +// clang-format off +#include +#include +// clang-format on + #include #include #include -#include #include -#include -#include +#include #include @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -40,7 +39,7 @@ namespace fs = std::filesystem; -void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session) +void export_result_to_folder(std::string output_folder_name, ObservationPicking& observation_picking, Session& session) { fs::path path(output_folder_name); std::string file_name_rms = "rms.csv"; @@ -71,7 +70,7 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking outfile_initial.open(path_initial, std::ios_base::app); outfile_result.open(path_result, std::ios_base::app); - const auto &intersection = observation_picking.intersections[i]; + const auto& intersection = observation_picking.intersections[i]; TaitBryanPose pose; pose.px = intersection.translation[0]; pose.py = intersection.translation[1]; @@ -90,8 +89,8 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking for (int pc_index = 0; pc_index < session.point_clouds_container.point_clouds.size(); pc_index++) { - const auto &pc = session.point_clouds_container.point_clouds[pc_index]; - for (const auto &p : pc.points_local) + const auto& pc = session.point_clouds_container.point_clouds[pc_index]; + for (const auto& p : pc.points_local) { Eigen::Vector3d vpi = pc.m_initial_pose * p; Eigen::Vector3d vpr = pc.m_pose * p; @@ -105,7 +104,8 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking { if (fabs(vpit.z()) < h) { - outfile_initial << vpit.x() << ";" << vpit.y() << ";" << vpit.z() << ";" << pc_index << ";1;" << i << ";" << pc.file_name << std::endl; + outfile_initial << vpit.x() << ";" << vpit.y() << ";" << vpit.z() << ";" << pc_index << ";1;" << i << ";" + << pc.file_name << std::endl; } } } @@ -115,7 +115,8 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking { if (fabs(vprt.z()) < h) { - outfile_result << vprt.x() << ";" << vprt.y() << ";" << vprt.z() << ";" << pc_index << ";0;" << i << ";" << pc.file_name << std::endl; + outfile_result << vprt.x() << ";" << vprt.y() << ";" << vprt.z() << ";" << pc_index << ";0;" << i << ";" + << pc.file_name << std::endl; } } } @@ -124,14 +125,14 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking outfile_initial.close(); outfile_result.close(); - const auto &obs = observation_picking.observations[i]; + const auto& obs = observation_picking.observations[i]; double rms_initial = 0.0; int sum = 0; double rms_result = 0.0; - for (const auto &[key1, value1] : obs) + for (const auto& [key1, value1] : obs) { - for (const auto &[key2, value2] : obs) + for (const auto& [key2, value2] : obs) { if (key1 != key2) { @@ -167,7 +168,7 @@ void export_result_to_folder(std::string output_folder_name, ObservationPicking session.point_clouds_container.save_poses(path_poses.string(), false); } -void export_result_to_folder(std::string output_folder_name, int method_id, ObservationPicking &observation_picking, Session &session) +void export_result_to_folder(std::string output_folder_name, int method_id, ObservationPicking& observation_picking, Session& session) { fs::path path(output_folder_name); path /= std::to_string(method_id); @@ -175,14 +176,16 @@ void export_result_to_folder(std::string output_folder_name, int method_id, Obse export_result_to_folder(path.string(), observation_picking, session); } -template -void append_to_result_file(std::string file_name, std::string method, const T &t, float rms, int id_method, std::chrono::milliseconds elapsed) +template +void append_to_result_file( + std::string file_name, std::string method, const T& t, float rms, int id_method, std::chrono::milliseconds elapsed) { std::ofstream outfile; outfile.open(file_name, std::ios_base::app); - outfile << method << ";" << id_method << ";" << int(t.is_gauss_newton) << ";" << int(t.is_levenberg_marguardt) << ";" << int(t.is_wc) << ";" << int(t.is_cw) - << ";" << int(t.is_tait_bryan_angles) << ";" << int(t.is_quaternion) << ";" << int(t.is_rodrigues) << ";" << int(t.is_lie_algebra_left_jacobian) - << ";" << int(t.is_lie_algebra_right_jacobian) << ";" << rms << ";" << elapsed.count() << std::endl; + outfile << method << ";" << id_method << ";" << int(t.is_gauss_newton) << ";" << int(t.is_levenberg_marguardt) << ";" << int(t.is_wc) + << ";" << int(t.is_cw) << ";" << int(t.is_tait_bryan_angles) << ";" << int(t.is_quaternion) << ";" << int(t.is_rodrigues) << ";" + << int(t.is_lie_algebra_left_jacobian) << ";" << int(t.is_lie_algebra_right_jacobian) << ";" << rms << ";" << elapsed.count() + << std::endl; outfile.close(); } @@ -190,7 +193,9 @@ void create_header(std::string file_name) { std::ofstream outfile; outfile.open(file_name); - outfile << "method;id_method;gauss_newton;levenberg_marguardt;wc;cw;tait_bryan_angles;quaternion;rodrigues;Lie_algebra_left_jacobian;Lie_algebra_right_jacobian;rms;elapsed_time_miliseconds" << std::endl; + outfile << "method;id_method;gauss_newton;levenberg_marguardt;wc;cw;tait_bryan_angles;quaternion;rodrigues;Lie_algebra_left_jacobian;" + "Lie_algebra_right_jacobian;rms;elapsed_time_miliseconds" + << std::endl; outfile.close(); } @@ -198,35 +203,41 @@ void add_initial_rms_to_file(std::string file_name, float rms) { std::ofstream outfile; outfile.open(file_name, std::ios_base::app); - // outfile << "method;is_gauss_newton;is_levenberg_marguardt;is_wc;is_cw;is_tait_bryan_angles;is_quaternion;is_rodrigues;is_Lie_algebra_left;Lie_algebra_right;rms;elapsed_time_miliseconds" << std::endl; + // outfile << + // "method;is_gauss_newton;is_levenberg_marguardt;is_wc;is_cw;is_tait_bryan_angles;is_quaternion;is_rodrigues;is_Lie_algebra_left;Lie_algebra_right;rms;elapsed_time_miliseconds" + // << std::endl; outfile << "initial_rms;0;0;0;0;0;0;0;0;0;0;" << rms << ";0" << std::endl; outfile.close(); } -void reset_poses(Session &session) +void reset_poses(Session& session) { for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) { session.point_clouds_container.point_clouds[i].m_pose = session.point_clouds_container.point_clouds[i].m_initial_pose; - 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].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)rad2deg(session.point_clouds_container.point_clouds[i].pose.om); - session.point_clouds_container.point_clouds[i].gui_rotation[1] = (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); - session.point_clouds_container.point_clouds[i].gui_rotation[2] = (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); + session.point_clouds_container.point_clouds[i].gui_rotation[0] = + (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.om); + session.point_clouds_container.point_clouds[i].gui_rotation[1] = + (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); + session.point_clouds_container.point_clouds[i].gui_rotation[2] = + (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); } } -double compute_rms(bool initial, Session &session, ObservationPicking &observation_picking) +double compute_rms(bool initial, Session& session, ObservationPicking& observation_picking) { double rms = 0.0; int sum = 0; - for (const auto &obs : observation_picking.observations) + for (const auto& obs : observation_picking.observations) { - for (const auto &[key1, value1] : obs) + for (const auto& [key1, value1] : obs) { - for (const auto &[key2, value2] : obs) + for (const auto& [key2, value2] : obs) { if (key1 != key2) { @@ -261,8 +272,13 @@ double compute_rms(bool initial, Session &session, ObservationPicking &observati } } -void perform_experiment_on_windows(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, - RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam) +void perform_experiment_on_windows( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam) { bool compute_mean_and_cov_for_bucket = false; session.point_clouds_container.show_with_initial_pose = false; @@ -314,8 +330,7 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa auto start = std::chrono::system_clock::now(); icp.optimization_point_to_point_source_to_target(session.point_clouds_container); auto end = std::chrono::system_clock::now(); - auto elapsed = - std::chrono::duration_cast(end - start); + auto elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); std::cout << "final RMS: " << rms << std::endl; @@ -1058,7 +1073,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa registration_plane_feature.is_lie_algebra_right_jacobian = false; start = std::chrono::system_clock::now(); - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); end = std::chrono::system_clock::now(); elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); @@ -1072,7 +1088,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa registration_plane_feature.is_levenberg_marguardt = true; start = std::chrono::system_clock::now(); - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); end = std::chrono::system_clock::now(); elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); @@ -1088,7 +1105,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa registration_plane_feature.is_lie_algebra_right_jacobian = true; start = std::chrono::system_clock::now(); - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); end = std::chrono::system_clock::now(); elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); @@ -1102,7 +1120,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa registration_plane_feature.is_levenberg_marguardt = true; start = std::chrono::system_clock::now(); - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); end = std::chrono::system_clock::now(); elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); @@ -1862,7 +1881,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 90; - append_to_result_file(result_file, "pose_graph_slam (point_to_point_lie_algebra_left_jacobian)", pose_graph_slam, rms, id_method, elapsed); + append_to_result_file( + result_file, "pose_graph_slam (point_to_point_lie_algebra_left_jacobian)", pose_graph_slam, rms, id_method, elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); //--91 @@ -1877,7 +1897,8 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 91; - append_to_result_file(result_file, "pose_graph_slam (point_to_point_lie_algebra_right_jacobian)", pose_graph_slam, rms, id_method, elapsed); + append_to_result_file( + result_file, "pose_graph_slam (point_to_point_lie_algebra_right_jacobian)", pose_graph_slam, rms, id_method, elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); //--92 @@ -1892,7 +1913,13 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 92; - append_to_result_file(result_file, "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_left_jacobian)", pose_graph_slam, rms, id_method, elapsed); + append_to_result_file( + result_file, + "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_left_jacobian)", + pose_graph_slam, + rms, + id_method, + elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); //--93 @@ -1907,12 +1934,23 @@ void perform_experiment_on_windows(Session &session, ObservationPicking &observa elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 93; - append_to_result_file(result_file, "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_right_jacobian)", pose_graph_slam, rms, id_method, elapsed); + append_to_result_file( + result_file, + "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_right_jacobian)", + pose_graph_slam, + rms, + id_method, + elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); } -void perform_experiment_on_linux(Session &session, ObservationPicking &observation_picking, ICP &icp, NDT &ndt, - RegistrationPlaneFeature ®istration_plane_feature, PoseGraphSLAM &pose_graph_slam) +void perform_experiment_on_linux( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam) { fs::path path_result(session.working_directory); path_result /= "results_linux"; @@ -1973,8 +2011,7 @@ void perform_experiment_on_linux(Session &session, ObservationPicking &observati auto start = std::chrono::system_clock::now(); pose_graph_slam.optimize(session.point_clouds_container); auto end = std::chrono::system_clock::now(); - auto elapsed = - std::chrono::duration_cast(end - start); + auto elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 94; @@ -1990,8 +2027,7 @@ void perform_experiment_on_linux(Session &session, ObservationPicking &observati start = std::chrono::system_clock::now(); pose_graph_slam.optimize(session.point_clouds_container); end = std::chrono::system_clock::now(); - elapsed = - std::chrono::duration_cast(end - start); + elapsed = std::chrono::duration_cast(end - start); rms = compute_rms(false, session, observation_picking); id_method = 95; @@ -2022,8 +2058,7 @@ void perform_experiment_on_linux(Session &session, ObservationPicking &observati append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_ndt)", pose_graph_slam, rms, id_method, elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); session.point_clouds_container = temp_data; - } - catch (std::exception &e) + } catch (std::exception& e) { std::cout << e.what() << std::endl; rms = compute_rms(false, session, observation_picking); @@ -2049,8 +2084,7 @@ void perform_experiment_on_linux(Session &session, ObservationPicking &observati append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_icp)", pose_graph_slam, rms, id_method, elapsed); export_result_to_folder(path_result.string(), id_method, observation_picking, session); session.point_clouds_container = temp_data; - } - catch (std::exception &e) + } catch (std::exception& e) { std::cout << e.what() << std::endl; rms = compute_rms(false, session, observation_picking); diff --git a/apps/precision_forestry_tools/CMakeLists.txt b/apps/precision_forestry_tools/CMakeLists.txt index 745c4ac9..a2ede28f 100644 --- a/apps/precision_forestry_tools/CMakeLists.txt +++ b/apps/precision_forestry_tools/CMakeLists.txt @@ -24,15 +24,15 @@ target_include_directories( precision_forestry_tools PRIVATE include ${REPOSITORY_DIRECTORY}/core/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include) if(WIN32) diff --git a/apps/precision_forestry_tools/precision_forestry_tools.cpp b/apps/precision_forestry_tools/precision_forestry_tools.cpp index 03f07f27..43c8bb76 100644 --- a/apps/precision_forestry_tools/precision_forestry_tools.cpp +++ b/apps/precision_forestry_tools/precision_forestry_tools.cpp @@ -1,5 +1,8 @@ +// clang-format off #include #include +// clang-format on + #include #include @@ -7,6 +10,7 @@ #include #include #include + #include #include @@ -39,12 +43,12 @@ float translate_z = -50.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}; +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_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; bool is_decimate = true; double bucket_x = 0.05; @@ -64,620 +68,640 @@ double local_shape_feature_radious = 0.2; namespace fs = std::filesystem; -std::vector get_lowest_points_indexes(const PointCloud &pc, Eigen::Vector2d bucket_dim_xy) +std::vector get_lowest_points_indexes(const PointCloud& pc, Eigen::Vector2d bucket_dim_xy) { - std::vector indexes; - - std::vector> indexes_tuple; + std::vector indexes; - for (size_t i = 0; i < pc.points_local.size(); i++) - { - unsigned long long int index = get_rgd_index_2D(pc.points_local[i], bucket_dim_xy); - indexes_tuple.emplace_back(index, i, pc.points_local[i].z()); - } + std::vector> indexes_tuple; - std::sort(indexes_tuple.begin(), indexes_tuple.end(), - [](const std::tuple &a, const std::tuple &b) - { return (std::get<0>(a) == std::get<0>(b)) ? (std::get<2>(a) < std::get<2>(b)) : (std::get<0>(a) < std::get<0>(b)); }); - - for (int i = 0; i < indexes_tuple.size(); i++) - { - if (i == 0) + for (size_t i = 0; i < pc.points_local.size(); i++) { - indexes.push_back(std::get<1>(indexes_tuple[i])); + unsigned long long int index = get_rgd_index_2D(pc.points_local[i], bucket_dim_xy); + indexes_tuple.emplace_back(index, i, pc.points_local[i].z()); } - else + + std::sort( + indexes_tuple.begin(), + indexes_tuple.end(), + [](const std::tuple& a, + const std::tuple& b) + { + return (std::get<0>(a) == std::get<0>(b)) ? (std::get<2>(a) < std::get<2>(b)) : (std::get<0>(a) < std::get<0>(b)); + }); + + for (int i = 0; i < indexes_tuple.size(); i++) { - if (std::get<0>(indexes_tuple[i - 1]) != std::get<0>(indexes_tuple[i])) - { - indexes.push_back(std::get<1>(indexes_tuple[i])); - } + if (i == 0) + { + indexes.push_back(std::get<1>(indexes_tuple[i])); + } + else + { + if (std::get<0>(indexes_tuple[i - 1]) != std::get<0>(indexes_tuple[i])) + { + indexes.push_back(std::get<1>(indexes_tuple[i])); + } + } } - } - return indexes; + return indexes; } 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); + 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, + 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(); + 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); + 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 + if (!io.WantCaptureMouse) { - 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; - } - } + 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(); + mouse_old_x = x; + mouse_old_y = y; + } + glutPostRedisplay(); } void project_gui() { - if (ImGui::Begin("main gui window")) - { - ImGui::Checkbox("is_decimate", &is_decimate); - if (is_decimate) + if (ImGui::Begin("main gui window")) { - ImGui::InputDouble("bucket_x", &bucket_x); - ImGui::InputDouble("bucket_y", &bucket_y); - ImGui::InputDouble("bucket_z", &bucket_z); - } - ImGui::Checkbox("calculate_offset", &calculate_offset); - - ImGui::InputDouble("lowest_points_resolution", &surface.lowest_points_resolution); - ImGui::InputDouble("lowest_points_filter_resolution", &surface.lowest_points_filter_resolution); - ImGui::InputDouble("z_sigma_threshold", &surface.z_sigma_threshold); - - ImGui::InputDouble("surface_resolution", &surface.surface_resolution); + ImGui::Checkbox("is_decimate", &is_decimate); + if (is_decimate) + { + ImGui::InputDouble("bucket_x", &bucket_x); + ImGui::InputDouble("bucket_y", &bucket_y); + ImGui::InputDouble("bucket_z", &bucket_z); + } + ImGui::Checkbox("calculate_offset", &calculate_offset); - ImGui::InputInt("viewer_decmiate_point_cloud", &viewer_decmiate_point_cloud); - if (viewer_decmiate_point_cloud < 1) - { - viewer_decmiate_point_cloud = 1; - } + ImGui::InputDouble("lowest_points_resolution", &surface.lowest_points_resolution); + ImGui::InputDouble("lowest_points_filter_resolution", &surface.lowest_points_filter_resolution); + ImGui::InputDouble("z_sigma_threshold", &surface.z_sigma_threshold); - if (ImGui::Button("Load *.laz file")) - { - std::vector input_file_names; - input_file_names = mandeye::fd::OpenFileDialog("Load laz files", {}, true); + ImGui::InputDouble("surface_resolution", &surface.surface_resolution); - if (input_file_names.size() == 1) - { - // std::cout << "Las/Laz file (only 1):" << std::endl; - for (size_t i = 0; i < input_file_names.size(); i++) + ImGui::InputInt("viewer_decmiate_point_cloud", &viewer_decmiate_point_cloud); + if (viewer_decmiate_point_cloud < 1) { - std::cout << input_file_names[i] << std::endl; + viewer_decmiate_point_cloud = 1; } - session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + if (ImGui::Button("Load *.laz file")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load laz files", {}, true); + + if (input_file_names.size() == 1) + { + // std::cout << "Las/Laz file (only 1):" << std::endl; + for (size_t i = 0; i < input_file_names.size(); i++) + { + std::cout << input_file_names[i] << std::endl; + } + + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + + if (!session.point_clouds_container.load_whu_tls( + input_file_names, is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset, session.load_cache_mode)) + { + std::cout << "check input files laz/las" << std::endl; + } + else + { + std::cout << "loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + } + } + else + { + std::cout << "please mark only 1 file" << std::endl; + } + } - if (!session.point_clouds_container.load_whu_tls(input_file_names, is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset, session.load_cache_mode)) + if (ImGui::Button("get indexes lowest points")) { - std::cout << "check input files laz/las" << std::endl; + if (session.point_clouds_container.point_clouds.size() > 0) + { + lowest_points_indexes = get_lowest_points_indexes( + session.point_clouds_container.point_clouds[0], + Eigen::Vector2d(surface.lowest_points_resolution, surface.lowest_points_resolution)); + } + else + { + std::cout << "point cloud data is empty, please load data" << std::endl; + } } - else + + if (ImGui::Button("Filter lower points indexes")) { - std::cout << "loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + lowest_points_indexes = surface.get_filtered_indexes( + session.point_clouds_container.point_clouds[0].points_local, + lowest_points_indexes, + { surface.lowest_points_filter_resolution, surface.lowest_points_filter_resolution }); } - } - else - { - std::cout << "please mark only 1 file" << std::endl; - } - } - if (ImGui::Button("get indexes lowest points")) - { - if (session.point_clouds_container.point_clouds.size() > 0) - { - lowest_points_indexes = get_lowest_points_indexes(session.point_clouds_container.point_clouds[0], Eigen::Vector2d(surface.lowest_points_resolution, surface.lowest_points_resolution)); - } - else - { - std::cout << "point cloud data is empty, please load data" << std::endl; - } - } + if (ImGui::Button("generate_ground_mesh")) + { + std::vector lowest_points; + for (int i = 0; i < lowest_points_indexes.size(); i++) + { + lowest_points.push_back(session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]]); + } - if (ImGui::Button("Filter lower points indexes")) - { - lowest_points_indexes = surface.get_filtered_indexes(session.point_clouds_container.point_clouds[0].points_local, lowest_points_indexes, {surface.lowest_points_filter_resolution, surface.lowest_points_filter_resolution}); - } + surface.generate_initial_surface(lowest_points); + } - if (ImGui::Button("generate_ground_mesh")) - { - std::vector lowest_points; - for (int i = 0; i < lowest_points_indexes.size(); i++) - { - lowest_points.push_back(session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]]); - } + if (ImGui::Button("align surface to ground points")) + { + std::vector lowest_points; + for (int i = 0; i < lowest_points_indexes.size(); i++) + { + lowest_points.push_back(session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]]); + } + surface.align_surface_to_ground_points(lowest_points); + } + ImGui::SameLine(); + ImGui::Checkbox("robust_kernel", &surface.robust_kernel); - surface.generate_initial_surface(lowest_points); - } + ImGui::InputDouble("distance_to_ground_threshold_bottom", &distance_to_ground_threshold_bottom); + ImGui::InputDouble("distance_to_ground_threshold_up", &distance_to_ground_threshold_up); - if (ImGui::Button("align surface to ground points")) - { - std::vector lowest_points; - for (int i = 0; i < lowest_points_indexes.size(); i++) - { - lowest_points.push_back(session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]]); - } - surface.align_surface_to_ground_points(lowest_points); - } - ImGui::SameLine(); - ImGui::Checkbox("robust_kernel", &surface.robust_kernel); + if (ImGui::Button("remove surface points")) + { + if (session.point_clouds_container.point_clouds.size() > 0) + { + std::vector points_without_surface = surface.get_points_without_surface( + session.point_clouds_container.point_clouds[0].points_local, + distance_to_ground_threshold_bottom, + distance_to_ground_threshold_up, + Eigen::Vector2d(surface.surface_resolution, surface.surface_resolution)); + + session.point_clouds_container.point_clouds[0].points_local = points_without_surface; + session.point_clouds_container.point_clouds[0].normal_vectors_local.clear(); + session.point_clouds_container.point_clouds[0].colors.clear(); + session.point_clouds_container.point_clouds[0].points_type.clear(); + session.point_clouds_container.point_clouds[0].intensities.clear(); + session.point_clouds_container.point_clouds[0].timestamps.clear(); + + lowest_points_indexes.clear(); + points_with_lsf.clear(); + + for (int i = 0; i < session.point_clouds_container.point_clouds[0].points_local.size(); i++) + { + session.point_clouds_container.point_clouds[0].normal_vectors_local.push_back({ 0, 0, 0 }); + session.point_clouds_container.point_clouds[0].colors.push_back({ 0, 0, 0 }); + session.point_clouds_container.point_clouds[0].points_type.push_back(0); + session.point_clouds_container.point_clouds[0].intensities.push_back(0); + session.point_clouds_container.point_clouds[0].timestamps.push_back(0); + } + } + } - ImGui::InputDouble("distance_to_ground_threshold_bottom", &distance_to_ground_threshold_bottom); - ImGui::InputDouble("distance_to_ground_threshold_up", &distance_to_ground_threshold_up); + ImGui::InputDouble("local_shape_feature_radious", &local_shape_feature_radious); - if (ImGui::Button("remove surface points")) - { - if (session.point_clouds_container.point_clouds.size() > 0) - { - std::vector points_without_surface = - surface.get_points_without_surface(session.point_clouds_container.point_clouds[0].points_local, distance_to_ground_threshold_bottom, distance_to_ground_threshold_up, Eigen::Vector2d(surface.surface_resolution, surface.surface_resolution)); - - session.point_clouds_container.point_clouds[0].points_local = points_without_surface; - session.point_clouds_container.point_clouds[0].normal_vectors_local.clear(); - session.point_clouds_container.point_clouds[0].colors.clear(); - session.point_clouds_container.point_clouds[0].points_type.clear(); - session.point_clouds_container.point_clouds[0].intensities.clear(); - session.point_clouds_container.point_clouds[0].timestamps.clear(); - - lowest_points_indexes.clear(); - points_with_lsf.clear(); - - for (int i = 0; i < session.point_clouds_container.point_clouds[0].points_local.size(); i++) + if (ImGui::Button("get local shape features")) + { + std::cout << "get local shape features" << std::endl; + if (session.point_clouds_container.point_clouds.size() > 0) + { + // std::vector points_with_lsf; + points_with_lsf.clear(); + LocalShapeFeatures::Params params; + + params.search_radious = + Eigen::Vector3d(local_shape_feature_radious, local_shape_feature_radious, local_shape_feature_radious); + params.radious = local_shape_feature_radious; + + for (int i = 0; i < session.point_clouds_container.point_clouds[0].points_local.size(); i++) + { + LocalShapeFeatures::PointWithLocalShapeFeatures point; + point.coordinates_global = session.point_clouds_container.point_clouds[0].points_local[i]; + points_with_lsf.push_back(point); + } + + LocalShapeFeatures lsf; + lsf.calculate_local_shape_features(points_with_lsf, params); + + // for (const auto &p : points_with_lsf) + //{ + // if (p.valid) + // std::cout << p.planarity << "," << p.cylindrical_likeness << "," << p.plane_likeness << "," << p.sphericity << "," << + // p.change_of_curvature << "," << p.omnivariance << "," << p.eigen_entropy << std::endl; + // } + } + else + { + std::cout << "please load point cloud" << std::endl; + } + } + if (points_with_lsf.size() > 0) { - session.point_clouds_container.point_clouds[0].normal_vectors_local.push_back({0, 0, 0}); - session.point_clouds_container.point_clouds[0].colors.push_back({0, 0, 0}); - session.point_clouds_container.point_clouds[0].points_type.push_back(0); - session.point_clouds_container.point_clouds[0].intensities.push_back(0); - session.point_clouds_container.point_clouds[0].timestamps.push_back(0); + if (ImGui::Button("save features")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save local shape features", {}, ""); + std::cout << "file to save: '" << output_file_name << "'" << std::endl; + + if (output_file_name.size() > 0) + { + std::ofstream file; + + file.open(output_file_name); + + file << "x,y,z,planarity,cylindrical_likeness,plane_likeness,sphericity,change_of_curvature,omnivariance,verticality" + << std::endl; + + for (const auto& p : points_with_lsf) + { + file << p.coordinates_global.x() << "," << p.coordinates_global.y() << "," << p.coordinates_global.z() << "," + << p.planarity << "," << p.cylindrical_likeness << "," << p.plane_likeness << "," << p.sphericity << "," + << p.change_of_curvature << "," << p.omnivariance << "," << p.eigen_entropy << "," << p.verticality + << std::endl; + } + file.close(); + } + } + ImGui::Checkbox("show_normal_vectors", &show_normal_vectors); } - } + ImGui::End(); } + return; +} - ImGui::InputDouble("local_shape_feature_radious", &local_shape_feature_radious); +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 (ImGui::Button("get local shape features")) + if (is_ortho) { - std::cout << "get local shape features" << std::endl; - if (session.point_clouds_container.point_clouds.size() > 0) - { - // std::vector points_with_lsf; - points_with_lsf.clear(); - LocalShapeFeatures::Params params; + 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); - params.search_radious = Eigen::Vector3d(local_shape_feature_radious, local_shape_feature_radious, local_shape_feature_radious); - params.radious = local_shape_feature_radious; + 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); - for (int i = 0; i < session.point_clouds_container.point_clouds[0].points_local.size(); i++) - { - LocalShapeFeatures::PointWithLocalShapeFeatures point; - point.coordinates_global = session.point_clouds_container.point_clouds[0].points_local[i]; - points_with_lsf.push_back(point); - } + 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; - LocalShapeFeatures lsf; - lsf.calculate_local_shape_features(points_with_lsf, params); - - // for (const auto &p : points_with_lsf) - //{ - // if (p.valid) - // std::cout << p.planarity << "," << p.cylindrical_likeness << "," << p.plane_likeness << "," << p.sphericity << "," << p.change_of_curvature << "," << p.omnivariance << "," << p.eigen_entropy << std::endl; - // } - } - else - { - std::cout << "please load point cloud" << std::endl; - } + 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); } - if (points_with_lsf.size() > 0) + + 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) { - if (ImGui::Button("save features")) - { - std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("Save local shape features", {}, ""); - std::cout << "file to save: '" << output_file_name << "'" << std::endl; + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - if (output_file_name.size() > 0) - { - std::ofstream file; + 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())); - file.open(output_file_name); + Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); + viewTranslation2.translate(-rotation_center); - file << "x,y,z,planarity,cylindrical_likeness,plane_likeness,sphericity,change_of_curvature,omnivariance,verticality" << std::endl; + Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; - for (const auto &p : points_with_lsf) - { - file << p.coordinates_global.x() << "," << p.coordinates_global.y() << "," << p.coordinates_global.z() << "," << p.planarity << "," << p.cylindrical_likeness << "," << p.plane_likeness << "," << p.sphericity << "," << p.change_of_curvature << "," << p.omnivariance << "," << p.eigen_entropy << "," << p.verticality << std::endl; - } - file.close(); - } - } - ImGui::Checkbox("show_normal_vectors", &show_normal_vectors); + 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(); } - 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 (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 (is_ortho) - { + if (show_axes) + { + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(100, 0.0f, 0.0f); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 100, 0.0f); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 100); + glEnd(); + } - 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(); - } + for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + session.point_clouds_container.point_clouds[i].render( + false, ObservationPicking(), viewer_decmiate_point_cloud, false, false, false, 100000, false); + } - 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(100, 0.0f, 0.0f); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 100, 0.0f); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 100); - glEnd(); - } - - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - session.point_clouds_container.point_clouds[i].render(false, ObservationPicking(), viewer_decmiate_point_cloud, false, false, false, 100000, false); - } - - if (session.point_clouds_container.point_clouds.size() == 1) - { - glColor3f(1.0f, 0.0f, 0.0f); - glPointSize(3); - glBegin(GL_POINTS); - for (int i = 0; i < lowest_points_indexes.size(); i++) + if (session.point_clouds_container.point_clouds.size() == 1) { - glVertex3f(session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x(), - session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y(), - session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z()); + glColor3f(1.0f, 0.0f, 0.0f); + glPointSize(3); + glBegin(GL_POINTS); + for (int i = 0; i < lowest_points_indexes.size(); i++) + { + glVertex3f( + session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x(), + session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y(), + session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z()); + + // std::cout << session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x() << " " << + // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y() << " " << + // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z() << std::endl; + } + glEnd(); + glPointSize(1); + } + // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud); - // std::cout << session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].x() << " " << - // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].y() << " " << - // session.point_clouds_container.point_clouds[0].points_local[lowest_points_indexes[i]].z() << std::endl; + if (show_normal_vectors) + { + glBegin(GL_LINES); + + for (int i = 0; i < points_with_lsf.size(); i += viewer_decmiate_point_cloud) + { + const auto& p = points_with_lsf[i]; + if (p.valid) + { + glColor3f(fabs(p.normal_vector.x()), fabs(p.normal_vector.y()), fabs(p.normal_vector.z())); + glVertex3f(p.coordinates_global.x(), p.coordinates_global.y(), p.coordinates_global.z()); + glVertex3f( + p.coordinates_global.x() + p.normal_vector.x(), + p.coordinates_global.y() + p.normal_vector.y(), + p.coordinates_global.z() + p.normal_vector.z()); + } + } + glEnd(); } - glEnd(); - glPointSize(1); - } - // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud); - if (show_normal_vectors) - { + // + /*glColor3f(0, 0, 0); glBegin(GL_LINES); - - for (int i = 0; i < points_with_lsf.size(); i += viewer_decmiate_point_cloud) + for (const auto &edge : ground_mesh.edges) { - const auto &p = points_with_lsf[i]; - if (p.valid) - { - glColor3f(fabs(p.normal_vector.x()), fabs(p.normal_vector.y()), fabs(p.normal_vector.z())); - glVertex3f(p.coordinates_global.x(), p.coordinates_global.y(), p.coordinates_global.z()); - glVertex3f(p.coordinates_global.x() + p.normal_vector.x(), p.coordinates_global.y() + p.normal_vector.y(), p.coordinates_global.z() + p.normal_vector.z()); - } + glVertex3f(ground_mesh.nodes[edge.index_from].pose.position.x(), ground_mesh.nodes[edge.index_from].pose.position.y(), + ground_mesh.nodes[edge.index_from].pose.position.z()); glVertex3f(ground_mesh.nodes[edge.index_to].pose.position.x(), + ground_mesh.nodes[edge.index_to].pose.position.y(), ground_mesh.nodes[edge.index_to].pose.position.z()); } - glEnd(); - } + glEnd();*/ - // - /*glColor3f(0, 0, 0); - glBegin(GL_LINES); - for (const auto &edge : ground_mesh.edges) - { - glVertex3f(ground_mesh.nodes[edge.index_from].pose.position.x(), ground_mesh.nodes[edge.index_from].pose.position.y(), ground_mesh.nodes[edge.index_from].pose.position.z()); - glVertex3f(ground_mesh.nodes[edge.index_to].pose.position.x(), ground_mesh.nodes[edge.index_to].pose.position.y(), ground_mesh.nodes[edge.index_to].pose.position.z()); - } - glEnd();*/ + // + surface.render(); - // - surface.render(); + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + project_gui(); - project_gui(); + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); + glutSwapBuffers(); + glutPostRedisplay(); } -bool initGL(int *argc, char **argv) +bool initGL(int* argc, char** argv) { - glutInit(argc, argv); - glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); - glutInitWindowSize(window_width, window_height); - - glutCreateWindow("precision_forestry_tools " 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; + glutInit(argc, argv); + glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); + glutInitWindowSize(window_width, window_height); + + glutCreateWindow("precision_forestry_tools " 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 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; - - if (!io.WantCaptureMouse) - { - if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) + 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; + + 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; + 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; } - mouse_old_x = x; - mouse_old_y = y; - } } void wheel(int button, int dir, int x, int y) { - if (dir > 0) - { - if (is_ortho) + if (dir > 0) { - camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; + 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; + if (camera_ortho_xy_view_zoom < 0.1) + { + camera_ortho_xy_view_zoom = 0.1; + } + } + else + { + translate_z -= 0.05f * translate_z; + } } else { - translate_z += 0.05f * translate_z; + if (is_ortho) + { + camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + } + else + { + translate_z += 0.05f * translate_z; + } } - } - return; + return; } -int main(int argc, char *argv[]) +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; + 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 diff --git a/apps/quick_start_demo/CMakeLists.txt b/apps/quick_start_demo/CMakeLists.txt index fcad585a..0402513d 100644 --- a/apps/quick_start_demo/CMakeLists.txt +++ b/apps/quick_start_demo/CMakeLists.txt @@ -14,19 +14,19 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) target_link_libraries( quick_start_demo diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index 41fd0191..175bf6e1 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -1,12 +1,15 @@ +// clang-format off +#include +#include +//clang-format on #include #include #include -#include #include -#include -#include +#include + #include #include diff --git a/apps/single_session_manual_coloring/CMakeLists.txt b/apps/single_session_manual_coloring/CMakeLists.txt index 5d2b43b1..bcc1f1ea 100644 --- a/apps/single_session_manual_coloring/CMakeLists.txt +++ b/apps/single_session_manual_coloring/CMakeLists.txt @@ -9,17 +9,17 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${FREEGLUT_INCLUDE_DIR}) target_compile_definitions(single_session_manual_coloring PRIVATE WITH_GUI=1) diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp index 42ed5e3d..ce166575 100644 --- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp +++ b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp @@ -1,5 +1,4 @@ -#include #include #include "imgui.h" @@ -13,14 +12,13 @@ #include "color_las_loader.h" #include "pfd_wrapper.hpp" -#include -#include -#include -#include #include #include +#include #include +#include +#include #include @@ -28,10 +26,13 @@ #include #include -#include #include +#include +#include +#include + const unsigned int window_width = 800; const unsigned int window_height = 600; double camera_ortho_xy_view_zoom = 10; @@ -51,18 +52,12 @@ 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}; +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_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = {1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1}; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; int decimation_step = 1000; Session session; @@ -77,17 +72,17 @@ namespace SystemData { std::vector points; std::pair clickedRay; - int closestPointIndex{-1}; + int closestPointIndex{ -1 }; std::vector pointPickedImage; std::vector pointPickedPointCloud; - unsigned char *imageData; + unsigned char* imageData; int imageWidth, imageHeight, imageNrChannels; Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); int point_size = 1; -} +} // namespace SystemData void reshape(int w, int h) { @@ -100,12 +95,16 @@ void reshape(int w, int h) } else { - ImGuiIO &io = ImGui::GetIO(); + 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( + -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); @@ -114,7 +113,7 @@ void reshape(int w, int h) void motion(int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); if (!io.WantCaptureMouse) @@ -128,8 +127,10 @@ void motion(int x, int y) 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); + 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; @@ -164,15 +165,36 @@ void motion(int x, int y) glutPostRedisplay(); } -std::vector ApplyColorToPointcloud(const std::vector &pointsRGB, const unsigned char *imageData, int imageWidth, int imageHeight, int nrChannels, const Eigen::Affine3d &transfom) +std::vector ApplyColorToPointcloud( + const std::vector& pointsRGB, + const unsigned char* imageData, + int imageWidth, + int imageHeight, + int nrChannels, + const Eigen::Affine3d& transfom) { std::vector newCloud(pointsRGB.size()); - std::transform(std::execution::par_unseq, pointsRGB.begin(), pointsRGB.end(), newCloud.begin(), [&](mandeye::PointRGB p) - { + std::transform( + std::execution::par_unseq, + pointsRGB.begin(), + pointsRGB.end(), + newCloud.begin(), + [&](mandeye::PointRGB p) + { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); double du, dv; - equrectangular_camera_colinearity_tait_bryan_wc(du,dv, imageHeight, imageWidth, - M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, + equrectangular_camera_colinearity_tait_bryan_wc( + du, + dv, + imageHeight, + imageWidth, + M_PI, + pose.px, + pose.py, + pose.pz, + pose.om, + pose.fi, + pose.ka, p.point.x(), p.point.y(), p.point.z()); @@ -184,11 +206,14 @@ std::vector ApplyColorToPointcloud(const std::vector pointsRGB; - for (const auto &p : session.point_clouds_container.point_clouds){ - if(p.visible){ - for (int index = 0; index < p.points_local.size(); index++){ + for (const auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int index = 0; index < p.points_local.size(); index++) + { mandeye::PointRGB out; out.point = p.m_pose * p.points_local[index]; out.intensity = p.intensities[index]; @@ -269,7 +296,7 @@ void project_gui() std::cout << "input_images list begin" << std::endl; std::cout << "----------------------" << std::endl; - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::cout << "'" << fn << "'" << std::endl; images_file_names.push_back(fn); @@ -344,7 +371,7 @@ void project_gui() if (ImGui::Button("select all")) { - for (auto &pc : session.point_clouds_container.point_clouds) + for (auto& pc : session.point_clouds_container.point_clouds) { pc.visible = true; } @@ -354,15 +381,16 @@ void project_gui() if (ImGui::Button("unselect all")) { - for (auto &pc : session.point_clouds_container.point_clouds) + for (auto& pc : session.point_clouds_container.point_clouds) { pc.visible = false; } } - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++ /*auto &pc : session.point_clouds_container.point_clouds*/) + for (int i = 0; i < session.point_clouds_container.point_clouds.size(); + i++ /*auto &pc : session.point_clouds_container.point_clouds*/) { - auto &pc = session.point_clouds_container.point_clouds[i]; + auto& pc = session.point_clouds_container.point_clouds[i]; ImGui::Text("----------------------------"); ImGui::Checkbox(pc.file_name.c_str(), &pc.visible); @@ -372,7 +400,7 @@ void project_gui() if (session.point_clouds_container.point_clouds.size() == corresponding_images.size() && images_file_names.size() > 0) { int prev = corresponding_images[i]; - + ImGui::InputInt((std::string("image[") + std::to_string(i) + std::string("]")).c_str(), &corresponding_images[i]); if (corresponding_images[i] < 0) { @@ -383,9 +411,11 @@ void project_gui() corresponding_images[i] = images_file_names.size() - 1; } - if (prev != corresponding_images[i]){ + if (prev != corresponding_images[i]) + { namespace SD = SystemData; - SD::imageData = stbi_load(images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); + SD::imageData = stbi_load( + images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); std::cout << "imageWidth: " << SD::imageWidth << std::endl; std::cout << "imageHeight: " << SD::imageHeight << std::endl; std::cout << "imageNrChannels: " << SD::imageNrChannels << std::endl; @@ -393,8 +423,9 @@ void project_gui() /////////////// // std::vector newCloud(session.point_clouds_container.point_clouds[i]..size()); - session.point_clouds_container.point_clouds[i].colors.resize(session.point_clouds_container.point_clouds[i].points_local.size()); - for (auto &c : session.point_clouds_container.point_clouds[i].colors) + session.point_clouds_container.point_clouds[i].colors.resize( + session.point_clouds_container.point_clouds[i].points_local.size()); + for (auto& c : session.point_clouds_container.point_clouds[i].colors) { c.x() = c.y() = c.z() = 0.0; } @@ -409,12 +440,12 @@ void project_gui() point.point = session.point_clouds_container.point_clouds[i].points_local[p]; // point.point = pc.local_trajectory[0].m_pose.inverse() * point.point; // point.point = pc.local_trajectory[offsets[i]].m_pose * point.point; - point.rgb = {0.f, 0.f, 0.f, 1.f}; + point.rgb = { 0.f, 0.f, 0.f, 1.f }; pointsRGB.push_back(point); } - std::vector - pc = ApplyColorToPointcloud(pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, transfom); + std::vector pc = ApplyColorToPointcloud( + pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, transfom); for (int color_idx = 0; color_idx < pc.size(); color_idx++) { @@ -426,18 +457,16 @@ void project_gui() session.point_clouds_container.point_clouds[i].show_color = true; } - //ImGui::InputInt((std::string("trajectory offset[") + std::to_string(i) + std::string("]")).c_str(), &offsets[i]); - //if (offsets[i] < 0) + // ImGui::InputInt((std::string("trajectory offset[") + std::to_string(i) + std::string("]")).c_str(), &offsets[i]); + // if (offsets[i] < 0) //{ // offsets[i] = 0; //} - //if (offsets[i] >= pc.local_trajectory.size() - 1) + // if (offsets[i] >= pc.local_trajectory.size() - 1) //{ // offsets[i] = pc.local_trajectory.size() - 1; //} - - /* if (ImGui::Button(("colorize with '" + images_file_names[corresponding_images[i]] + "'").c_str())) { @@ -448,10 +477,9 @@ void project_gui() // namespace SD = SystemData; - SD::imageData = stbi_load(images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); - std::cout << "imageWidth: " << SD::imageWidth << std::endl; - std::cout << "imageHeight: " << SD::imageHeight << std::endl; - std::cout << "imageNrChannels: " << SD::imageNrChannels << std::endl; + SD::imageData = stbi_load(images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, + &SD::imageNrChannels, 0); std::cout << "imageWidth: " << SD::imageWidth << std::endl; std::cout << "imageHeight: " << + SD::imageHeight << std::endl; std::cout << "imageNrChannels: " << SD::imageNrChannels << std::endl; /////////////// // std::vector newCloud(session.point_clouds_container.point_clouds[i]..size()); @@ -477,7 +505,8 @@ void project_gui() } std::vector - pc = ApplyColorToPointcloud(pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, transfom); + pc = ApplyColorToPointcloud(pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, + transfom); for (int color_idx = 0; color_idx < pc.size(); color_idx++) { @@ -493,7 +522,8 @@ void project_gui() } } - // ImGui::Checkbox(session.point_clouds_container.point_clouds[i].file_name.c_str(), &session.point_clouds_container.point_clouds[i].visible); + // ImGui::Checkbox(session.point_clouds_container.point_clouds[i].file_name.c_str(), + // &session.point_clouds_container.point_clouds[i].visible); #if 0 for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) { @@ -725,7 +755,7 @@ void project_gui() void display() { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); glMatrixMode(GL_PROJECTION); glLoadIdentity(); @@ -733,14 +763,21 @@ void display() 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); + 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); @@ -759,12 +796,11 @@ void display() Eigen::Vector3d v_t = m * v; - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), - v_center_t.x(), v_center_t.y(), v_center_t.z(), - v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt(glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - 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())); + 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); } @@ -824,15 +860,17 @@ void display() session.point_clouds_container.point_clouds[k].point_size = SystemData::point_size; } - session.point_clouds_container.render(observation_picking, decimation_step, false, false, false, false, false, false, false, false, false, false, false, false, 10000); + session.point_clouds_container.render( + observation_picking, decimation_step, false, false, false, false, false, false, false, false, false, false, false, false, 10000); // session.ground_control_points.render(session.point_clouds_container); - /*session.point_clouds_container.render({}, {}, 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.point_clouds_container.render({}, {}, 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);*/ /*if (ImGui::GetIO().KeyCtrl) { @@ -923,7 +961,7 @@ void display() ImGui_ImplOpenGL2_NewFrame(); ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + ImGui::NewFrame(); project_gui(); @@ -969,7 +1007,7 @@ 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(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); int button = -1; if (glut_button == GLUT_LEFT_BUTTON) @@ -984,8 +1022,7 @@ void mouse(int glut_button, int state, int x, int y) io.MouseDown[button] = false; static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && - glutMajorVersion < 3) + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) { wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); } @@ -1009,7 +1046,7 @@ void mouse(int glut_button, int state, int x, int y) } } -bool initGL(int *argc, char **argv) +bool initGL(int* argc, char** argv) { glutInit(argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); @@ -1031,7 +1068,7 @@ bool initGL(int *argc, char **argv) gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); glutReshapeFunc(reshape); ImGui::CreateContext(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); (void)io; // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls @@ -1042,7 +1079,7 @@ bool initGL(int *argc, char **argv) return true; } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { initGL(&argc, argv); glutDisplayFunc(display); diff --git a/apps/split_multi_livox/CMakeLists.txt b/apps/split_multi_livox/CMakeLists.txt index abb15e52..6712d274 100644 --- a/apps/split_multi_livox/CMakeLists.txt +++ b/apps/split_multi_livox/CMakeLists.txt @@ -9,17 +9,17 @@ target_include_directories( PRIVATE include ${REPOSITORY_DIRECTORY}/core/include ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/glew-cmake/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${FREEGLUT_INCLUDE_DIR}) target_link_libraries(split_multi_livox PRIVATE core ${PLATFORM_LASZIP_LIB} diff --git a/apps/split_multi_livox/split_multi_livox.cpp b/apps/split_multi_livox/split_multi_livox.cpp index 76023d0e..a8f50406 100644 --- a/apps/split_multi_livox/split_multi_livox.cpp +++ b/apps/split_multi_livox/split_multi_livox.cpp @@ -1,11 +1,11 @@ -#include -#include -#include #include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include struct Point3Dis { @@ -22,7 +22,7 @@ struct Point3Dil int livoxId; }; -std::vector load_point_cloud(const std::string &lazFile, bool ommit_points_with_timestamp_equals_zero) +std::vector load_point_cloud(const std::string& lazFile, bool ommit_points_with_timestamp_equals_zero) { std::vector points; laszip_POINTER laszip_reader; @@ -39,7 +39,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_p std::abort(); } std::cout << "compressed : " << is_compressed << std::endl; - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -47,7 +47,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_p std::abort(); } fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_reader, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); @@ -67,7 +67,10 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_p } Point3Dil p; - Eigen::Vector3d pf(header->x_offset + header->x_scale_factor * static_cast(point->X), header->y_offset + header->y_scale_factor * static_cast(point->Y), header->z_offset + header->z_scale_factor * static_cast(point->Z)); + Eigen::Vector3d pf( + header->x_offset + header->x_scale_factor * static_cast(point->X), + header->y_offset + header->y_scale_factor * static_cast(point->Y), + header->z_offset + header->z_scale_factor * static_cast(point->Z)); p.point.x() = pf.x(); p.point.y() = pf.y(); p.point.z() = pf.z(); @@ -92,7 +95,7 @@ std::vector load_point_cloud(const std::string &lazFile, bool ommit_p return points; } -std::unordered_map GetIdToStringMapping(const std::string &filename) +std::unordered_map GetIdToStringMapping(const std::string& filename) { std::unordered_map dataMap; std::ifstream fst(filename); @@ -115,7 +118,7 @@ std::unordered_map GetIdToStringMapping(const std::string &fil return dataMap; } -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { std::cout << "Version " HDMAPPING_VERSION_STRING << std::endl; std::vector arguments; @@ -129,9 +132,9 @@ int main(int argc, char *argv[]) std::cout << " " << argv[0] << " \n"; return 0; } - const std::filesystem::path inputLazFile{arguments[0]}; + const std::filesystem::path inputLazFile{ arguments[0] }; const std::filesystem::path inputSnFile = inputLazFile.parent_path() / inputLazFile.stem().concat(".sn"); - const std::filesystem::path outputFile{arguments[1]}; + const std::filesystem::path outputFile{ arguments[1] }; std::cout << "Input Laz file " << inputLazFile << std::endl; std::cout << "Input Sn file " << inputSnFile << std::endl; @@ -150,7 +153,7 @@ int main(int argc, char *argv[]) std::unordered_map> data_separated; - for (const auto &pInput : data) + for (const auto& pInput : data) { const int id = pInput.livoxId; @@ -161,14 +164,14 @@ int main(int argc, char *argv[]) data_separated[id].push_back(pOutput); } - for (const auto &[id, sn] : mapping) + for (const auto& [id, sn] : mapping) { const std::filesystem::path outputFileSn = outputFile.parent_path() / outputFile.stem().concat("_" + sn + ".laz"); - const auto &lidarData = data_separated[id]; + const auto& lidarData = data_separated[id]; std::vector global_pointcloud; - std::vector intensity; + std::vector intensity; std::vector timestamps; - for (const auto &p : lidarData) + for (const auto& p : lidarData) { global_pointcloud.push_back(p.point); intensity.push_back(p.intensity); diff --git a/cmake/imgui.cmake b/cmake/imgui.cmake index 661dd8bf..0da72db6 100644 --- a/cmake/imgui.cmake +++ b/cmake/imgui.cmake @@ -1,6 +1,6 @@ include_guard() -set(IMGUI_LIBRARY_DIRECTORY ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui) +set(IMGUI_LIBRARY_DIRECTORY ${THIRDPARTY_DIRECTORY}/imgui) set(IMGUI_LIBRARY_BACKEND_DIRECTORY ${IMGUI_LIBRARY_DIRECTORY}/backends) set(IMGUI_SOURCE_FILES @@ -22,4 +22,4 @@ add_definitions(-DImDrawIdx=unsigned\ int) add_library(imgui STATIC ${IMGUI_FILES}) target_include_directories( imgui PRIVATE ${IMGUI_LIBRARY_DIRECTORY} ${IMGUI_LIBRARY_BACKEND_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut/include) + ${THIRDPARTY_DIRECTORY}/freeglut/include) diff --git a/cmake/imguizmo.cmake b/cmake/imguizmo.cmake index 4b280aff..a0104b72 100644 --- a/cmake/imguizmo.cmake +++ b/cmake/imguizmo.cmake @@ -1,6 +1,6 @@ include_guard() -set(IMGUIZMO_LIBRARY_DIRECTORY ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo) +set(IMGUIZMO_LIBRARY_DIRECTORY ${THIRDPARTY_DIRECTORY}/ImGuizmo) set(IMGUIZMO_SOURCE_FILES ${IMGUIZMO_LIBRARY_DIRECTORY}/GraphEditor.cpp @@ -22,4 +22,4 @@ set(IMGUIZMO_FILES ${IMGUIZMO_SOURCE_FILES} ${IMGUIZMO_HEADER_FILES}) add_library(imguizmo STATIC ${IMGUIZMO_FILES}) target_include_directories( imguizmo PRIVATE ${IMGUIZMO_LIBRARY_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui) + ${THIRDPARTY_DIRECTORY}/imgui) diff --git a/cmake/implot.cmake b/cmake/implot.cmake index 80f7fee5..c08e0752 100644 --- a/cmake/implot.cmake +++ b/cmake/implot.cmake @@ -1,6 +1,6 @@ include_guard() -set(IMPLOT_LIBRARY_DIRECTORY ${EXTERNAL_LIBRARIES_DIRECTORY}/implot) +set(IMPLOT_LIBRARY_DIRECTORY ${THIRDPARTY_DIRECTORY}/implot) set(IMPLOT_SOURCE_FILES @@ -19,4 +19,4 @@ set(IMPLOT_FILES ${IMPLOT_SOURCE_FILES} ${IMPLOT_HEADER_FILES}) add_library(implot STATIC ${IMPLOT_FILES}) target_include_directories( implot PRIVATE ${IMPLOT_LIBRARY_DIRECTORY} ${IMPLOT_LIBRARY_BACKEND_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui) + ${THIRDPARTY_DIRECTORY}/imgui) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 6cc70dc1..c41a0887 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -2,97 +2,79 @@ cmake_minimum_required(VERSION 4.0.0) project(core) -# Source files and headers -set(CORE_SOURCE_FILES +set(CORE_BASE_SOURCES src/color_las_loader.cpp + src/control_points.cpp src/gnss.cpp + src/ground_control_points.cpp + src/hash_utils.cpp src/icp.cpp src/ndt.cpp - src/nns.cpp src/nmea.cpp src/optimization_point_to_point_source_to_target.cpp src/optimize_distance_point_to_plane_source_to_target.cpp src/optimize_plane_to_plane_source_to_target.cpp src/optimize_point_to_plane_source_to_target.cpp src/optimize_point_to_projection_onto_plane_source_to_target.cpp - src/pcl_wrapper.cpp - src/plycpp.cpp + src/pair_wise_iterative_closest_point.cpp src/point_cloud.cpp src/point_clouds.cpp - src/pose_graph_slam.cpp - src/pose_graph_slam.cpp src/pose_graph_loop_closure.cpp - src/session.cpp - src/wgs84_do_puwg92.cc - src/pair_wise_iterative_closest_point.cpp - src/hash_utils.cpp - src/ground_control_points.cpp - src/control_points.cpp + src/pose_graph_slam.cpp src/registration_plane_feature.cpp + src/session.cpp + ${EXTERNAL_LIBRARIES_DIRECTORY}/src/wgs84_do_puwg92.cc + ${EXTERNAL_LIBRARIES_DIRECTORY}/src/plycpp.cpp ) -set(CORE_SOURCE_FILES_GUI - src/pfd_wrapper.cpp +set(CORE_GUI_SOURCES src/manual_pose_graph_loop_closure.cpp src/observation_picking.cpp + src/pfd_wrapper.cpp ) -set(CORE_INCLUDE_FILES - include/color_las_loader.h - include/gnss.h - include/icp.h - include/m_estimators.h - include/pose_graph_loop_closure.h - include/ndt.h - include/nns.h - include/nmea.h - include/pcl_wrapper.h - include/plycpp.h - include/point_cloud.h - include/point_clouds.h - include/pose_graph_slam.h - include/registration_plane_feature.h - include/session.h - include/structures.h - include/transformations.h - include/wgs84_do_puwg92.h - include/WGS84toCartesian.hpp -) +function(add_core_target target_name with_gui) + if(${with_gui}) + set(SOURCES ${CORE_BASE_SOURCES} ${CORE_GUI_SOURCES}) + set(DEFINES WITH_GUI=1) + else() + set(SOURCES ${CORE_BASE_SOURCES}) + set(DEFINES WITH_GUI=0) + endif() -set(CORE_INCLUDE_FILES_GUI - include/manual_pose_graph_loop_closure.h - include/observation_picking.h - include/pfd_wrapper.hpp -) + add_library(${target_name} STATIC ${SOURCES}) + target_compile_definitions(${target_name} PRIVATE ${DEFINES}) + target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) + target_include_directories(${target_name} PRIVATE + include + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/include + ) -# GUI-independent target -add_library(core_no_gui STATIC ${CORE_SOURCE_FILES} ${CORE_INCLUDE_FILES}) -target_compile_definitions(core_no_gui PRIVATE WITH_GUI=0) -set_target_properties(core_no_gui PROPERTIES - POSITION_INDEPENDENT_CODE ON) -target_link_libraries(core_no_gui PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) -target_include_directories(core_no_gui PRIVATE - include - ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + if(${with_gui}) + target_include_directories(${target_name} PRIVATE + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/freeglut/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ) + endif() +endfunction() + +add_core_target(core_no_gui FALSE) +add_core_target(core TRUE) + +target_precompile_headers(core_no_gui + PRIVATE include/pch/pch.h ) -# GUI target -add_library(core STATIC ${CORE_SOURCE_FILES} ${CORE_SOURCE_FILES_GUI} ${CORE_INCLUDE_FILES} ${CORE_INCLUDE_FILES_GUI}) -target_compile_definitions(core PRIVATE WITH_GUI=1) -target_link_libraries(core PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) -target_include_directories(core PRIVATE - include - ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master +target_precompile_headers(core_no_gui + PRIVATE include/pch/pch.h ) +set_target_properties(core_no_gui PROPERTIES + POSITION_INDEPENDENT_CODE ON) \ No newline at end of file diff --git a/core/include/GL_assert.h b/core/include/GL_assert.h index 679cd287..bdc1c4ff 100644 --- a/core/include/GL_assert.h +++ b/core/include/GL_assert.h @@ -1,7 +1,9 @@ #pragma once + +#include + #include #include -#include // Macro to wrap OpenGL calls and check for errors #define GL_CALL(x) \ @@ -19,17 +21,18 @@ // Macro to wrap OpenGL calls that return a value. Evaluates the call once, checks for GL errors, // and returns the call result. Implemented as a lambda to preserve the expression type. #define GL_CALL_RET(x) \ - ([&]() -> decltype(x) \ - { \ - auto _gl_call_result = (x); \ - GLenum _gl_call_err = glGetError(); \ - if (_gl_call_err != GL_NO_ERROR) \ - { \ - std::cerr << "OpenGL Error (" << _gl_call_err << "): " << __FILE__ << " at line " << __LINE__ << std::endl; \ - assert(false && "OpenGL Error"); \ - } \ - return _gl_call_result; \ - }()) + ( \ + [&]() -> decltype(x) \ + { \ + auto _gl_call_result = (x); \ + GLenum _gl_call_err = glGetError(); \ + if (_gl_call_err != GL_NO_ERROR) \ + { \ + std::cerr << "OpenGL Error (" << _gl_call_err << "): " << __FILE__ << " at line " << __LINE__ << std::endl; \ + assert(false && "OpenGL Error"); \ + } \ + return _gl_call_result; \ + }()) static bool CheckUniformLocation(GLint location, const char* name) { diff --git a/core/include/color_las_loader.h b/core/include/color_las_loader.h index af379a61..11fa7915 100644 --- a/core/include/color_las_loader.h +++ b/core/include/color_las_loader.h @@ -1,28 +1,34 @@ #pragma once -#include + #include #include +#include + namespace mandeye { - struct Point { + struct Point + { double timestamp; float intensity; Eigen::Vector3d point; }; - - struct PointRGB { + struct PointRGB + { double timestamp; float intensity; Eigen::Vector3d point; Eigen::Vector4f rgb; + PointRGB() = default; - PointRGB(const mandeye::Point& p) : - timestamp(p.timestamp), - intensity(p.intensity), - point(p.point) {}; + PointRGB(const mandeye::Point& p) + : timestamp(p.timestamp) + , intensity(p.intensity) + , point(p.point) + { + } }; std::vector load(const std::string& lazFile); bool saveLaz(const std::string& filename, const std::vector& buffer); -} \ No newline at end of file +} // namespace mandeye diff --git a/core/include/control_points.h b/core/include/control_points.h index 771199fc..42e05382 100644 --- a/core/include/control_points.h +++ b/core/include/control_points.h @@ -1,5 +1,4 @@ -#ifndef _CONTROL_POINTS_H_ -#define _CONTROL_POINTS_H_ +#pragma once #include #include @@ -48,5 +47,3 @@ class ControlPoints{ void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd = 1); #endif }; - -#endif \ No newline at end of file diff --git a/core/include/export_laz.h b/core/include/export_laz.h index 854c055b..aacaecef 100644 --- a/core/include/export_laz.h +++ b/core/include/export_laz.h @@ -1,22 +1,24 @@ -#ifndef _EXPORT_LAZ_H_ -#define _EXPORT_LAZ_H_ +#pragma once -#include -#include -#include #include #include #include +#include +#include + +#include #include -// #include namespace fs = std::filesystem; -inline bool exportLaz(const std::string &filename, - const std::vector &pointcloud, - const std::vector &intensity, - const std::vector ×tamps, - double offset_x = 0.0, double offset_y = 0.0, double offset_alt = 0.0) +inline bool exportLaz( + const std::string& filename, + const std::vector& pointcloud, + const std::vector& intensity, + const std::vector& timestamps, + double offset_x = 0.0, + double offset_y = 0.0, + double offset_alt = 0.0) { double min_ts = std::numeric_limits::max(); double max_ts = std::numeric_limits::lowest(); @@ -24,10 +26,11 @@ inline bool exportLaz(const std::string &filename, constexpr float scale = 0.0001f; // one tenth of milimeter // find max - Eigen::Vector3d _max(std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); + Eigen::Vector3d _max( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); Eigen::Vector3d _min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); - for (auto &p : pointcloud) + for (auto& p : pointcloud) { if (p.x() < _min.x()) { @@ -66,7 +69,7 @@ inline bool exportLaz(const std::string &filename, // get a pointer to the header of the writer so we can populate it - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_writer, &header)) { @@ -117,7 +120,7 @@ inline bool exportLaz(const std::string &filename, // get a pointer to the point of the writer that we will populate and write - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_writer, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); @@ -129,7 +132,7 @@ inline bool exportLaz(const std::string &filename, for (int i = 0; i < pointcloud.size(); i++) { - const auto &p = pointcloud[i]; + const auto& p = pointcloud[i]; point->intensity = intensity[i]; point->gps_time = timestamps[i] * 1e9; @@ -198,10 +201,10 @@ inline bool exportLaz(const std::string &filename, return true; } -inline void adjustHeader(laszip_header *header, const Eigen::Affine3d &m_pose, const Eigen::Vector3d &offset_in) +inline void adjustHeader(laszip_header* header, const Eigen::Affine3d& m_pose, const Eigen::Vector3d& offset_in) { - Eigen::Vector3d max{header->max_x, header->max_y, header->max_z}; - Eigen::Vector3d min{header->min_x, header->min_y, header->min_z}; + Eigen::Vector3d max{ header->max_x, header->max_y, header->max_z }; + Eigen::Vector3d min{ header->min_x, header->min_y, header->min_z }; // max -= offset_in; // min -= offset_in; @@ -236,7 +239,12 @@ inline void adjustHeader(laszip_header *header, const Eigen::Affine3d &m_pose, c // return o; //} -inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file_path_put, const Eigen::Affine3d &m_pose, const Eigen::Vector3d &offset, bool override_compressed = false) +inline void save_processed_pc( + const fs::path& file_path_in, + const fs::path& file_path_put, + const Eigen::Affine3d& m_pose, + const Eigen::Vector3d& offset, + bool override_compressed = false) { std::cout << "processing: " << file_path_in << std::endl; @@ -265,7 +273,7 @@ inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file } std::cout << "compressed : " << is_compressed << std::endl; - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { @@ -293,8 +301,7 @@ inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file std::cout << "header after:" << std::endl; std::cout << header->x_offset << " " << header->y_offset << " " << header->z_offset << std::endl; - std::cout << "m_pose: " << std::endl - << m_pose.matrix() << std::endl; + std::cout << "m_pose: " << std::endl << m_pose.matrix() << std::endl; if (laszip_set_header(laszip_writer, header)) { @@ -316,14 +323,14 @@ inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file return; } - laszip_point *input_point; + laszip_point* input_point; if (laszip_get_point_pointer(laszip_reader, &input_point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); std::abort(); } - laszip_point *output_point; + laszip_point* output_point; if (laszip_get_point_pointer(laszip_writer, &output_point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); @@ -387,7 +394,7 @@ inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file return; } - laszip_I64 p_count{0}; + laszip_I64 p_count{ 0 }; if (laszip_get_point_count(laszip_writer, &p_count)) { fprintf(stderr, "DLL ERROR: getting point count\n"); @@ -417,11 +424,18 @@ inline void save_processed_pc(const fs::path &file_path_in, const fs::path &file } inline void points_to_vector( - const std::vector points, std::vector &trajectory, double threshold_output_filter, std::vector *index_poses, - std::vector &pointcloud, std::vector &intensity, std::vector ×tamps, bool use_first_pose, bool save_index_pose) + const std::vector points, + std::vector& trajectory, + double threshold_output_filter, + std::vector* index_poses, + std::vector& pointcloud, + std::vector& intensity, + std::vector& timestamps, + bool use_first_pose, + bool save_index_pose) { Eigen::Affine3d m_pose = trajectory[0].inverse(); - for (const auto &org_p : points) + for (const auto& org_p : points) { Point3Di p = org_p; if (p.point.norm() > threshold_output_filter) @@ -448,7 +462,7 @@ inline void points_to_vector( } } -inline void save_all_to_las(const Session &session, std::string output_las_name, bool as_local, bool skip_ts_0) +inline void save_all_to_las(const Session& session, std::string output_las_name, bool as_local, bool skip_ts_0) { std::vector pointcloud; std::vector intensity; @@ -457,7 +471,7 @@ inline void save_all_to_las(const Session &session, std::string output_las_name, Eigen::Affine3d first_pose = Eigen::Affine3d::Identity(); bool found_first_pose = false; - for (const auto &p : session.point_clouds_container.point_clouds) + for (const auto& p : session.point_clouds_container.point_clouds) { if (p.visible) { @@ -469,7 +483,7 @@ inline void save_all_to_las(const Session &session, std::string output_las_name, for (size_t i = 0; i < p.points_local.size(); ++i) { - const auto &pp = p.points_local[i]; + const auto& pp = p.points_local[i]; Eigen::Vector3d vp = p.m_pose * pp; if (skip_ts_0) @@ -510,11 +524,11 @@ inline void save_all_to_las(const Session &session, std::string output_las_name, if (i < p.timestamps.size()) { - timestamps.push_back(p.timestamps[i]); + timestamps.push_back(p.timestamps[i]); } else { - timestamps.push_back(0.0);//ToDo this is caused by BUG in data + timestamps.push_back(0.0); // ToDo this is caused by BUG in data } } } @@ -523,7 +537,7 @@ inline void save_all_to_las(const Session &session, std::string output_las_name, if (as_local) { - for (auto &pt : pointcloud) + for (auto& pt : pointcloud) { pt = first_pose * pt; } @@ -541,4 +555,3 @@ inline void save_all_to_las(const Session &session, std::string output_las_name, std::cout << "problem with saving file: " << output_las_name << std::endl; } } -#endif \ No newline at end of file diff --git a/core/include/gnss.h b/core/include/gnss.h index 55bfff8a..6aa06e85 100644 --- a/core/include/gnss.h +++ b/core/include/gnss.h @@ -1,14 +1,15 @@ -#ifndef _GNSS_H_ -#define _GNSS_H_ +#pragma once -#include -#include #include -class GNSS{ - public: +#include +#include - struct GlobalPose{ +class GNSS +{ +public: + struct GlobalPose + { double timestamp; double lat; double lon; @@ -24,27 +25,31 @@ class GNSS{ double dist_xy_along; }; - GNSS(){;}; - ~GNSS(){;}; + GNSS() + { + ; + }; + ~GNSS() + { + ; + }; //! \brief Load GNSS data from file and converts to PUWG92 //! \param input_file_names - vector of file names //! \param localize - if true, the data is moved to the first point - bool load(const std::vector &input_file_names, Eigen::Vector3d &out_offset, bool localize = false); - bool load_mercator_projection(const std::vector &input_file_names); - bool load_nmea_mercator_projection(const std::vector &input_file_names); - void render(const PointClouds &point_clouds_container); - bool save_to_laz(const std::string &output_file_names, double offset_x, double offset_y, double offset_alt); + bool load(const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize = false); + bool load_mercator_projection(const std::vector& input_file_names); + bool load_nmea_mercator_projection(const std::vector& input_file_names); + void render(const PointClouds& point_clouds_container); + bool save_to_laz(const std::string& output_file_names, double offset_x, double offset_y, double offset_alt); std::vector gnss_poses; - //double offset_x = 0; - //double offset_y = 0; - //double offset_alt = 0; + // double offset_x = 0; + // double offset_y = 0; + // double offset_alt = 0; bool show_correspondences = false; double WGS84ReferenceLatitude = 0; double WGS84ReferenceLongitude = 0; bool setWGS84ReferenceFromFirstPose = true; }; - -#endif \ No newline at end of file diff --git a/core/include/ground_control_points.h b/core/include/ground_control_points.h index adaad430..e0b0966d 100644 --- a/core/include/ground_control_points.h +++ b/core/include/ground_control_points.h @@ -1,14 +1,16 @@ -#ifndef _GROUND_CONTROL_POINTS_H_ -#define _GROUND_CONTROL_POINTS_H_ +#pragma once -#include -#include #include + +#include +#include + #if WITH_GUI == 1 #include #endif -struct GroundControlPoint{ +struct GroundControlPoint +{ char name[64]; double x; double y; @@ -21,24 +23,29 @@ struct GroundControlPoint{ int index_to_node_outer; }; -class GroundControlPoints{ - public: - GroundControlPoints(){;}; - ~GroundControlPoints(){;}; +class GroundControlPoints +{ +public: + GroundControlPoints() + { + ; + }; + ~GroundControlPoints() + { + ; + }; - std::vector gpcs; - double default_lidar_height_above_ground = 0.15; + std::vector gpcs; + double default_lidar_height_above_ground = 0.15; #if WITH_GUI == 1 - bool is_imgui = false; - bool picking_mode = false; - int picking_mode_index_to_node_inner = -1; - int picking_mode_index_to_node_outer = -1; - bool draw_uncertainty = false; + bool is_imgui = false; + bool picking_mode = false; + int picking_mode_index_to_node_inner = -1; + int picking_mode_index_to_node_outer = -1; + bool draw_uncertainty = false; - void imgui(PointClouds &point_clouds_container); - void render(const PointClouds &point_clouds_container); - void draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd = 1); + void imgui(PointClouds& point_clouds_container); + void render(const PointClouds& point_clouds_container); + void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd = 1); #endif - }; - -#endif \ No newline at end of file +}; diff --git a/core/include/hash_utils.h b/core/include/hash_utils.h index 84263700..35973eb6 100644 --- a/core/include/hash_utils.h +++ b/core/include/hash_utils.h @@ -1,5 +1,4 @@ -#ifndef _HASH_UTILS_H_ -#define _HASH_UTILS_H_ +#pragma once #include @@ -8,5 +7,3 @@ unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t // this function provides unique index for input point p and 3D space decomposition into buckets b unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); - -#endif \ No newline at end of file diff --git a/core/include/icp.h b/core/include/icp.h index 2f03fb15..2a3aedad 100644 --- a/core/include/icp.h +++ b/core/include/icp.h @@ -1,80 +1,94 @@ -#ifndef _ICP_H_ -#define _ICP_H_ +#pragma once #include #include -class ICP { +class ICP +{ public: - enum PoseConvention { - cw, - wc - }; - enum OptimizationAlgorithm { - gauss_newton, - levenberg_marguardt - }; - enum RotationMatrixParametrization { - tait_bryan_xyz, - rodrigues, - quaternion - }; + enum PoseConvention + { + cw, + wc + }; + enum OptimizationAlgorithm + { + gauss_newton, + levenberg_marguardt + }; + enum RotationMatrixParametrization + { + tait_bryan_xyz, + rodrigues, + quaternion + }; - struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; - }; - std::vector get_jobs(long long unsigned int size, int num_threads = 8); + struct Job + { + long long unsigned int index_begin_inclusive; + long long unsigned int index_end_exclusive; + }; + std::vector get_jobs(long long unsigned int size, int num_threads = 8); - ICP() { - search_radius = 0.1; - number_of_threads = std::thread::hardware_concurrency(); - number_of_iterations = 6; - is_adaptive_robust_kernel = false; - is_ballanced_horizontal_vs_vertical = true; - barron_c = 1.0; - }; - ~ICP() { ; }; - - bool optimize_source_to_target_wc(PointClouds& point_clouds_container, bool fix_first_node); - bool compute_uncertainty(PointClouds& point_clouds_container); - - bool optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container, bool fix_first_node); - bool optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container, bool fix_first_node); - //- - bool optimization_point_to_point_source_to_target(PointClouds& point_clouds_container); - bool optimization_point_to_point_source_to_target_compute_rms(PointClouds& point_clouds_container, double &rms); - bool optimization_point_to_point_source_to_target(PointClouds& point_clouds_container, - PoseConvention pose_convention, OptimizationAlgorithm optimization_algorithm, RotationMatrixParametrization rotation_matrix_parametrization, double &out_rms, bool compute_only_rms); - std::vector> compute_covariance_matrices_tait_bryan_point_to_point_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_rodrigues_point_to_point_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_quaternion_point_to_point_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); + ICP() + { + search_radius = 0.1; + number_of_threads = std::thread::hardware_concurrency(); + number_of_iterations = 6; + is_adaptive_robust_kernel = false; + is_ballanced_horizontal_vs_vertical = true; + barron_c = 1.0; + }; + ~ICP() + { + ; + }; - bool optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container); - std::vector> compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container); - bool optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container); - std::vector> compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container); + bool optimize_source_to_target_wc(PointClouds& point_clouds_container, bool fix_first_node); + bool compute_uncertainty(PointClouds& point_clouds_container); + bool optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container, bool fix_first_node); + bool optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container, bool fix_first_node); + //- + bool optimization_point_to_point_source_to_target(PointClouds& point_clouds_container); + bool optimization_point_to_point_source_to_target_compute_rms(PointClouds& point_clouds_container, double& rms); + bool optimization_point_to_point_source_to_target( + PointClouds& point_clouds_container, + PoseConvention pose_convention, + OptimizationAlgorithm optimization_algorithm, + RotationMatrixParametrization rotation_matrix_parametrization, + double& out_rms, + bool compute_only_rms); + std::vector> compute_covariance_matrices_tait_bryan_point_to_point_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_rodrigues_point_to_point_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_quaternion_point_to_point_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); - float search_radius; - int number_of_threads; - int number_of_iterations; - bool is_adaptive_robust_kernel; - double barron_c = 1.0; - bool is_ballanced_horizontal_vs_vertical; - bool is_fix_first_node = false; - bool is_gauss_newton = true; - bool is_levenberg_marguardt = false; - bool is_cw = false; - bool is_wc = true; - bool is_tait_bryan_angles = true; - bool is_quaternion = false; - bool is_rodrigues = false; - bool is_lie_algebra_left_jacobian = false; - bool is_lie_algebra_right_jacobian = false; -}; + bool optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container); + std::vector> + compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_left_jacobian( + PointClouds& point_clouds_container); + bool optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container); + std::vector> + compute_covariance_matrices_point_to_point_source_to_target_source_to_target_lie_algebra_right_jacobian( + PointClouds& point_clouds_container); -#endif \ No newline at end of file + float search_radius; + int number_of_threads; + int number_of_iterations; + bool is_adaptive_robust_kernel; + double barron_c = 1.0; + bool is_ballanced_horizontal_vs_vertical; + bool is_fix_first_node = false; + bool is_gauss_newton = true; + bool is_levenberg_marguardt = false; + bool is_cw = false; + bool is_wc = true; + bool is_tait_bryan_angles = true; + bool is_quaternion = false; + bool is_rodrigues = false; + bool is_lie_algebra_left_jacobian = false; + bool is_lie_algebra_right_jacobian = false; +}; diff --git a/core/include/local_shape_features.h b/core/include/local_shape_features.h index 3654cc1f..b6aa2dd3 100644 --- a/core/include/local_shape_features.h +++ b/core/include/local_shape_features.h @@ -1,5 +1,4 @@ -#ifndef _LOCAL_SHAPE_FEATURES_H_ -#define _LOCAL_SHAPE_FEATURES_H_ +#pragma once #include #include @@ -24,16 +23,15 @@ class LocalShapeFeatures double verticality; }; - struct Params{ - Eigen::Vector3d search_radious = {0.5, 0.5, 0.5}; + struct Params + { + Eigen::Vector3d search_radious = { 0.5, 0.5, 0.5 }; double radious = 0.5; bool multithread = true; }; - LocalShapeFeatures(){;}; - ~LocalShapeFeatures(){;}; + LocalShapeFeatures() = default; + ~LocalShapeFeatures() = default; - bool calculate_local_shape_features(std::vector &points, const Params ¶ms); + bool calculate_local_shape_features(std::vector& points, const Params& params); }; - -#endif \ No newline at end of file diff --git a/core/include/m_estimators.h b/core/include/m_estimators.h index 8aeb9456..7bbcbba8 100644 --- a/core/include/m_estimators.h +++ b/core/include/m_estimators.h @@ -1,200 +1,267 @@ -#ifndef _M_ESTIMATORS_H_ -#define _M_ESTIMATORS_H_ +#pragma once #include -inline double get_l1_rho(double r){ - return fabs(r); +inline double get_l1_rho(double r) +{ + return fabs(r); } -inline double get_l1_upsilon(double r){ - return (r > 0) ? 1 : ((r < 0) ? -1 : 0); +inline double get_l1_upsilon(double r) +{ + return (r > 0) ? 1 : ((r < 0) ? -1 : 0); } -inline double get_l1_w(double r){ - return 1/fabs(r); +inline double get_l1_w(double r) +{ + return 1 / fabs(r); } -inline double get_l2_rho(double r){ - return (r*r)/2; +inline double get_l2_rho(double r) +{ + return (r * r) / 2; } -inline double get_l2_upsilon(double r){ - return r; +inline double get_l2_upsilon(double r) +{ + return r; } -inline double get_l2_w(double r){ - return 1; +inline double get_l2_w(double r) +{ + return 1; } -inline double get_l1l2_rho(double r){ - return 2*(sqrt(1 + (r*r)/2.0) - 1); +inline double get_l1l2_rho(double r) +{ + return 2 * (sqrt(1 + (r * r) / 2.0) - 1); } -inline double get_l1l2_upsilon(double r){ - return r/(sqrt(1+(r*r)/2.0)); +inline double get_l1l2_upsilon(double r) +{ + return r / (sqrt(1 + (r * r) / 2.0)); } -inline double get_l1l2_w(double r){ - return 1/(sqrt(1+(r*r)/2.0)); +inline double get_l1l2_w(double r) +{ + return 1 / (sqrt(1 + (r * r) / 2.0)); } -inline double get_lp_rho(double r, double nu){ - return (pow(fabs(r),nu))/nu; +inline double get_lp_rho(double r, double nu) +{ + return (pow(fabs(r), nu)) / nu; } -inline double get_lp_upsilon(double r, double nu){ - return ((r > 0) ? 1 : ((r < 0) ? -1 : 0)) * pow(fabs(r),nu-1); +inline double get_lp_upsilon(double r, double nu) +{ + return ((r > 0) ? 1 : ((r < 0) ? -1 : 0)) * pow(fabs(r), nu - 1); } -inline double get_lp_w(double r, double nu){ - return pow(fabs(r),nu-2); +inline double get_lp_w(double r, double nu) +{ + return pow(fabs(r), nu - 2); } -inline double get_fair_rho(double r, double c){ - return c*c*(fabs(r)/c - log(1 + fabs(r)/c)); +inline double get_fair_rho(double r, double c) +{ + return c * c * (fabs(r) / c - log(1 + fabs(r) / c)); } -inline double get_fair_upsilon(double r, double c){ - return r/(1+fabs(r)/c); +inline double get_fair_upsilon(double r, double c) +{ + return r / (1 + fabs(r) / c); } -inline double get_fair_w(double r, double c){ - return 1/(1+fabs(r)/c); +inline double get_fair_w(double r, double c) +{ + return 1 / (1 + fabs(r) / c); } -inline double get_huber_rho(double r, double k){ - if(fabs(r) < k){ - return (r*r)/2; - }else{ - return k*(fabs(r)-k/2); - } +inline double get_huber_rho(double r, double k) +{ + if (fabs(r) < k) + { + return (r * r) / 2; + } + else + { + return k * (fabs(r) - k / 2); + } } -inline double get_huber_upsilon(double r, double k){ - if(fabs(r) < k){ - return r; - }else{ - return k*((r > 0) ? 1 : ((r < 0) ? -1 : 0)); - } +inline double get_huber_upsilon(double r, double k) +{ + if (fabs(r) < k) + { + return r; + } + else + { + return k * ((r > 0) ? 1 : ((r < 0) ? -1 : 0)); + } } -inline double get_huber_w(double r, double k){ - if(fabs(r) < k){ - return 1; - }else{ - return k/fabs(r); - } +inline double get_huber_w(double r, double k) +{ + if (fabs(r) < k) + { + return 1; + } + else + { + return k / fabs(r); + } } -inline double get_cauchy_rho(double r, double c){ - return (c*c)/2 * log(1 + (r/c)*(r/c)); +inline double get_cauchy_rho(double r, double c) +{ + return (c * c) / 2 * log(1 + (r / c) * (r / c)); } -inline double get_cauchy_upsilon(double r, double c){ - return r/(1 + (r/c)*(r/c)); +inline double get_cauchy_upsilon(double r, double c) +{ + return r / (1 + (r / c) * (r / c)); } -inline double get_cauchy_w(double r, double c){ - return 1/(1 + (r/c)*(r/c)); +inline double get_cauchy_w(double r, double c) +{ + return 1 / (1 + (r / c) * (r / c)); } -inline double get_geman_mcclure_rho(double r){ - return ((r*r)/2)/(1 +r*r); +inline double get_geman_mcclure_rho(double r) +{ + return ((r * r) / 2) / (1 + r * r); } -inline double get_geman_mcclure_upsilon(double r){ - return r/( (1+r*r)*(1+r*r) ); +inline double get_geman_mcclure_upsilon(double r) +{ + return r / ((1 + r * r) * (1 + r * r)); } -inline double get_geman_mcclure_w(double r){ - return 1/( (1+r*r)*(1+r*r) ); +inline double get_geman_mcclure_w(double r) +{ + return 1 / ((1 + r * r) * (1 + r * r)); +} + +inline double get_welsch_rho(double r, double c) +{ + return (c * c) / 2 * (1 - exp(-(r / c) * (r / c))); +} + +inline double get_welsch_upsilon(double r, double c) +{ + return r * exp(-(r / c) * (r / c)); +} + +inline double get_welsch_w(double r, double c) +{ + return exp(-(r / c) * (r / c)); +} + +inline double get_tukey_rho(double r, double c) +{ + if (fabs(r) < c) + { + double temp = 1 - (r / c) * (r / c); + return (c * c) / 6 * (1 - temp * temp * temp); + } + else + { + return (c * c) / 6; + } +} + +inline double get_tukey_upsilon(double r, double c) +{ + if (fabs(r) < c) + { + double temp = 1 - (r / c) * (r / c); + return r * temp * temp; + } + else + { + return 0; + } +} + +inline double get_tukey_w(double r, double c) +{ + if (fabs(r) < c) + { + double temp = 1 - (r / c) * (r / c); + return temp * temp; + } + else + { + return 0; + } +} + +inline double get_barron_rho(double r, double alpha, double c) +{ + if (alpha <= -1000000000000.0) + { + return 1 - exp(-0.5 * (r / c) * (r / c)); + } + else if (alpha == 0) + { + return log(0.5 * (r / c) * (r / c) + 1); + } + else if (alpha == 2) + { + return 0.5 * (r / c) * (r / c); + } + else + { + return fabs(alpha - 2) / alpha * ((pow(((r / c) * (r / c)) / fabs(alpha - 2) + 1, alpha / 2)) - 1); + } +} + +inline double get_barron_upsilon(double r, double alpha, double c) +{ + if (alpha <= -1000000000000.0) + { + return r / (c * c) * exp(-0.5 * (r / c) * (r / c)); + } + else if (alpha == 0) + { + return 2 * r / (r * r + 2 * c * c); + } + else if (alpha == 2) + { + return r / (c * c); + } + else + { + return r / (c * c) * (pow(((r / c) * (r / c)) / fabs(alpha - 2) + 1, alpha / 2 - 1)); + } +} + +inline double get_barron_w(double r, double alpha, double c) +{ + if (r == 0.0) + return 1.0; + return get_barron_upsilon(r, alpha, c) / r; +} + +inline double get_approximate_partition_function(double tau_begin, double tau_end, double alpha, double c, double num_steps) +{ + double step = (tau_end - tau_begin) / num_steps; + double area = 0.0; + for (int i = 0; i < num_steps; i++) + { + area += exp(-get_barron_rho(tau_begin + (i + 0.5) * step, alpha, c)) * step; + } + return area; +} + +inline double get_truncated_robust_kernel(double r, double alpha, double c, double Z_tilde) +{ + return get_barron_rho(r, alpha, c) + log(c * Z_tilde); +} + +inline double get_modified_probability_distribution(double r, double alpha, double c, double tau_begin, double tau_end, double num_steps) +{ + double Z_tilde = get_approximate_partition_function(tau_begin, tau_end, alpha, 1, num_steps); + return (1.0 / (c * Z_tilde)) * exp(-(get_barron_rho(r, alpha, c))); } - -inline double get_welsch_rho(double r, double c){ - return (c*c)/2 * (1 - exp(-(r/c)*(r/c))); -} - -inline double get_welsch_upsilon(double r, double c){ - return r*exp(-(r/c)*(r/c)); -} - -inline double get_welsch_w(double r, double c){ - return exp(-(r/c)*(r/c)); -} - -inline double get_tukey_rho(double r, double c){ - if(fabs(r) < c){ - double temp = 1-(r/c)*(r/c); - return (c*c)/6 * (1 - temp*temp*temp); - }else{ - return (c*c)/6; - } -} - -inline double get_tukey_upsilon(double r, double c){ - if(fabs(r) < c){ - double temp = 1-(r/c)*(r/c); - return r*temp*temp; - }else{ - return 0; - } -} - -inline double get_tukey_w(double r, double c){ - if(fabs(r) < c){ - double temp = 1-(r/c)*(r/c); - return temp*temp; - }else{ - return 0; - } -} - -inline double get_barron_rho(double r, double alpha, double c){ - if(alpha <= -1000000000000.0){ - return 1 - exp(-0.5 * (r/c)* (r/c)); - }else if (alpha == 0){ - return log(0.5 * (r/c)* (r/c) + 1); - }else if (alpha == 2){ - return 0.5 * (r/c)* (r/c); - }else{ - return fabs(alpha-2)/alpha * (( pow(((r/c)* (r/c))/fabs(alpha-2) + 1, alpha/2) ) - 1); - } -} - -inline double get_barron_upsilon(double r, double alpha, double c){ - if(alpha <= -1000000000000.0){ - return r/(c*c)*exp(-0.5*(r/c)* (r/c)); - }else if (alpha == 0){ - return 2*r/(r*r+2*c*c); - }else if (alpha == 2){ - return r/(c*c); - }else{ - return r/(c*c) * ( pow(((r/c)* (r/c))/fabs(alpha-2) + 1, alpha/2-1) ); - } -} - -inline double get_barron_w(double r, double alpha, double c){ - if (r == 0.0)return 1.0; - return get_barron_upsilon(r, alpha, c)/r; -} - -inline double get_approximate_partition_function(double tau_begin, double tau_end, double alpha, double c, double num_steps){ - double step = (tau_end - tau_begin) / num_steps; - double area = 0.0; - for (int i = 0; i < num_steps; i ++) { - area += exp(-get_barron_rho(tau_begin + (i + 0.5) * step, alpha, c)) * step; - } - return area; -} - -inline double get_truncated_robust_kernel(double r, double alpha, double c, double Z_tilde){ - return get_barron_rho(r, alpha, c) + log(c*Z_tilde); -} - -inline double get_modified_probability_distribution(double r, double alpha, double c, double tau_begin, double tau_end, double num_steps) { - double Z_tilde = get_approximate_partition_function(tau_begin, tau_end, alpha, 1, num_steps); - return (1.0 / (c * Z_tilde)) * exp(-(get_barron_rho(r, alpha, c))); -} - -#endif diff --git a/core/include/manual_pose_graph_loop_closure.h b/core/include/manual_pose_graph_loop_closure.h index b1d0e38a..adc217b5 100644 --- a/core/include/manual_pose_graph_loop_closure.h +++ b/core/include/manual_pose_graph_loop_closure.h @@ -1,13 +1,13 @@ -#ifndef _manual_pose_graph_loop_closure_h_ -#define _manual_pose_graph_loop_closure_h_ +#pragma once + #if WITH_GUI == 1 -#include +#include #include +#include #include +#include #include -#include -#include class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure { @@ -17,15 +17,25 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure bool gizmo = false; double search_radious = 0.1; - ManualPoseGraphLoopClosure() {}; - ~ManualPoseGraphLoopClosure() {}; + ManualPoseGraphLoopClosure() = default; + ~ManualPoseGraphLoopClosure() = default; - void Gui(PointClouds &point_clouds_container, int &index_loop_closure_source, int &index_loop_closure_target, float *m_gizmo, GNSS &gnss, - GroundControlPoints &gcps, ControlPoints &cps, int num_edge_extended_before, int num_edge_extended_after); - void Render(PointClouds &point_clouds_container, - int index_loop_closure_source, int index_loop_closure_target, - int num_edge_extended_before, int num_edge_extended_after); + void Gui( + PointClouds& point_clouds_container, + int& index_loop_closure_source, + int& index_loop_closure_target, + float* m_gizmo, + GNSS& gnss, + GroundControlPoints& gcps, + ControlPoints& cps, + int num_edge_extended_before, + int num_edge_extended_after); + void Render( + PointClouds& point_clouds_container, + int index_loop_closure_source, + int index_loop_closure_target, + int num_edge_extended_before, + int num_edge_extended_after); }; #endif -#endif diff --git a/core/include/ndt.h b/core/include/ndt.h index 69e94e96..c6043788 100644 --- a/core/include/ndt.h +++ b/core/include/ndt.h @@ -1,168 +1,198 @@ -#ifndef _NDT_H_ -#define _NDT_H_ +#pragma once -#include #include -#include +#include #include +#include class NDT { public: - struct GridParameters - { - double bounding_box_min_X; - double bounding_box_min_Y; - double bounding_box_min_Z; - double bounding_box_max_X; - double bounding_box_max_Y; - double bounding_box_max_Z; - double bounding_box_extension; - int number_of_buckets_X; - int number_of_buckets_Y; - int number_of_buckets_Z; - long long unsigned int number_of_buckets; - double resolution_X; - double resolution_Y; - double resolution_Z; - }; - - struct PointBucketIndexPair - { - int index_of_point; - long long unsigned int index_of_bucket; - int index_pose; - }; - - struct Bucket - { - long long unsigned int index_begin; - long long unsigned int index_end; - long long unsigned int number_of_points; - Eigen::Vector3d mean; - Eigen::Matrix3d cov; - Eigen::Vector3d normal_vector; - }; - - struct BucketCoef - { - Eigen::Vector3d mean; - Eigen::Matrix3d cov; - Eigen::Vector3d normal_vector; - std::vector point_indexes; - bool valid = false; - }; - - struct Bucket2 - { - int classification = 0; // 1 - ceiling, 2 - floor - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; - // long long unsigned int number_of_points; - std::unordered_map buckets; - }; - - struct Job - { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; - }; - - enum PoseConvention - { - cw, - wc - }; - enum OptimizationAlgorithm - { - gauss_newton, - levenberg_marguardt - }; - enum RotationMatrixParametrization - { - tait_bryan_xyz, - rodrigues, - quaternion, - lie_algebra_left_jacobian, - lie_algebra_right_jacobian - }; - - NDT() - { - bucket_size[0] = 0.3; - bucket_size[1] = 0.3; - bucket_size[2] = 0.3; - bucket_size_external[0] = 5.0; - bucket_size_external[1] = 5.0; - bucket_size_external[2] = 5.0; - number_of_threads = std::thread::hardware_concurrency(); - number_of_iterations = 30; - }; - ~NDT() { ; }; - - void grid_calculate_params(const std::vector &point_cloud_global, GridParameters &in_out_params); - void grid_calculate_params(const std::vector &point_cloud_global, GridParameters &in_out_params); - void grid_calculate_params(GridParameters &in_out_params, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z); - - void build_rgd(std::vector &points, std::vector &index_pair, std::vector &buckets, GridParameters &rgd_params, - int num_threads = std::thread::hardware_concurrency()); - void build_rgd(std::vector &points, std::vector &index_pair, std::vector &buckets, GridParameters &rgd_params, - int num_threads = std::thread::hardware_concurrency()); - std::vector get_jobs(long long unsigned int size, int num_threads = std::thread::hardware_concurrency()); - void reindex(std::vector &points, std::vector &index_pair, NDT::GridParameters &rgd_params, int num_threads); - void reindex(std::vector &points, std::vector &index_pair, NDT::GridParameters &rgd_params, int num_threads); - - bool optimize(std::vector &point_clouds, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket); - bool optimize(std::vector &sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket); - - std::vector> compute_covariance_matrices_and_rms(std::vector &point_clouds, double &rms); - - bool optimize(std::vector &point_clouds, double &rms_initial, double &rms_final, double &mui, bool compute_mean_and_cov_for_bucket); - - //std::vector sessions; - - - bool optimize_lie_algebra_left_jacobian(std::vector &point_clouds, bool compute_mean_and_cov_for_bucket); - bool optimize_lie_algebra_right_jacobian(std::vector &point_clouds, bool compute_mean_and_cov_for_bucket); - // std::vector> compute_covariance_matrices7x7(PointClouds& point_clouds_container); - - bool compute_cov_mean(std::vector &points, std::vector &index_pair, std::vector &buckets, GridParameters &rgd_params, - double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, int num_threads = std::thread::hardware_concurrency()); - - bool compute_cov_mean(std::vector &points, - std::vector &index_pair, - std::vector &buckets, GridParameters &rgd_params, - int num_threads = std::thread::hardware_concurrency()); - - bool compute_cov_mean(std::vector &points, - std::vector &index_pair, - std::map &buckets, GridParameters &rgd_params, - int num_threads = std::thread::hardware_concurrency()); - - void build_rgd(std::vector &points, std::vector &index_pair, std::map &buckets, GridParameters &rgd_params, - int num_threads = std::thread::hardware_concurrency()); - - float bucket_size[3]; - float bucket_size_external[3]; - int number_of_threads; - int number_of_iterations; - - bool is_fix_first_node = false; - bool is_gauss_newton = true; - bool is_levenberg_marguardt = false; - bool is_wc = true; - bool is_cw = false; - bool is_tait_bryan_angles = true; - bool is_quaternion = false; - bool is_rodrigues = false; - bool is_lie_algebra_left_jacobian = false; - bool is_lie_algebra_right_jacobian = false; - - bool is_generalized = false; - double sigma_r = 0.01; - double sigma_polar_angle = 0.0001; - double sigma_azimuthal_angle = 0.0001; - int num_extended_points = 10; + struct GridParameters + { + double bounding_box_min_X; + double bounding_box_min_Y; + double bounding_box_min_Z; + double bounding_box_max_X; + double bounding_box_max_Y; + double bounding_box_max_Z; + double bounding_box_extension; + int number_of_buckets_X; + int number_of_buckets_Y; + int number_of_buckets_Z; + long long unsigned int number_of_buckets; + double resolution_X; + double resolution_Y; + double resolution_Z; + }; + + struct PointBucketIndexPair + { + int index_of_point; + long long unsigned int index_of_bucket; + int index_pose; + }; + + struct Bucket + { + long long unsigned int index_begin; + long long unsigned int index_end; + long long unsigned int number_of_points; + Eigen::Vector3d mean; + Eigen::Matrix3d cov; + Eigen::Vector3d normal_vector; + }; + + struct BucketCoef + { + Eigen::Vector3d mean; + Eigen::Matrix3d cov; + Eigen::Vector3d normal_vector; + std::vector point_indexes; + bool valid = false; + }; + + struct Bucket2 + { + int classification = 0; // 1 - ceiling, 2 - floor + long long unsigned int index_begin_inclusive; + long long unsigned int index_end_exclusive; + // long long unsigned int number_of_points; + std::unordered_map buckets; + }; + + struct Job + { + long long unsigned int index_begin_inclusive; + long long unsigned int index_end_exclusive; + }; + + enum PoseConvention + { + cw, + wc + }; + enum OptimizationAlgorithm + { + gauss_newton, + levenberg_marguardt + }; + enum RotationMatrixParametrization + { + tait_bryan_xyz, + rodrigues, + quaternion, + lie_algebra_left_jacobian, + lie_algebra_right_jacobian + }; + + NDT() + { + bucket_size[0] = 0.3; + bucket_size[1] = 0.3; + bucket_size[2] = 0.3; + bucket_size_external[0] = 5.0; + bucket_size_external[1] = 5.0; + bucket_size_external[2] = 5.0; + number_of_threads = std::thread::hardware_concurrency(); + number_of_iterations = 30; + } + + ~NDT() = default; + + void grid_calculate_params(const std::vector& point_cloud_global, GridParameters& in_out_params); + void grid_calculate_params(const std::vector& point_cloud_global, GridParameters& in_out_params); + void grid_calculate_params( + GridParameters& in_out_params, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z); + + void build_rgd( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + int num_threads = std::thread::hardware_concurrency()); + void build_rgd( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + int num_threads = std::thread::hardware_concurrency()); + std::vector get_jobs(long long unsigned int size, int num_threads = std::thread::hardware_concurrency()); + void reindex( + std::vector& points, std::vector& index_pair, NDT::GridParameters& rgd_params, int num_threads); + void reindex( + std::vector& points, + std::vector& index_pair, + NDT::GridParameters& rgd_params, + int num_threads); + + bool optimize(std::vector& point_clouds, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket); + bool optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket); + + std::vector> compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms); + + bool optimize( + std::vector& point_clouds, double& rms_initial, double& rms_final, double& mui, bool compute_mean_and_cov_for_bucket); + + // std::vector sessions; + + bool optimize_lie_algebra_left_jacobian(std::vector& point_clouds, bool compute_mean_and_cov_for_bucket); + bool optimize_lie_algebra_right_jacobian(std::vector& point_clouds, bool compute_mean_and_cov_for_bucket); + // std::vector> compute_covariance_matrices7x7(PointClouds& point_clouds_container); + + bool compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + double min_x, + double max_x, + double min_y, + double max_y, + double min_z, + double max_z, + int num_threads = std::thread::hardware_concurrency()); + + bool compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + int num_threads = std::thread::hardware_concurrency()); + + bool compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::map& buckets, + GridParameters& rgd_params, + int num_threads = std::thread::hardware_concurrency()); + + void build_rgd( + std::vector& points, + std::vector& index_pair, + std::map& buckets, + GridParameters& rgd_params, + int num_threads = std::thread::hardware_concurrency()); + + float bucket_size[3]; + float bucket_size_external[3]; + int number_of_threads; + int number_of_iterations; + + bool is_fix_first_node = false; + bool is_gauss_newton = true; + bool is_levenberg_marguardt = false; + bool is_wc = true; + bool is_cw = false; + bool is_tait_bryan_angles = true; + bool is_quaternion = false; + bool is_rodrigues = false; + bool is_lie_algebra_left_jacobian = false; + bool is_lie_algebra_right_jacobian = false; + + bool is_generalized = false; + double sigma_r = 0.01; + double sigma_polar_angle = 0.0001; + double sigma_azimuthal_angle = 0.0001; + int num_extended_points = 10; }; - -#endif \ No newline at end of file diff --git a/core/include/nmea.h b/core/include/nmea.h index 9b609c00..461cc188 100644 --- a/core/include/nmea.h +++ b/core/include/nmea.h @@ -1,57 +1,60 @@ #pragma once + +#include #include #include #include #include #include #include -#include -namespace hd_mapping::nmea { -struct GNRMCData { - std::string time_utc; - char status; - double latitude; - char lat_dir; - double longitude; - char lon_dir; - double speed_knots; - double track_angle; - std::string date; - double magnetic_variation; - char mag_var_dir; -}; +namespace hd_mapping::nmea +{ + struct GNRMCData + { + std::string time_utc; + char status; + double latitude; + char lat_dir; + double longitude; + char lon_dir; + double speed_knots; + double track_angle; + std::string date; + double magnetic_variation; + char mag_var_dir; + }; -struct GNGGAData { - double latitude; - char lat_dir; - double longitude; - char lon_dir; - double altitude; - double hdop; - int fix_quality; - int satellites_tracked; - double age_of_data; -}; + struct GNGGAData + { + double latitude; + char lat_dir; + double longitude; + char lon_dir; + double altitude; + double hdop; + int fix_quality; + int satellites_tracked; + double age_of_data; + }; -//! Breaks a line from an NMEA file into its components. -//! Returns a tuple containing the timestamp in nanosecs, the Unix timestamp in -//! nanosecs, and the NMEA sentence. -std::tuple -BreakLineFromNMEAFile(const std::string &line); + //! Breaks a line from an NMEA file into its components. + //! Returns a tuple containing the timestamp in nanosecs, the Unix timestamp in + //! nanosecs, and the NMEA sentence. + std::tuple BreakLineFromNMEAFile(const std::string& line); -//! Validates the checksum of an NMEA sentence. -bool validateNMEAChecksum(const std::string &nmea); + //! Validates the checksum of an NMEA sentence. + bool validateNMEAChecksum(const std::string& nmea); -//! Parses a GNRMC or GPRMC NMEA sentence and returns a GNRMCData structure. -//! Example sentence: `$GPRMC,184942.40,A,5212.01834,N,02056.49288,E,1.953,254.11,080725,,,R,V*00` -//! If the sentence is invalid or does not contain the required fields, returns -//! std::nullopt. -std::optional parseGNRMC(const std::string &nmea); + //! Parses a GNRMC or GPRMC NMEA sentence and returns a GNRMCData structure. + //! Example sentence: `$GPRMC,184942.40,A,5212.01834,N,02056.49288,E,1.953,254.11,080725,,,R,V*00` + //! If the sentence is invalid or does not contain the required fields, returns + //! std::nullopt. + std::optional parseGNRMC(const std::string& nmea); -//! Parses a GNGGA or GPGGA NMEA sentence and returns a GNGGAData structure. -//! Example sentence: `$GPGGA,184852.00,5212.02878,N,02056.51988,E,4,12,0.94,109.1,M,34.4,M,2.0,0093*74` -//! If the sentence is invalid or does not contain the required fields, returns -//! std::nullopt. -std::optional parseGNGGA(const std::string &nmea); + //! Parses a GNGGA or GPGGA NMEA sentence and returns a GNGGAData structure. + //! Example sentence: `$GPGGA,184852.00,5212.02878,N,02056.51988,E,4,12,0.94,109.1,M,34.4,M,2.0,0093*74` + //! If the sentence is invalid or does not contain the required fields, returns + //! std::nullopt. + std::optional parseGNGGA(const std::string& nmea); } // namespace hd_mapping::nmea diff --git a/core/include/nns.h b/core/include/nns.h deleted file mode 100644 index c0df97a5..00000000 --- a/core/include/nns.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef _NNS_H_ -#define _NNS_H_ - -#include - - - - - -#endif \ No newline at end of file diff --git a/core/include/observation_picking.h b/core/include/observation_picking.h index 935cf86b..3608a11a 100644 --- a/core/include/observation_picking.h +++ b/core/include/observation_picking.h @@ -1,154 +1,152 @@ -#ifndef _OBSERVATION_PICKING_H_ -#define _OBSERVATION_PICKING_H_ +#pragma once -#include #include -#include -#include #include +#include +#include +#include #include #include struct PointInsideIntersection { - double x_local; - double y_local; - double z_local; - int index_scan; - std::string source_filename; - Eigen::Vector3d color; + double x_local; + double y_local; + double z_local; + int index_scan; + std::string source_filename; + Eigen::Vector3d color; }; class Intersection { public: - Intersection() { ; }; - ~Intersection() { ; }; - float color[3]; - float translation[3]; - float rotation[3]; - float width_length_height[3]; - std::vector points; + Intersection() = default; + ~Intersection() = default; - void render() - { - TaitBryanPose pose; - pose.px = translation[0]; - pose.py = translation[1]; - pose.pz = translation[2]; - pose.om = rotation[0]; - pose.fi = rotation[1]; - pose.ka = rotation[2]; - Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(pose); + float color[3]; + float translation[3]; + float rotation[3]; + float width_length_height[3]; + std::vector points; - float &x_length = width_length_height[0]; - float &y_width = width_length_height[1]; - float &z_height = width_length_height[2]; + void render() + { + TaitBryanPose pose; + pose.px = translation[0]; + pose.py = translation[1]; + pose.pz = translation[2]; + pose.om = rotation[0]; + pose.fi = rotation[1]; + pose.ka = rotation[2]; + Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(pose); - glColor3f(color[0], color[1], color[2]); + float& x_length = width_length_height[0]; + float& y_width = width_length_height[1]; + float& z_height = width_length_height[2]; - Eigen::Vector3d v000(-x_length * 0.5, -y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v100(x_length * 0.5, -y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v110(x_length * 0.5, y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v010(-x_length * 0.5, y_width * 0.5, -z_height * 0.5); + glColor3f(color[0], color[1], color[2]); - Eigen::Vector3d v001(-x_length * 0.5, -y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v101(x_length * 0.5, -y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v111(x_length * 0.5, y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v011(-x_length * 0.5, y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v000(-x_length * 0.5, -y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v100(x_length * 0.5, -y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v110(x_length * 0.5, y_width * 0.5, -z_height * 0.5); + Eigen::Vector3d v010(-x_length * 0.5, y_width * 0.5, -z_height * 0.5); - Eigen::Vector3d v000t = m_pose * v000; - Eigen::Vector3d v100t = m_pose * v100; - Eigen::Vector3d v110t = m_pose * v110; - Eigen::Vector3d v010t = m_pose * v010; + Eigen::Vector3d v001(-x_length * 0.5, -y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v101(x_length * 0.5, -y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v111(x_length * 0.5, y_width * 0.5, z_height * 0.5); + Eigen::Vector3d v011(-x_length * 0.5, y_width * 0.5, z_height * 0.5); - Eigen::Vector3d v001t = m_pose * v001; - Eigen::Vector3d v101t = m_pose * v101; - Eigen::Vector3d v111t = m_pose * v111; - Eigen::Vector3d v011t = m_pose * v011; + Eigen::Vector3d v000t = m_pose * v000; + Eigen::Vector3d v100t = m_pose * v100; + Eigen::Vector3d v110t = m_pose * v110; + Eigen::Vector3d v010t = m_pose * v010; - glBegin(GL_LINES); + Eigen::Vector3d v001t = m_pose * v001; + Eigen::Vector3d v101t = m_pose * v101; + Eigen::Vector3d v111t = m_pose * v111; + Eigen::Vector3d v011t = m_pose * v011; - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); + glBegin(GL_LINES); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); + glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); - // - glVertex3d(v000t.x(), v000t.y(), v000t.z()); - glVertex3d(v110t.x(), v110t.y(), v110t.z()); + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v010t.x(), v010t.y(), v010t.z()); - glVertex3d(v100t.x(), v100t.y(), v100t.z()); + // + glVertex3d(v000t.x(), v000t.y(), v000t.z()); + glVertex3d(v110t.x(), v110t.y(), v110t.z()); - glVertex3d(v001t.x(), v001t.y(), v001t.z()); - glVertex3d(v111t.x(), v111t.y(), v111t.z()); + glVertex3d(v010t.x(), v010t.y(), v010t.z()); + glVertex3d(v100t.x(), v100t.y(), v100t.z()); - glVertex3d(v011t.x(), v011t.y(), v011t.z()); - glVertex3d(v101t.x(), v101t.y(), v101t.z()); + glVertex3d(v001t.x(), v001t.y(), v001t.z()); + glVertex3d(v111t.x(), v111t.y(), v111t.z()); - glEnd(); - } + glVertex3d(v011t.x(), v011t.y(), v011t.z()); + glVertex3d(v101t.x(), v101t.y(), v101t.z()); + + glEnd(); + } }; class ObservationPicking { public: - ObservationPicking() { ; }; - ~ObservationPicking() { ; }; - - bool is_observation_picking_mode = false; - float picking_plane_height = 0.0f; - float picking_plane_threshold = 0.1f; - float max_xy = 200.0f; - int point_size = 1; - // bool high_density_grid = false; - bool grid10x10m = false; - bool grid1x1m = false; - bool grid01x01m = false; - bool grid001x001m = false; - void render(); - void add_picked_to_current_observation(int index_picked, Eigen::Vector3d p); - void accept_current_observation(std::vector m_poses); - void import_observations(const std::string &filename); - void export_observation(const std::string &filename); - void add_intersection(Eigen::Vector3d translation); - - std::map current_observation; - std::vector> observations; - std::vector intersections; - float label_dist = 100.0f; + ObservationPicking() = default; + ~ObservationPicking() = default; + + bool is_observation_picking_mode = false; + float picking_plane_height = 0.0f; + float picking_plane_threshold = 0.1f; + float max_xy = 200.0f; + int point_size = 1; + // bool high_density_grid = false; + bool grid10x10m = false; + bool grid1x1m = false; + bool grid01x01m = false; + bool grid001x001m = false; + void render(); + void add_picked_to_current_observation(int index_picked, Eigen::Vector3d p); + void accept_current_observation(std::vector m_poses); + void import_observations(const std::string& filename); + void export_observation(const std::string& filename); + void add_intersection(Eigen::Vector3d translation); + + std::map current_observation; + std::vector> observations; + std::vector intersections; + float label_dist = 100.0f; }; - -#endif \ No newline at end of file diff --git a/core/include/pair_wise_iterative_closest_point.h b/core/include/pair_wise_iterative_closest_point.h index c5e2c100..8e9db634 100644 --- a/core/include/pair_wise_iterative_closest_point.h +++ b/core/include/pair_wise_iterative_closest_point.h @@ -1,16 +1,25 @@ -#ifndef _PAIR_WISE_ITERATIVE_CLOSEST_POINT_H_ -#define _PAIR_WISE_ITERATIVE_CLOSEST_POINT_H_ +#pragma once -#include #include +#include -class PairWiseICP{ +class PairWiseICP +{ public: - PairWiseICP(){;}; - ~PairWiseICP() { ; }; + PairWiseICP() = default; + ~PairWiseICP() = default; - bool compute(const std::vector &source, const std::vector &target, double search_radious, int number_of_iterations, Eigen::Affine3d &m_pose_result); - bool compute_fast(const std::vector &source, const std::vector &target, double search_radious, int number_of_iterations, Eigen::Affine3d &m_pose_result, int dec); + bool compute( + const std::vector& source, + const std::vector& target, + double search_radious, + int number_of_iterations, + Eigen::Affine3d& m_pose_result); + bool compute_fast( + const std::vector& source, + const std::vector& target, + double search_radious, + int number_of_iterations, + Eigen::Affine3d& m_pose_result, + int dec); }; - -#endif \ No newline at end of file diff --git a/core/include/pch/pch.h b/core/include/pch/pch.h new file mode 100644 index 00000000..0a8c4c67 --- /dev/null +++ b/core/include/pch/pch.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include \ No newline at end of file diff --git a/core/include/pcl_wrapper.h b/core/include/pcl_wrapper.h deleted file mode 100644 index 428b6b05..00000000 --- a/core/include/pcl_wrapper.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef _PCL_WRAPPER_H_ -#define _PCL_WRAPPER_H_ - -#ifdef WITH_PCL - - -#endif //WITH_PCL - - -#endif //_PCL_WRAPPER_H_ diff --git a/core/include/pfd_wrapper.hpp b/core/include/pfd_wrapper.hpp index 54cc84ee..0ee51c6a 100644 --- a/core/include/pfd_wrapper.hpp +++ b/core/include/pfd_wrapper.hpp @@ -1,34 +1,52 @@ #pragma once -#include + #include -namespace mandeye::fd{ +#include + +namespace mandeye::fd +{ namespace internal { - static std::string lastLocationHint = ".";//"C:\\"; + static std::string lastLocationHint = "."; //"C:\\"; } - const std::vector LAS_LAZ_filter = {"LASzip file (*.laz)", "*.laz", "LAS file (*.las)", "*.las", "All files", "*"}; - const std::vector LazFilter = { "LAS/LAZ files (*.laz)", "*.las *.laz" }; - const std::vector ImageFilter = { "Image files (*.bmp, *.jpg, *.jpeg, *.png)", "*.bmp *.jpg *.jpeg *.png", "All files", "*" }; + + const std::vector LAS_LAZ_filter = { "LASzip file (*.laz)", "*.laz", "LAS file (*.las)", "*.las", "All files", "*" }; + const std::vector LazFilter = { "LAS/LAZ files (*.laz)", "*.las *.laz" }; + const std::vector ImageFilter = { + "Image files (*.bmp, *.jpg, *.jpeg, *.png)", "*.bmp *.jpg *.jpeg *.png", "All files", "*" + }; const std::vector Calibration_filter = { "Mandeye JSON Calibration (*.mjc)", "*.mjc", "Generic JSON (*.json)", "*.json" }; const std::vector Session_filter = { "Mandeye JSON Session (*.mjs)", "*.mjs", "Generic JSON (*.json)", "*.json" }; const std::vector Project_filter = { "Mandeye JSON Project (*.mjp)", "*.mjp", "Generic JSON (*.json)", "*.json" }; - const std::vector IniPoses_filter = { "Mandeye REG Initial poses (*.mri)", "*.mri", "Generic REG initial poses (*.reg)", "*.reg" }; + const std::vector IniPoses_filter = { + "Mandeye REG Initial poses (*.mri)", "*.mri", "Generic REG initial poses (*.reg)", "*.reg" + }; const std::vector Poses_filter = { "Mandeye REG Poses (*.mrp)", "*.mrp", "Generic REG poses (*.reg)", "*.reg" }; const std::vector Resso_filter = { "RESSO (*.reg)", "*.reg" }; const std::vector Dxf_filter = { "DXF file (*.dxf)", "*.dxf" }; const std::vector Csv_filter = { "CSV file (*.csv)", "*.csv" }; - const std::vector Toml_filter = {"TOML file (*.toml)", "*.toml"}; - const std::vector All_Filter = {"All supported files (*.bmp, *.csv, *.dxf, *.jpg, *.jpeg, *.json, *.las, *.laz, *.mjc, *.png, *.sn)", "*.bmp *.csv *.dxf *.jpg *.jpeg *.json *.las *.laz *.mjc *.png *.sn", "All files", "*" }; + const std::vector Toml_filter = { "TOML file (*.toml)", "*.toml" }; + const std::vector All_Filter = { + "All supported files (*.bmp, *.csv, *.dxf, *.jpg, *.jpeg, *.json, *.las, *.laz, *.mjc, *.png, *.sn)", + "*.bmp *.csv *.dxf *.jpg *.jpeg *.json *.las *.laz *.mjc *.png *.sn", + "All files", + "*" + }; + const std::vector json_filter = { "Calibration file (*.json)", "*.json", "All files", "*" }; const std::vector sn_filter = { "SN file (*.sn)", "*.sn", "All files", "*" }; - std::string OpenFileDialogOneFile(const std::string& title, const std::vector&filter); - std::vector OpenFileDialog(const std::string& title, const std::vector&filter, bool multiselect); - std::string SaveFileDialog(const std::string& title, const std::vector& filter, const std::string& defaultExtension = "", const std::string& defaultFileName = ""); + std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter); + std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect); + std::string SaveFileDialog( + const std::string& title, + const std::vector& filter, + const std::string& defaultExtension = "", + const std::string& defaultFileName = ""); std::string SelectFolder(const std::string& title); void OutOfMemMessage(); -} \ No newline at end of file +} // namespace mandeye::fd diff --git a/core/include/point_cloud.h b/core/include/point_cloud.h index 59d2f6a6..c55e2a43 100644 --- a/core/include/point_cloud.h +++ b/core/include/point_cloud.h @@ -1,150 +1,164 @@ -#ifndef _POINT_CLOUD_H_ -#define _POINT_CLOUD_H_ +#pragma once -#include -#include #include +#include #include +#include + #if WITH_GUI == 1 #include #include #endif -class PointCloud { +class PointCloud +{ public: - struct GridParameters { - double bounding_box_min_X = 0.0; - double bounding_box_min_Y = 0.0; - double bounding_box_min_Z = 0.0; - double bounding_box_max_X = 0.0; - double bounding_box_max_Y = 0.0; - double bounding_box_max_Z = 0.0; - double bounding_box_extension = 1.0; - int number_of_buckets_X = 0; - int number_of_buckets_Y = 0; - int number_of_buckets_Z = 0; - long long unsigned int number_of_buckets = 0; - double resolution_X = 2.0; - double resolution_Y = 2.0; - double resolution_Z = 2.0; - }; - - struct PointBucketIndexPair { - int index_of_point = 0; - long long unsigned int index_of_bucket = 0; - //int index_pose; - }; - - struct Bucket { - long long unsigned int index_begin = 0; - long long unsigned int index_end = 0; - long long unsigned int number_of_points = 0; - }; - - struct Job { - long long unsigned int index_begin_inclusive = 0; - long long unsigned int index_end_exclusive = 0; - }; - - struct NearestNeighbour { - int index_source = 0; - int index_target = 0; - bool found = false; - }; - - struct LocalTrajectoryNode{ - std::pair timestamps; - Eigen::Affine3d m_pose; - Eigen::Vector3d imu_om_fi_ka; - Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; - }; - - PointCloud() { - m_pose = Eigen::Affine3d::Identity(); - pose.px = pose.py = pose.pz = pose.om = pose.fi = pose.ka = 0.0; - gui_translation[0] = gui_translation[1] = gui_translation[2] = 0.0f; - gui_rotation[0] = gui_rotation[1] = gui_rotation[2] = 0.0; - - render_color[0] = (float(rand() % 255)) / 255.0f; - render_color[1] = (float(rand() % 255)) / 255.0f; - render_color[2] = (float(rand() % 255)) / 255.0f; - - visible = true; - gizmo = false; - }; - ~PointCloud() { ; }; - - GridParameters rgd_params; - std::vector index_pairs; - std::vector < Bucket> buckets; - std::string file_name; - std::vector points_local; - std::vector normal_vectors_local; - std::vector colors; - std::vector points_type; - std::vector intensities; - std::vector timestamps; - Eigen::Affine3d m_pose_temp; - Eigen::Affine3d m_pose; - Eigen::Affine3d m_initial_pose; - Eigen::Matrix covariance_matrix_tait_bryan; - Eigen::Matrix information_matrix_tait_bryan; - int number_points_vertical = 0; - int number_points_horizontal = 0; - int point_size = 1; - //std::vector available_geo_points; - bool choosing_geo = false; - std::vector local_trajectory; - bool show_local_trajectory = false; - bool show_color = true; - bool fuse_inclination_from_IMU = false; - - bool show_IMU = false; - bool show_pose = false; - - TaitBryanPose pose; - float gui_translation[3]; - float gui_rotation[3]; - float render_color[3]; - bool visible; - //bool visible_imu_diff = false; - bool gizmo; - int num_threads = 16; - //bool fixed = false; - //double search_radious = 2.0; - bool fixed_x = false; - bool fixed_y = false; - bool fixed_z = false; - bool fixed_om = false; - bool fixed_fi = false; - bool fixed_ka = false; - - bool load(const std::string& file_name); - bool load_pc(std::string input_file_name, bool load_cache_mode); - void update_from_gui(); - bool save_as_global(std::string file_name); - //rgd - bool build_rgd(); - void grid_calculate_params(std::vector& points, GridParameters& params); - void reindex(std::vector& ip, std::vector& points, GridParameters rgd_params); - std::vector get_jobs(long long unsigned int size, int num_threads); - void cout_rgd(); - std::vector> nns(PointCloud& pc_target, double search_radious); - void clean(); - void compute_normal_vectors(double search_radious); - void decimate(double bucket_x, double bucket_y, double bucket_z); - void shift_to_center(); + struct GridParameters + { + double bounding_box_min_X = 0.0; + double bounding_box_min_Y = 0.0; + double bounding_box_min_Z = 0.0; + double bounding_box_max_X = 0.0; + double bounding_box_max_Y = 0.0; + double bounding_box_max_Z = 0.0; + double bounding_box_extension = 1.0; + int number_of_buckets_X = 0; + int number_of_buckets_Y = 0; + int number_of_buckets_Z = 0; + long long unsigned int number_of_buckets = 0; + double resolution_X = 2.0; + double resolution_Y = 2.0; + double resolution_Z = 2.0; + }; + + struct PointBucketIndexPair + { + int index_of_point = 0; + long long unsigned int index_of_bucket = 0; + // int index_pose; + }; + + struct Bucket + { + long long unsigned int index_begin = 0; + long long unsigned int index_end = 0; + long long unsigned int number_of_points = 0; + }; + + struct Job + { + long long unsigned int index_begin_inclusive = 0; + long long unsigned int index_end_exclusive = 0; + }; + + struct NearestNeighbour + { + int index_source = 0; + int index_target = 0; + bool found = false; + }; + + struct LocalTrajectoryNode + { + std::pair timestamps; + Eigen::Affine3d m_pose; + Eigen::Vector3d imu_om_fi_ka; + Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; + }; + + PointCloud() + { + m_pose = Eigen::Affine3d::Identity(); + pose.px = pose.py = pose.pz = pose.om = pose.fi = pose.ka = 0.0; + gui_translation[0] = gui_translation[1] = gui_translation[2] = 0.0f; + gui_rotation[0] = gui_rotation[1] = gui_rotation[2] = 0.0; + + render_color[0] = (float(rand() % 255)) / 255.0f; + render_color[1] = (float(rand() % 255)) / 255.0f; + render_color[2] = (float(rand() % 255)) / 255.0f; + + visible = true; + gizmo = false; + }; + + ~PointCloud() = default; + + GridParameters rgd_params; + std::vector index_pairs; + std::vector buckets; + std::string file_name; + std::vector points_local; + std::vector normal_vectors_local; + std::vector colors; + std::vector points_type; + std::vector intensities; + std::vector timestamps; + Eigen::Affine3d m_pose_temp; + Eigen::Affine3d m_pose; + Eigen::Affine3d m_initial_pose; + Eigen::Matrix covariance_matrix_tait_bryan; + Eigen::Matrix information_matrix_tait_bryan; + int number_points_vertical = 0; + int number_points_horizontal = 0; + int point_size = 1; + // std::vector available_geo_points; + bool choosing_geo = false; + std::vector local_trajectory; + bool show_local_trajectory = false; + bool show_color = true; + bool fuse_inclination_from_IMU = false; + + bool show_IMU = false; + bool show_pose = false; + + TaitBryanPose pose; + float gui_translation[3]; + float gui_rotation[3]; + float render_color[3]; + bool visible; + // bool visible_imu_diff = false; + bool gizmo; + int num_threads = 16; + // bool fixed = false; + // double search_radious = 2.0; + bool fixed_x = false; + bool fixed_y = false; + bool fixed_z = false; + bool fixed_om = false; + bool fixed_fi = false; + bool fixed_ka = false; + + bool load(const std::string& file_name); + bool load_pc(std::string input_file_name, bool load_cache_mode); + void update_from_gui(); + bool save_as_global(std::string file_name); + // rgd + bool build_rgd(); + void grid_calculate_params(std::vector& points, GridParameters& params); + void reindex(std::vector& ip, std::vector& points, GridParameters rgd_params); + std::vector get_jobs(long long unsigned int size, int num_threads); + void cout_rgd(); + std::vector> nns(PointCloud& pc_target, double search_radious); + void clean(); + void compute_normal_vectors(double search_radious); + void decimate(double bucket_x, double bucket_y, double bucket_z); + void shift_to_center(); #if WITH_GUI == 1 - //void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, - // bool xz_intersection, bool yz_intersection, bool xy_intersection, - // bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, - // bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, - // bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, - // double intersection_width); - void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, - bool xz_intersection, bool yz_intersection, bool xy_intersection, double intersection_width, bool visible_imu_diff); - void render(Eigen::Affine3d pose, int viewer_decmiate_point_cloud); + // void render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, + // bool xz_intersection, bool yz_intersection, bool xy_intersection, + // bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, + // bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, + // bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, + // double intersection_width); + void render( + bool show_with_initial_pose, + const ObservationPicking& observation_picking, + int viewer_decmiate_point_cloud, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width, + bool visible_imu_diff); + void render(Eigen::Affine3d pose, int viewer_decmiate_point_cloud); #endif }; - -#endif \ No newline at end of file diff --git a/core/include/point_clouds.h b/core/include/point_clouds.h index 3f5387fb..cc383681 100644 --- a/core/include/point_clouds.h +++ b/core/include/point_clouds.h @@ -1,5 +1,4 @@ -#ifndef _POINT_CLOUDS_H_ -#define _POINT_CLOUDS_H_ +#pragma once #include #include @@ -10,77 +9,122 @@ #include #endif -class PointClouds { +class PointClouds +{ public: - PointClouds() { ; }; - ~PointClouds() { ; }; + PointClouds() = default; + ~PointClouds() = default; - bool xz_intersection = false; - bool yz_intersection = false; - bool xy_intersection = false; + bool xz_intersection = false; + bool yz_intersection = false; + bool xy_intersection = false; - double intersection_width = 0.05; - bool xz_grid_10x10 = false; - bool xz_grid_1x1 = false; - bool xz_grid_01x01 = false; - bool yz_grid_10x10 = false; - bool yz_grid_1x1 = false; - bool yz_grid_01x01 = false; - bool xy_grid_10x10 = false; - bool xy_grid_1x1 = false; - bool xy_grid_01x01 = false; + double intersection_width = 0.05; + bool xz_grid_10x10 = false; + bool xz_grid_1x1 = false; + bool xz_grid_01x01 = false; + bool yz_grid_10x10 = false; + bool yz_grid_1x1 = false; + bool yz_grid_01x01 = false; + bool xy_grid_10x10 = false; + bool xy_grid_1x1 = false; + bool xy_grid_01x01 = false; - Eigen::Vector3d offset = Eigen::Vector3d(0, 0, 0); - Eigen::Vector3d offset_to_apply = Eigen::Vector3d(0, 0, 0); - std::string folder_name; - std::string out_folder_name; - std::string poses_file_name; - std::string initial_poses_file_name; - std::string out_poses_file_name; - bool show_imu_to_lio_diff = false; + Eigen::Vector3d offset = Eigen::Vector3d(0, 0, 0); + Eigen::Vector3d offset_to_apply = Eigen::Vector3d(0, 0, 0); + std::string folder_name; + std::string out_folder_name; + std::string poses_file_name; + std::string initial_poses_file_name; + std::string out_poses_file_name; + bool show_imu_to_lio_diff = false; - struct PointCloudDimensions { - double x_min, x_max, y_min, y_max, z_min, z_max; - double length, width, height; - }; + struct PointCloudDimensions + { + double x_min, x_max, y_min, y_max, z_min, z_max; + double length, width, height; + }; - bool load(const std::string &folder_with_point_clouds, const std::string &poses_file_name, bool decimation, - double bucket_x, double bucket_y, double bucket_z, bool load_cache_mode); - bool update_poses_from_RESSO(const std::string& folder_with_point_clouds, const std::string& poses_file_name); - bool update_poses_from_RESSO_inverse(const std::string &folder_with_point_clouds, const std::string &poses_file_name); - bool update_initial_poses_from_RESSO(const std::string &folder_with_point_clouds, const std::string &poses_file_name); - bool load_eth(const std::string& folder_with_point_clouds, const std::string& poses_file_name, bool decimation, double bucket_x, double bucket_y, double bucket_z); - //std::vector load_points(const std::string& point_clouds_file_name); + bool load( + const std::string& folder_with_point_clouds, + const std::string& poses_file_name, + bool decimation, + double bucket_x, + double bucket_y, + double bucket_z, + bool load_cache_mode); + bool update_poses_from_RESSO(const std::string& folder_with_point_clouds, const std::string& poses_file_name); + bool update_poses_from_RESSO_inverse(const std::string& folder_with_point_clouds, const std::string& poses_file_name); + bool update_initial_poses_from_RESSO(const std::string& folder_with_point_clouds, const std::string& poses_file_name); + bool load_eth( + const std::string& folder_with_point_clouds, + const std::string& poses_file_name, + bool decimation, + double bucket_x, + double bucket_y, + double bucket_z); + // std::vector load_points(const std::string& point_clouds_file_name); #if WITH_GUI == 1 - void draw_grids(bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, - bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, - bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, PointClouds::PointCloudDimensions dims); - void render(const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, bool xz_intersection, bool yz_intersection, bool xy_intersection, - bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, bool yz_grid_10x10, - bool yz_grid_1x1, bool yz_grid_01x01, bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, double intersection_width, PointClouds::PointCloudDimensions dims = {}); + void draw_grids( + bool xz_grid_10x10, + bool xz_grid_1x1, + bool xz_grid_01x01, + bool yz_grid_10x10, + bool yz_grid_1x1, + bool yz_grid_01x01, + bool xy_grid_10x10, + bool xy_grid_1x1, + bool xy_grid_01x01, + PointClouds::PointCloudDimensions dims); + void render( + const ObservationPicking& observation_picking, + int viewer_decmiate_point_cloud, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + bool xz_grid_10x10, + bool xz_grid_1x1, + bool xz_grid_01x01, + bool yz_grid_10x10, + bool yz_grid_1x1, + bool yz_grid_01x01, + bool xy_grid_10x10, + bool xy_grid_1x1, + bool xy_grid_01x01, + double intersection_width, + PointClouds::PointCloudDimensions dims = {}); #endif - //bool save_poses(); - bool save_poses(const std::string file_name, bool is_subsession); - bool save_scans(); - void show_all(); - void show_all_from_range(int index_begin, int index_end); - void hide_all(); + // bool save_poses(); + bool save_poses(const std::string file_name, bool is_subsession); + bool save_scans(); + void show_all(); + void show_all_from_range(int index_begin, int index_end); + void hide_all(); - std::vector point_clouds; - bool show_with_initial_pose = false; - - bool load_pose_ETH(const std::string& fn, Eigen::Affine3d &m_increment); - bool load_whu_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset, bool load_cache_mode); - PointCloudDimensions compute_point_cloud_dimension() const; - void print_point_cloud_dimension(); - bool load_3DTK_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z); + std::vector point_clouds; + bool show_with_initial_pose = false; - bool load_pc(PointCloud &pc, std::string input_file_name, bool load_cache_mode); -}; - -double get_mean_uncertainty_xyz_impact6x6(std::vector>& uncertainty_before, std::vector>& uncertainty_after); -double get_mean_uncertainty_xyz_impact7x7(std::vector>& uncertainty_before, std::vector>& uncertainty_after); -double get_mean_uncertainty_xyz_impact(std::vector>& uncertainty_before, std::vector>& uncertainty_after); + bool load_pose_ETH(const std::string& fn, Eigen::Affine3d& m_increment); + bool load_whu_tls( + std::vector input_file_names, + bool is_decimate, + double bucket_x, + double bucket_y, + double bucket_z, + bool calculate_offset, + bool load_cache_mode); + PointCloudDimensions compute_point_cloud_dimension() const; + void print_point_cloud_dimension(); + bool load_3DTK_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z); + bool load_pc(PointCloud& pc, std::string input_file_name, bool load_cache_mode); +}; -#endif \ No newline at end of file +double get_mean_uncertainty_xyz_impact6x6( + std::vector>& uncertainty_before, + std::vector>& uncertainty_after); +double get_mean_uncertainty_xyz_impact7x7( + std::vector>& uncertainty_before, + std::vector>& uncertainty_after); +double get_mean_uncertainty_xyz_impact( + std::vector>& uncertainty_before, std::vector>& uncertainty_after); diff --git a/core/include/pose_graph_loop_closure.h b/core/include/pose_graph_loop_closure.h index d89501c4..29f40e11 100644 --- a/core/include/pose_graph_loop_closure.h +++ b/core/include/pose_graph_loop_closure.h @@ -1,63 +1,66 @@ -#ifndef _graph_loop_closure_h_ -#define _graph_loop_closure_h_ +#pragma once -#include +#include #include #include -#include +#include + +class PoseGraphLoopClosure +{ +public: + enum EdgeType + { + motion_model_1, + motion_model_2, + loop_closure + }; -class PoseGraphLoopClosure{ - public: - enum EdgeType - { - motion_model_1, - motion_model_2, - loop_closure - }; - - struct Edge - { - TaitBryanPose relative_pose_tb; - TaitBryanPose relative_pose_tb_weights; - int index_from; - int index_to; - bool is_fixed_px = false; - bool is_fixed_py = false; - bool is_fixed_pz = false; - bool is_fixed_om = false; - bool is_fixed_fi = false; - bool is_fixed_ka = false; - EdgeType type; - }; - - std::vector edges; - std::vector poses_motion_model; - - //edge.relative_pose_tb_weights.om = 10000.0; - //edge.relative_pose_tb_weights.fi = 10000.0; - //edge.relative_pose_tb_weights.ka = 10000.0; - //edge.relative_pose_tb_weights.px = 100.0; - //edge.relative_pose_tb_weights.py = 10000.0; - //edge.relative_pose_tb_weights.pz = 10000.0; - - double motion_model_w_px_1_sigma_m = 0.1; - double motion_model_w_py_1_sigma_m = 0.01; - double motion_model_w_pz_1_sigma_m = 0.01; - - double motion_model_w_om_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; //0.01; - double motion_model_w_fi_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; - double motion_model_w_ka_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; - - PoseGraphLoopClosure(){}; - ~PoseGraphLoopClosure(){}; - - void add_edge( - PointClouds &point_clouds_container, int index_loop_closure_source, int index_loop_closure_target); - void set_initial_poses_as_motion_model(PointClouds &point_clouds_container); - void set_current_poses_as_motion_model(PointClouds &point_clouds_container); - void graph_slam(PointClouds &point_clouds_container, GNSS &gnss, GroundControlPoints &gcps, ControlPoints &cps); - void FuseTrajectoryWithGNSS(PointClouds &point_clouds_container, GNSS &gnss); - void run_icp(PointClouds &point_clouds_container, int index_active_edge, float search_radius, int number_of_iterations, int num_edge_extended_before, int num_edge_extended_after); + struct Edge + { + TaitBryanPose relative_pose_tb; + TaitBryanPose relative_pose_tb_weights; + int index_from; + int index_to; + bool is_fixed_px = false; + bool is_fixed_py = false; + bool is_fixed_pz = false; + bool is_fixed_om = false; + bool is_fixed_fi = false; + bool is_fixed_ka = false; + EdgeType type; }; -#endif + std::vector edges; + std::vector poses_motion_model; + + // edge.relative_pose_tb_weights.om = 10000.0; + // edge.relative_pose_tb_weights.fi = 10000.0; + // edge.relative_pose_tb_weights.ka = 10000.0; + // edge.relative_pose_tb_weights.px = 100.0; + // edge.relative_pose_tb_weights.py = 10000.0; + // edge.relative_pose_tb_weights.pz = 10000.0; + + double motion_model_w_px_1_sigma_m = 0.1; + double motion_model_w_py_1_sigma_m = 0.01; + double motion_model_w_pz_1_sigma_m = 0.01; + + double motion_model_w_om_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; // 0.01; + double motion_model_w_fi_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; + double motion_model_w_ka_1_sigma_deg = 1.0 / 100.0 * 180.0 / M_PI; + + PoseGraphLoopClosure(){}; + ~PoseGraphLoopClosure(){}; + + void add_edge(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target); + void set_initial_poses_as_motion_model(PointClouds& point_clouds_container); + void set_current_poses_as_motion_model(PointClouds& point_clouds_container); + void graph_slam(PointClouds& point_clouds_container, GNSS& gnss, GroundControlPoints& gcps, ControlPoints& cps); + void FuseTrajectoryWithGNSS(PointClouds& point_clouds_container, GNSS& gnss); + void run_icp( + PointClouds& point_clouds_container, + int index_active_edge, + float search_radius, + int number_of_iterations, + int num_edge_extended_before, + int num_edge_extended_after); +}; diff --git a/core/include/pose_graph_slam.h b/core/include/pose_graph_slam.h index c6416dc4..5f9d5bed 100644 --- a/core/include/pose_graph_slam.h +++ b/core/include/pose_graph_slam.h @@ -1,99 +1,104 @@ -#ifndef _POSE_GRAPH_SLAM_H_ -#define _POSE_GRAPH_SLAM_H_ +#pragma once -#include #include +#include -class PoseGraphSLAM { +class PoseGraphSLAM +{ public: - enum PairWiseMatchingType{ - general, - pcl_ndt, - pcl_icp - }; - - struct Edge { - int index_from; - int index_to; - Eigen::Affine3d m_relative_pose; - Eigen::SparseMatrix information_matrix; - }; - - PoseGraphSLAM() { ndt_bucket_size[0] = 1.0; ndt_bucket_size[1] = 1.0; ndt_bucket_size[2] = 1.0;/*icp.is_adaptive_robust_kernel = false;*/ }; - ~PoseGraphSLAM() { ; }; - - float overlap_threshold = 0.3; - int iterations = 6; - std::vector edges; - - float search_radius = 0.1; - int number_of_threads = 16; - int number_of_iterations_pair_wise_matching = 6; - - //-- - bool is_adaptive_robust_kernel = false; - bool is_fix_first_node = false; - bool is_gauss_newton = true; - bool is_levenberg_marguardt = false; - bool is_cw = false; - bool is_wc = true; - bool is_tait_bryan_angles = true; - bool is_quaternion = false; - bool is_rodrigues = false; - //-- - - bool is_ndt = true; - bool is_optimization_point_to_point_source_to_target = false; - bool is_optimize_point_to_projection_onto_plane_source_to_target = false; - bool is_optimize_point_to_plane_source_to_target = false; - bool is_optimize_distance_point_to_plane_source_to_target = false; - bool is_optimize_plane_to_plane_source_to_target = false; - bool is_optimize_pcl_ndt = false; - bool is_optimize_pcl_icp = false; - bool is_ndt_lie_algebra_left_jacobian = false; - bool is_ndt_lie_algebra_right_jacobian = false; - bool is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = false; - bool is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = false; - bool is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = false; - bool is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = false; - bool is_lie_algebra_left_jacobian = false; - bool is_lie_algebra_right_jacobian = false; - - - float ndt_bucket_size[3]; - - PairWiseMatchingType pair_wise_matching_type = PairWiseMatchingType::general; - - void add_random_noise(PointClouds& point_clouds_container); - - //bool optimize(PointClouds& point_clouds_container, double& rms_initial, double& rms_final, double& mui); - bool optimize(PointClouds& point_clouds_container); - std::vector> compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms); - - void calculate_edges(std::vector& point_clouds); - - bool optimize_with_GTSAM(PointClouds& point_clouds_container); - bool optimize_with_manif(PointClouds& point_clouds_container); - - void set_all_to_false() { - is_ndt_lie_algebra_left_jacobian = false; - is_ndt_lie_algebra_right_jacobian = false; - is_ndt = false; - is_optimization_point_to_point_source_to_target = false; - is_optimize_point_to_projection_onto_plane_source_to_target = false; - is_optimize_point_to_plane_source_to_target = false; - is_optimize_distance_point_to_plane_source_to_target = false; - is_optimize_plane_to_plane_source_to_target = false; - is_optimize_pcl_ndt = false; - is_optimize_pcl_icp = false; - is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = false; - is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = false; - is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = false; - is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = false; - is_lie_algebra_left_jacobian = false; - is_lie_algebra_right_jacobian = false; - } + enum PairWiseMatchingType + { + general, + pcl_ndt, + pcl_icp + }; + + struct Edge + { + int index_from; + int index_to; + Eigen::Affine3d m_relative_pose; + Eigen::SparseMatrix information_matrix; + }; + + PoseGraphSLAM() + { + ndt_bucket_size[0] = 1.0; + ndt_bucket_size[1] = 1.0; + ndt_bucket_size[2] = 1.0; /*icp.is_adaptive_robust_kernel = false;*/ + } + + ~PoseGraphSLAM() = default; + + float overlap_threshold = 0.3; + int iterations = 6; + std::vector edges; + + float search_radius = 0.1; + int number_of_threads = 16; + int number_of_iterations_pair_wise_matching = 6; + + //-- + bool is_adaptive_robust_kernel = false; + bool is_fix_first_node = false; + bool is_gauss_newton = true; + bool is_levenberg_marguardt = false; + bool is_cw = false; + bool is_wc = true; + bool is_tait_bryan_angles = true; + bool is_quaternion = false; + bool is_rodrigues = false; + //-- + + bool is_ndt = true; + bool is_optimization_point_to_point_source_to_target = false; + bool is_optimize_point_to_projection_onto_plane_source_to_target = false; + bool is_optimize_point_to_plane_source_to_target = false; + bool is_optimize_distance_point_to_plane_source_to_target = false; + bool is_optimize_plane_to_plane_source_to_target = false; + bool is_optimize_pcl_ndt = false; + bool is_optimize_pcl_icp = false; + bool is_ndt_lie_algebra_left_jacobian = false; + bool is_ndt_lie_algebra_right_jacobian = false; + bool is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = false; + bool is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = false; + bool is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = false; + bool is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = false; + bool is_lie_algebra_left_jacobian = false; + bool is_lie_algebra_right_jacobian = false; + + float ndt_bucket_size[3]; + + PairWiseMatchingType pair_wise_matching_type = PairWiseMatchingType::general; + + void add_random_noise(PointClouds& point_clouds_container); + + // bool optimize(PointClouds& point_clouds_container, double& rms_initial, double& rms_final, double& mui); + bool optimize(PointClouds& point_clouds_container); + std::vector> compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms); + + void calculate_edges(std::vector& point_clouds); + + bool optimize_with_GTSAM(PointClouds& point_clouds_container); + bool optimize_with_manif(PointClouds& point_clouds_container); + + void set_all_to_false() + { + is_ndt_lie_algebra_left_jacobian = false; + is_ndt_lie_algebra_right_jacobian = false; + is_ndt = false; + is_optimization_point_to_point_source_to_target = false; + is_optimize_point_to_projection_onto_plane_source_to_target = false; + is_optimize_point_to_plane_source_to_target = false; + is_optimize_distance_point_to_plane_source_to_target = false; + is_optimize_plane_to_plane_source_to_target = false; + is_optimize_pcl_ndt = false; + is_optimize_pcl_icp = false; + is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = false; + is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = false; + is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = false; + is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = false; + is_lie_algebra_left_jacobian = false; + is_lie_algebra_right_jacobian = false; + } }; - - -#endif diff --git a/core/include/registration_plane_feature.h b/core/include/registration_plane_feature.h index b948225b..2207f437 100644 --- a/core/include/registration_plane_feature.h +++ b/core/include/registration_plane_feature.h @@ -1,131 +1,149 @@ -#ifndef _REGISTRATION_PLANE_FEATURE_H_ -#define _REGISTRATION_PLANE_FEATURE_H_ +#pragma once #include -class RegistrationPlaneFeature { +class RegistrationPlaneFeature +{ public: - enum PoseConvention { - cw, - wc - }; - enum OptimizationAlgorithm { - gauss_newton, - levenberg_marguardt - }; - enum RotationMatrixParametrization { - tait_bryan_xyz, - rodrigues, - quaternion - }; - - struct Plane { - Plane() { - a = b = c = d = 0.0; - } - double a; - double b; - double c; - double d; - - Plane(Eigen::Vector3d p, Eigen::Vector3d nv) { - a = nv.x(); - b = nv.y(); - c = nv.z(); - d = -a * p.x() - b * p.y() - c * p.z(); - } - }; - - struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; - }; - std::vector get_jobs(long long unsigned int size, int num_threads = 8); - - RegistrationPlaneFeature() { - search_radius = 0.1; - number_of_threads = 16; - number_of_iterations = 6; - is_adaptive_robust_kernel = false; - barron_c = 1.0; - }; - ~RegistrationPlaneFeature() {}; - - //optimize_point_to_projection_onto_plane_source_to_target - bool optimize_point_to_projection_onto_plane_source_to_target(PointClouds& point_clouds_container); - bool optimize_point_to_projection_onto_plane_source_to_target(PointClouds& point_clouds_container, - PoseConvention pose_convention, OptimizationAlgorithm optimization_algorithm, RotationMatrixParametrization rotation_matrix_parametrization); - std::vector> compute_covariance_matrices_tait_bryan_point_to_projection_onto_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_rodrigues_point_to_projection_onto_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_quaternion_point_to_projection_onto_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - - bool optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container); - bool optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container); - - - - - //optimize_point_to_plane_source_to_target - bool optimize_point_to_plane_source_to_target(PointClouds& point_clouds_container); - bool optimize_point_to_plane_source_to_target(PointClouds& point_clouds_container, - PoseConvention pose_convention, OptimizationAlgorithm optimization_algorithm, RotationMatrixParametrization rotation_matrix_parametrization); - std::vector> compute_covariance_matrices_tait_bryan_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_rodrigues_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_quaternion_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - //optimize_point_to_plane_source_to_target - - //optimize_distance_point_to_plane_source_to_target - bool optimize_distance_point_to_plane_source_to_target(PointClouds& point_clouds_container); - bool optimize_distance_point_to_plane_source_to_target(PointClouds& point_clouds_container, - PoseConvention pose_convention, OptimizationAlgorithm optimization_algorithm, RotationMatrixParametrization rotation_matrix_parametrization); - std::vector> compute_covariance_matrices_tait_bryan_distance_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_rodrigues_distance_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_quaternion_distance_point_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - - //optimize_plane_to_plane_source_to_target - bool optimize_plane_to_plane_source_to_target(PointClouds& point_clouds_container); - std::vector> compute_covariance_matrices_tait_bryan_plane_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_rodrigues_plane_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - std::vector> compute_covariance_matrices_quaternion_plane_to_plane_source_to_target( - PointClouds& point_clouds_container, PoseConvention pose_convention); - bool optimize_plane_to_plane_source_to_target(PointClouds& point_clouds_container, - PoseConvention pose_convention, OptimizationAlgorithm optimization_algorithm, RotationMatrixParametrization rotation_matrix_parametrization); - - float search_radius = 0.1; - int number_of_threads = 16; - int number_of_iterations = 6; - bool is_adaptive_robust_kernel = true; - double barron_c = 1.0; - - //bool is_newton = false; - bool is_gauss_newton = true; - bool is_levenberg_marguardt = false; - - bool is_wc = true; - bool is_cw = false; - - bool is_tait_bryan_angles = true; - bool is_quaternion = false; - bool is_rodrigues = false; - - bool is_fix_first_node = false; - - bool is_lie_algebra_left_jacobian = false; - bool is_lie_algebra_right_jacobian = false; + enum PoseConvention + { + cw, + wc + }; + enum OptimizationAlgorithm + { + gauss_newton, + levenberg_marguardt + }; + enum RotationMatrixParametrization + { + tait_bryan_xyz, + rodrigues, + quaternion + }; + + struct Plane + { + Plane() + { + a = b = c = d = 0.0; + } + double a; + double b; + double c; + double d; + + Plane(Eigen::Vector3d p, Eigen::Vector3d nv) + { + a = nv.x(); + b = nv.y(); + c = nv.z(); + d = -a * p.x() - b * p.y() - c * p.z(); + } + }; + + struct Job + { + long long unsigned int index_begin_inclusive; + long long unsigned int index_end_exclusive; + }; + std::vector get_jobs(long long unsigned int size, int num_threads = 8); + + RegistrationPlaneFeature() + { + search_radius = 0.1; + number_of_threads = 16; + number_of_iterations = 6; + is_adaptive_robust_kernel = false; + barron_c = 1.0; + } + ~RegistrationPlaneFeature() = default; + + // optimize_point_to_projection_onto_plane_source_to_target + bool optimize_point_to_projection_onto_plane_source_to_target(PointClouds& point_clouds_container); + bool optimize_point_to_projection_onto_plane_source_to_target( + PointClouds& point_clouds_container, + PoseConvention pose_convention, + OptimizationAlgorithm optimization_algorithm, + RotationMatrixParametrization rotation_matrix_parametrization); + std::vector> + compute_covariance_matrices_tait_bryan_point_to_projection_onto_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> + compute_covariance_matrices_rodrigues_point_to_projection_onto_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> + compute_covariance_matrices_quaternion_point_to_projection_onto_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + + bool optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container); + bool optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container); + + // optimize_point_to_plane_source_to_target + bool optimize_point_to_plane_source_to_target(PointClouds& point_clouds_container); + bool optimize_point_to_plane_source_to_target( + PointClouds& point_clouds_container, + PoseConvention pose_convention, + OptimizationAlgorithm optimization_algorithm, + RotationMatrixParametrization rotation_matrix_parametrization); + std::vector> compute_covariance_matrices_tait_bryan_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_rodrigues_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_quaternion_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + // optimize_point_to_plane_source_to_target + + // optimize_distance_point_to_plane_source_to_target + bool optimize_distance_point_to_plane_source_to_target(PointClouds& point_clouds_container); + bool optimize_distance_point_to_plane_source_to_target( + PointClouds& point_clouds_container, + PoseConvention pose_convention, + OptimizationAlgorithm optimization_algorithm, + RotationMatrixParametrization rotation_matrix_parametrization); + std::vector> + compute_covariance_matrices_tait_bryan_distance_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> + compute_covariance_matrices_rodrigues_distance_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> + compute_covariance_matrices_quaternion_distance_point_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + + // optimize_plane_to_plane_source_to_target + bool optimize_plane_to_plane_source_to_target(PointClouds& point_clouds_container); + std::vector> compute_covariance_matrices_tait_bryan_plane_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_rodrigues_plane_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + std::vector> compute_covariance_matrices_quaternion_plane_to_plane_source_to_target( + PointClouds& point_clouds_container, PoseConvention pose_convention); + bool optimize_plane_to_plane_source_to_target( + PointClouds& point_clouds_container, + PoseConvention pose_convention, + OptimizationAlgorithm optimization_algorithm, + RotationMatrixParametrization rotation_matrix_parametrization); + + float search_radius = 0.1; + int number_of_threads = 16; + int number_of_iterations = 6; + bool is_adaptive_robust_kernel = true; + double barron_c = 1.0; + + // bool is_newton = false; + bool is_gauss_newton = true; + bool is_levenberg_marguardt = false; + + bool is_wc = true; + bool is_cw = false; + + bool is_tait_bryan_angles = true; + bool is_quaternion = false; + bool is_rodrigues = false; + + bool is_fix_first_node = false; + + bool is_lie_algebra_left_jacobian = false; + bool is_lie_algebra_right_jacobian = false; }; - - - - -#endif \ No newline at end of file diff --git a/core/include/session.h b/core/include/session.h index f5ce8b12..2897a241 100644 --- a/core/include/session.h +++ b/core/include/session.h @@ -1,13 +1,14 @@ -#ifndef _SESSION_H_ -#define _SESSION_H_ +#pragma once +#include #include #include -#include + #if WITH_GUI == 1 -#include -#include #include +#include +#include + #else #include #endif @@ -15,8 +16,8 @@ class Session { public: - Session() { ; }; - ~Session() { ; }; + Session() = default; + ~Session() = default; PointClouds point_clouds_container; std::string working_directory = ""; @@ -36,12 +37,13 @@ class Session PoseGraphLoopClosure pose_graph_loop_closure; #endif - bool load(const std::string &file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset); - bool save(const std::string &file_name, const std::string &poses_file_name, const std::string &initial_poses_file_name, bool is_subsession); + bool load(const std::string& file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset); + bool save( + const std::string& file_name, const std::string& poses_file_name, const std::string& initial_poses_file_name, bool is_subsession); void fill_session_from_worker_data( - const std::vector &worker_data, bool save_selected, - bool filter_on_export, bool apply_pose, double threshould_output_filter - ); + const std::vector& worker_data, + bool save_selected, + bool filter_on_export, + bool apply_pose, + double threshould_output_filter); }; - -#endif diff --git a/core/include/structures.h b/core/include/structures.h index a202ed17..b574ee3d 100644 --- a/core/include/structures.h +++ b/core/include/structures.h @@ -1,75 +1,76 @@ -#ifndef _STRUCTURES_H_ -#define _STRUCTURES_H_ +#pragma once -#include -#include #include #include +#include +#include #include #include -struct PerspectiveCameraParams { - double cx; - double cy; - double fx; - double fy; +struct PerspectiveCameraParams +{ + double cx; + double cy; + double fx; + double fy; }; -struct MetricCameraParams{ - double c; - double ksi_0; - double eta_0; +struct MetricCameraParams +{ + double c; + double ksi_0; + double eta_0; }; struct TaitBryanPose { - double px = 0.0; - double py = 0.0; - double pz = 0.0; - double om = 0.0; - double fi = 0.0; - double ka = 0.0; + double px = 0.0; + double py = 0.0; + double pz = 0.0; + double om = 0.0; + double fi = 0.0; + double ka = 0.0; }; struct RodriguesPose { - double px; - double py; - double pz; - double sx; - double sy; - double sz; + double px; + double py; + double pz; + double sx; + double sy; + double sz; }; struct QuaternionPose { - double px; - double py; - double pz; - double q0; - double q1; - double q2; - double q3; + double px; + double py; + double pz; + double q0; + double q1; + double q2; + double q3; }; struct Point3D { - float x; - float y; - float z; - int index_pose; + float x; + float y; + float z; + int index_pose; }; struct Point3Di { - Eigen::Vector3d point; - double timestamp; + Eigen::Vector3d point; + double timestamp; float intensity; int index_pose; uint8_t lidarid; - int index_point; + int index_point; }; // Point3Di POD (Plain Old Data) variant @@ -78,335 +79,344 @@ struct Point3Di #pragma pack(push, 1) struct Point3DiDisk { - double x, y, z; - double timestamp; - float intensity; - int32_t index_pose; - uint8_t lidarid; - int32_t index_point; + double x, y, z; + double timestamp; + float intensity; + int32_t index_pose; + uint8_t lidarid; + int32_t index_point; }; #pragma pack(pop) struct Point { - double x = 0.0f; - double y = 0.0f; - double z = 0.0f; - uint32_t intensity = 0; - double time = 0.0; + double x = 0.0f; + double y = 0.0f; + double z = 0.0f; + uint32_t intensity = 0; + double time = 0.0; }; struct PointMesh { - Eigen::Vector3d coordinates; - Eigen::Vector3d normal_vector; - int index_pose; + Eigen::Vector3d coordinates; + Eigen::Vector3d normal_vector; + int index_pose; }; -struct ChunkFile { - int filename; - double time_begin_inclusive; - double time_end_inclusive; +struct ChunkFile +{ + int filename; + double time_begin_inclusive; + double time_end_inclusive; }; -//Extra measures are needed when writting/reading non-trivial types -//Point3Di containing Eigen::Vector3d to avoid: +// Extra measures are needed when writting/reading non-trivial types +// Point3Di containing Eigen::Vector3d to avoid: //- structure alignment & padding written to file //- unreadable file on another build //- undefined behavior if read back via raw load inline void convert_to_disk(const Point3Di& src, Point3DiDisk& dst) { - dst.x = src.point.x(); - dst.y = src.point.y(); - dst.z = src.point.z(); - dst.timestamp = src.timestamp; - dst.intensity = src.intensity; - dst.index_pose = src.index_pose; - dst.lidarid = src.lidarid; - dst.index_point = src.index_point; + dst.x = src.point.x(); + dst.y = src.point.y(); + dst.z = src.point.z(); + dst.timestamp = src.timestamp; + dst.intensity = src.intensity; + dst.index_pose = src.index_pose; + dst.lidarid = src.lidarid; + dst.index_point = src.index_point; } inline void convert_from_disk(const Point3DiDisk& src, Point3Di& dst) { - dst.point = Eigen::Vector3d(src.x, src.y, src.z); - dst.timestamp = src.timestamp; - dst.intensity = src.intensity; - dst.index_pose = src.index_pose; - dst.lidarid = src.lidarid; - dst.index_point = src.index_point; + dst.point = Eigen::Vector3d(src.x, src.y, src.z); + dst.timestamp = src.timestamp; + dst.intensity = src.intensity; + dst.index_pose = src.index_pose; + dst.lidarid = src.lidarid; + dst.index_point = src.index_point; } /*template inline bool save_vector_data(const std::string& file_name, std::vector& vector_data) { - std::ofstream ofs(file_name, std::ios::binary); - if (!ofs.good()) { - return false; - } - ofs.write(reinterpret_cast(vector_data.data()), vector_data.size()* sizeof(T)); - return true; + std::ofstream ofs(file_name, std::ios::binary); + if (!ofs.good()) { + return false; + } + ofs.write(reinterpret_cast(vector_data.data()), vector_data.size()* sizeof(T)); + return true; }*/ template bool save_vector_data(const std::string& file_name, const std::vector& out) { - static_assert( - std::is_trivially_copyable_v || std::is_same_v, - "Binary loading only supported for POD or Point3Di" - ); - - std::ofstream file(file_name, std::ios::binary); - if (!file) - return false; - - if constexpr (std::is_trivially_copyable_v) - // direct write for POD - return file.write(reinterpret_cast(out.data()), out.size() * sizeof(T)).good(); - else - { - // convert to disk type first - std::vector disk(out.size()); - for (size_t i = 0; i < out.size(); ++i) - convert_to_disk(out[i], disk[i]); - - return file.write(reinterpret_cast(disk.data()), disk.size() * sizeof(Point3DiDisk)).good(); - } + static_assert(std::is_trivially_copyable_v || std::is_same_v, "Binary loading only supported for POD or Point3Di"); + + std::ofstream file(file_name, std::ios::binary); + if (!file) + return false; + + if constexpr (std::is_trivially_copyable_v) + // direct write for POD + return file.write(reinterpret_cast(out.data()), out.size() * sizeof(T)).good(); + else + { + // convert to disk type first + std::vector disk(out.size()); + for (size_t i = 0; i < out.size(); ++i) + convert_to_disk(out[i], disk[i]); + + return file.write(reinterpret_cast(disk.data()), disk.size() * sizeof(Point3DiDisk)).good(); + } } /*template inline bool load_vector_data(const std::string& file_name, std::vector& vector_data) { - std::basic_ifstream vd_str(file_name, std::ios::binary); - if (!vd_str.good()) { - return false; - } - std::vector data((std::istreambuf_iterator(vd_str)), std::istreambuf_iterator()); - std::vector v(data.size() / sizeof(T)); - memcpy(v.data(), data.data(), data.size()); - vector_data = v; - return true; + std::basic_ifstream vd_str(file_name, std::ios::binary); + if (!vd_str.good()) { + return false; + } + std::vector data((std::istreambuf_iterator(vd_str)), std::istreambuf_iterator()); + std::vector v(data.size() / sizeof(T)); + memcpy(v.data(), data.data(), data.size()); + vector_data = v; + return true; }*/ /*template bool load_vector_data(const std::string& file_name, std::vector& out) { - //static_assert(std::is_trivially_copyable_v, - // "T must be trivially copyable"); + //static_assert(std::is_trivially_copyable_v, + // "T must be trivially copyable"); - std::ifstream file(file_name, std::ios::binary | std::ios::ate); - if (!file) - return false; + std::ifstream file(file_name, std::ios::binary | std::ios::ate); + if (!file) + return false; - const std::streamsize size = file.tellg(); - if (size < 0 || size % sizeof(T) != 0) - return false; + const std::streamsize size = file.tellg(); + if (size < 0 || size % sizeof(T) != 0) + return false; - const size_t count = size / sizeof(T); - out.resize(count); + const size_t count = size / sizeof(T); + out.resize(count); - file.seekg(0, std::ios::beg); - if (!file.read(reinterpret_cast(out.data()), size)) - return false; + file.seekg(0, std::ios::beg); + if (!file.read(reinterpret_cast(out.data()), size)) + return false; - return true; + return true; }*/ template bool load_vector_data(const std::string& file_name, std::vector& out) { - static_assert( - std::is_trivially_copyable_v || std::is_same_v, - "Binary loading only supported for POD or Point3Di" - ); + static_assert(std::is_trivially_copyable_v || std::is_same_v, "Binary loading only supported for POD or Point3Di"); - std::ifstream file(file_name, std::ios::binary | std::ios::ate); - if (!file) - return false; + std::ifstream file(file_name, std::ios::binary | std::ios::ate); + if (!file) + return false; - const std::streamsize size = file.tellg(); - if (size < 0) - return false; + const std::streamsize size = file.tellg(); + if (size < 0) + return false; - file.seekg(0, std::ios::beg); + file.seekg(0, std::ios::beg); - if constexpr (std::is_trivially_copyable_v) - { - if (size % sizeof(T) != 0) - return false; + if constexpr (std::is_trivially_copyable_v) + { + if (size % sizeof(T) != 0) + return false; - const size_t count = size / sizeof(T); - out.resize(count); + const size_t count = size / sizeof(T); + out.resize(count); - return file.read(reinterpret_cast(out.data()), size).good(); - } - else - { - // Non-trivial type path (Point3Di) - if (size % sizeof(Point3DiDisk) != 0) - return false; + return file.read(reinterpret_cast(out.data()), size).good(); + } + else + { + // Non-trivial type path (Point3Di) + if (size % sizeof(Point3DiDisk) != 0) + return false; - const size_t count = size / sizeof(Point3DiDisk); - std::vector disk(count); + const size_t count = size / sizeof(Point3DiDisk); + std::vector disk(count); - if (!file.read(reinterpret_cast(disk.data()), size)) - return false; + if (!file.read(reinterpret_cast(disk.data()), size)) + return false; - out.resize(count); - for (size_t i = 0; i < count; ++i) - convert_from_disk(disk[i], out[i]); + out.resize(count); + for (size_t i = 0; i < count; ++i) + convert_from_disk(disk[i], out[i]); - return true; - } + return true; + } } -struct LaserBeam { - Eigen::Vector3d position; - Eigen::Vector3d direction; - float distance; - float range; +struct LaserBeam +{ + Eigen::Vector3d position; + Eigen::Vector3d direction; + float distance; + float range; }; -struct Plane { - Plane() { - a = b = c = d = 0.0; - } - double a; - double b; - double c; - double d; - - Plane(Eigen::Vector3d p, Eigen::Vector3d nv) { - a = nv.x(); - b = nv.y(); - c = nv.z(); - d = -a * p.x() - b * p.y() - c * p.z(); - } +struct Plane +{ + Plane() + { + a = b = c = d = 0.0; + } + double a; + double b; + double c; + double d; + + Plane(Eigen::Vector3d p, Eigen::Vector3d nv) + { + a = nv.x(); + b = nv.y(); + c = nv.z(); + d = -a * p.x() - b * p.y() - c * p.z(); + } }; -struct CommonData { - Eigen::Vector3d roi = Eigen::Vector3d(0, 0, 0); - float roi_size = 50; - float rotate_x = 0.0, rotate_y = 0.0; - float translate_x, translate_y = 0.0; - float translate_z = -5000; - bool is_ortho = true; - bool roi_exorter = false; - bool laz_wrapper = false; - bool single_trajectory_viewer = false; - bool odo_with_gnss_fusion = false; - float shift_x = 0.0; - float shift_y = 0.0; +struct CommonData +{ + Eigen::Vector3d roi = Eigen::Vector3d(0, 0, 0); + float roi_size = 50; + float rotate_x = 0.0, rotate_y = 0.0; + float translate_x, translate_y = 0.0; + float translate_z = -5000; + bool is_ortho = true; + bool roi_exorter = false; + bool laz_wrapper = false; + bool single_trajectory_viewer = false; + bool odo_with_gnss_fusion = false; + float shift_x = 0.0; + float shift_y = 0.0; }; -struct PointCloudWithPose { - bool visible = true; - double timestamp; - std::string trajectory_file_name; - Eigen::Affine3d m_pose; - std::vector points_global; - int point_size = 1; +struct PointCloudWithPose +{ + bool visible = true; + double timestamp; + std::string trajectory_file_name; + Eigen::Affine3d m_pose; + std::vector points_global; + int point_size = 1; }; -struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; +struct Job +{ + long long unsigned int index_begin_inclusive; + long long unsigned int index_end_exclusive; }; -inline std::vector get_jobs(long long unsigned int size, int num_threads) { - - int hc = size / num_threads; - if (hc < 1)hc = 1; - - std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; - if (i + hc >= size) { - sequence_length = size - i; - } - if (sequence_length == 0)break; - - Job j; - j.index_begin_inclusive = i; - j.index_end_exclusive = i + sequence_length; - jobs.push_back(j); - } - - return jobs; +inline std::vector get_jobs(long long unsigned int size, int num_threads) +{ + int hc = size / num_threads; + if (hc < 1) + hc = 1; + + std::vector jobs; + for (long long unsigned int i = 0; i < size; i += hc) + { + long long unsigned int sequence_length = hc; + if (i + hc >= size) + { + sequence_length = size - i; + } + if (sequence_length == 0) + break; + + Job j; + j.index_begin_inclusive = i; + j.index_end_exclusive = i + sequence_length; + jobs.push_back(j); + } + + return jobs; } struct LAZPoint { - double x = 0.0f; - double y = 0.0f; - double z = 0.0f; - //uint16_t r; - //uint16_t g; - //uint16_t b; - float r; - float g; - float b; - uint8_t classsification; - double timestamp; - //unsigned short RGB[4]; + double x = 0.0f; + double y = 0.0f; + double z = 0.0f; + // uint16_t r; + // uint16_t g; + // uint16_t b; + float r; + float g; + float b; + uint8_t classsification; + double timestamp; + // unsigned short RGB[4]; }; -struct LAZSector { - std::string file_name; - std::vector point_cloud; - double min_x; - double max_x; - double min_y; - double max_y; - bool visible = true; - int point_size = 1; +struct LAZSector +{ + std::string file_name; + std::vector point_cloud; + double min_x; + double max_x; + double min_y; + double max_y; + bool visible = true; + int point_size = 1; }; -struct ConstraintToGeoreference { - std::string trajectory_file_name; - double time_stamp; - Eigen::Affine3d m_pose; +struct ConstraintToGeoreference +{ + std::string trajectory_file_name; + double time_stamp; + Eigen::Affine3d m_pose; }; -struct ROIwithConstraints { - std::vector constraints; +struct ROIwithConstraints +{ + std::vector constraints; }; -struct Node { - double timestamp = 0.0; - Eigen::Affine3d m_pose = Eigen::Affine3d::Identity(); - int index_to_lidar_odometry_odo = -1; - int index_to_gnss = -1; +struct Node +{ + double timestamp = 0.0; + Eigen::Affine3d m_pose = Eigen::Affine3d::Identity(); + int index_to_lidar_odometry_odo = -1; + int index_to_gnss = -1; }; -struct BetweenNode { - Node node_outer; - std::vector nodes_between; - float color_x = 0; - float color_y = 0; - float color_z = 0; +struct BetweenNode +{ + Node node_outer; + std::vector nodes_between; + float color_x = 0; + float color_y = 0; + float color_z = 0; }; -struct GeoPoint { - std::string name; - Eigen::Vector3d coordinates; - bool choosen = false; - double w_x = 1000000.0; - double w_y = 1000000.0; - double w_z = 1000000.0; +struct GeoPoint +{ + std::string name; + Eigen::Vector3d coordinates; + bool choosen = false; + double w_x = 1000000.0; + double w_y = 1000000.0; + double w_z = 1000000.0; }; - struct WorkerData { - // std::vector intermediate_points; - std::filesystem::path intermediate_points_cache_file_name; - // std::vector original_points_to_save; - std::filesystem::path original_points_cache_file_name; - std::filesystem::path original_points_to_save_cache_file_name; - std::vector intermediate_trajectory; - std::vector intermediate_trajectory_motion_model; - std::vector> intermediate_trajectory_timestamps; - std::vector imu_om_fi_ka; - bool show = false; + // std::vector intermediate_points; + std::filesystem::path intermediate_points_cache_file_name; + // std::vector original_points_to_save; + std::filesystem::path original_points_cache_file_name; + std::filesystem::path original_points_to_save_cache_file_name; + std::vector intermediate_trajectory; + std::vector intermediate_trajectory_motion_model; + std::vector> intermediate_trajectory_timestamps; + std::vector imu_om_fi_ka; + bool show = false; }; - -#endif diff --git a/core/include/surface.h b/core/include/surface.h index c478a4f6..22d9d864 100644 --- a/core/include/surface.h +++ b/core/include/surface.h @@ -1,8 +1,8 @@ -#ifndef _SURFACE_H_ -#define _SURFACE_H_ +#pragma once -#include #include +#include + inline unsigned long long int get_index_2D(const int16_t x, const int16_t y /*, const int16_t z*/) { @@ -10,7 +10,7 @@ inline unsigned long long int get_index_2D(const int16_t x, const int16_t y /*, // ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | // ((static_cast(z) << 0) & (0x000000000000FFFFull)); return ((static_cast(x) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(y) << 0) & (0x000000000000FFFFull)); + ((static_cast(y) << 0) & (0x000000000000FFFFull)); } inline unsigned long long int get_rgd_index_2D(const Eigen::Vector3d p, const Eigen::Vector2d b) @@ -31,8 +31,14 @@ class Surface // int index_after; // }; - Surface() { ; }; - ~Surface() { ; }; + Surface() + { + ; + }; + ~Surface() + { + ; + }; std::vector vertices; std::vector vertices_odo; @@ -46,13 +52,15 @@ class Surface double lowest_points_filter_resolution = 0.3; bool robust_kernel = false; - void generate_initial_surface(const std::vector &point_cloud); + void generate_initial_surface(const std::vector& point_cloud); void render(); - void align_surface_to_ground_points(const std::vector &point_cloud); - bool find_nearest_neighbour(Eigen::Vector3d &p_t, Eigen::Affine3d vertex, const std::vector &reference_points); - std::vector get_filtered_indexes(const std::vector &pc, const std::vector &lowest_points_indexes, Eigen::Vector2d bucket_dim_xy); - std::vector get_points_without_surface(const std::vector &points, double distance_to_ground_threshold_bottom, - double distance_to_ground_threshold_up, Eigen::Vector2d bucket_dim_xy); + void align_surface_to_ground_points(const std::vector& point_cloud); + bool find_nearest_neighbour(Eigen::Vector3d& p_t, Eigen::Affine3d vertex, const std::vector& reference_points); + std::vector get_filtered_indexes( + const std::vector& pc, const std::vector& lowest_points_indexes, Eigen::Vector2d bucket_dim_xy); + std::vector get_points_without_surface( + const std::vector& points, + double distance_to_ground_threshold_bottom, + double distance_to_ground_threshold_up, + Eigen::Vector2d bucket_dim_xy); }; - -#endif \ No newline at end of file diff --git a/core/include/transformations.h b/core/include/transformations.h index 90e6cf48..f6eda9bc 100644 --- a/core/include/transformations.h +++ b/core/include/transformations.h @@ -1,11 +1,10 @@ -#ifndef _TRANSFORMATIONS_H_ -#define _TRANSFORMATIONS_H_ +#pragma once #include "structures.h" //#include #ifndef MAX -#define MAX(a,b) a>b?a:b +#define MAX(a, b) a > b ? a : b #endif #ifndef M_PI #define M_PI 3.14159265358979323846 @@ -13,160 +12,165 @@ #include -inline double deg2rad(double deg) { - return (deg * M_PI) / 180.0; +inline double deg2rad(double deg) +{ + return (deg * M_PI) / 180.0; } -inline double rad2deg(double rad) { - return (rad * 180.0) / M_PI; +inline double rad2deg(double rad) +{ + return (rad * 180.0) / M_PI; } -inline TaitBryanPose pose_tait_bryan_from_affine_matrix(Eigen::Affine3d m){ - TaitBryanPose pose; - - pose.px = m(0,3); - pose.py = m(1,3); - pose.pz = m(2,3); - - if (m(0,2) < 1) { - if (m(0,2) > -1) { - //case 1 - pose.fi = asin(m(0,2)); - pose.om = atan2(-m(1,2), m(2,2)); - pose.ka = atan2(-m(0,1), m(0,0)); - - return pose; - } - else //r02 = −1 - { - //case 2 - // not a unique solution: thetaz − thetax = atan2 ( r10 , r11 ) - pose.fi = -M_PI / 2.0; - pose.om = -atan2(m(1,0), m(1,1)); - pose.ka = 0; - return pose; - } - } - else { - //case 3 - // r02 = +1 - // not a unique solution: thetaz + thetax = atan2 ( r10 , r11 ) - pose.fi = M_PI / 2.0; - pose.om = atan2(m(1,0), m(1,1)); - pose.ka = 0.0; - return pose; - } - - return pose; +inline TaitBryanPose pose_tait_bryan_from_affine_matrix(Eigen::Affine3d m) +{ + TaitBryanPose pose; + + pose.px = m(0, 3); + pose.py = m(1, 3); + pose.pz = m(2, 3); + + if (m(0, 2) < 1) + { + if (m(0, 2) > -1) + { + // case 1 + pose.fi = asin(m(0, 2)); + pose.om = atan2(-m(1, 2), m(2, 2)); + pose.ka = atan2(-m(0, 1), m(0, 0)); + + return pose; + } + else // r02 = −1 + { + // case 2 + // not a unique solution: thetaz − thetax = atan2 ( r10 , r11 ) + pose.fi = -M_PI / 2.0; + pose.om = -atan2(m(1, 0), m(1, 1)); + pose.ka = 0; + return pose; + } + } + else + { + // case 3 + // r02 = +1 + // not a unique solution: thetaz + thetax = atan2 ( r10 , r11 ) + pose.fi = M_PI / 2.0; + pose.om = atan2(m(1, 0), m(1, 1)); + pose.ka = 0.0; + return pose; + } + + return pose; } inline Eigen::Affine3d affine_matrix_from_pose_tait_bryan(TaitBryanPose pose) { - Eigen::Affine3d m = Eigen::Affine3d::Identity(); + Eigen::Affine3d m = Eigen::Affine3d::Identity(); - double sx = sin(pose.om); - double cx = cos(pose.om); - double sy = sin(pose.fi); - double cy = cos(pose.fi); - double sz = sin(pose.ka); - double cz = cos(pose.ka); + double sx = sin(pose.om); + double cx = cos(pose.om); + double sy = sin(pose.fi); + double cy = cos(pose.fi); + double sz = sin(pose.ka); + double cz = cos(pose.ka); - m(0,0) = cy * cz; - m(1,0) = cz * sx * sy + cx * sz; - m(2,0) = -cx * cz * sy + sx * sz; + m(0, 0) = cy * cz; + m(1, 0) = cz * sx * sy + cx * sz; + m(2, 0) = -cx * cz * sy + sx * sz; - m(0,1) = -cy * sz; - m(1,1) = cx * cz - sx * sy * sz; - m(2,1) = cz * sx + cx * sy * sz; + m(0, 1) = -cy * sz; + m(1, 1) = cx * cz - sx * sy * sz; + m(2, 1) = cz * sx + cx * sy * sz; - m(0,2) = sy; - m(1,2) = -cy * sx; - m(2,2) = cx * cy; + m(0, 2) = sy; + m(1, 2) = -cy * sx; + m(2, 2) = cx * cy; - m(0,3) = pose.px; - m(1,3) = pose.py; - m(2,3) = pose.pz; + m(0, 3) = pose.px; + m(1, 3) = pose.py; + m(2, 3) = pose.pz; - return m; + return m; } - inline RodriguesPose pose_rodrigues_from_affine_matrix(Eigen::Affine3d m) { -//https://github.com/shimat/opencv_files_343/blob/master/include/x64/opencv2/core/affine.hpp -//https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac -//https://docs.rs/opencv/0.22.1/opencv/calib3d/fn.rodrigues.html -//https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp - RodriguesPose rp; - - double r_x = m(2,1) - m(1,2); - double r_y = m(0,2) - m(2,0); - double r_z = m(1,0) - m(0,1); - - // Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); - double s = std::sqrt((r_x*r_x + r_y*r_y + r_z*r_z)*0.25); - double c = (m(0, 0) + m(1, 1) + m(2, 2) - 1)*0.5; - c = c > 1. ? 1. : c < -1. ? -1. : c; - - double theta = acos(c); - - - if( s < 1e-8 ) - { - double t; - - if( c > 0 ) - r_x = r_y = r_z = 0.0; - else - { - t = (m(0, 0) + 1)*0.5; - r_x = std::sqrt(MAX(t,0.)); - t = (m(1, 1) + 1)*0.5; - r_y = std::sqrt(MAX(t,0.))*(m(0, 1) < 0 ? -1. : 1.); - t = (m(2, 2) + 1)*0.5; - r_z = std::sqrt(MAX(t,0.))*(m(0, 2) < 0 ? -1. : 1.); - if( fabs(r_x) < fabs(r_y) && fabs(r_x) < fabs(r_z) && (m(1, 2) > 0) != (r_y*r_z > 0) ) - r_z = -r_z; - theta /= sqrt(r_x*r_x + r_y*r_y + r_z*r_z);// norm(r); - r_x *= theta; - r_y *= theta; - r_z *= theta; - } - - rp.sx = r_x; - rp.sy = r_y; - rp.sz = r_z; - - rp.px = m(0,3); - rp.py = m(1,3); - rp.pz = m(2,3); - } - else - { - double vth = 1/(2*s); - vth *= theta; - r_x *= vth; - r_y *= vth; - r_z *= vth; - - rp.sx = r_x; - rp.sy = r_y; - rp.sz = r_z; - - rp.px = m(0,3); - rp.py = m(1,3); - rp.pz = m(2,3); - } - return rp; + // https://github.com/shimat/opencv_files_343/blob/master/include/x64/opencv2/core/affine.hpp + // https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac + // https://docs.rs/opencv/0.22.1/opencv/calib3d/fn.rodrigues.html + // https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp + RodriguesPose rp; + + double r_x = m(2, 1) - m(1, 2); + double r_y = m(0, 2) - m(2, 0); + double r_z = m(1, 0) - m(0, 1); + + // Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + double s = std::sqrt((r_x * r_x + r_y * r_y + r_z * r_z) * 0.25); + double c = (m(0, 0) + m(1, 1) + m(2, 2) - 1) * 0.5; + c = c > 1. ? 1. : c < -1. ? -1. : c; + + double theta = acos(c); + + if (s < 1e-8) + { + double t; + + if (c > 0) + r_x = r_y = r_z = 0.0; + else + { + t = (m(0, 0) + 1) * 0.5; + r_x = std::sqrt(MAX(t, 0.)); + t = (m(1, 1) + 1) * 0.5; + r_y = std::sqrt(MAX(t, 0.)) * (m(0, 1) < 0 ? -1. : 1.); + t = (m(2, 2) + 1) * 0.5; + r_z = std::sqrt(MAX(t, 0.)) * (m(0, 2) < 0 ? -1. : 1.); + if (fabs(r_x) < fabs(r_y) && fabs(r_x) < fabs(r_z) && (m(1, 2) > 0) != (r_y * r_z > 0)) + r_z = -r_z; + theta /= sqrt(r_x * r_x + r_y * r_y + r_z * r_z); // norm(r); + r_x *= theta; + r_y *= theta; + r_z *= theta; + } + + rp.sx = r_x; + rp.sy = r_y; + rp.sz = r_z; + + rp.px = m(0, 3); + rp.py = m(1, 3); + rp.pz = m(2, 3); + } + else + { + double vth = 1 / (2 * s); + vth *= theta; + r_x *= vth; + r_y *= vth; + r_z *= vth; + + rp.sx = r_x; + rp.sy = r_y; + rp.sz = r_z; + + rp.px = m(0, 3); + rp.py = m(1, 3); + rp.pz = m(2, 3); + } + return rp; } -inline void orthogonalize_rotation(Eigen::Affine3d& m){ - Eigen::Matrix4d ret = m.matrix(); - Eigen::JacobiSVD svd(ret.block<3,3>(0,0), Eigen::ComputeFullU | Eigen::ComputeFullV); - double d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); - Eigen::Matrix3d diag = Eigen::Matrix3d::Identity() * d; - ret.block<3,3>(0,0) = svd.matrixU() * diag * svd.matrixV().transpose(); - m = Eigen::Affine3d (ret); +inline void orthogonalize_rotation(Eigen::Affine3d& m) +{ + Eigen::Matrix4d ret = m.matrix(); + Eigen::JacobiSVD svd(ret.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV); + double d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); + Eigen::Matrix3d diag = Eigen::Matrix3d::Identity() * d; + ret.block<3, 3>(0, 0) = svd.matrixU() * diag * svd.matrixV().transpose(); + m = Eigen::Affine3d(ret); #if 0 Eigen::Affine3d mtemp = m; @@ -194,11 +198,12 @@ inline void orthogonalize_rotation(Eigen::Affine3d& m){ #endif } -inline Eigen::Affine3d affine_matrix_from_pose_rodrigues(const RodriguesPose& pr){ -//https://github.com/shimat/opencv_files_343/blob/master/include/x64/opencv2/core/affine.hpp -//https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac -//https://docs.rs/opencv/0.22.1/opencv/calib3d/fn.rodrigues.html -//https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp +inline Eigen::Affine3d affine_matrix_from_pose_rodrigues(const RodriguesPose& pr) +{ +// https://github.com/shimat/opencv_files_343/blob/master/include/x64/opencv2/core/affine.hpp +// https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac +// https://docs.rs/opencv/0.22.1/opencv/calib3d/fn.rodrigues.html +// https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp #if 0 double c = cos(theta); double s = sin(theta); @@ -216,203 +221,197 @@ inline Eigen::Affine3d affine_matrix_from_pose_rodrigues(const RodriguesPose& pr Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; #endif - Eigen::Affine3d m = Eigen::Affine3d::Identity(); - - double norm = sqrt( (pr.sx)*(pr.sx) + (pr.sy)*(pr.sy) + (pr.sz)*(pr.sz) ); - double theta = norm; - double c = cos(theta); - double s = sin(theta); - double c1 = 1. - c; - - double x = pr.sx; - double y = pr.sy; - double z = pr.sz; + Eigen::Affine3d m = Eigen::Affine3d::Identity(); - double itheta = theta ? 1./theta : 0.; + double norm = sqrt((pr.sx) * (pr.sx) + (pr.sy) * (pr.sy) + (pr.sz) * (pr.sz)); + double theta = norm; + double c = cos(theta); + double s = sin(theta); + double c1 = 1. - c; - x *= itheta; - y *= itheta; - z *= itheta; + double x = pr.sx; + double y = pr.sy; + double z = pr.sz; - double rrt[9] = { - x*x, x*y, x*z, x*y, y*y, y*z, x*z, y*z, z*z - }; + double itheta = theta ? 1. / theta : 0.; + x *= itheta; + y *= itheta; + z *= itheta; - double r_x[9] = { - 0, -z, y, - z, 0, -x, - -y, x, 0 - }; + double rrt[9] = { x * x, x * y, x * z, x * y, y * y, y * z, x * z, y * z, z * z }; - double c_eye[9] = {c, 0, 0, 0, c ,0, 0, 0, c }; + double r_x[9] = { 0, -z, y, z, 0, -x, -y, x, 0 }; - double c1_rrt[9] = {c1 * rrt[0], c1 * rrt[1], c1 * rrt[2], c1 * rrt[3], c1 * rrt[4], c1 * rrt[5], c1 * rrt[6], c1 * rrt[7], c1 * rrt[8] }; + double c_eye[9] = { c, 0, 0, 0, c, 0, 0, 0, c }; - double s_r_x[9] = {s*r_x[0], s*r_x[1], s*r_x[2], s*r_x[3], s*r_x[4], s*r_x[5], s*r_x[6], s*r_x[7], s*r_x[8]}; + double c1_rrt[9] = { + c1 * rrt[0], c1 * rrt[1], c1 * rrt[2], c1 * rrt[3], c1 * rrt[4], c1 * rrt[5], c1 * rrt[6], c1 * rrt[7], c1 * rrt[8] + }; + double s_r_x[9] = { s * r_x[0], s * r_x[1], s * r_x[2], s * r_x[3], s * r_x[4], s * r_x[5], s * r_x[6], s * r_x[7], s * r_x[8] }; - // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - //Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; - - double R[9] = { c_eye[0] + c1_rrt[0] + s_r_x[0], - c_eye[1] + c1_rrt[1] + s_r_x[1], - c_eye[2] + c1_rrt[2] + s_r_x[2], - c_eye[3] + c1_rrt[3] + s_r_x[3], - c_eye[4] + c1_rrt[4] + s_r_x[4], - c_eye[5] + c1_rrt[5] + s_r_x[5], - c_eye[6] + c1_rrt[6] + s_r_x[6], - c_eye[7] + c1_rrt[7] + s_r_x[7], - c_eye[8] + c1_rrt[8] + s_r_x[8] }; + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + // Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; + double R[9] = { c_eye[0] + c1_rrt[0] + s_r_x[0], c_eye[1] + c1_rrt[1] + s_r_x[1], c_eye[2] + c1_rrt[2] + s_r_x[2], + c_eye[3] + c1_rrt[3] + s_r_x[3], c_eye[4] + c1_rrt[4] + s_r_x[4], c_eye[5] + c1_rrt[5] + s_r_x[5], + c_eye[6] + c1_rrt[6] + s_r_x[6], c_eye[7] + c1_rrt[7] + s_r_x[7], c_eye[8] + c1_rrt[8] + s_r_x[8] }; - m(0,0) = R[0]; - m(0,1) = R[1]; - m(0,2) = R[2]; + m(0, 0) = R[0]; + m(0, 1) = R[1]; + m(0, 2) = R[2]; - m(1,0) = R[3]; - m(1,1) = R[4]; - m(1,2) = R[5]; + m(1, 0) = R[3]; + m(1, 1) = R[4]; + m(1, 2) = R[5]; - m(2,0) = R[6]; - m(2,1) = R[7]; - m(2,2) = R[8]; + m(2, 0) = R[6]; + m(2, 1) = R[7]; + m(2, 2) = R[8]; + m(0, 3) = pr.px; + m(1, 3) = pr.py; + m(2, 3) = pr.pz; - m(0,3) = pr.px; - m(1,3) = pr.py; - m(2,3) = pr.pz; + // orthogonalize_rotation(m); - //orthogonalize_rotation(m); - - return m; + return m; } -inline void normalize_quaternion(QuaternionPose &pq) +inline void normalize_quaternion(QuaternionPose& pq) { - double norm = sqrt((pq.q0*pq.q0 + pq.q1*pq.q1 + pq.q2*pq.q2 + pq.q3*pq.q3)); + double norm = sqrt((pq.q0 * pq.q0 + pq.q1 * pq.q1 + pq.q2 * pq.q2 + pq.q3 * pq.q3)); - pq.q0 /= norm; - pq.q1 /= norm; - pq.q2 /= norm; - pq.q3 /= norm; + pq.q0 /= norm; + pq.q1 /= norm; + pq.q2 /= norm; + pq.q3 /= norm; } inline Eigen::Affine3d affine_matrix_from_pose_quaternion(const QuaternionPose& pq) { - Eigen::Affine3d m = Eigen::Affine3d::Identity(); - double q0 = pq.q0; - double q1 = pq.q1; - double q2 = pq.q2; - double q3 = pq.q3; - double t0 = pq.px; - double t1 = pq.py; - double t2 = pq.pz; - - double q11 = q1*q1; - double q22 = q2*q2; - double q33 = q3*q3; - double q03 = q0*q3; - double q13 = q1*q3; - double q23 = q2*q3; - double q02 = q0*q2; - double q12 = q1*q2; - double q01 = q0*q1; - - m(0,0) = 1 - 2 * (q22 + q33); - m(1,0) = 2.0*(q12+q03); - m(2,0) = 2.0*(q13-q02); - - m(0,1) = 2.0*(q12-q03); - m(1,1) = 1 - 2 * (q11 + q33); - m(2,1) = 2.0*(q23+q01); - - m(0,2) = 2.0*(q13+q02); - m(1,2) = 2.0*(q23-q01); - m(2,2) = 1 - 2 * (q11 + q22); - - m(0,3) = t0; - m(1,3) = t1; - m(2,3) = t2; - - orthogonalize_rotation(m); - - return m; + Eigen::Affine3d m = Eigen::Affine3d::Identity(); + double q0 = pq.q0; + double q1 = pq.q1; + double q2 = pq.q2; + double q3 = pq.q3; + double t0 = pq.px; + double t1 = pq.py; + double t2 = pq.pz; + + double q11 = q1 * q1; + double q22 = q2 * q2; + double q33 = q3 * q3; + double q03 = q0 * q3; + double q13 = q1 * q3; + double q23 = q2 * q3; + double q02 = q0 * q2; + double q12 = q1 * q2; + double q01 = q0 * q1; + + m(0, 0) = 1 - 2 * (q22 + q33); + m(1, 0) = 2.0 * (q12 + q03); + m(2, 0) = 2.0 * (q13 - q02); + + m(0, 1) = 2.0 * (q12 - q03); + m(1, 1) = 1 - 2 * (q11 + q33); + m(2, 1) = 2.0 * (q23 + q01); + + m(0, 2) = 2.0 * (q13 + q02); + m(1, 2) = 2.0 * (q23 - q01); + m(2, 2) = 1 - 2 * (q11 + q22); + + m(0, 3) = t0; + m(1, 3) = t1; + m(2, 3) = t2; + + orthogonalize_rotation(m); + + return m; } -inline QuaternionPose pose_quaternion_from_affine_matrix(const Eigen::Affine3d& m){ - QuaternionPose pq; - - double T, S, X, Y, Z, W; - - T = 1 + m(0,0) + m(1,1) + m(2,2); - if ( T > 0.00000001 ) { // to avoid large distortions! - S = sqrt(T) * 2; - X = ( m(1,2) - m(2,1) ) / S; - Y = ( m(2,0) - m(0,2) ) / S; - Z = ( m(0,1) - m(1,0) ) / S; - W = 0.25 * S; - } else if ( m(0,0) > m(1,1) && m(0,0) > m(2,2) ) { // Column 0: - S = sqrt( 1.0 + m(0,0) - m(1,1) - m(2,2) ) * 2; - X = 0.25 * S; - Y = (m(0,1) + m(1,0) ) / S; - Z = (m(2,0) + m(0,2) ) / S; - W = (m(1,2) - m(2,1) ) / S; - } else if ( m(1,1) > m(2,2) ) { // Column 1: - S = sqrt( 1.0 + m(1,1) - m(0,0) - m(2,2) ) * 2; - X = (m(0,1) + m(1,0) ) / S; - Y = 0.25 * S; - Z = (m(1,2) + m(2,1) ) / S; - W = (m(2,0) - m(0,2) ) / S; - } else { // Column 2: - S = sqrt( 1.0 + m(2,2) - m(0,0) - m(1,1) ) * 2; - X = (m(2,0) + m(0,2) ) / S; - Y = (m(1,2) + m(2,1) ) / S; - Z = 0.25 * S; - W = (m(0,1) - m(1,0) ) / S; - } - pq.q0 = W; - pq.q1 = -X; - pq.q2 = -Y; - pq.q3 = -Z; - - normalize_quaternion(pq); - - pq.px = m(0,3); - pq.py = m(1,3); - pq.pz = m(2,3); - - return pq; +inline QuaternionPose pose_quaternion_from_affine_matrix(const Eigen::Affine3d& m) +{ + QuaternionPose pq; + + double T, S, X, Y, Z, W; + + T = 1 + m(0, 0) + m(1, 1) + m(2, 2); + if (T > 0.00000001) + { // to avoid large distortions! + S = sqrt(T) * 2; + X = (m(1, 2) - m(2, 1)) / S; + Y = (m(2, 0) - m(0, 2)) / S; + Z = (m(0, 1) - m(1, 0)) / S; + W = 0.25 * S; + } + else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) + { // Column 0: + S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; + X = 0.25 * S; + Y = (m(0, 1) + m(1, 0)) / S; + Z = (m(2, 0) + m(0, 2)) / S; + W = (m(1, 2) - m(2, 1)) / S; + } + else if (m(1, 1) > m(2, 2)) + { // Column 1: + S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; + X = (m(0, 1) + m(1, 0)) / S; + Y = 0.25 * S; + Z = (m(1, 2) + m(2, 1)) / S; + W = (m(2, 0) - m(0, 2)) / S; + } + else + { // Column 2: + S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; + X = (m(2, 0) + m(0, 2)) / S; + Y = (m(1, 2) + m(2, 1)) / S; + Z = 0.25 * S; + W = (m(0, 1) - m(1, 0)) / S; + } + pq.q0 = W; + pq.q1 = -X; + pq.q2 = -Y; + pq.q3 = -Z; + + normalize_quaternion(pq); + + pq.px = m(0, 3); + pq.py = m(1, 3); + pq.pz = m(2, 3); + + return pq; } inline double normalize_angle(double src_rad) { - double arg; - - arg = fmod(src_rad, 2.0 * M_PI); - if (arg < 0) - arg = arg + 2.0 * M_PI; - if (arg > M_PI) - arg = arg - 2.0 * M_PI; - return arg; + double arg; + + arg = fmod(src_rad, 2.0 * M_PI); + if (arg < 0) + arg = arg + 2.0 * M_PI; + if (arg > M_PI) + arg = arg - 2.0 * M_PI; + return arg; } -inline Eigen::Affine3d pose_interpolation(double t, double t1, double t2, Eigen::Affine3d const& aff1, Eigen::Affine3d const& aff2) { - // assume here t1 <= t <= t2 - double alpha = 0.0; - if (t2 != t1) - alpha = (t - t1) / (t2 - t1); +inline Eigen::Affine3d pose_interpolation(double t, double t1, double t2, Eigen::Affine3d const& aff1, Eigen::Affine3d const& aff2) +{ + // assume here t1 <= t <= t2 + double alpha = 0.0; + if (t2 != t1) + alpha = (t - t1) / (t2 - t1); - Eigen::Quaternion rot1(aff1.linear()); - Eigen::Quaternion rot2(aff2.linear()); + Eigen::Quaternion rot1(aff1.linear()); + Eigen::Quaternion rot2(aff2.linear()); - Eigen::Vector3d trans1 = aff1.translation(); - Eigen::Vector3d trans2 = aff2.translation(); + Eigen::Vector3d trans1 = aff1.translation(); + Eigen::Vector3d trans2 = aff2.translation(); - Eigen::Affine3d result; - result.translation() = (1.0 - alpha) * trans1 + alpha * trans2; - result.linear() = rot1.slerp(alpha, rot2).toRotationMatrix(); + Eigen::Affine3d result; + result.translation() = (1.0 - alpha) * trans1 + alpha * trans2; + result.linear() = rot1.slerp(alpha, rot2).toRotationMatrix(); - return result; + return result; } -#endif diff --git a/core/include/utils.hpp b/core/include/utils.hpp index e41bd9ab..fb316ae5 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -2,9 +2,9 @@ #include #include -#include #include #include +#include /////////////////////////////////////////////////////////////////////////////////// @@ -28,7 +28,8 @@ const unsigned int window_height = 600; const float camera_transition_speed = 1.0f; // higher = faster -enum CameraPreset { +enum CameraPreset +{ CAMERA_FRONT, CAMERA_BACK, CAMERA_LEFT, @@ -80,13 +81,14 @@ extern float new_translate_z; // Transition timing extern bool camera_transition_active; -struct ShortcutEntry { +struct ShortcutEntry +{ std::string type; std::string shortcut; std::string description; }; -#define IDI_ICON1 101 //application icon (double defined in resource.h) +#define IDI_ICON1 101 // application icon (double defined in resource.h) /////////////////////////////////////////////////////////////////////////////////// @@ -114,9 +116,15 @@ Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationP LaserBeam GetLaserBeam(int x, int y); double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); void getClosestTrajectoryPoint(Session& session, int x, int y, bool gcpPicking, int& picked_index); -void getClosestTrajectoriesPoint(std::vector& sessions, int x, int y, - const int first_session_index, const int second_session_index, const int number_visible_sessions, - int& index_loop_closure_source, int& index_loop_closure_target, bool KeyShift); - +void getClosestTrajectoriesPoint( + std::vector& sessions, + int x, + int y, + const int first_session_index, + const int second_session_index, + const int number_visible_sessions, + int& index_loop_closure_source, + int& index_loop_closure_target, + bool KeyShift); void setNewRotationCenter(int x, int y); \ No newline at end of file diff --git a/core/src/color_las_loader.cpp b/core/src/color_las_loader.cpp index 73640e47..e9c0424e 100644 --- a/core/src/color_las_loader.cpp +++ b/core/src/color_las_loader.cpp @@ -1,249 +1,246 @@ +#include + #include "color_las_loader.h" -#include -#include -std::vector mandeye::load(const std::string &lazFile) +std::vector mandeye::load(const std::string& lazFile) { - std::vector points; - laszip_POINTER laszip_reader; - if (laszip_create(&laszip_reader)) - { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); - std::abort(); - } - - laszip_BOOL is_compressed = 0; - if (laszip_open_reader(laszip_reader, lazFile.c_str(), &is_compressed)) - { - fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", lazFile.c_str()); - std::abort(); - } - std::cout << "compressed : " << is_compressed << std::endl; - laszip_header *header; - - if (laszip_get_header_pointer(laszip_reader, &header)) - { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); - std::abort(); - } - // fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); - - // laszip_point *point; - // if (laszip_get_point_pointer(laszip_reader, &point)) - //{ - // fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); - // std::abort(); - // } - - /*std::cout << "header->number_of_point_records: " << header->number_of_point_records << std::endl; - std::cout << "header->extended_number_of_point_records: " << header->extended_number_of_point_records << std::endl; - - //for (int j = 0; j < header->number_of_point_records; j++) - for (int j = 0; j < header->extended_number_of_point_records; j++) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: reading point %u\n", j); - std::abort(); - } - - Point p; - p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.point.z() = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - p.intensity = point->intensity; - - points.emplace_back(p); - }*/ - - laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - - // report how many points the file has - - fprintf(stderr, "file '%s' contains %I64d points\n", lazFile.c_str(), npoints); - - laszip_point *point; - - if (laszip_get_point_pointer(laszip_reader, &point)) - { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); - } - - laszip_I64 p_count = 0; - - while (p_count < npoints) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); - - } - Point p; - p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.point.z() = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - p.intensity = point->intensity; - - points.emplace_back(p); - - p_count++; - } - - return points; + std::vector points; + laszip_POINTER laszip_reader; + if (laszip_create(&laszip_reader)) + { + fprintf(stderr, "DLL ERROR: creating laszip reader\n"); + std::abort(); + } + + laszip_BOOL is_compressed = 0; + if (laszip_open_reader(laszip_reader, lazFile.c_str(), &is_compressed)) + { + fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", lazFile.c_str()); + std::abort(); + } + std::cout << "compressed : " << is_compressed << std::endl; + laszip_header* header; + + if (laszip_get_header_pointer(laszip_reader, &header)) + { + fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + std::abort(); + } + // fprintf(stderr, "file '%s' contains %u points\n", lazFile.c_str(), header->number_of_point_records); + + // laszip_point *point; + // if (laszip_get_point_pointer(laszip_reader, &point)) + //{ + // fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); + // std::abort(); + // } + + /*std::cout << "header->number_of_point_records: " << header->number_of_point_records << std::endl; + std::cout << "header->extended_number_of_point_records: " << header->extended_number_of_point_records << std::endl; + + //for (int j = 0; j < header->number_of_point_records; j++) + for (int j = 0; j < header->extended_number_of_point_records; j++) + { + if (laszip_read_point(laszip_reader)) + { + fprintf(stderr, "DLL ERROR: reading point %u\n", j); + std::abort(); + } + + Point p; + p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); + p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); + p.point.z() = header->z_offset + header->z_scale_factor * static_cast(point->Z); + p.timestamp = point->gps_time; + p.intensity = point->intensity; + + points.emplace_back(p); + }*/ + + laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); + + // report how many points the file has + + fprintf(stderr, "file '%s' contains %I64d points\n", lazFile.c_str(), npoints); + + laszip_point* point; + + if (laszip_get_point_pointer(laszip_reader, &point)) + { + fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); + } + + laszip_I64 p_count = 0; + + while (p_count < npoints) + { + if (laszip_read_point(laszip_reader)) + { + fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + } + Point p; + p.point.x() = header->x_offset + header->x_scale_factor * static_cast(point->X); + p.point.y() = header->y_offset + header->y_scale_factor * static_cast(point->Y); + p.point.z() = header->z_offset + header->z_scale_factor * static_cast(point->Z); + p.timestamp = point->gps_time; + p.intensity = point->intensity; + + points.emplace_back(p); + + p_count++; + } + + return points; } -bool mandeye::saveLaz(const std::string &filename, const std::vector &buffer) +bool mandeye::saveLaz(const std::string& filename, const std::vector& buffer) { - - constexpr float scale = 0.0001f; // one tenth of milimeter - // find max - double max_x{std::numeric_limits::lowest()}; - double max_y{std::numeric_limits::lowest()}; - double max_z{std::numeric_limits::lowest()}; - - double min_x{std::numeric_limits::max()}; - double min_y{std::numeric_limits::max()}; - double min_z{std::numeric_limits::max()}; - - for (auto p : buffer) - { - double x = p.point.x(); - double y = p.point.y(); - double z = p.point.z(); - - max_x = std::max(max_x, x); - max_y = std::max(max_y, y); - max_z = std::max(max_z, z); - - min_x = std::min(min_x, x); - min_y = std::min(min_y, y); - min_z = std::min(min_z, z); - } - - std::cout << "processing: " << filename << "points " << buffer.size() << std::endl; - - laszip_POINTER laszip_writer; - if (laszip_create(&laszip_writer)) - { - fprintf(stderr, "DLL ERROR: creating laszip writer\n"); - return false; - } - - // get a pointer to the header of the writer so we can populate it - - laszip_header *header; - - if (laszip_get_header_pointer(laszip_writer, &header)) - { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n"); - return false; - } - - // populate the header - - header->file_source_ID = 4711; - header->global_encoding = (1 << 0); // see LAS specification for details - header->version_major = 1; - header->version_minor = 2; - // header->file_creation_day = 120; - // header->file_creation_year = 2013; - header->point_data_format = 2; - header->point_data_record_length = 0; - header->number_of_point_records = buffer.size(); - header->number_of_points_by_return[0] = buffer.size(); - header->number_of_points_by_return[1] = 0; - header->point_data_record_length = 26; - header->x_scale_factor = scale; - header->y_scale_factor = scale; - header->z_scale_factor = scale; - - header->max_x = max_x; - header->min_x = min_x; - header->max_y = max_y; - header->min_y = min_y; - header->max_z = max_z; - header->min_z = min_z; - - // optional: use the bounding box and the scale factor to create a "good" offset - // open the writer - laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); - - if (laszip_open_writer(laszip_writer, filename.c_str(), compress)) - { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str()); - return false; - } - - fprintf(stderr, "writing file '%s' %scompressed\n", filename.c_str(), (compress ? "" : "un")); - - // get a pointer to the point of the writer that we will populate and write - - laszip_point *point; - if (laszip_get_point_pointer(laszip_writer, &point)) - { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); - return false; - } - - laszip_I64 p_count = 0; - laszip_F64 coordinates[3]; - - for (int i = 0; i < buffer.size(); i++) - { - - const auto &p = buffer.at(i); - point->intensity = p.intensity; - point->rgb[0] = 255 * p.rgb[0]; - point->rgb[1] = 255 * p.rgb[1]; - point->rgb[2] = 255 * p.rgb[2]; - point->rgb[3] = 255 * p.rgb[3]; - - point->gps_time = p.timestamp * 1e-9; - // point->user_data = p.line_id; - // point->classification = p.tag; - p_count++; - coordinates[0] = p.point[0]; - coordinates[1] = p.point[1]; - coordinates[2] = p.point[2]; - if (laszip_set_coordinates(laszip_writer, coordinates)) - { - fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count); - return false; - } - - if (laszip_write_point(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count); - return false; - } - } - - if (laszip_get_point_count(laszip_writer, &p_count)) - { - fprintf(stderr, "DLL ERROR: getting point count\n"); - return false; - } - - fprintf(stderr, "successfully written %I64d points\n", p_count); - - // close the writer - - if (laszip_close_writer(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); - return false; - } - - // destroy the writer - - if (laszip_destroy(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); - return false; - } - - std::cout << "exportLaz DONE" << std::endl; - return true; -} \ No newline at end of file + constexpr float scale = 0.0001f; // one tenth of milimeter + // find max + double max_x{ std::numeric_limits::lowest() }; + double max_y{ std::numeric_limits::lowest() }; + double max_z{ std::numeric_limits::lowest() }; + + double min_x{ std::numeric_limits::max() }; + double min_y{ std::numeric_limits::max() }; + double min_z{ std::numeric_limits::max() }; + + for (auto p : buffer) + { + double x = p.point.x(); + double y = p.point.y(); + double z = p.point.z(); + + max_x = std::max(max_x, x); + max_y = std::max(max_y, y); + max_z = std::max(max_z, z); + + min_x = std::min(min_x, x); + min_y = std::min(min_y, y); + min_z = std::min(min_z, z); + } + + std::cout << "processing: " << filename << "points " << buffer.size() << std::endl; + + laszip_POINTER laszip_writer; + if (laszip_create(&laszip_writer)) + { + fprintf(stderr, "DLL ERROR: creating laszip writer\n"); + return false; + } + + // get a pointer to the header of the writer so we can populate it + + laszip_header* header; + + if (laszip_get_header_pointer(laszip_writer, &header)) + { + fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n"); + return false; + } + + // populate the header + + header->file_source_ID = 4711; + header->global_encoding = (1 << 0); // see LAS specification for details + header->version_major = 1; + header->version_minor = 2; + // header->file_creation_day = 120; + // header->file_creation_year = 2013; + header->point_data_format = 2; + header->point_data_record_length = 0; + header->number_of_point_records = buffer.size(); + header->number_of_points_by_return[0] = buffer.size(); + header->number_of_points_by_return[1] = 0; + header->point_data_record_length = 26; + header->x_scale_factor = scale; + header->y_scale_factor = scale; + header->z_scale_factor = scale; + + header->max_x = max_x; + header->min_x = min_x; + header->max_y = max_y; + header->min_y = min_y; + header->max_z = max_z; + header->min_z = min_z; + + // optional: use the bounding box and the scale factor to create a "good" offset + // open the writer + laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); + + if (laszip_open_writer(laszip_writer, filename.c_str(), compress)) + { + fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str()); + return false; + } + + fprintf(stderr, "writing file '%s' %scompressed\n", filename.c_str(), (compress ? "" : "un")); + + // get a pointer to the point of the writer that we will populate and write + + laszip_point* point; + if (laszip_get_point_pointer(laszip_writer, &point)) + { + fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); + return false; + } + + laszip_I64 p_count = 0; + laszip_F64 coordinates[3]; + + for (int i = 0; i < buffer.size(); i++) + { + const auto& p = buffer.at(i); + point->intensity = p.intensity; + point->rgb[0] = 255 * p.rgb[0]; + point->rgb[1] = 255 * p.rgb[1]; + point->rgb[2] = 255 * p.rgb[2]; + point->rgb[3] = 255 * p.rgb[3]; + + point->gps_time = p.timestamp * 1e-9; + // point->user_data = p.line_id; + // point->classification = p.tag; + p_count++; + coordinates[0] = p.point[0]; + coordinates[1] = p.point[1]; + coordinates[2] = p.point[2]; + if (laszip_set_coordinates(laszip_writer, coordinates)) + { + fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count); + return false; + } + + if (laszip_write_point(laszip_writer)) + { + fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count); + return false; + } + } + + if (laszip_get_point_count(laszip_writer, &p_count)) + { + fprintf(stderr, "DLL ERROR: getting point count\n"); + return false; + } + + fprintf(stderr, "successfully written %I64d points\n", p_count); + + // close the writer + + if (laszip_close_writer(laszip_writer)) + { + fprintf(stderr, "DLL ERROR: closing laszip writer\n"); + return false; + } + + // destroy the writer + + if (laszip_destroy(laszip_writer)) + { + fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); + return false; + } + + std::cout << "exportLaz DONE" << std::endl; + return true; +} diff --git a/core/src/control_points.cpp b/core/src/control_points.cpp index 0bf39892..d46532dc 100644 --- a/core/src/control_points.cpp +++ b/core/src/control_points.cpp @@ -1,5 +1,9 @@ +#include + #include + #include + #if WITH_GUI == 1 #include #include @@ -7,11 +11,11 @@ #include static constexpr float ImGuiNumberWidth = 120.0f; -static constexpr const char *xText = "Longitudinal (forward/backward)"; -static constexpr const char *yText = "Lateral (left/right)"; -static constexpr const char *zText = "Vertical (up/down)"; +static constexpr const char* xText = "Longitudinal (forward/backward)"; +static constexpr const char* yText = "Lateral (left/right)"; +static constexpr const char* zText = "Vertical (up/down)"; -void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f &rotation_center) +void ControlPoints::imgui(PointClouds& point_clouds_container, Eigen::Vector3f& rotation_center) { if (ImGui::Begin("Control Point", &is_imgui)) { @@ -70,7 +74,7 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & int remove_gcp_index = -1; for (int i = 0; i < cps.size(); i++) { - const auto gui_text = (std::string("CP_") + std::to_string(i) + " [" + cps[i].name + "]"); + const auto gui_text = (std::string("CP_") + std::to_string(i) + " [" + cps[i].name + "]"); ImGui::Text("%s", gui_text.c_str()); ImGui::SameLine(); if (ImGui::Button(("Remove##" + std::to_string(i)).c_str())) @@ -159,16 +163,29 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; Eigen::Vector3d p_t(cps[i].x_target_global, cps[i].y_target_global, cps[i].z_target_global); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = 0; @@ -200,16 +217,29 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; Eigen::Vector3d p_t(cps[i].x_target_global, cps[i].y_target_global, 0.0); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = 0; @@ -293,7 +323,8 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { point_clouds_container.point_clouds[i].m_pose = m_pose * point_clouds_container.point_clouds[i].m_pose; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -315,7 +346,7 @@ void ControlPoints::imgui(PointClouds &point_clouds_container, Eigen::Vector3f & return; } -void ControlPoints::render(const PointClouds &point_clouds_container, bool show_pc) +void ControlPoints::render(const PointClouds& point_clouds_container, bool show_pc) { if (show_pc) { @@ -326,8 +357,12 @@ void ControlPoints::render(const PointClouds &point_clouds_container, bool show_ glBegin(GL_POINTS); for (int i = 0; i < point_clouds_container.point_clouds[index_pose].points_local.size(); i++) { - glColor3f(point_clouds_container.point_clouds[index_pose].intensities[i], 0.0, 1 - point_clouds_container.point_clouds[index_pose].intensities[i]); - auto p = point_clouds_container.point_clouds[index_pose].m_pose * point_clouds_container.point_clouds[index_pose].points_local[i]; + glColor3f( + point_clouds_container.point_clouds[index_pose].intensities[i], + 0.0, + 1 - point_clouds_container.point_clouds[index_pose].intensities[i]); + auto p = point_clouds_container.point_clouds[index_pose].m_pose * + point_clouds_container.point_clouds[index_pose].points_local[i]; glVertex3f(p.x(), p.y(), p.z()); } glEnd(); @@ -335,8 +370,10 @@ void ControlPoints::render(const PointClouds &point_clouds_container, bool show_ glColor3f(0.0, 1.0, 0.0); glLineWidth(2.0); glBegin(GL_LINE_STRIP); - for (int i = 0; i < point_clouds_container.point_clouds[index_pose].local_trajectory.size(); i++){ - auto pose = point_clouds_container.point_clouds[index_pose].m_pose * point_clouds_container.point_clouds[index_pose].local_trajectory[i].m_pose; + for (int i = 0; i < point_clouds_container.point_clouds[index_pose].local_trajectory.size(); i++) + { + auto pose = point_clouds_container.point_clouds[index_pose].m_pose * + point_clouds_container.point_clouds[index_pose].local_trajectory[i].m_pose; glVertex3f(pose(0, 3), pose(1, 3), pose(2, 3)); } @@ -347,7 +384,6 @@ void ControlPoints::render(const PointClouds &point_clouds_container, bool show_ for (int i = 0; i < cps.size(); i++) { - Eigen::Vector3d p(cps[i].x_source_local, cps[i].y_source_local, cps[i].z_source_local); Eigen::Vector3d c = point_clouds_container.point_clouds[cps[i].index_to_pose].m_pose * p; @@ -414,22 +450,21 @@ void ControlPoints::render(const PointClouds &point_clouds_container, bool show_ glColor3f(0, 0, 0); glRasterPos3f(cps[i].x_target_global, cps[i].y_target_global, cps[i].z_target_global + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)cps[i].name); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)cps[i].name); glRasterPos3f(cps[i].x_target_global, cps[i].y_target_global, cps[i].z_target_global); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("CP_" + std::to_string(i)).c_str())); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char*)(("CP_" + std::to_string(i)).c_str())); glColor3f(0.7f, 0.3f, 0.5f); glRasterPos3f(c.x(), c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("CP_" + std::to_string(i) + ": initial location").c_str())); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char*)(("CP_" + std::to_string(i) + ": initial location").c_str())); } return; } -void ControlPoints::draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd) +void ControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) { - Eigen::LLT> cholSolver(covar); Eigen::Matrix3d transform = cholSolver.matrixL(); @@ -444,7 +479,7 @@ void ControlPoints::draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d & { for (double j = 0; j < 1.0; j += dj) // vertical { - double u = i * 2 * pi; // 0 to 2pi + double u = i * 2 * pi; // 0 to 2pi double v = (j - 0.5) * pi; //-pi/2 to pi/2 const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); diff --git a/core/src/gnss.cpp b/core/src/gnss.cpp index cb33f1fe..84755318 100644 --- a/core/src/gnss.cpp +++ b/core/src/gnss.cpp @@ -1,18 +1,17 @@ -#include -#include -#include -#include -#include -#include -#include +#include + #include -#include +#include #include +#include + #if WITH_GUI == 1 #include #endif -inline void split(std::string &str, char delim, std::vector &out) +#include + +inline void split(std::string& str, char delim, std::vector& out) { size_t start; size_t end = 0; @@ -24,7 +23,7 @@ inline void split(std::string &str, char delim, std::vector &out) } } -bool GNSS::load(const std::vector &input_file_names, Eigen::Vector3d &out_offset, bool localize) +bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize) { // 54651848940 5156.43798828125 2009.1610107421875 122.1999969482421875 1.25 8 35.799999237060546875 nan 14:51:42 1 // timestamp lat lon alt hdop satelites_tracked height age time fix_quality @@ -36,12 +35,12 @@ bool GNSS::load(const std::vector &input_file_names, Eigen::Vector3 gnss_poses.clear(); std::cout << "loading GNSS data from following files:" << std::endl; - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::cout << fn << std::endl; } - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::ifstream infile(fn); if (!infile.good()) @@ -86,11 +85,21 @@ bool GNSS::load(const std::vector &input_file_names, Eigen::Vector3 infile.close(); } - std::sort(gnss_poses.begin(), gnss_poses.end(), [](GNSS::GlobalPose &a, GNSS::GlobalPose &b) - { return (a.timestamp < b.timestamp); }); + std::sort( + gnss_poses.begin(), + gnss_poses.end(), + [](GNSS::GlobalPose& a, GNSS::GlobalPose& b) + { + return (a.timestamp < b.timestamp); + }); - auto firstGNSSIt = std::find_if(gnss_poses.begin(), gnss_poses.end(), [](const GNSS::GlobalPose &gp) - { return gp.x != 0 && gp.y != 0 && gp.alt != 0; }); + auto firstGNSSIt = std::find_if( + gnss_poses.begin(), + gnss_poses.end(), + [](const GNSS::GlobalPose& gp) + { + return gp.x != 0 && gp.y != 0 && gp.alt != 0; + }); if (firstGNSSIt == gnss_poses.end()) { std::cout << "no valid GNSS data" << std::endl; @@ -113,33 +122,33 @@ bool GNSS::load(const std::vector &input_file_names, Eigen::Vector3 if (localize) { - for (auto &pose : gnss_poses) + for (auto& pose : gnss_poses) { pose.x = pose.x - firstGNSS.x; pose.y = pose.y - firstGNSS.y; pose.alt = pose.alt - firstGNSS.alt; - //std::cout << "pose.x: " << pose.x << " pose.y: " << pose.y << " pose.alt: " << pose.alt << std::endl; + // std::cout << "pose.x: " << pose.x << " pose.y: " << pose.y << " pose.alt: " << pose.alt << std::endl; } } std::cout << std::setprecision(20); std::cout << "firstGNSS: " << firstGNSS.lat << " " << firstGNSS.lon << " " << firstGNSS.alt << std::endl; std::cout << "firstGNSS: " << firstGNSS.x << " " << firstGNSS.y << " " << firstGNSS.alt << std::endl; - + return true; } -bool GNSS::load_mercator_projection(const std::vector &input_file_names) +bool GNSS::load_mercator_projection(const std::vector& input_file_names) { gnss_poses.clear(); std::cout << "loading GNSS data from following files:" << std::endl; - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::cout << fn << std::endl; } - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::ifstream infile(fn); if (!infile.good()) @@ -179,8 +188,8 @@ bool GNSS::load_mercator_projection(const std::vector &input_file_n if (gp.lon != 0) { gnss_poses.push_back(gp); - // std::cout << std::setprecision(20); - // std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << " gp.alt " << gp.alt << std::endl; + // std::cout << std::setprecision(20); + // std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << " gp.alt " << gp.alt << std::endl; } } } @@ -191,10 +200,15 @@ bool GNSS::load_mercator_projection(const std::vector &input_file_n infile.close(); } - std::sort(gnss_poses.begin(), gnss_poses.end(), [](GNSS::GlobalPose &a, GNSS::GlobalPose &b) - { return (a.timestamp < b.timestamp); }); + std::sort( + gnss_poses.begin(), + gnss_poses.end(), + [](GNSS::GlobalPose& a, GNSS::GlobalPose& b) + { + return (a.timestamp < b.timestamp); + }); - std::array WGS84Reference{0, 0}; + std::array WGS84Reference{ 0, 0 }; if (gnss_poses.size() > 0) { @@ -214,8 +228,8 @@ bool GNSS::load_mercator_projection(const std::vector &input_file_n for (int i = 0; i < gnss_poses.size(); i++) { - std::array WGS84Position{gnss_poses[i].lat, gnss_poses[i].lon}; - std::array result{wgs84::toCartesian(WGS84Reference, WGS84Position)}; + std::array WGS84Position{ gnss_poses[i].lat, gnss_poses[i].lon }; + std::array result{ wgs84::toCartesian(WGS84Reference, WGS84Position) }; gnss_poses[i].x = result[0]; gnss_poses[i].y = result[1]; } @@ -223,7 +237,7 @@ bool GNSS::load_mercator_projection(const std::vector &input_file_n return true; } -double dm_to_dd(const std::string &dm, char direction, bool is_latitude) +double dm_to_dd(const std::string& dm, char direction, bool is_latitude) { if (dm.empty()) return 0.0; @@ -240,19 +254,18 @@ double dm_to_dd(const std::string &dm, char direction, bool is_latitude) return dd; } - -bool GNSS::load_nmea_mercator_projection(const std::vector &input_file_names) +bool GNSS::load_nmea_mercator_projection(const std::vector& input_file_names) { gnss_poses.clear(); std::cout << "loading NMEA data from following files:" << std::endl; - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::cout << fn << std::endl; } std::optional lastGGAData; std::optional lastRMCData; - for (const auto &fn : input_file_names) + for (const auto& fn : input_file_names) { std::ifstream infile(fn); if (!infile.good()) @@ -267,48 +280,57 @@ bool GNSS::load_nmea_mercator_projection(const std::vector &input_f using namespace hd_mapping::nmea; auto [timestampLidar, timestampUnix, nmeaSentence] = BreakLineFromNMEAFile(line); - if (!validateNMEAChecksum(nmeaSentence)) { - std::cout << "Invalid NMEA checksum in line: " << line << std::endl; - continue; + if (!validateNMEAChecksum(nmeaSentence)) + { + std::cout << "Invalid NMEA checksum in line: " << line << std::endl; + continue; } const auto gga = parseGNGGA(nmeaSentence); const auto rmc = parseGNRMC(nmeaSentence); - if (rmc.has_value()) { + if (rmc.has_value()) + { lastRMCData = rmc; } - if (gga.has_value()) { - lastGGAData = gga; + if (gga.has_value()) + { + lastGGAData = gga; } - if (gga.has_value()) { - GlobalPose gp; - gp.timestamp = timestampLidar; - gp.lat = gga->latitude; - gp.lon = gga->longitude; - gp.alt = gga->altitude; - gp.hdop = gga->hdop; - gp.height = gga->altitude; - gp.age = gga->age_of_data; - gp.time = 0; // todo convert to seconds from string - gp.fix_quality = gga->fix_quality; - - // register if there is not nans - if (gp.lat == gp.lat && gp.lon == gp.lon && - gp.alt == gp.alt) { - //std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << std::endl; - gnss_poses.push_back(gp); - } + if (gga.has_value()) + { + GlobalPose gp; + gp.timestamp = timestampLidar; + gp.lat = gga->latitude; + gp.lon = gga->longitude; + gp.alt = gga->altitude; + gp.hdop = gga->hdop; + gp.height = gga->altitude; + gp.age = gga->age_of_data; + gp.time = 0; // todo convert to seconds from string + gp.fix_quality = gga->fix_quality; + + // register if there is not nans + if (gp.lat == gp.lat && gp.lon == gp.lon && gp.alt == gp.alt) + { + // std::cout << "gp.lat " << gp.lat << " gp.lon " << gp.lon << std::endl; + gnss_poses.push_back(gp); + } } } infile.close(); } std::cout << "loaded " << gnss_poses.size() << " gps poses" << std::endl; - - std::sort(gnss_poses.begin(), gnss_poses.end(), [](GNSS::GlobalPose &a, GNSS::GlobalPose &b) - { return (a.timestamp < b.timestamp); }); - std::array WGS84Reference{0, 0}; + std::sort( + gnss_poses.begin(), + gnss_poses.end(), + [](GNSS::GlobalPose& a, GNSS::GlobalPose& b) + { + return (a.timestamp < b.timestamp); + }); + + std::array WGS84Reference{ 0, 0 }; if (gnss_poses.size() > 0) { @@ -328,25 +350,28 @@ bool GNSS::load_nmea_mercator_projection(const std::vector &input_f for (int i = 0; i < gnss_poses.size(); i++) { - std::array WGS84Position{gnss_poses[i].lat, gnss_poses[i].lon}; - std::array result{wgs84::toCartesian(WGS84Reference, WGS84Position)}; + std::array WGS84Position{ gnss_poses[i].lat, gnss_poses[i].lon }; + std::array result{ wgs84::toCartesian(WGS84Reference, WGS84Position) }; gnss_poses[i].x = result[0]; gnss_poses[i].y = result[1]; - //std::cout << "gnss_poses[i].x " << gnss_poses[i].x << " gnss_poses[i].y " << gnss_poses[i].y << std::endl; + // std::cout << "gnss_poses[i].x " << gnss_poses[i].x << " gnss_poses[i].y " << gnss_poses[i].y << std::endl; } return true; } #if WITH_GUI == 1 -void GNSS::render(const PointClouds &point_clouds_container) +void GNSS::render(const PointClouds& point_clouds_container) { glColor3f(1, 1, 1); glBegin(GL_LINE_STRIP); for (int i = 0; i < gnss_poses.size(); i++) { - glVertex3f(gnss_poses[i].x - point_clouds_container.offset.x(), gnss_poses[i].y - point_clouds_container.offset.y(), gnss_poses[i].alt - point_clouds_container.offset.z()); + glVertex3f( + gnss_poses[i].x - point_clouds_container.offset.x(), + gnss_poses[i].y - point_clouds_container.offset.y(), + gnss_poses[i].alt - point_clouds_container.offset.z()); } glEnd(); @@ -354,27 +379,34 @@ void GNSS::render(const PointClouds &point_clouds_container) { glColor3f(1, 0, 0); glBegin(GL_LINES); - for (const auto &pc : point_clouds_container.point_clouds) + for (const auto& pc : point_clouds_container.point_clouds) { for (int i = 0; i < gnss_poses.size(); i++) { double time_stamp = gnss_poses[i].timestamp; - auto it = std::lower_bound(pc.local_trajectory.begin(), pc.local_trajectory.end(), - time_stamp, [](const PointCloud::LocalTrajectoryNode &lhs, const double &time) -> bool - { return lhs.timestamps.first < time; }); + auto it = std::lower_bound( + pc.local_trajectory.begin(), + pc.local_trajectory.end(), + time_stamp, + [](const PointCloud::LocalTrajectoryNode& lhs, const double& time) -> bool + { + return lhs.timestamps.first < time; + }); int index = it - pc.local_trajectory.begin(); if (index > 0 && index < pc.local_trajectory.size()) { - if (fabs(time_stamp - pc.local_trajectory[index].timestamps.first) < 10e12) { auto m = pc.m_pose * pc.local_trajectory[index].m_pose; glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(gnss_poses[i].x - point_clouds_container.offset.x(), gnss_poses[i].y - point_clouds_container.offset.y(), gnss_poses[i].alt - point_clouds_container.offset.z()); + glVertex3f( + gnss_poses[i].x - point_clouds_container.offset.x(), + gnss_poses[i].y - point_clouds_container.offset.y(), + gnss_poses[i].alt - point_clouds_container.offset.z()); } } } @@ -384,11 +416,12 @@ void GNSS::render(const PointClouds &point_clouds_container) } #endif -bool GNSS::save_to_laz(const std::string &output_file_names, double offset_x, double offset_y, double offset_alt) +bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, double offset_y, double offset_alt) { constexpr float scale = 0.0001f; // one tenth of milimeter // find max - Eigen::Vector3d max(std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); + Eigen::Vector3d max( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); Eigen::Vector3d min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); for (int i = 0; i < gnss_poses.size(); i++) @@ -412,7 +445,7 @@ bool GNSS::save_to_laz(const std::string &output_file_names, double offset_x, do // get a pointer to the header of the writer so we can populate it - laszip_header *header; + laszip_header* header; if (laszip_get_header_pointer(laszip_writer, &header)) { @@ -463,7 +496,7 @@ bool GNSS::save_to_laz(const std::string &output_file_names, double offset_x, do // get a pointer to the point of the writer that we will populate and write - laszip_point *point; + laszip_point* point; if (laszip_get_point_pointer(laszip_writer, &point)) { fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); @@ -477,7 +510,7 @@ bool GNSS::save_to_laz(const std::string &output_file_names, double offset_x, do { point->intensity = 0; - const auto &p = gnss_poses[i]; + const auto& p = gnss_poses[i]; p_count++; coordinates[0] = p.x; coordinates[1] = p.y; @@ -521,4 +554,4 @@ bool GNSS::save_to_laz(const std::string &output_file_names, double offset_x, do std::cout << "exportLaz DONE" << std::endl; return true; -} \ No newline at end of file +} diff --git a/core/src/ground_control_points.cpp b/core/src/ground_control_points.cpp index d83e6706..02fc2519 100644 --- a/core/src/ground_control_points.cpp +++ b/core/src/ground_control_points.cpp @@ -1,3 +1,5 @@ +#include + #include #include #if WITH_GUI == 1 @@ -11,7 +13,7 @@ static constexpr const char* xText = "Longitudinal (forward/backward)"; static constexpr const char* yText = "Lateral (left/right)"; static constexpr const char* zText = "Vertical (up/down)"; -void GroundControlPoints::imgui(PointClouds &point_clouds_container) +void GroundControlPoints::imgui(PointClouds& point_clouds_container) { if (ImGui::Begin("Ground Control Point", &is_imgui)) { @@ -19,7 +21,7 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputDouble("Default lidar height above ground [m]", &default_lidar_height_above_ground, 0.0, 10.0, "%.3f"); - //ImGui::Text("To show Ground Control Point (GCP) candidate please press 'CTRL'"); + // ImGui::Text("To show Ground Control Point (GCP) candidate please press 'CTRL'"); ImGui::Text("To pick trajectory node please press 'CTRL + left/middle mouse'"); ImGui::Text("At least 3 GCPs needed!"); @@ -34,7 +36,9 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) gcp.sigma_x = 0.1; gcp.sigma_y = 0.1; gcp.sigma_z = 0.1; - const auto &p = point_clouds_container.point_clouds[gcp.index_to_node_inner].local_trajectory[gcp.index_to_node_outer].m_pose.translation(); + const auto& p = point_clouds_container.point_clouds[gcp.index_to_node_inner] + .local_trajectory[gcp.index_to_node_outer] + .m_pose.translation(); Eigen::Vector3d position_global = point_clouds_container.point_clouds[gcp.index_to_node_inner].m_pose * p; gcp.x = position_global.x(); @@ -48,7 +52,7 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) // int picking_mode_index_to_node_outer = -1; } - ImGui::NewLine(); + ImGui::NewLine(); ImGui::Separator(); ImGui::NewLine(); @@ -65,7 +69,7 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) ImGui::PushItemWidth(ImGuiNumberWidth); ImGui::InputText(("GCP name##" + std::to_string(i)).c_str(), gpcs[i].name, IM_ARRAYSIZE(gpcs[i].name)); - ImGui::Text("sigma [m]:"); + ImGui::Text("sigma [m]:"); ImGui::InputDouble(("X##" + std::to_string(i)).c_str(), &gpcs[i].sigma_x, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); @@ -90,9 +94,9 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) ImGui::InputDouble(("Z##c" + std::to_string(i)).c_str(), &gpcs[i].z); if (ImGui::IsItemHovered()) ImGui::SetTooltip(zText); - + ImGui::InputDouble(("lidar_height_above_ground [m]##" + std::to_string(i)).c_str(), &gpcs[i].lidar_height_above_ground); - ImGui::PopItemWidth(); + ImGui::PopItemWidth(); ImGui::NewLine(); } @@ -130,24 +134,36 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) // GCPs for (int i = 0; i < gpcs.size(); i++) { - Eigen::Vector3d p_s = - point_clouds_container.point_clouds[gpcs[i].index_to_node_inner].m_pose * + Eigen::Vector3d p_s = point_clouds_container.point_clouds[gpcs[i].index_to_node_inner].m_pose * point_clouds_container.point_clouds[gpcs[i].index_to_node_inner] .local_trajectory[gpcs[i].index_to_node_outer] .m_pose.translation(); Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; Eigen::Vector3d p_t(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = 0; @@ -230,7 +246,8 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { point_clouds_container.point_clouds[i].m_pose = m_pose * point_clouds_container.point_clouds[i].m_pose; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -244,7 +261,8 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) std::cout << "AtPA=AtPB FAILED" << std::endl; } } - if (ImGui::IsItemHovered()){ + if (ImGui::IsItemHovered()) + { ImGui::SetTooltip("trajectory is rigid"); } } @@ -255,11 +273,13 @@ void GroundControlPoints::imgui(PointClouds &point_clouds_container) return; } -void GroundControlPoints::render(const PointClouds &point_clouds_container) +void GroundControlPoints::render(const PointClouds& point_clouds_container) { for (int i = 0; i < gpcs.size(); i++) { - const auto &p = point_clouds_container.point_clouds[gpcs[i].index_to_node_inner].local_trajectory[gpcs[i].index_to_node_outer].m_pose.translation(); + const auto& p = point_clouds_container.point_clouds[gpcs[i].index_to_node_inner] + .local_trajectory[gpcs[i].index_to_node_outer] + .m_pose.translation(); Eigen::Vector3d c = point_clouds_container.point_clouds[gpcs[i].index_to_node_inner].m_pose * p; Eigen::Vector3d g(gpcs[i].x, gpcs[i].y, gpcs[i].z); @@ -308,25 +328,26 @@ void GroundControlPoints::render(const PointClouds &point_clouds_container) glColor3f(0.7f, 0.3f, 0.5f); glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": LiDAR center").c_str())); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char*)(("GCP_" + std::to_string(i) + ": LiDAR center").c_str())); glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": 'plane on the ground'").c_str())); + glutBitmapString( + GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char*)(("GCP_" + std::to_string(i) + ": 'plane on the ground'").c_str())); glColor3f(0, 0, 0); glRasterPos3f(gpcs[i].x, gpcs[i].y, gpcs[i].z + gpcs[i].lidar_height_above_ground + 0.1); glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)gpcs[i].name); glRasterPos3f(c.x(), c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char *)(("GCP_" + std::to_string(i) + ": assigned trajectory node").c_str())); + glutBitmapString( + GLUT_BITMAP_TIMES_ROMAN_10, (const unsigned char*)(("GCP_" + std::to_string(i) + ": assigned trajectory node").c_str())); } return; } -void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vector3d &mean, Eigen::Vector3f color, float nstd) +void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd) { - Eigen::LLT> cholSolver(covar); Eigen::Matrix3d transform = cholSolver.matrixL(); @@ -341,7 +362,7 @@ void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vect { for (double j = 0; j < 1.0; j += dj) // vertical { - double u = i * 2 * pi; // 0 to 2pi + double u = i * 2 * pi; // 0 to 2pi double v = (j - 0.5) * pi; //-pi/2 to pi/2 const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); @@ -362,4 +383,4 @@ void GroundControlPoints::draw_ellipse(const Eigen::Matrix3d &covar, Eigen::Vect } } } -#endif \ No newline at end of file +#endif diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp index 82b6bec6..e866947c 100644 --- a/core/src/hash_utils.cpp +++ b/core/src/hash_utils.cpp @@ -1,10 +1,12 @@ +#include + #include unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z) { return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); + ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + ((static_cast(z) << 0) & (0x000000000000FFFFull)); } unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) diff --git a/core/src/icp.cpp b/core/src/icp.cpp index 3c572f60..59f42af3 100644 --- a/core/src/icp.cpp +++ b/core/src/icp.cpp @@ -1,25 +1,29 @@ +#include + #include -#include -#include -#include #include #include -#include -#include +#include +#include -std::vector ICP::get_jobs(long long unsigned int size, int num_threads) { +std::vector ICP::get_jobs(long long unsigned int size, int num_threads) +{ int hc = size / num_threads; - if (hc < 1)hc = 1; + if (hc < 1) + hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) { + for (long long unsigned int i = 0; i < size; i += hc) + { long long unsigned int sequence_length = hc; - if (i + hc >= size) { + if (i + hc >= size) + { sequence_length = size - i; } - if (sequence_length == 0)break; + if (sequence_length == 0) + break; Job j; j.index_begin_inclusive = i; @@ -29,34 +33,56 @@ std::vector ICP::get_jobs(long long unsigned int size, int num_threads return jobs; } -void alpha_point_to_point_job(ICP::Job* job, std::vector* alphas, float barron_c, std::vector>>* all_nns, - std::vector* point_clouds, std::vector* j_indexes, TaitBryanPose pose_s, float scale_factor_x, float scale_factor_y, float scale_factor_z, - std::vector* sums_x, std::vector* sums_y, std::vector* sums_z, int index_source) { - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { +void alpha_point_to_point_job( + ICP::Job* job, + std::vector* alphas, + float barron_c, + std::vector>>* all_nns, + std::vector* point_clouds, + std::vector* j_indexes, + TaitBryanPose pose_s, + float scale_factor_x, + float scale_factor_y, + float scale_factor_z, + std::vector* sums_x, + std::vector* sums_y, + std::vector* sums_z, + int index_source) +{ + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { double& alpha = (*alphas)[ii]; double Z_tilde = get_approximate_partition_function(-10, 10, alpha, barron_c, 100); double sum_x = 0; double sum_y = 0; double sum_z = 0; - for (size_t ni = 0; ni < (*all_nns).size(); ni++) { - for (size_t nj = 0; nj < (*all_nns)[ni].size(); nj++) { - //Eigen::Vector3d p_s((*point_clouds)[index_source].points_local[(*all_nns)[ni][nj].first]); - Eigen::Vector3d p_s((*point_clouds)[index_source].m_pose * (*point_clouds)[index_source].points_local[(*all_nns)[ni][nj].first]); - Eigen::Vector3d p_t((*point_clouds)[(*j_indexes)[ni]].m_pose * (*point_clouds)[(*j_indexes)[ni]].points_local[(*all_nns)[ni][nj].second]); - + for (size_t ni = 0; ni < (*all_nns).size(); ni++) + { + for (size_t nj = 0; nj < (*all_nns)[ni].size(); nj++) + { + // Eigen::Vector3d p_s((*point_clouds)[index_source].points_local[(*all_nns)[ni][nj].first]); + Eigen::Vector3d p_s( + (*point_clouds)[index_source].m_pose * (*point_clouds)[index_source].points_local[(*all_nns)[ni][nj].first]); + Eigen::Vector3d p_t( + (*point_clouds)[(*j_indexes)[ni]].m_pose * (*point_clouds)[(*j_indexes)[ni]].points_local[(*all_nns)[ni][nj].second]); + double delta_x = p_t.x() - p_s.x(); double delta_y = p_t.y() - p_s.y(); double delta_z = p_t.z() - p_s.z(); - //point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + // point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, + // pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); - if (!(delta_x == delta_x)) { + if (!(delta_x == delta_x)) + { continue; } - if (!(delta_y == delta_y)) { + if (!(delta_y == delta_y)) + { continue; } - if (!(delta_z == delta_z)) { + if (!(delta_z == delta_z)) + { continue; } @@ -77,21 +103,24 @@ void alpha_point_to_point_job(ICP::Job* job, std::vector* alphas, float bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool fix_first_node) { - for (auto& pc : point_clouds_container.point_clouds) { + for (auto& pc : point_clouds_container.point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } - - for (size_t iter = 0; iter < number_of_iterations; iter++) { + + for (size_t iter = 0; iter < number_of_iterations; iter++) + { std::cout << "ICP iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; - + std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - //barron + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + // barron double min_sum_x = std::numeric_limits::max(); double min_sum_y = std::numeric_limits::max(); double min_sum_z = std::numeric_limits::max(); @@ -99,19 +128,23 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool double barron_alpha_x = -10.; double barron_alpha_y = -10.; double barron_alpha_z = -10.; - + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); std::vector>> all_nns; std::vector j_indexes; - + float scale_factor_x = 10; float scale_factor_y = 10; float scale_factor_z = 10; - if (is_adaptive_robust_kernel) { - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + if (is_adaptive_robust_kernel) + { + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); all_nns.push_back(nns); j_indexes.push_back(j); } @@ -122,7 +155,8 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool std::vector sums_y; std::vector sums_z; - for (double alpha = -10; alpha <= 2; alpha += 0.1) { + for (double alpha = -10; alpha <= 2; alpha += 0.1) + { alphas.push_back(alpha); sums_x.push_back(0); sums_y.push_back(0); @@ -131,91 +165,153 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool std::vector jobs = get_jobs(alphas.size(), this->number_of_threads); std::vector threads; - for (size_t k = 0; k < jobs.size(); k++) { - threads.push_back(std::thread(alpha_point_to_point_job, &jobs[k], &alphas, barron_c, &all_nns, &point_clouds_container.point_clouds, &j_indexes, pose_s, - scale_factor_x, scale_factor_y, scale_factor_z, &sums_x, &sums_y, &sums_z, i)); + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + alpha_point_to_point_job, + &jobs[k], + &alphas, + barron_c, + &all_nns, + &point_clouds_container.point_clouds, + &j_indexes, + pose_s, + scale_factor_x, + scale_factor_y, + scale_factor_z, + &sums_x, + &sums_y, + &sums_z, + i)); } - for (size_t j = 0; j < threads.size(); j++) { + for (size_t j = 0; j < threads.size(); j++) + { threads[j].join(); } - for (size_t s = 0; s < sums_x.size(); s++) { - if (sums_x[s] < min_sum_x) { + for (size_t s = 0; s < sums_x.size(); s++) + { + if (sums_x[s] < min_sum_x) + { min_sum_x = sums_x[s]; barron_alpha_x = alphas[s]; } - if (sums_y[s] < min_sum_y) { + if (sums_y[s] < min_sum_y) + { min_sum_y = sums_y[s]; barron_alpha_y = alphas[s]; } - if (sums_z[s] < min_sum_z) { + if (sums_z[s] < min_sum_z) + { min_sum_z = sums_z[s]; barron_alpha_z = alphas[s]; } } - std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y << " barron_alpha_z: " << barron_alpha_z << std::endl; + std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y + << " barron_alpha_z: " << barron_alpha_z << std::endl; } - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - for (size_t k = 0; k < nns.size(); k++) { + for (size_t k = 0; k < nns.size(); k++) + { Eigen::Vector3d p_s(point_clouds_container.point_clouds[i].points_local[nns[k].first]); - Eigen::Vector3d p_t(point_clouds_container.point_clouds[j].m_pose * point_clouds_container.point_clouds[j].points_local[nns[k].second]); - + Eigen::Vector3d p_t( + point_clouds_container.point_clouds[j].m_pose * + point_clouds_container.point_clouds[j].points_local[nns[k].second]); + double delta_x; double delta_y; double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); int ir = tripletListB.size(); int ic = i * 6; - for (int row = 0; row < 3; row++) { - for (int col = 0; col < 6; col++) { - if (jacobian(row, col) != 0.0) { + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); } } } - + float factor_ballanced_horizontal_vs_vertical = 1; - if (is_ballanced_horizontal_vs_vertical) { - if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) { - //number_points_horisontal - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal);// + if (is_ballanced_horizontal_vs_vertical) + { + if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) + { + // number_points_horisontal + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal); // } - else { - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical);// + else + { + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical); // } } - if (is_adaptive_robust_kernel) { - tripletListP.emplace_back(ir, ir, get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 1, ir + 1, get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 2, ir + 2, get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); + if (is_adaptive_robust_kernel) + { + tripletListP.emplace_back( + ir, + ir, + get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 1, + ir + 1, + get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 2, + ir + 2, + get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); } - else { - tripletListP.emplace_back(ir, ir, 1 * factor_ballanced_horizontal_vs_vertical); + else + { + tripletListP.emplace_back(ir, ir, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 1, ir + 1, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 2, ir + 2, 1 * factor_ballanced_horizontal_vs_vertical); - //exit(0); + // exit(0); } - tripletListB.emplace_back(ir , 0, delta_x); + tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); } } } } - if (fix_first_node) { + if (fix_first_node) + { int ir = tripletListB.size(); tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); @@ -224,7 +320,7 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool tripletListA.emplace_back(ir + 4, 4, 1); tripletListA.emplace_back(ir + 5, 5, 1); - tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir, ir, 1000000); tripletListP.emplace_back(ir + 1, ir + 1, 1000000); tripletListP.emplace_back(ir + 2, ir + 2, 1000000); tripletListP.emplace_back(ir + 3, ir + 3, 1000000); @@ -263,21 +359,27 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); } } - if (h_x.size() == point_clouds_container.point_clouds.size() * 6) { + if (h_x.size() == point_clouds_container.point_clouds.size() * 6) + { std::cout << "ICP solution" << std::endl; std::cout << "x,y,z,om,fi,ka" << std::endl; - for (size_t i = 0; i < h_x.size(); i+=6) { - std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + for (size_t i = 0; i < h_x.size(); i += 6) + { + std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + << std::endl; } int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); pose.px += h_x[counter++] * 0.5; pose.py += h_x[counter++] * 0.5; @@ -286,13 +388,15 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool pose.fi += h_x[counter++] * 0.5; pose.ka += h_x[counter++] * 0.5; - if (i == 0 && fix_first_node) { + if (i == 0 && fix_first_node) + { continue; } /*if (!point_clouds_container.point_clouds[i].fixed) { point_clouds_container.point_clouds[i].m_pose = affine_matrix_from_pose_tait_bryan(pose); - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -342,37 +446,42 @@ bool ICP::optimize_source_to_target_wc(PointClouds& point_clouds_container, bool point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(pose_src.ka); } } - else { + else + { std::cout << "AtPA=AtPB FAILED" << std::endl; return false; } } - //clean - for (auto& pc : point_clouds_container.point_clouds) { + // clean + for (auto& pc : point_clouds_container.point_clouds) + { pc.clean(); } - return true; + return true; } bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point_clouds_container, bool fix_first_node) { - for (auto& pc : point_clouds_container.point_clouds) { + for (auto& pc : point_clouds_container.point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } - for (size_t iter = 0; iter < number_of_iterations; iter++) { + for (size_t iter = 0; iter < number_of_iterations; iter++) + { std::cout << "ICP iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - //barron + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + // barron double min_sum_x = std::numeric_limits::max(); double min_sum_y = std::numeric_limits::max(); double min_sum_z = std::numeric_limits::max(); @@ -389,10 +498,14 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point float scale_factor_y = 10; float scale_factor_z = 10; - if (is_adaptive_robust_kernel) { - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + if (is_adaptive_robust_kernel) + { + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); all_nns.push_back(nns); j_indexes.push_back(j); } @@ -403,7 +516,8 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point std::vector sums_y; std::vector sums_z; - for (double alpha = -10; alpha <= 2; alpha += 0.1) { + for (double alpha = -10; alpha <= 2; alpha += 0.1) + { alphas.push_back(alpha); sums_x.push_back(0); sums_y.push_back(0); @@ -412,42 +526,67 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point std::vector jobs = get_jobs(alphas.size(), this->number_of_threads); std::vector threads; - for (size_t k = 0; k < jobs.size(); k++) { - threads.push_back(std::thread(alpha_point_to_point_job, &jobs[k], &alphas, barron_c, &all_nns, &point_clouds_container.point_clouds, &j_indexes, pose_s, - scale_factor_x, scale_factor_y, scale_factor_z, &sums_x, &sums_y, &sums_z, i)); + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + alpha_point_to_point_job, + &jobs[k], + &alphas, + barron_c, + &all_nns, + &point_clouds_container.point_clouds, + &j_indexes, + pose_s, + scale_factor_x, + scale_factor_y, + scale_factor_z, + &sums_x, + &sums_y, + &sums_z, + i)); } - for (size_t j = 0; j < threads.size(); j++) { + for (size_t j = 0; j < threads.size(); j++) + { threads[j].join(); } - for (size_t s = 0; s < sums_x.size(); s++) { - if (sums_x[s] < min_sum_x) { + for (size_t s = 0; s < sums_x.size(); s++) + { + if (sums_x[s] < min_sum_x) + { min_sum_x = sums_x[s]; barron_alpha_x = alphas[s]; } - if (sums_y[s] < min_sum_y) { + if (sums_y[s] < min_sum_y) + { min_sum_y = sums_y[s]; barron_alpha_y = alphas[s]; } - if (sums_z[s] < min_sum_z) { + if (sums_z[s] < min_sum_z) + { min_sum_z = sums_z[s]; barron_alpha_z = alphas[s]; } } - std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y << " barron_alpha_z: " << barron_alpha_z << std::endl; + std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y + << " barron_alpha_z: " << barron_alpha_z << std::endl; } - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); - //TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - - + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - for (size_t k = 0; k < nns.size(); k++) { + for (size_t k = 0; k < nns.size(); k++) + { Eigen::Vector3d p_s(point_clouds_container.point_clouds[i].points_local[nns[k].first]); - Eigen::Vector3d p_t(point_clouds_container.point_clouds[j].m_pose * point_clouds_container.point_clouds[j].points_local[nns[k].second]); + Eigen::Vector3d p_t( + point_clouds_container.point_clouds[j].m_pose * + point_clouds_container.point_clouds[j].points_local[nns[k].second]); Eigen::Matrix3d R = point_clouds_container.point_clouds[i].m_pose.rotation(); Eigen::Vector3d Rp = R * p_s; @@ -466,34 +605,39 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point int ic = i * 6; tripletListA.emplace_back(ir, ic + 0, 1); - //tripletListA.emplace_back(ir, ic + 1, 0); - //tripletListA.emplace_back(ir, ic + 2, 0); + // tripletListA.emplace_back(ir, ic + 1, 0); + // tripletListA.emplace_back(ir, ic + 2, 0); tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - //tripletListA.emplace_back(ir + 1, ic + 0, 0); + // tripletListA.emplace_back(ir + 1, ic + 0, 0); tripletListA.emplace_back(ir + 1, ic + 1, 1); - //tripletListA.emplace_back(ir + 1, ic + 2, 0); + // tripletListA.emplace_back(ir + 1, ic + 2, 0); tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - //tripletListA.emplace_back(ir + 2, ic + 0, 0); - //tripletListA.emplace_back(ir + 2, ic + 1, 0); + // tripletListA.emplace_back(ir + 2, ic + 0, 0); + // tripletListA.emplace_back(ir + 2, ic + 1, 0); tripletListA.emplace_back(ir + 2, ic + 2, 1); tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); float factor_ballanced_horizontal_vs_vertical = 1; - if (is_ballanced_horizontal_vs_vertical) { - if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) { - //number_points_horisontal - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal);// + if (is_ballanced_horizontal_vs_vertical) + { + if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) + { + // number_points_horisontal + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal); // } - else { - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical);// + else + { + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical); // } } @@ -504,24 +648,36 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point double delta_y = target.y() - source.y(); double delta_z = target.z() - source.z(); - if (is_adaptive_robust_kernel) { - tripletListP.emplace_back(ir, ir, get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 1, ir + 1, get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 2, ir + 2, get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); + if (is_adaptive_robust_kernel) + { + tripletListP.emplace_back( + ir, + ir, + get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 1, + ir + 1, + get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 2, + ir + 2, + get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); } - else { + else + { tripletListP.emplace_back(ir, ir, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 1, ir + 1, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 2, ir + 2, 1 * factor_ballanced_horizontal_vs_vertical); } - tripletListB.emplace_back(ir , 0, delta_x); + tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); } } } } - if (fix_first_node) { + if (fix_first_node) + { int ir = tripletListB.size(); tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); @@ -569,21 +725,27 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); } } - if (h_x.size() == point_clouds_container.point_clouds.size() * 6) { + if (h_x.size() == point_clouds_container.point_clouds.size() * 6) + { std::cout << "ICP solution" << std::endl; std::cout << "x,y,z,s_x,s_y,s_z" << std::endl; - for (size_t i = 0; i < h_x.size(); i += 6) { - std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + for (size_t i = 0; i < h_x.size(); i += 6) + { + std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + << std::endl; } int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { RodriguesPose pose_update; pose_update.px = h_x[counter++] * 0.5; pose_update.py = h_x[counter++] * 0.5; @@ -591,15 +753,18 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point pose_update.sx = h_x[counter++] * 0.5; pose_update.sy = h_x[counter++] * 0.5; pose_update.sz = h_x[counter++] * 0.5; - - if (i == 0 && fix_first_node) { + + if (i == 0 && fix_first_node) + { continue; } /*if (!point_clouds_container.point_clouds[i].fixed) { - point_clouds_container.point_clouds[i].m_pose = affine_matrix_from_pose_rodrigues(pose_update) * point_clouds_container.point_clouds[i].m_pose; + point_clouds_container.point_clouds[i].m_pose = affine_matrix_from_pose_rodrigues(pose_update) * + point_clouds_container.point_clouds[i].m_pose; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -608,7 +773,8 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); }*/ - auto pose_res = pose_tait_bryan_from_affine_matrix(affine_matrix_from_pose_rodrigues(pose_update) * point_clouds_container.point_clouds[i].m_pose); + auto pose_res = pose_tait_bryan_from_affine_matrix( + affine_matrix_from_pose_rodrigues(pose_update) * point_clouds_container.point_clouds[i].m_pose); auto pose_src = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); if (!point_clouds_container.point_clouds[i].fixed_x) @@ -645,14 +811,16 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(pose_src.ka); } } - else { + else + { std::cout << "AtPA=AtPB FAILED" << std::endl; return false; } } - - //clean - for (auto& pc : point_clouds_container.point_clouds) { + + // clean + for (auto& pc : point_clouds_container.point_clouds) + { pc.clean(); } return true; @@ -660,21 +828,24 @@ bool ICP::optimize_source_to_target_lie_algebra_left_jacobian(PointClouds& point bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& point_clouds_container, bool fix_first_node) { - for (auto& pc : point_clouds_container.point_clouds) { + for (auto& pc : point_clouds_container.point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } - for (size_t iter = 0; iter < number_of_iterations; iter++) { + for (size_t iter = 0; iter < number_of_iterations; iter++) + { std::cout << "ICP iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - //barron + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + // barron double min_sum_x = std::numeric_limits::max(); double min_sum_y = std::numeric_limits::max(); double min_sum_z = std::numeric_limits::max(); @@ -691,10 +862,14 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin float scale_factor_y = 10; float scale_factor_z = 10; - if (is_adaptive_robust_kernel) { - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + if (is_adaptive_robust_kernel) + { + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); all_nns.push_back(nns); j_indexes.push_back(j); } @@ -705,7 +880,8 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin std::vector sums_y; std::vector sums_z; - for (double alpha = -10; alpha <= 2; alpha += 0.1) { + for (double alpha = -10; alpha <= 2; alpha += 0.1) + { alphas.push_back(alpha); sums_x.push_back(0); sums_y.push_back(0); @@ -714,39 +890,66 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin std::vector jobs = get_jobs(alphas.size(), this->number_of_threads); std::vector threads; - for (size_t k = 0; k < jobs.size(); k++) { - threads.push_back(std::thread(alpha_point_to_point_job, &jobs[k], &alphas, barron_c, &all_nns, &point_clouds_container.point_clouds, &j_indexes, pose_s, - scale_factor_x, scale_factor_y, scale_factor_z, &sums_x, &sums_y, &sums_z, i)); + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + alpha_point_to_point_job, + &jobs[k], + &alphas, + barron_c, + &all_nns, + &point_clouds_container.point_clouds, + &j_indexes, + pose_s, + scale_factor_x, + scale_factor_y, + scale_factor_z, + &sums_x, + &sums_y, + &sums_z, + i)); } - for (size_t j = 0; j < threads.size(); j++) { + for (size_t j = 0; j < threads.size(); j++) + { threads[j].join(); } - for (size_t s = 0; s < sums_x.size(); s++) { - if (sums_x[s] < min_sum_x) { + for (size_t s = 0; s < sums_x.size(); s++) + { + if (sums_x[s] < min_sum_x) + { min_sum_x = sums_x[s]; barron_alpha_x = alphas[s]; } - if (sums_y[s] < min_sum_y) { + if (sums_y[s] < min_sum_y) + { min_sum_y = sums_y[s]; barron_alpha_y = alphas[s]; } - if (sums_z[s] < min_sum_z) { + if (sums_z[s] < min_sum_z) + { min_sum_z = sums_z[s]; barron_alpha_z = alphas[s]; } } - std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y << " barron_alpha_z: " << barron_alpha_z << std::endl; + std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y + << " barron_alpha_z: " << barron_alpha_z << std::endl; } - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); - - for (size_t k = 0; k < nns.size(); k++) { + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + + for (size_t k = 0; k < nns.size(); k++) + { Eigen::Vector3d p_s(point_clouds_container.point_clouds[i].points_local[nns[k].first]); - Eigen::Vector3d p_t(point_clouds_container.point_clouds[j].m_pose * point_clouds_container.point_clouds[j].points_local[nns[k].second]); + Eigen::Vector3d p_t( + point_clouds_container.point_clouds[j].m_pose * + point_clouds_container.point_clouds[j].points_local[nns[k].second]); Eigen::Matrix3d px; px(0, 0) = 0; @@ -786,13 +989,18 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); float factor_ballanced_horizontal_vs_vertical = 1; - if (is_ballanced_horizontal_vs_vertical) { - if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) { - //number_points_horisontal - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal);// + if (is_ballanced_horizontal_vs_vertical) + { + if (point_clouds_container.point_clouds[i].points_type[nns[k].first] == 0) + { + // number_points_horisontal + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_horizontal); // } - else { - factor_ballanced_horizontal_vs_vertical = 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical);// + else + { + factor_ballanced_horizontal_vs_vertical = + 1.0 / float(point_clouds_container.point_clouds[i].number_points_vertical); // } } @@ -803,12 +1011,23 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin double delta_y = target.y() - source.y(); double delta_z = target.z() - source.z(); - if (is_adaptive_robust_kernel) { - tripletListP.emplace_back(ir, ir, get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 1, ir + 1, get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); - tripletListP.emplace_back(ir + 2, ir + 2, get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); + if (is_adaptive_robust_kernel) + { + tripletListP.emplace_back( + ir, + ir, + get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 1, + ir + 1, + get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c) * factor_ballanced_horizontal_vs_vertical); + tripletListP.emplace_back( + ir + 2, + ir + 2, + get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c) * factor_ballanced_horizontal_vs_vertical); } - else { + else + { tripletListP.emplace_back(ir, ir, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 1, ir + 1, 1 * factor_ballanced_horizontal_vs_vertical); tripletListP.emplace_back(ir + 2, ir + 2, 1 * factor_ballanced_horizontal_vs_vertical); @@ -820,7 +1039,8 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin } } } - if (fix_first_node) { + if (fix_first_node) + { int ir = tripletListB.size(); tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); @@ -868,21 +1088,27 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); } } - if (h_x.size() == point_clouds_container.point_clouds.size() * 6) { + if (h_x.size() == point_clouds_container.point_clouds.size() * 6) + { std::cout << "ICP solution" << std::endl; std::cout << "x,y,z,s_x,s_y,s_z" << std::endl; - for (size_t i = 0; i < h_x.size(); i += 6) { - std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + for (size_t i = 0; i < h_x.size(); i += 6) + { + std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + << std::endl; } int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { RodriguesPose pose_update; pose_update.px = h_x[counter++] * 0.5; pose_update.py = h_x[counter++] * 0.5; @@ -891,14 +1117,17 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin pose_update.sy = h_x[counter++] * 0.5; pose_update.sz = h_x[counter++] * 0.5; - if (i == 0 && fix_first_node) { + if (i == 0 && fix_first_node) + { continue; } /*if (!point_clouds_container.point_clouds[i].fixed) { - point_clouds_container.point_clouds[i].m_pose = point_clouds_container.point_clouds[i].m_pose * affine_matrix_from_pose_rodrigues(pose_update); + point_clouds_container.point_clouds[i].m_pose = point_clouds_container.point_clouds[i].m_pose * + affine_matrix_from_pose_rodrigues(pose_update); - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -906,7 +1135,8 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); }*/ - auto pose_res = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose * affine_matrix_from_pose_rodrigues(pose_update)); + auto pose_res = pose_tait_bryan_from_affine_matrix( + point_clouds_container.point_clouds[i].m_pose * affine_matrix_from_pose_rodrigues(pose_update)); auto pose_src = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); if (!point_clouds_container.point_clouds[i].fixed_x) @@ -943,14 +1173,16 @@ bool ICP::optimize_source_to_target_lie_algebra_right_jacobian(PointClouds& poin point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(pose_src.ka); } } - else { + else + { std::cout << "AtPA=AtPB FAILED" << std::endl; return false; } } - //clean - for (auto& pc : point_clouds_container.point_clouds) { + // clean + for (auto& pc : point_clouds_container.point_clouds) + { pc.clean(); } return true; @@ -960,44 +1192,70 @@ bool ICP::compute_uncertainty(PointClouds& point_clouds_container) { std::cout << "compute_uncertainty" << std::endl; - for (auto& pc : point_clouds_container.point_clouds) { + for (auto& pc : point_clouds_container.point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } std::vector> tripletListA; - + double ssr = 0.0; int num_obs = 0; - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) { - if (i != j) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + for (int j = 0; j < point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + std::vector> nns = + point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], search_radius); TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - for (size_t k = 0; k < nns.size(); k++) { + for (size_t k = 0; k < nns.size(); k++) + { Eigen::Vector3d p_s(point_clouds_container.point_clouds[i].points_local[nns[k].first]); - Eigen::Vector3d p_t(point_clouds_container.point_clouds[j].m_pose * point_clouds_container.point_clouds[j].points_local[nns[k].second]); + Eigen::Vector3d p_t( + point_clouds_container.point_clouds[j].m_pose * point_clouds_container.point_clouds[j].points_local[nns[k].second]); double delta_x; double delta_y; double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); int ir = num_obs; int ic = i * 6; - for (int row = 0; row < 3; row++) { - for (int col = 0; col < 6; col++) { - if (jacobian(row, col) != 0.0) { + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); } } } - + ssr += delta_x * delta_x; ssr += delta_y * delta_y; ssr += delta_z * delta_z; @@ -1010,7 +1268,6 @@ bool ICP::compute_uncertainty(PointClouds& point_clouds_container) Eigen::SparseMatrix matA(num_obs, point_clouds_container.point_clouds.size() * 6); matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - Eigen::SparseMatrix AtA(point_clouds_container.point_clouds.size() * 6, point_clouds_container.point_clouds.size() * 6); AtA = matA.transpose() * matA; @@ -1026,19 +1283,23 @@ bool ICP::compute_uncertainty(PointClouds& point_clouds_container) AtAinv = AtAinv * sq; - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - for (int r = 0; r < 6; r++) { - for (int c = 0; c < 6; c++) { + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + for (int r = 0; r < 6; r++) + { + for (int c = 0; c < 6; c++) + { point_clouds_container.point_clouds[i].covariance_matrix_tait_bryan(r, c) = AtAinv.coeff(i * 6 + r, i * 6 + c); } } - point_clouds_container.point_clouds[i].information_matrix_tait_bryan = point_clouds_container.point_clouds[i].covariance_matrix_tait_bryan.inverse(); + point_clouds_container.point_clouds[i].information_matrix_tait_bryan = + point_clouds_container.point_clouds[i].covariance_matrix_tait_bryan.inverse(); } - - //clean - for (auto& pc : point_clouds_container.point_clouds) { + + // clean + for (auto& pc : point_clouds_container.point_clouds) + { pc.clean(); } return true; } - diff --git a/core/src/local_shape_features.cpp b/core/src/local_shape_features.cpp index ff9035d0..628869c7 100644 --- a/core/src/local_shape_features.cpp +++ b/core/src/local_shape_features.cpp @@ -1,9 +1,9 @@ -#include +#include + #include -#include -#include +#include -bool LocalShapeFeatures::calculate_local_shape_features(std::vector &points, const Params ¶ms) +bool LocalShapeFeatures::calculate_local_shape_features(std::vector& points, const Params& params) { std::vector> indexes; @@ -13,9 +13,13 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -33,9 +37,9 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector &index) + const auto hessian_fun = [&](const std::pair& index) { - int index_element_source = index.second; + int index_element_source = index.second; Eigen::Vector3d source = points[index_element_source].coordinates_global; Eigen::Vector3d mean(0.0, 0.0, 0.0); Eigen::Matrix3d cov; @@ -48,7 +52,6 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector= 3) { points[index_element_source].valid = true; @@ -96,7 +99,10 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector + #if WITH_GUI == 1 +#include #include #include -#include #include -#include -#include - -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include -#include #include - -#include -#include +#include #include @@ -26,19 +26,26 @@ #include -void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, - int &index_loop_closure_source, int &index_loop_closure_target, - float *m_gizmo, GNSS &gnss, GroundControlPoints &gcps, ControlPoints &cps, - int num_edge_extended_before, int num_edge_extended_after) +void ManualPoseGraphLoopClosure::Gui( + PointClouds& point_clouds_container, + int& index_loop_closure_source, + int& index_loop_closure_target, + float* m_gizmo, + GNSS& gnss, + GroundControlPoints& gcps, + ControlPoints& cps, + int num_edge_extended_before, + int num_edge_extended_after) { if (point_clouds_container.point_clouds.size() > 0) { if (!manipulate_active_edge) { - //suboptimal as distance is computed each frame! - double distance = (point_clouds_container.point_clouds[index_loop_closure_target].m_pose.translation() - - point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation()).norm(); - ImGui::Text("Loop closure indexes: (Length of selected edge: %.3f [m])", distance); + // suboptimal as distance is computed each frame! + double distance = (point_clouds_container.point_clouds[index_loop_closure_target].m_pose.translation() - + point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation()) + .norm(); + ImGui::Text("Loop closure indexes: (Length of selected edge: %.3f [m])", distance); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Select indexes from going/returning trips to same point that should overlap"); @@ -108,21 +115,21 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, // ImGui::Text("motion model"); ImGui::Separator(); - + ImGui::Text("Motion model sigmas [m] / [deg]:"); if (ImGui::IsItemHovered()) ImGui::SetTooltip("position/rotational uncertainties"); ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputDouble("px_1", & motion_model_w_px_1_sigma_m, 0.0, 0.0, "%.4f"); + ImGui::InputDouble("px_1", &motion_model_w_px_1_sigma_m, 0.0, 0.0, "%.4f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(xText); ImGui::SameLine(); - ImGui::InputDouble("om_1", & motion_model_w_om_1_sigma_deg, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("om_1", &motion_model_w_om_1_sigma_deg, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(omText); - ImGui::InputDouble("py_1", & motion_model_w_py_1_sigma_m, 0.0, 0.0, "%.4f"); + ImGui::InputDouble("py_1", &motion_model_w_py_1_sigma_m, 0.0, 0.0, "%.4f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(yText); ImGui::SameLine(); @@ -130,7 +137,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, if (ImGui::IsItemHovered()) ImGui::SetTooltip(fiText); - ImGui::InputDouble("pz_1", & motion_model_w_pz_1_sigma_m, 0.0, 0.0, "%.4f"); + ImGui::InputDouble("pz_1", &motion_model_w_pz_1_sigma_m, 0.0, 0.0, "%.4f"); if (ImGui::IsItemHovered()) ImGui::SetTooltip(zText); ImGui::SameLine(); @@ -141,8 +148,6 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, ImGui::PopItemWidth(); ImGui::Separator(); - - if (ImGui::Button("Fuse trajectory with GNSS (trajectory is rigid)")) { FuseTrajectoryWithGNSS(point_clouds_container, gnss); @@ -150,7 +155,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, } } - if (edges.size() > 0) + if (edges.size() > 0) ImGui::Checkbox("Manipulate active edge", &manipulate_active_edge); if (edges.size() > 0 && manipulate_active_edge) @@ -162,7 +167,8 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, if (!gizmo) { if (ImGui::Button("ICP")) - run_icp(point_clouds_container, index_active_edge, search_radious, 10, num_edge_extended_before, num_edge_extended_after); + run_icp( + point_clouds_container, index_active_edge, search_radious, 10, num_edge_extended_before, num_edge_extended_after); ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); ImGui::InputDouble("Search_radius [m]", &search_radious); @@ -192,7 +198,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, if (output_file_name.size() > 0) { std::vector source; - auto &e = edges[index_active_edge]; + auto& e = edges[index_active_edge]; for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) { int index_src = e.index_to + i; @@ -207,16 +213,16 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, // point_clouds_container.point_clouds.at(index_src).render(m_src, 1); } } - + Eigen::Affine3d m_src_inv = point_clouds_container.point_clouds[e.index_to].m_pose.inverse(); - for (auto &p : source) + for (auto& p : source) { p = m_src_inv * p; } auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - for (auto &p : source) + for (auto& p : source) { p = m_pose * p; } @@ -229,11 +235,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, intensity.push_back(0); timestamps.push_back(0.0); } - exportLaz( - output_file_name, - source, - intensity, - timestamps); + exportLaz(output_file_name, source, intensity, timestamps); } } ImGui::SameLine(); @@ -245,7 +247,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, if (output_file_name.size() > 0) { std::vector target; - auto &e = edges[index_active_edge]; + auto& e = edges[index_active_edge]; for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) { int index_trg = e.index_from + i; @@ -262,7 +264,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, Eigen::Affine3d m_trg_inv = point_clouds_container.point_clouds[e.index_from].m_pose.inverse(); - for (auto &p : target) + for (auto& p : target) { p = m_trg_inv * p; } @@ -276,11 +278,7 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, timestamps.push_back(0.0); } - exportLaz( - output_file_name, - target, - intensity, - timestamps); + exportLaz(output_file_name, target, intensity, timestamps); } } } @@ -292,25 +290,27 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, { for (size_t i = 0; i < edges.size(); i++) { - if (i > 0){ + if (i > 0) + { ImGui::Separator(); } ImGui::SetWindowFontScale(1.25f); ImGui::RadioButton(("Active edge " + std::to_string(i)).c_str(), &index_active_edge, i); ImGui::SetWindowFontScale(1.0f); - //suboptimal as distance is computed each frame! - double distance = (point_clouds_container.point_clouds[edges[i].index_to].m_pose.translation() - - point_clouds_container.point_clouds[edges[i].index_from].m_pose.translation()).norm(); + // suboptimal as distance is computed each frame! + double distance = (point_clouds_container.point_clouds[edges[i].index_to].m_pose.translation() - + point_clouds_container.point_clouds[edges[i].index_from].m_pose.translation()) + .norm(); ImGui::SameLine(); ImGui::Text("(length [m]: %.3f)", distance); ImGui::SameLine(); - + if (ImGui::Button(("Remove##" + std::to_string(i)).c_str())) { gizmo = false; - + std::vector new_edges; for (int ni = 0; ni < edges.size(); ni++) { @@ -319,11 +319,12 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, } edges = new_edges; - index_active_edge = std::min(index_active_edge, static_cast(edges.size()-1)); + index_active_edge = std::min(index_active_edge, static_cast(edges.size() - 1)); manipulate_active_edge = (edges.size() > 0); } - if (index_active_edge == i){ + if (index_active_edge == i) + { ImGui::SameLine(); // if(session.) @@ -337,11 +338,15 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, std::cout << point_clouds_container.point_clouds[edges[index_active_edge].index_from].file_name << std::endl; std::cout << point_clouds_container.point_clouds[edges[index_active_edge].index_to].file_name << std::endl; - point_clouds_container.point_clouds[edges[index_active_edge].index_from].load_pc(point_clouds_container.point_clouds[edges[index_active_edge].index_from].file_name, true); - point_clouds_container.point_clouds[edges[index_active_edge].index_from].decimate(downsampling_voxel_size, downsampling_voxel_size, downsampling_voxel_size); + point_clouds_container.point_clouds[edges[index_active_edge].index_from].load_pc( + point_clouds_container.point_clouds[edges[index_active_edge].index_from].file_name, true); + point_clouds_container.point_clouds[edges[index_active_edge].index_from].decimate( + downsampling_voxel_size, downsampling_voxel_size, downsampling_voxel_size); - point_clouds_container.point_clouds[edges[index_active_edge].index_to].load_pc(point_clouds_container.point_clouds[edges[index_active_edge].index_to].file_name, true); - point_clouds_container.point_clouds[edges[index_active_edge].index_to].decimate(downsampling_voxel_size, downsampling_voxel_size, downsampling_voxel_size); + point_clouds_container.point_clouds[edges[index_active_edge].index_to].load_pc( + point_clouds_container.point_clouds[edges[index_active_edge].index_to].file_name, true); + point_clouds_container.point_clouds[edges[index_active_edge].index_to].decimate( + downsampling_voxel_size, downsampling_voxel_size, downsampling_voxel_size); // gizmo = false; @@ -356,23 +361,22 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, // index_active_edge = std::min(index_active_edge, static_cast(edges.size() - 1)); // manipulate_active_edge = (edges.size() > 0); - //for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) - // { - // } + // for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + // { + // } } ImGui::SameLine(); ImGui::InputDouble("downsampling_voxel_size ", &downsampling_voxel_size); } - ImGui::BeginDisabled(true); ImGui::PushItemWidth(ImGuiNumberWidth); ImGui::InputInt(("Source##" + std::to_string(i)).c_str(), &edges[i].index_from); ImGui::SameLine(); ImGui::InputInt(("Target##" + std::to_string(i)).c_str(), &edges[i].index_to); - ImGui::PopItemWidth(); - ImGui::EndDisabled(); + ImGui::PopItemWidth(); + ImGui::EndDisabled(); } } ImGui::EndChild(); @@ -403,10 +407,12 @@ void ManualPoseGraphLoopClosure::Gui(PointClouds &point_clouds_container, } } -void ManualPoseGraphLoopClosure::Render(PointClouds &point_clouds_container, - int index_loop_closure_source, int index_loop_closure_target, - int num_edge_extended_before, - int num_edge_extended_after) +void ManualPoseGraphLoopClosure::Render( + PointClouds& point_clouds_container, + int index_loop_closure_source, + int index_loop_closure_target, + int num_edge_extended_before, + int num_edge_extended_after) { point_clouds_container.point_clouds.at(index_loop_closure_source).visible = true; point_clouds_container.point_clouds.at(index_loop_closure_target).visible = true; @@ -456,7 +462,7 @@ void ManualPoseGraphLoopClosure::Render(PointClouds &point_clouds_container, Eigen::Affine3d m_trg = m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); Eigen::Affine3d m_rel = point_clouds_container.point_clouds.at(index_trg).m_pose.inverse() * - point_clouds_container.point_clouds.at(index_trg + i).m_pose; + point_clouds_container.point_clouds.at(index_trg + i).m_pose; m_trg = m_trg * m_rel; point_clouds_container.point_clouds.at(index_trg + i).render(m_trg, 1); } @@ -467,17 +473,17 @@ void ManualPoseGraphLoopClosure::Render(PointClouds &point_clouds_container, glColor3f(0, 1, 0); glBegin(GL_LINE_STRIP); - for (auto &pc : point_clouds_container.point_clouds) + for (auto& pc : point_clouds_container.point_clouds) { glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); } glEnd(); int i = 0; - for (auto &pc : point_clouds_container.point_clouds) + for (auto& pc : point_clouds_container.point_clouds) { glRasterPos3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3) + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(i).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); i++; } @@ -512,8 +518,8 @@ void ManualPoseGraphLoopClosure::Render(PointClouds &point_clouds_container, glEnd(); glRasterPos3f(m2.x(), m2.y(), m2.z() + 0.1); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)std::to_string(i).c_str()); + glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char*)std::to_string(i).c_str()); } } -#endif \ No newline at end of file +#endif diff --git a/core/src/ndt.cpp b/core/src/ndt.cpp index f729364d..1f4b4ed9 100644 --- a/core/src/ndt.cpp +++ b/core/src/ndt.cpp @@ -1,3697 +1,4029 @@ +#include + #include #include -// #include -#include -#include -#include -#include - -#include #include #include -#include -#include -#include -#include - -#include -#include #include - #include +#include +#include +#include +#include +#include +#include -void NDT::grid_calculate_params(const std::vector &point_cloud_global, GridParameters &in_out_params) +void NDT::grid_calculate_params(const std::vector& point_cloud_global, GridParameters& in_out_params) { - double min_x = std::numeric_limits::max(); - double max_x = std::numeric_limits::lowest(); - - double min_y = std::numeric_limits::max(); - double max_y = std::numeric_limits::lowest(); - - double min_z = std::numeric_limits::max(); - double max_z = std::numeric_limits::lowest(); - - for (size_t i = 0; i < point_cloud_global.size(); i++) - { - if (point_cloud_global[i].x < min_x) - min_x = point_cloud_global[i].x; - if (point_cloud_global[i].x > max_x) - max_x = point_cloud_global[i].x; - - if (point_cloud_global[i].y < min_y) - min_y = point_cloud_global[i].y; - if (point_cloud_global[i].y > max_y) - max_y = point_cloud_global[i].y; - - if (point_cloud_global[i].z < min_z) - min_z = point_cloud_global[i].z; - if (point_cloud_global[i].z > max_z) - max_z = point_cloud_global[i].z; - } - - max_x += in_out_params.bounding_box_extension; - min_x -= in_out_params.bounding_box_extension; - - max_y += in_out_params.bounding_box_extension; - min_y -= in_out_params.bounding_box_extension; - - max_z += in_out_params.bounding_box_extension; - min_z -= in_out_params.bounding_box_extension; - - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; - - in_out_params.number_of_buckets_X = number_of_buckets_X; - in_out_params.number_of_buckets_Y = number_of_buckets_Y; - in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); - - in_out_params.bounding_box_max_X = max_x; - in_out_params.bounding_box_min_X = min_x; - in_out_params.bounding_box_max_Y = max_y; - in_out_params.bounding_box_min_Y = min_y; - in_out_params.bounding_box_max_Z = max_z; - in_out_params.bounding_box_min_Z = min_z; + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); + + double min_z = std::numeric_limits::max(); + double max_z = std::numeric_limits::lowest(); + + for (size_t i = 0; i < point_cloud_global.size(); i++) + { + if (point_cloud_global[i].x < min_x) + min_x = point_cloud_global[i].x; + if (point_cloud_global[i].x > max_x) + max_x = point_cloud_global[i].x; + + if (point_cloud_global[i].y < min_y) + min_y = point_cloud_global[i].y; + if (point_cloud_global[i].y > max_y) + max_y = point_cloud_global[i].y; + + if (point_cloud_global[i].z < min_z) + min_z = point_cloud_global[i].z; + if (point_cloud_global[i].z > max_z) + max_z = point_cloud_global[i].z; + } + + max_x += in_out_params.bounding_box_extension; + min_x -= in_out_params.bounding_box_extension; + + max_y += in_out_params.bounding_box_extension; + min_y -= in_out_params.bounding_box_extension; + + max_z += in_out_params.bounding_box_extension; + min_z -= in_out_params.bounding_box_extension; + + long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + + in_out_params.number_of_buckets_X = number_of_buckets_X; + in_out_params.number_of_buckets_Y = number_of_buckets_Y; + in_out_params.number_of_buckets_Z = number_of_buckets_Z; + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * + static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + + in_out_params.bounding_box_max_X = max_x; + in_out_params.bounding_box_min_X = min_x; + in_out_params.bounding_box_max_Y = max_y; + in_out_params.bounding_box_min_Y = min_y; + in_out_params.bounding_box_max_Z = max_z; + in_out_params.bounding_box_min_Z = min_z; } -void NDT::grid_calculate_params(const std::vector &point_cloud_global, GridParameters &in_out_params) +void NDT::grid_calculate_params(const std::vector& point_cloud_global, GridParameters& in_out_params) { - double min_x = std::numeric_limits::max(); - double max_x = std::numeric_limits::lowest(); - - double min_y = std::numeric_limits::max(); - double max_y = std::numeric_limits::lowest(); - - double min_z = std::numeric_limits::max(); - double max_z = std::numeric_limits::lowest(); - - for (size_t i = 0; i < point_cloud_global.size(); i++) - { - if (point_cloud_global[i].point.x() < min_x) - min_x = point_cloud_global[i].point.x(); - if (point_cloud_global[i].point.x() > max_x) - max_x = point_cloud_global[i].point.x(); - - if (point_cloud_global[i].point.y() < min_y) - min_y = point_cloud_global[i].point.y(); - if (point_cloud_global[i].point.y() > max_y) - max_y = point_cloud_global[i].point.y(); - - if (point_cloud_global[i].point.z() < min_z) - min_z = point_cloud_global[i].point.z(); - if (point_cloud_global[i].point.z() > max_z) - max_z = point_cloud_global[i].point.z(); - } + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); + + double min_z = std::numeric_limits::max(); + double max_z = std::numeric_limits::lowest(); + + for (size_t i = 0; i < point_cloud_global.size(); i++) + { + if (point_cloud_global[i].point.x() < min_x) + min_x = point_cloud_global[i].point.x(); + if (point_cloud_global[i].point.x() > max_x) + max_x = point_cloud_global[i].point.x(); + + if (point_cloud_global[i].point.y() < min_y) + min_y = point_cloud_global[i].point.y(); + if (point_cloud_global[i].point.y() > max_y) + max_y = point_cloud_global[i].point.y(); + + if (point_cloud_global[i].point.z() < min_z) + min_z = point_cloud_global[i].point.z(); + if (point_cloud_global[i].point.z() > max_z) + max_z = point_cloud_global[i].point.z(); + } + + max_x += in_out_params.bounding_box_extension; + min_x -= in_out_params.bounding_box_extension; + + max_y += in_out_params.bounding_box_extension; + min_y -= in_out_params.bounding_box_extension; + + max_z += in_out_params.bounding_box_extension; + min_z -= in_out_params.bounding_box_extension; + + long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + + in_out_params.number_of_buckets_X = number_of_buckets_X; + in_out_params.number_of_buckets_Y = number_of_buckets_Y; + in_out_params.number_of_buckets_Z = number_of_buckets_Z; + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * + static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + + in_out_params.bounding_box_max_X = max_x; + in_out_params.bounding_box_min_X = min_x; + in_out_params.bounding_box_max_Y = max_y; + in_out_params.bounding_box_min_Y = min_y; + in_out_params.bounding_box_max_Z = max_z; + in_out_params.bounding_box_min_Z = min_z; +} - max_x += in_out_params.bounding_box_extension; - min_x -= in_out_params.bounding_box_extension; +void NDT::grid_calculate_params( + GridParameters& in_out_params, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z) +{ + long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + + in_out_params.number_of_buckets_X = number_of_buckets_X; + in_out_params.number_of_buckets_Y = number_of_buckets_Y; + in_out_params.number_of_buckets_Z = number_of_buckets_Z; + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * + static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + + in_out_params.bounding_box_max_X = max_x; + in_out_params.bounding_box_min_X = min_x; + in_out_params.bounding_box_max_Y = max_y; + in_out_params.bounding_box_min_Y = min_y; + in_out_params.bounding_box_max_Z = max_z; + in_out_params.bounding_box_min_Z = min_z; +} - max_y += in_out_params.bounding_box_extension; - min_y -= in_out_params.bounding_box_extension; +std::vector NDT::get_jobs(long long unsigned int size, int num_threads) +{ + int hc = size / num_threads; + if (hc < 1) + hc = 1; + + std::vector jobs; + for (long long unsigned int i = 0; i < size; i += hc) + { + long long unsigned int sequence_length = hc; + if (i + hc >= size) + { + sequence_length = size - i; + } + if (sequence_length == 0) + break; + + Job j; + j.index_begin_inclusive = i; + j.index_end_exclusive = i + sequence_length; + jobs.push_back(j); + } + + return jobs; +} - max_z += in_out_params.bounding_box_extension; - min_z -= in_out_params.bounding_box_extension; +void reindex_job( + int i, NDT::Job* job, std::vector* points, std::vector* pairs, NDT::GridParameters rgd_params) +{ + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + Point3D& p = (*points)[ii]; + + (*pairs)[ii].index_of_point = ii; + (*pairs)[ii].index_of_bucket = 0; + (*pairs)[ii].index_pose = p.index_pose; + + if (p.x < rgd_params.bounding_box_min_X) + { + continue; + } + if (p.x > rgd_params.bounding_box_max_X) + { + continue; + } + if (p.y < rgd_params.bounding_box_min_Y) + { + continue; + } + if (p.y > rgd_params.bounding_box_max_Y) + { + continue; + } + if (p.z < rgd_params.bounding_box_min_Z) + { + continue; + } + if (p.z > rgd_params.bounding_box_max_Z) + { + continue; + } + + long long unsigned int ix = (p.x - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + long long unsigned int iy = (p.y - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + long long unsigned int iz = (p.z - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + + (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * + static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + } +} - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; +void reindex_jobi( + int i, NDT::Job* job, std::vector* points, std::vector* pairs, NDT::GridParameters rgd_params) +{ + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + Point3Di& p = (*points)[ii]; + + (*pairs)[ii].index_of_point = ii; + (*pairs)[ii].index_of_bucket = 0; + (*pairs)[ii].index_pose = p.index_pose; + + if (p.point.x() < rgd_params.bounding_box_min_X) + { + continue; + } + if (p.point.x() > rgd_params.bounding_box_max_X) + { + continue; + } + if (p.point.y() < rgd_params.bounding_box_min_Y) + { + continue; + } + if (p.point.y() > rgd_params.bounding_box_max_Y) + { + continue; + } + if (p.point.z() < rgd_params.bounding_box_min_Z) + { + continue; + } + if (p.point.z() > rgd_params.bounding_box_max_Z) + { + continue; + } + + long long unsigned int ix = (p.point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + long long unsigned int iy = (p.point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + long long unsigned int iz = (p.point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + + (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * + static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + } +} - in_out_params.number_of_buckets_X = number_of_buckets_X; - in_out_params.number_of_buckets_Y = number_of_buckets_Y; - in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); +void NDT::reindex( + std::vector& points, std::vector& index_pair, NDT::GridParameters& rgd_params, int num_threads) +{ + index_pair.resize(points.size()); + + std::vector jobs = get_jobs(index_pair.size(), num_threads); + + std::vector threads; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(reindex_job, i, &jobs[i], &points, &index_pair, rgd_params)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + std::sort( + index_pair.begin(), + index_pair.end(), + [](const NDT::PointBucketIndexPair& a, const NDT::PointBucketIndexPair& b) + { + return ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); + }); +} - in_out_params.bounding_box_max_X = max_x; - in_out_params.bounding_box_min_X = min_x; - in_out_params.bounding_box_max_Y = max_y; - in_out_params.bounding_box_min_Y = min_y; - in_out_params.bounding_box_max_Z = max_z; - in_out_params.bounding_box_min_Z = min_z; +void NDT::reindex( + std::vector& points, std::vector& index_pair, NDT::GridParameters& rgd_params, int num_threads) +{ + index_pair.resize(points.size()); + + std::vector jobs = get_jobs(index_pair.size(), num_threads); + + std::vector threads; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(reindex_jobi, i, &jobs[i], &points, &index_pair, rgd_params)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + std::sort( + index_pair.begin(), + index_pair.end(), + [](const NDT::PointBucketIndexPair& a, const NDT::PointBucketIndexPair& b) + { + return ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); + }); } -void NDT::grid_calculate_params(GridParameters &in_out_params, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z) +void build_rgd_init_job(int i, NDT::Job* job, std::vector* buckets) { - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; - - in_out_params.number_of_buckets_X = number_of_buckets_X; - in_out_params.number_of_buckets_Y = number_of_buckets_Y; - in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); - - in_out_params.bounding_box_max_X = max_x; - in_out_params.bounding_box_min_X = min_x; - in_out_params.bounding_box_max_Y = max_y; - in_out_params.bounding_box_min_Y = min_y; - in_out_params.bounding_box_max_Z = max_z; - in_out_params.bounding_box_min_Z = min_z; + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + (*buckets)[ii].index_begin = 0; + (*buckets)[ii].index_end = 0; + (*buckets)[ii].number_of_points = 0; + } } -std::vector NDT::get_jobs(long long unsigned int size, int num_threads) +void build_rgd_job(int i, NDT::Job* job, std::vector* index_pair, std::vector* buckets) { + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + int ind = ii; + if (ind == 0) + { + long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; + long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + + (*buckets)[index_of_bucket].index_begin = ind; + if (index_of_bucket != index_of_bucket_1) + { + (*buckets)[index_of_bucket].index_end = ind + 1; + (*buckets)[index_of_bucket_1].index_end = ind + 1; + } + } + else if (ind == (*index_pair).size() - 1) + { + if ((*index_pair)[ind].index_of_bucket < (*buckets).size()) + { + (*buckets)[(*index_pair)[ind].index_of_bucket].index_end = ind + 1; + } + } + else if (ind + 1 < (*index_pair).size()) + { + long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; + long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + + if (index_of_bucket != index_of_bucket_1) + { + (*buckets)[index_of_bucket].index_end = ind + 1; + (*buckets)[index_of_bucket_1].index_begin = ind + 1; + } + } + } +} - int hc = size / num_threads; - if (hc < 1) - hc = 1; +void build_rgd_final_job(int i, NDT::Job* job, std::vector* buckets) +{ + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + long long unsigned int index_begin = (*buckets)[ii].index_begin; + long long unsigned int index_end = (*buckets)[ii].index_end; + if (index_end > index_begin) + { + (*buckets)[ii].number_of_points = index_end - index_begin; + } + } +} - std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) - { - long long unsigned int sequence_length = hc; - if (i + hc >= size) - { - sequence_length = size - i; - } - if (sequence_length == 0) - break; +void NDT::build_rgd( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + NDT::GridParameters& rgd_params, + int num_threads) +{ + if (num_threads < 1) + num_threads = 1; + + std::cout << "points.size(): " << points.size() << std::endl; + + index_pair.resize(points.size()); + reindex(points, index_pair, rgd_params, num_threads); + + buckets.resize(rgd_params.number_of_buckets); + + std::vector jobs = get_jobs(buckets.size(), num_threads); + std::vector threads; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + jobs = get_jobs(points.size(), num_threads); + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); + } + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + jobs = get_jobs(buckets.size(), num_threads); + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); +} - Job j; - j.index_begin_inclusive = i; - j.index_end_exclusive = i + sequence_length; - jobs.push_back(j); - } +void NDT::build_rgd( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + NDT::GridParameters& rgd_params, + int num_threads) +{ + if (num_threads < 1) + num_threads = 1; + + index_pair.resize(points.size()); + std::cout << "reindex start" << std::endl; + reindex(points, index_pair, rgd_params, num_threads); + std::cout << "reindex finished" << std::endl; + + buckets.resize(rgd_params.number_of_buckets); + + std::vector jobs = get_jobs(buckets.size(), num_threads); + std::vector threads; + + std::cout << "build_rgd_init_jobs start" << std::endl; + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + std::cout << "build_rgd_init_jobs finished" << std::endl; + + jobs = get_jobs(points.size(), num_threads); + + std::cout << "build_rgd_jobs start jobs.size():" << jobs.size() << std::endl; + std::cout << "points.size() " << points.size() << std::endl; + std::cout << "index_pair.size() " << index_pair.size() << std::endl; + std::cout << "buckets.size() " << buckets.size() << std::endl; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); + } + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + std::cout << "build_rgd_jobs finished" << std::endl; + + jobs = get_jobs(buckets.size(), num_threads); + + std::cout << "build_rgd_final_jobs start" << std::endl; + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + std::cout << "build_rgd_final_jobs finished" << std::endl; +} - return jobs; +std::vector get_points_normal_distribution( + const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, const int num_points = 100) +{ + Eigen::LLT> cholSolver(covar); + Eigen::Matrix3d transform = cholSolver.matrixL(); + std::random_device rd{}; + std::mt19937 gen{ rd() }; + std::vector res; + for (int i = 0; i < num_points; i++) + { + std::normal_distribution x{ 0.0, 1.0 }; + std::normal_distribution y{ 0.0, 1.0 }; + std::normal_distribution z{ 0.0, 1.0 }; + Eigen::Vector3d xyz(x(gen), y(gen), z(gen)); + res.emplace_back(transform * xyz + mean); + } + return res; } -void reindex_job(int i, NDT::Job *job, std::vector *points, std::vector *pairs, NDT::GridParameters rgd_params) +void ndt_job( + int i, + NDT::Job* job, + std::vector* buckets, + Eigen::SparseMatrix* AtPA, + Eigen::SparseMatrix* AtPB, + std::vector* index_pair_internal, + std::vector* pp, + std::vector* mposes, + std::vector* mposes_inv, + size_t trajectory_size, + NDT::PoseConvention pose_convention, + NDT::RotationMatrixParametrization rotation_matrix_parametrization, + int number_of_unknowns, + double* sumssr, + int* sums_obs, + bool is_generalized, + double sigma_r, + double sigma_polar_angle, + double sigma_azimuthal_angle, + int num_extended_points, + double* md_out, + double* md_count_out, + bool compute_only_mean_and_cov, + bool compute_mean_and_cov_for_bucket) { - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + Eigen::SparseMatrix AtPAt_tmp(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt_tmp(trajectory_size * number_of_unknowns, 1); + + double ssr = 0.0; + int sum_obs = 0; + bool init = true; + double md = 0.0; + double md_count = 0.0; + + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + NDT::Bucket& b = (*buckets)[ii]; + if (b.number_of_points < 5) + continue; + + Eigen::Vector3d mean(0, 0, 0); + Eigen::Matrix3d cov; + cov.setZero(); + + int counter = 0; + + const auto& p_ = (*pp)[(*index_pair_internal)[b.index_begin].index_of_point]; + int index_first_pose = p_.index_pose; + + for (int index = b.index_begin; index < b.index_end; index++) + { + if (is_generalized && compute_mean_and_cov_for_bucket) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + int index_pose = p.index_pose; + + if (index_pose == index_first_pose) + { + const auto& m = (*mposes)[p.index_pose]; + const auto& minv = (*mposes_inv)[p.index_pose]; + + Eigen::Vector3d point_local(p.x, p.y, p.z); + point_local = minv * point_local; + + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); + + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); + + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; + + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; + + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; + + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); + + for (const auto& ppl : points) + { + mean += m * ppl; + counter++; + } + } + } + else + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + mean += Eigen::Vector3d(p.x, p.y, p.z); + counter++; + } + } + + mean /= counter; + + counter = 0; + for (int index = b.index_begin; index < b.index_end; index++) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + const auto& m = (*mposes)[p.index_pose]; + const auto& minv = (*mposes_inv)[p.index_pose]; + + if (is_generalized && compute_mean_and_cov_for_bucket) + { + int index_pose = p.index_pose; + + if (index_pose == index_first_pose) + { + Eigen::Vector3d point_local(p.x, p.y, p.z); + point_local = minv * point_local; + + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); + + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); + + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; + + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; + + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; + + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); + + // for (const auto &ppp : points) + //{ + // std::cout << "---" << std::endl; + // std::cout << ppp << std::endl; + // } + // exit(1); + + for (const auto& pp : points) + { + auto ppg = m * pp; + + cov(0, 0) += (mean.x() - ppg.x()) * (mean.x() - ppg.x()); + cov(0, 1) += (mean.x() - ppg.x()) * (mean.y() - ppg.y()); + cov(0, 2) += (mean.x() - ppg.x()) * (mean.z() - ppg.z()); + cov(1, 0) += (mean.y() - ppg.y()) * (mean.x() - ppg.x()); + cov(1, 1) += (mean.y() - ppg.y()) * (mean.y() - ppg.y()); + cov(1, 2) += (mean.y() - ppg.y()) * (mean.z() - ppg.z()); + cov(2, 0) += (mean.z() - ppg.z()) * (mean.x() - ppg.x()); + cov(2, 1) += (mean.z() - ppg.z()) * (mean.y() - ppg.y()); + cov(2, 2) += (mean.z() - ppg.z()) * (mean.z() - ppg.z()); + counter++; + } + } + } + else + { + cov(0, 0) += (mean.x() - p.x) * (mean.x() - p.x); + cov(0, 1) += (mean.x() - p.x) * (mean.y() - p.y); + cov(0, 2) += (mean.x() - p.x) * (mean.z() - p.z); + cov(1, 0) += (mean.y() - p.y) * (mean.x() - p.x); + cov(1, 1) += (mean.y() - p.y) * (mean.y() - p.y); + cov(1, 2) += (mean.y() - p.y) * (mean.z() - p.z); + cov(2, 0) += (mean.z() - p.z) * (mean.x() - p.x); + cov(2, 1) += (mean.z() - p.z) * (mean.y() - p.y); + cov(2, 2) += (mean.z() - p.z) * (mean.z() - p.z); + counter++; + } + } + + if (counter < 5) + { + continue; + } + + if (is_generalized) + { + Eigen::SelfAdjointEigenSolver eigen_solver(cov, Eigen::ComputeEigenvectors); + // Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); + Eigen::Vector3d eigen_values = eigen_solver.eigenvalues(); + + double min_eigen_value = eigen_values.x(); + if (eigen_values.y() < min_eigen_value) + { + min_eigen_value = eigen_values.y(); + } + + if (eigen_values.z() < min_eigen_value) + { + min_eigen_value = eigen_values.z(); + } + + if (min_eigen_value > 3 * sigma_r) + { + continue; + } // else{ + // std::cout << min_eigen_value << " "; + //} + // std::cout << min_eigen_value << " "; + + Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); + Eigen::Vector3d nv = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2)); + nv.normalize(); + + const auto& p = (*pp)[(*index_pair_internal)[b.index_begin].index_of_point]; + const auto& m = (*mposes)[p.index_pose]; + + // flip towards viewport + if (nv.dot(m.translation() - mean) < 0.0) + { + nv *= -1.0; + } + // this_bucket.normal_vector = nv; + (*buckets)[ii].normal_vector = nv; + } + + cov /= counter; + + (*buckets)[ii].mean = mean; + (*buckets)[ii].cov = cov; + + if (compute_only_mean_and_cov) + { + continue; + } + + Eigen::Matrix3d infm = cov.inverse(); + + if (!(infm(0, 0) == infm(0, 0))) + continue; + if (!(infm(0, 1) == infm(0, 1))) + continue; + if (!(infm(0, 2) == infm(0, 2))) + continue; + + if (!(infm(1, 0) == infm(1, 0))) + continue; + if (!(infm(1, 1) == infm(1, 1))) + continue; + if (!(infm(1, 2) == infm(1, 2))) + continue; + + if (!(infm(2, 0) == infm(2, 0))) + continue; + if (!(infm(2, 1) == infm(2, 1))) + continue; + if (!(infm(2, 2) == infm(2, 2))) + continue; + + double threshold = 1000000.0; + + if (infm(0, 0) > threshold) + continue; + if (infm(0, 1) > threshold) + continue; + if (infm(0, 2) > threshold) + continue; + if (infm(1, 0) > threshold) + continue; + if (infm(1, 1) > threshold) + continue; + if (infm(1, 2) > threshold) + continue; + if (infm(2, 0) > threshold) + continue; + if (infm(2, 1) > threshold) + continue; + if (infm(2, 2) > threshold) + continue; + + if (infm(0, 0) < -threshold) + continue; + if (infm(0, 1) < -threshold) + continue; + if (infm(0, 2) < -threshold) + continue; + if (infm(1, 0) < -threshold) + continue; + if (infm(1, 1) < -threshold) + continue; + if (infm(1, 2) < -threshold) + continue; + if (infm(2, 0) < -threshold) + continue; + if (infm(2, 1) < -threshold) + continue; + if (infm(2, 2) < -threshold) + continue; + + for (int index = b.index_begin; index < b.index_end; index++) + { + std::vector points_local; + + if (is_generalized) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + const auto& m = (*mposes)[p.index_pose]; + const auto& minv = (*mposes_inv)[p.index_pose]; + + if ((*buckets)[ii].normal_vector.dot(m.translation() - (*buckets)[ii].mean) < 0.0) + { + continue; + } + + Eigen::Vector3d point_local(p.x, p.y, p.z); + point_local = minv * point_local; + + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); + + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); + + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; + + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; + + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; + + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); + + for (const auto& ppl : points) + { + points_local.push_back(ppl); + } + } + else + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + Eigen::Vector3d point_local(p.x, p.y, p.z); + point_local = (*mposes_inv)[p.index_pose] * point_local; + points_local.push_back(point_local); + } + + for (const auto& point_local : points_local) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + int ir = tripletListB.size(); + double delta_x; + double delta_y; + double delta_z; + + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); + md += sqrt((p_s - p_t).transpose() * infm * (p_s - p_t)); + md_count += 1.0; + + if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::tait_bryan_xyz) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) + { + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); + + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z()); + } + else + { + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose].inverse()); + + point_to_point_source_to_target_tait_bryan_cw( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_tait_bryan_cw_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + point_local.x(), + point_local.y(), + point_local.z()); + } + //----------------------- + int c = p.index_pose * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::rodrigues) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) + { + RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose]); + + point_to_point_source_to_target_rodrigues_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.sx, + pose_s.sy, + pose_s.sz, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_rodrigues_wc_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.sx, + pose_s.sy, + pose_s.sz, + point_local.x(), + point_local.y(), + point_local.z()); + } + else + { + RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose].inverse()); + + point_to_point_source_to_target_rodrigues_cw( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.sx, + pose_s.sy, + pose_s.sz, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_rodrigues_cw_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.sx, + pose_s.sy, + pose_s.sz, + point_local.x(), + point_local.y(), + point_local.z()); + } + //----------------------- + int c = p.index_pose * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::quaternion) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) + { + QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose]); + + point_to_point_source_to_target_quaternion_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.q0, + pose_s.q1, + pose_s.q2, + pose_s.q3, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_quaternion_wc_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.q0, + pose_s.q1, + pose_s.q2, + pose_s.q3, + point_local.x(), + point_local.y(), + point_local.z()); + } + else + { + QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose].inverse()); + + point_to_point_source_to_target_quaternion_cw( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.q0, + pose_s.q1, + pose_s.q2, + pose_s.q3, + point_local.x(), + point_local.y(), + point_local.z(), + mean.x(), + mean.y(), + mean.z()); + + point_to_point_source_to_target_quaternion_cw_jacobian( + jacobian, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.q0, + pose_s.q1, + pose_s.q2, + pose_s.q3, + point_local.x(), + point_local.y(), + point_local.z()); + } + //----------------------- + int c = p.index_pose * 7; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 7; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_left_jacobian) + { + Eigen::Matrix jacobian; + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; + + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); + + Eigen::Matrix3d R = m_pose.rotation(); + Eigen::Vector3d Rp = R * p_s; + Eigen::Matrix3d Rpx; + Rpx(0, 0) = 0; + Rpx(0, 1) = -Rp.z(); + Rpx(0, 2) = Rp.y(); + Rpx(1, 0) = Rp.z(); + Rpx(1, 1) = 0; + Rpx(1, 2) = -Rp.x(); + Rpx(2, 0) = -Rp.y(); + Rpx(2, 1) = Rp.x(); + Rpx(2, 2) = 0; + + int ic = p.index_pose * 6; + + tripletListA.emplace_back(ir, ic + 0, 1); + // tripletListA.emplace_back(ir, ic + 1, 0); + // tripletListA.emplace_back(ir, ic + 2, 0); + tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); + + // tripletListA.emplace_back(ir + 1, ic + 0, 0); + tripletListA.emplace_back(ir + 1, ic + 1, 1); + // tripletListA.emplace_back(ir + 1, ic + 2, 0); + tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); + + // tripletListA.emplace_back(ir + 2, ic + 0, 0); + // tripletListA.emplace_back(ir + 2, ic + 1, 0); + tripletListA.emplace_back(ir + 2, ic + 2, 1); + tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); + + Eigen::Vector3d target = p_t; + Eigen::Vector3d source = m_pose * p_s; + + delta_x = target.x() - source.x(); + delta_y = target.y() - source.y(); + delta_z = target.z() - source.z(); + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_right_jacobian) + { + Eigen::Matrix jacobian; + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; + + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); + + Eigen::Matrix3d px; + px(0, 0) = 0; + px(0, 1) = -p_s.z(); + px(0, 2) = p_s.y(); + px(1, 0) = p_s.z(); + px(1, 1) = 0; + px(1, 2) = -p_s.x(); + px(2, 0) = -p_s.y(); + px(2, 1) = p_s.x(); + px(2, 2) = 0; + Eigen::Matrix3d R = m_pose.rotation(); + Eigen::Matrix3d Rpx = R * px; + + int ic = p.index_pose * 6; + + tripletListA.emplace_back(ir, ic + 0, R(0, 0)); + tripletListA.emplace_back(ir, ic + 1, R(0, 1)); + tripletListA.emplace_back(ir, ic + 2, R(0, 2)); + tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); + + tripletListA.emplace_back(ir + 1, ic + 0, R(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 1, R(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 2, R(1, 2)); + tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); + + tripletListA.emplace_back(ir + 2, ic + 0, R(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 1, R(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 2, R(2, 2)); + tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); + + Eigen::Vector3d target = p_t; + Eigen::Vector3d source = m_pose * p_s; + + delta_x = target.x() - source.x(); + delta_y = target.y() - source.y(); + delta_z = target.z() - source.z(); + } + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); + + ssr += delta_x * delta_x; + ssr += delta_y * delta_y; + ssr += delta_z * delta_z; + sum_obs += 3; + } + + if (tripletListB.size() > 100000) + { + Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPAt = AtP * matA; + AtPBt = AtP * matB; + + if (init) + { + AtPAt_tmp = AtPAt; + AtPBt_tmp = AtPBt; + init = false; + } + else + { + AtPAt_tmp += AtPAt; + AtPBt_tmp += AtPBt; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + } + } + } + (*sumssr) = ssr; + (*sums_obs) = sum_obs; + (*md_out) = md; + (*md_count_out) = md_count; + + // std::cout << md << " "; + + if (tripletListB.size() > 0) + { + Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPAt = AtP * matA; + AtPBt = AtP * matB; + + if (init) + { + (*AtPA) = AtPAt; + (*AtPB) = AtPBt; + } + else + { + AtPAt_tmp += AtPAt; + AtPBt_tmp += AtPBt; + + (*AtPA) = AtPAt_tmp; + (*AtPB) = AtPBt_tmp; + } + // std::cout << AtPAt << std::endl; + } + } + else + { + (*AtPA) = AtPAt_tmp; + (*AtPB) = AtPBt_tmp; + } +} - Point3D &p = (*points)[ii]; +void ndt_jobi( + int i, + NDT::Job* job, + std::vector* buckets, + Eigen::SparseMatrix* AtPA, + Eigen::SparseMatrix* AtPB, + std::vector* index_pair_internal, + std::vector* pp, + std::vector* mposes, + std::vector* mposes_inv, + size_t trajectory_size, + NDT::PoseConvention pose_convention, + NDT::RotationMatrixParametrization rotation_matrix_parametrization, + int number_of_unknowns, + double* sumssr, + int* sums_obs, + bool is_generalized, + double sigma_r, + double sigma_polar_angle, + double sigma_azimuthal_angle, + int num_extended_points, + double* md_out, + double* md_count_out, + bool compute_only_mean_and_cov) +{ + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + Eigen::SparseMatrix AtPAt_tmp(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt_tmp(trajectory_size * number_of_unknowns, 1); + + double ssr = 0.0; + int sum_obs = 0; + bool init = true; + double md = 0.0; + double md_count = 0.0; + + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + // std::cout << ii << " "; + NDT::Bucket& b = (*buckets)[ii]; + if (b.number_of_points < 5) + { + continue; + } + + Eigen::Vector3d mean(0, 0, 0); + Eigen::Matrix3d cov; + cov.setZero(); + + int counter = 0; + for (int index = b.index_begin; index < b.index_end; index++) + { + if (is_generalized) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + const auto& m = (*mposes)[p.index_pose]; + const auto& minv = (*mposes_inv)[p.index_pose]; + + // Eigen::Vector3d point_local(p.x, p.y, p.z); + Eigen::Vector3d point_local = p.point; + point_local = minv * point_local; + + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); + + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); + + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; + + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; + + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; + + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); + + for (const auto& ppl : points) + { + mean += m * ppl; + counter++; + } + } + else + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + mean += p.point; //::Vector3d(p.x, p.y, p.z); + counter++; + // std::cout << counter << " "; + } + } + + mean /= counter; + + counter = 0; + for (int index = b.index_begin; index < b.index_end; index++) + { + const auto& p = (*pp)[(*index_pair_internal)[index].index_of_point]; + const auto& m = (*mposes)[p.index_pose]; + const auto& minv = (*mposes_inv)[p.index_pose]; + + if (is_generalized) + { + Eigen::Vector3d point_local = p.point; //(p.x, p.y, p.z); + point_local = minv * point_local; + + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); + + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); + + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; + + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; + + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; + + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); + + for (const auto& pp : points) + { + auto ppg = m * pp; + + cov(0, 0) += (mean.x() - ppg.x()) * (mean.x() - ppg.x()); + cov(0, 1) += (mean.x() - ppg.x()) * (mean.y() - ppg.y()); + cov(0, 2) += (mean.x() - ppg.x()) * (mean.z() - ppg.z()); + cov(1, 0) += (mean.y() - ppg.y()) * (mean.x() - ppg.x()); + cov(1, 1) += (mean.y() - ppg.y()) * (mean.y() - ppg.y()); + cov(1, 2) += (mean.y() - ppg.y()) * (mean.z() - ppg.z()); + cov(2, 0) += (mean.z() - ppg.z()) * (mean.x() - ppg.x()); + cov(2, 1) += (mean.z() - ppg.z()) * (mean.y() - ppg.y()); + cov(2, 2) += (mean.z() - ppg.z()) * (mean.z() - ppg.z()); + counter++; + } + } + else + { + cov(0, 0) += (mean.x() - p.point.x()) * (mean.x() - p.point.x()); + cov(0, 1) += (mean.x() - p.point.x()) * (mean.y() - p.point.y()); + cov(0, 2) += (mean.x() - p.point.x()) * (mean.z() - p.point.z()); + cov(1, 0) += (mean.y() - p.point.y()) * (mean.x() - p.point.x()); + cov(1, 1) += (mean.y() - p.point.y()) * (mean.y() - p.point.y()); + cov(1, 2) += (mean.y() - p.point.y()) * (mean.z() - p.point.z()); + cov(2, 0) += (mean.z() - p.point.z()) * (mean.x() - p.point.x()); + cov(2, 1) += (mean.z() - p.point.z()) * (mean.y() - p.point.y()); + cov(2, 2) += (mean.z() - p.point.z()) * (mean.z() - p.point.z()); + counter++; + } + } + cov /= counter; + + (*buckets)[ii].mean = mean; + (*buckets)[ii].cov = cov; + + // std::cout << mean << " "; +#if 0 + if(compute_only_mean_and_cov){ + continue; + } - (*pairs)[ii].index_of_point = ii; - (*pairs)[ii].index_of_bucket = 0; - (*pairs)[ii].index_pose = p.index_pose; + Eigen::Matrix3d infm = cov.inverse(); - if (p.x < rgd_params.bounding_box_min_X) - { + if (!(infm(0, 0) == infm(0, 0))) continue; - } - if (p.x > rgd_params.bounding_box_max_X) - { + if (!(infm(0, 1) == infm(0, 1))) continue; - } - if (p.y < rgd_params.bounding_box_min_Y) - { + if (!(infm(0, 2) == infm(0, 2))) continue; - } - if (p.y > rgd_params.bounding_box_max_Y) - { + + if (!(infm(1, 0) == infm(1, 0))) continue; - } - if (p.z < rgd_params.bounding_box_min_Z) - { + if (!(infm(1, 1) == infm(1, 1))) continue; - } - if (p.z > rgd_params.bounding_box_max_Z) - { + if (!(infm(1, 2) == infm(1, 2))) continue; - } - - long long unsigned int ix = (p.x - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; - } -} - -void reindex_jobi(int i, NDT::Job *job, std::vector *points, std::vector *pairs, NDT::GridParameters rgd_params) -{ - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - Point3Di &p = (*points)[ii]; + if (!(infm(2, 0) == infm(2, 0))) + continue; + if (!(infm(2, 1) == infm(2, 1))) + continue; + if (!(infm(2, 2) == infm(2, 2))) + continue; - (*pairs)[ii].index_of_point = ii; - (*pairs)[ii].index_of_bucket = 0; - (*pairs)[ii].index_pose = p.index_pose; + double threshold = 10000.0; - if (p.point.x() < rgd_params.bounding_box_min_X) - { + if (infm(0, 0) > threshold) continue; - } - if (p.point.x() > rgd_params.bounding_box_max_X) - { + if (infm(0, 1) > threshold) continue; - } - if (p.point.y() < rgd_params.bounding_box_min_Y) - { + if (infm(0, 2) > threshold) continue; - } - if (p.point.y() > rgd_params.bounding_box_max_Y) - { + if (infm(1, 0) > threshold) continue; - } - if (p.point.z() < rgd_params.bounding_box_min_Z) - { + if (infm(1, 1) > threshold) continue; - } - if (p.point.z() > rgd_params.bounding_box_max_Z) - { + if (infm(1, 2) > threshold) + continue; + if (infm(2, 0) > threshold) + continue; + if (infm(2, 1) > threshold) + continue; + if (infm(2, 2) > threshold) continue; - } - - long long unsigned int ix = (p.point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; - } -} - -void NDT::reindex(std::vector &points, std::vector &index_pair, NDT::GridParameters &rgd_params, int num_threads) -{ - index_pair.resize(points.size()); - - std::vector jobs = get_jobs(index_pair.size(), num_threads); - - std::vector threads; - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(reindex_job, i, &jobs[i], &points, &index_pair, rgd_params)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - std::sort(index_pair.begin(), index_pair.end(), [](const NDT::PointBucketIndexPair &a, const NDT::PointBucketIndexPair &b) - { return ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); }); -} + if (infm(0, 0) < -threshold) + continue; + if (infm(0, 1) < -threshold) + continue; + if (infm(0, 2) < -threshold) + continue; + if (infm(1, 0) < -threshold) + continue; + if (infm(1, 1) < -threshold) + continue; + if (infm(1, 2) < -threshold) + continue; + if (infm(2, 0) < -threshold) + continue; + if (infm(2, 1) < -threshold) + continue; + if (infm(2, 2) < -threshold) + continue; -void NDT::reindex(std::vector &points, std::vector &index_pair, NDT::GridParameters &rgd_params, int num_threads) -{ - index_pair.resize(points.size()); + for (int index = b.index_begin; index < b.index_end; index++) + { + std::vector points_local; - std::vector jobs = get_jobs(index_pair.size(), num_threads); + if (is_generalized) + { + const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; + const auto &m = (*mposes)[p.index_pose]; + const auto &minv = (*mposes_inv)[p.index_pose]; - std::vector threads; + Eigen::Vector3d point_local = p.point;//(p.x, p.y, p.z); + point_local = minv * point_local; - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(reindex_jobi, i, &jobs[i], &points, &index_pair, rgd_params)); - } + double norm = point_local.norm(); + double r = norm; + double polar_angle = acos(point_local.z() / norm); + double azimuthal_angle = atan(point_local.y() / point_local.x()); - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); + Eigen::Matrix j; + elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - std::sort(index_pair.begin(), index_pair.end(), [](const NDT::PointBucketIndexPair &a, const NDT::PointBucketIndexPair &b) - { return ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); }); -} + Eigen::Matrix cov_r_alpha_theta; + cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; + cov_r_alpha_theta(0, 1) = 0.0; + cov_r_alpha_theta(0, 2) = 0.0; -void build_rgd_init_job(int i, NDT::Job *job, std::vector *buckets) -{ + cov_r_alpha_theta(1, 0) = 0.0; + cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; + cov_r_alpha_theta(1, 2) = 0.0; - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - (*buckets)[ii].index_begin = 0; - (*buckets)[ii].index_end = 0; - (*buckets)[ii].number_of_points = 0; - } -} + cov_r_alpha_theta(2, 0) = 0.0; + cov_r_alpha_theta(2, 1) = 0.0; + cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; -void build_rgd_job(int i, NDT::Job *job, std::vector *index_pair, std::vector *buckets) -{ - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - int ind = ii; - if (ind == 0) - { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); + std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - (*buckets)[index_of_bucket].index_begin = ind; - if (index_of_bucket != index_of_bucket_1) - { - (*buckets)[index_of_bucket].index_end = ind + 1; - (*buckets)[index_of_bucket_1].index_end = ind + 1; + for (const auto &ppl : points) + { + points_local.push_back(ppl); + } } - } - else if (ind == (*index_pair).size() - 1) - { - if ((*index_pair)[ind].index_of_bucket < (*buckets).size()) + else { - (*buckets)[(*index_pair)[ind].index_of_bucket].index_end = ind + 1; + const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; + Eigen::Vector3d point_local = p.point;//(p.x, p.y, p.z); + point_local = (*mposes_inv)[p.index_pose] * point_local; + points_local.push_back(point_local); } - } - else if (ind + 1 < (*index_pair).size()) - { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; - if (index_of_bucket != index_of_bucket_1) + for (const auto &point_local : points_local) { - (*buckets)[index_of_bucket].index_end = ind + 1; - (*buckets)[index_of_bucket_1].index_begin = ind + 1; - } - } - } -} - -void build_rgd_final_job(int i, NDT::Job *job, std::vector *buckets) -{ - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - long long unsigned int index_begin = (*buckets)[ii].index_begin; - long long unsigned int index_end = (*buckets)[ii].index_end; - if (index_end > index_begin) - { - (*buckets)[ii].number_of_points = index_end - index_begin; - } - } -} - -void NDT::build_rgd(std::vector &points, std::vector &index_pair, std::vector &buckets, NDT::GridParameters &rgd_params, int num_threads) -{ - if (num_threads < 1) - num_threads = 1; - - std::cout << "points.size(): " << points.size() << std::endl; + const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; + int ir = tripletListB.size(); + double delta_x; + double delta_y; + double delta_z; - index_pair.resize(points.size()); - reindex(points, index_pair, rgd_params, num_threads); + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); + md += sqrt((p_s - p_t).transpose() * infm * (p_s - p_t)); + md_count += 1.0; - buckets.resize(rgd_params.number_of_buckets); + if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::tait_bryan_xyz) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) + { + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - std::vector jobs = get_jobs(buckets.size(), num_threads); - std::vector threads; + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - - jobs = get_jobs(points.size(), num_threads); - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); - } - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - - jobs = get_jobs(buckets.size(), num_threads); - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); -} - -void NDT::build_rgd(std::vector &points, std::vector &index_pair, std::vector &buckets, NDT::GridParameters &rgd_params, int num_threads) -{ - if (num_threads < 1) - num_threads = 1; - - index_pair.resize(points.size()); - std::cout << "reindex start" << std::endl; - reindex(points, index_pair, rgd_params, num_threads); - std::cout << "reindex finished" << std::endl; - - buckets.resize(rgd_params.number_of_buckets); - - std::vector jobs = get_jobs(buckets.size(), num_threads); - std::vector threads; - - std::cout << "build_rgd_init_jobs start" << std::endl; - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - std::cout << "build_rgd_init_jobs finished" << std::endl; - - jobs = get_jobs(points.size(), num_threads); - - std::cout << "build_rgd_jobs start jobs.size():" << jobs.size() << std::endl; - std::cout << "points.size() " << points.size() << std::endl; - std::cout << "index_pair.size() " << index_pair.size() << std::endl; - std::cout << "buckets.size() " << buckets.size() << std::endl; - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); - } - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - std::cout << "build_rgd_jobs finished" << std::endl; - - jobs = get_jobs(buckets.size(), num_threads); - - std::cout << "build_rgd_final_jobs start" << std::endl; - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - std::cout << "build_rgd_final_jobs finished" << std::endl; -} - -std::vector get_points_normal_distribution(const Eigen::Matrix3d &covar, const Eigen::Vector3d &mean, const int num_points = 100) -{ - Eigen::LLT> cholSolver(covar); - Eigen::Matrix3d transform = cholSolver.matrixL(); - std::random_device rd{}; - std::mt19937 gen{rd()}; - std::vector res; - for (int i = 0; i < num_points; i++) - { - std::normal_distribution x{0.0, 1.0}; - std::normal_distribution y{0.0, 1.0}; - std::normal_distribution z{0.0, 1.0}; - Eigen::Vector3d xyz(x(gen), y(gen), z(gen)); - res.emplace_back(transform * xyz + mean); - } - return res; -} - -void ndt_job(int i, NDT::Job *job, std::vector *buckets, Eigen::SparseMatrix *AtPA, - Eigen::SparseMatrix *AtPB, std::vector *index_pair_internal, std::vector *pp, - std::vector *mposes, std::vector *mposes_inv, size_t trajectory_size, - NDT::PoseConvention pose_convention, NDT::RotationMatrixParametrization rotation_matrix_parametrization, int number_of_unknowns, double *sumssr, int *sums_obs, - bool is_generalized, double sigma_r, double sigma_polar_angle, double sigma_azimuthal_angle, int num_extended_points, double *md_out, double *md_count_out, - bool compute_only_mean_and_cov, bool compute_mean_and_cov_for_bucket) -{ - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - Eigen::SparseMatrix AtPAt_tmp(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt_tmp(trajectory_size * number_of_unknowns, 1); - - double ssr = 0.0; - int sum_obs = 0; - bool init = true; - double md = 0.0; - double md_count = 0.0; - - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - NDT::Bucket &b = (*buckets)[ii]; - if (b.number_of_points < 5) - continue; - - Eigen::Vector3d mean(0, 0, 0); - Eigen::Matrix3d cov; - cov.setZero(); - - int counter = 0; - - const auto &p_ = (*pp)[(*index_pair_internal)[b.index_begin].index_of_point]; - int index_first_pose = p_.index_pose; - - for (int index = b.index_begin; index < b.index_end; index++) - { - if (is_generalized && compute_mean_and_cov_for_bucket) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - int index_pose = p.index_pose; - - if (index_pose == index_first_pose) - { - - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - Eigen::Vector3d point_local(p.x, p.y, p.z); - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - for (const auto &ppl : points) - { - mean += m * ppl; - counter++; - } - } - } - else - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - mean += Eigen::Vector3d(p.x, p.y, p.z); - counter++; - } - } - - mean /= counter; - - counter = 0; - for (int index = b.index_begin; index < b.index_end; index++) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - if (is_generalized && compute_mean_and_cov_for_bucket) - { - int index_pose = p.index_pose; - - if (index_pose == index_first_pose) - { - - Eigen::Vector3d point_local(p.x, p.y, p.z); - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - // for (const auto &ppp : points) - //{ - // std::cout << "---" << std::endl; - // std::cout << ppp << std::endl; - // } - // exit(1); - - for (const auto &pp : points) - { - auto ppg = m * pp; - - cov(0, 0) += (mean.x() - ppg.x()) * (mean.x() - ppg.x()); - cov(0, 1) += (mean.x() - ppg.x()) * (mean.y() - ppg.y()); - cov(0, 2) += (mean.x() - ppg.x()) * (mean.z() - ppg.z()); - cov(1, 0) += (mean.y() - ppg.y()) * (mean.x() - ppg.x()); - cov(1, 1) += (mean.y() - ppg.y()) * (mean.y() - ppg.y()); - cov(1, 2) += (mean.y() - ppg.y()) * (mean.z() - ppg.z()); - cov(2, 0) += (mean.z() - ppg.z()) * (mean.x() - ppg.x()); - cov(2, 1) += (mean.z() - ppg.z()) * (mean.y() - ppg.y()); - cov(2, 2) += (mean.z() - ppg.z()) * (mean.z() - ppg.z()); - counter++; - } - } - } - else - { - cov(0, 0) += (mean.x() - p.x) * (mean.x() - p.x); - cov(0, 1) += (mean.x() - p.x) * (mean.y() - p.y); - cov(0, 2) += (mean.x() - p.x) * (mean.z() - p.z); - cov(1, 0) += (mean.y() - p.y) * (mean.x() - p.x); - cov(1, 1) += (mean.y() - p.y) * (mean.y() - p.y); - cov(1, 2) += (mean.y() - p.y) * (mean.z() - p.z); - cov(2, 0) += (mean.z() - p.z) * (mean.x() - p.x); - cov(2, 1) += (mean.z() - p.z) * (mean.y() - p.y); - cov(2, 2) += (mean.z() - p.z) * (mean.z() - p.z); - counter++; - } - } - - if (counter < 5) - { - continue; - } - - if (is_generalized) - { - Eigen::SelfAdjointEigenSolver eigen_solver(cov, Eigen::ComputeEigenvectors); - // Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); - Eigen::Vector3d eigen_values = eigen_solver.eigenvalues(); - - double min_eigen_value = eigen_values.x(); - if (eigen_values.y() < min_eigen_value) - { - min_eigen_value = eigen_values.y(); - } - - if (eigen_values.z() < min_eigen_value) - { - min_eigen_value = eigen_values.z(); - } - - if (min_eigen_value > 3 * sigma_r) - { - continue; - } // else{ - // std::cout << min_eigen_value << " "; - //} - // std::cout << min_eigen_value << " "; - - Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors(); - Eigen::Vector3d nv = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2)); - nv.normalize(); - - const auto &p = (*pp)[(*index_pair_internal)[b.index_begin].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - - // flip towards viewport - if (nv.dot(m.translation() - mean) < 0.0) - { - nv *= -1.0; - } - // this_bucket.normal_vector = nv; - (*buckets)[ii].normal_vector = nv; - } - - cov /= counter; - - (*buckets)[ii].mean = mean; - (*buckets)[ii].cov = cov; - - if (compute_only_mean_and_cov) - { - continue; - } - - Eigen::Matrix3d infm = cov.inverse(); - - if (!(infm(0, 0) == infm(0, 0))) - continue; - if (!(infm(0, 1) == infm(0, 1))) - continue; - if (!(infm(0, 2) == infm(0, 2))) - continue; - - if (!(infm(1, 0) == infm(1, 0))) - continue; - if (!(infm(1, 1) == infm(1, 1))) - continue; - if (!(infm(1, 2) == infm(1, 2))) - continue; - - if (!(infm(2, 0) == infm(2, 0))) - continue; - if (!(infm(2, 1) == infm(2, 1))) - continue; - if (!(infm(2, 2) == infm(2, 2))) - continue; - - double threshold = 1000000.0; - - if (infm(0, 0) > threshold) - continue; - if (infm(0, 1) > threshold) - continue; - if (infm(0, 2) > threshold) - continue; - if (infm(1, 0) > threshold) - continue; - if (infm(1, 1) > threshold) - continue; - if (infm(1, 2) > threshold) - continue; - if (infm(2, 0) > threshold) - continue; - if (infm(2, 1) > threshold) - continue; - if (infm(2, 2) > threshold) - continue; - - if (infm(0, 0) < -threshold) - continue; - if (infm(0, 1) < -threshold) - continue; - if (infm(0, 2) < -threshold) - continue; - if (infm(1, 0) < -threshold) - continue; - if (infm(1, 1) < -threshold) - continue; - if (infm(1, 2) < -threshold) - continue; - if (infm(2, 0) < -threshold) - continue; - if (infm(2, 1) < -threshold) - continue; - if (infm(2, 2) < -threshold) - continue; - - for (int index = b.index_begin; index < b.index_end; index++) - { - std::vector points_local; - - if (is_generalized) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - if ((*buckets)[ii].normal_vector.dot(m.translation() - (*buckets)[ii].mean) < 0.0) - { - continue; - } - - Eigen::Vector3d point_local(p.x, p.y, p.z); - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - for (const auto &ppl : points) - { - points_local.push_back(ppl); - } - } - else - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - Eigen::Vector3d point_local(p.x, p.y, p.z); - point_local = (*mposes_inv)[p.index_pose] * point_local; - points_local.push_back(point_local); - } - - for (const auto &point_local : points_local) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - int ir = tripletListB.size(); - double delta_x; - double delta_y; - double delta_z; - - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - md += sqrt((p_s - p_t).transpose() * infm * (p_s - p_t)); - md_count += 1.0; - - if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::tait_bryan_xyz) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_tait_bryan_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 6; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 6; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::rodrigues) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_rodrigues_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_rodrigues_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_rodrigues_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_rodrigues_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 6; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 6; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::quaternion) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_quaternion_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_quaternion_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_quaternion_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_quaternion_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 7; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 7; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_left_jacobian) - { - Eigen::Matrix jacobian; - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - - Eigen::Matrix3d R = m_pose.rotation(); - Eigen::Vector3d Rp = R * p_s; - Eigen::Matrix3d Rpx; - Rpx(0, 0) = 0; - Rpx(0, 1) = -Rp.z(); - Rpx(0, 2) = Rp.y(); - Rpx(1, 0) = Rp.z(); - Rpx(1, 1) = 0; - Rpx(1, 2) = -Rp.x(); - Rpx(2, 0) = -Rp.y(); - Rpx(2, 1) = Rp.x(); - Rpx(2, 2) = 0; - - int ic = p.index_pose * 6; - - tripletListA.emplace_back(ir, ic + 0, 1); - // tripletListA.emplace_back(ir, ic + 1, 0); - // tripletListA.emplace_back(ir, ic + 2, 0); - tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - - // tripletListA.emplace_back(ir + 1, ic + 0, 0); - tripletListA.emplace_back(ir + 1, ic + 1, 1); - // tripletListA.emplace_back(ir + 1, ic + 2, 0); - tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - - // tripletListA.emplace_back(ir + 2, ic + 0, 0); - // tripletListA.emplace_back(ir + 2, ic + 1, 0); - tripletListA.emplace_back(ir + 2, ic + 2, 1); - tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - - Eigen::Vector3d target = p_t; - Eigen::Vector3d source = m_pose * p_s; - - delta_x = target.x() - source.x(); - delta_y = target.y() - source.y(); - delta_z = target.z() - source.z(); - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_right_jacobian) - { - Eigen::Matrix jacobian; - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - - Eigen::Matrix3d px; - px(0, 0) = 0; - px(0, 1) = -p_s.z(); - px(0, 2) = p_s.y(); - px(1, 0) = p_s.z(); - px(1, 1) = 0; - px(1, 2) = -p_s.x(); - px(2, 0) = -p_s.y(); - px(2, 1) = p_s.x(); - px(2, 2) = 0; - Eigen::Matrix3d R = m_pose.rotation(); - Eigen::Matrix3d Rpx = R * px; - - int ic = p.index_pose * 6; - - tripletListA.emplace_back(ir, ic + 0, R(0, 0)); - tripletListA.emplace_back(ir, ic + 1, R(0, 1)); - tripletListA.emplace_back(ir, ic + 2, R(0, 2)); - tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - - tripletListA.emplace_back(ir + 1, ic + 0, R(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 1, R(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 2, R(1, 2)); - tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - - tripletListA.emplace_back(ir + 2, ic + 0, R(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 1, R(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 2, R(2, 2)); - tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - - Eigen::Vector3d target = p_t; - Eigen::Vector3d source = m_pose * p_s; - - delta_x = target.x() - source.x(); - delta_y = target.y() - source.y(); - delta_z = target.z() - source.z(); - } - - tripletListB.emplace_back(ir, 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - - tripletListP.emplace_back(ir, ir, infm(0, 0)); - tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); - tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); - tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); - tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); - tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); - tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); - tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); - tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); - - ssr += delta_x * delta_x; - ssr += delta_y * delta_y; - ssr += delta_z * delta_z; - sum_obs += 3; - } - - if (tripletListB.size() > 100000) - { - Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPAt = AtP * matA; - AtPBt = AtP * matB; - - if (init) - { - AtPAt_tmp = AtPAt; - AtPBt_tmp = AtPBt; - init = false; - } - else - { - AtPAt_tmp += AtPAt; - AtPBt_tmp += AtPBt; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - } - } - } - (*sumssr) = ssr; - (*sums_obs) = sum_obs; - (*md_out) = md; - (*md_count_out) = md_count; - - // std::cout << md << " "; - - if (tripletListB.size() > 0) - { - Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPAt = AtP * matA; - AtPBt = AtP * matB; - - if (init) - { - (*AtPA) = AtPAt; - (*AtPB) = AtPBt; - } - else - { - AtPAt_tmp += AtPAt; - AtPBt_tmp += AtPBt; - - (*AtPA) = AtPAt_tmp; - (*AtPB) = AtPBt_tmp; - } - // std::cout << AtPAt << std::endl; - } - } - else - { - (*AtPA) = AtPAt_tmp; - (*AtPB) = AtPBt_tmp; - } -} - -void ndt_jobi(int i, NDT::Job *job, std::vector *buckets, Eigen::SparseMatrix *AtPA, - Eigen::SparseMatrix *AtPB, std::vector *index_pair_internal, std::vector *pp, - std::vector *mposes, std::vector *mposes_inv, size_t trajectory_size, - NDT::PoseConvention pose_convention, NDT::RotationMatrixParametrization rotation_matrix_parametrization, int number_of_unknowns, double *sumssr, int *sums_obs, - bool is_generalized, double sigma_r, double sigma_polar_angle, double sigma_azimuthal_angle, int num_extended_points, double *md_out, double *md_count_out, - bool compute_only_mean_and_cov) -{ - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - Eigen::SparseMatrix AtPAt_tmp(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt_tmp(trajectory_size * number_of_unknowns, 1); - - double ssr = 0.0; - int sum_obs = 0; - bool init = true; - double md = 0.0; - double md_count = 0.0; - - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - // std::cout << ii << " "; - NDT::Bucket &b = (*buckets)[ii]; - if (b.number_of_points < 5) - { - continue; - } - - Eigen::Vector3d mean(0, 0, 0); - Eigen::Matrix3d cov; - cov.setZero(); - - int counter = 0; - for (int index = b.index_begin; index < b.index_end; index++) - { - if (is_generalized) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - // Eigen::Vector3d point_local(p.x, p.y, p.z); - Eigen::Vector3d point_local = p.point; - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - for (const auto &ppl : points) - { - mean += m * ppl; - counter++; - } - } - else - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - mean += p.point; //::Vector3d(p.x, p.y, p.z); - counter++; - // std::cout << counter << " "; - } - } - - mean /= counter; - - counter = 0; - for (int index = b.index_begin; index < b.index_end; index++) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - if (is_generalized) - { - Eigen::Vector3d point_local = p.point; //(p.x, p.y, p.z); - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - for (const auto &pp : points) - { - auto ppg = m * pp; - - cov(0, 0) += (mean.x() - ppg.x()) * (mean.x() - ppg.x()); - cov(0, 1) += (mean.x() - ppg.x()) * (mean.y() - ppg.y()); - cov(0, 2) += (mean.x() - ppg.x()) * (mean.z() - ppg.z()); - cov(1, 0) += (mean.y() - ppg.y()) * (mean.x() - ppg.x()); - cov(1, 1) += (mean.y() - ppg.y()) * (mean.y() - ppg.y()); - cov(1, 2) += (mean.y() - ppg.y()) * (mean.z() - ppg.z()); - cov(2, 0) += (mean.z() - ppg.z()) * (mean.x() - ppg.x()); - cov(2, 1) += (mean.z() - ppg.z()) * (mean.y() - ppg.y()); - cov(2, 2) += (mean.z() - ppg.z()) * (mean.z() - ppg.z()); - counter++; - } - } - else - { - cov(0, 0) += (mean.x() - p.point.x()) * (mean.x() - p.point.x()); - cov(0, 1) += (mean.x() - p.point.x()) * (mean.y() - p.point.y()); - cov(0, 2) += (mean.x() - p.point.x()) * (mean.z() - p.point.z()); - cov(1, 0) += (mean.y() - p.point.y()) * (mean.x() - p.point.x()); - cov(1, 1) += (mean.y() - p.point.y()) * (mean.y() - p.point.y()); - cov(1, 2) += (mean.y() - p.point.y()) * (mean.z() - p.point.z()); - cov(2, 0) += (mean.z() - p.point.z()) * (mean.x() - p.point.x()); - cov(2, 1) += (mean.z() - p.point.z()) * (mean.y() - p.point.y()); - cov(2, 2) += (mean.z() - p.point.z()) * (mean.z() - p.point.z()); - counter++; - } - } - cov /= counter; - - (*buckets)[ii].mean = mean; - (*buckets)[ii].cov = cov; - - // std::cout << mean << " "; -#if 0 - if(compute_only_mean_and_cov){ - continue; - } - - Eigen::Matrix3d infm = cov.inverse(); - - if (!(infm(0, 0) == infm(0, 0))) - continue; - if (!(infm(0, 1) == infm(0, 1))) - continue; - if (!(infm(0, 2) == infm(0, 2))) - continue; - - if (!(infm(1, 0) == infm(1, 0))) - continue; - if (!(infm(1, 1) == infm(1, 1))) - continue; - if (!(infm(1, 2) == infm(1, 2))) - continue; - - if (!(infm(2, 0) == infm(2, 0))) - continue; - if (!(infm(2, 1) == infm(2, 1))) - continue; - if (!(infm(2, 2) == infm(2, 2))) - continue; - - double threshold = 10000.0; - - if (infm(0, 0) > threshold) - continue; - if (infm(0, 1) > threshold) - continue; - if (infm(0, 2) > threshold) - continue; - if (infm(1, 0) > threshold) - continue; - if (infm(1, 1) > threshold) - continue; - if (infm(1, 2) > threshold) - continue; - if (infm(2, 0) > threshold) - continue; - if (infm(2, 1) > threshold) - continue; - if (infm(2, 2) > threshold) - continue; - - if (infm(0, 0) < -threshold) - continue; - if (infm(0, 1) < -threshold) - continue; - if (infm(0, 2) < -threshold) - continue; - if (infm(1, 0) < -threshold) - continue; - if (infm(1, 1) < -threshold) - continue; - if (infm(1, 2) < -threshold) - continue; - if (infm(2, 0) < -threshold) - continue; - if (infm(2, 1) < -threshold) - continue; - if (infm(2, 2) < -threshold) - continue; - - for (int index = b.index_begin; index < b.index_end; index++) - { - std::vector points_local; - - if (is_generalized) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - const auto &m = (*mposes)[p.index_pose]; - const auto &minv = (*mposes_inv)[p.index_pose]; - - Eigen::Vector3d point_local = p.point;//(p.x, p.y, p.z); - point_local = minv * point_local; - - double norm = point_local.norm(); - double r = norm; - double polar_angle = acos(point_local.z() / norm); - double azimuthal_angle = atan(point_local.y() / point_local.x()); - - Eigen::Matrix j; - elementary_error_theory_for_terrestrial_laser_scanner_jacobian(j, r, polar_angle, azimuthal_angle); - - Eigen::Matrix cov_r_alpha_theta; - cov_r_alpha_theta(0, 0) = sigma_r * sigma_r; - cov_r_alpha_theta(0, 1) = 0.0; - cov_r_alpha_theta(0, 2) = 0.0; - - cov_r_alpha_theta(1, 0) = 0.0; - cov_r_alpha_theta(1, 1) = sigma_polar_angle * sigma_polar_angle; - cov_r_alpha_theta(1, 2) = 0.0; - - cov_r_alpha_theta(2, 0) = 0.0; - cov_r_alpha_theta(2, 1) = 0.0; - cov_r_alpha_theta(2, 2) = sigma_azimuthal_angle * sigma_azimuthal_angle; - - Eigen::Matrix cov_xyz = j * cov_r_alpha_theta * j.transpose(); - std::vector points = get_points_normal_distribution(cov_xyz, point_local, num_extended_points); - - for (const auto &ppl : points) - { - points_local.push_back(ppl); - } - } - else - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - Eigen::Vector3d point_local = p.point;//(p.x, p.y, p.z); - point_local = (*mposes_inv)[p.index_pose] * point_local; - points_local.push_back(point_local); - } - - for (const auto &point_local : points_local) - { - const auto &p = (*pp)[(*index_pair_internal)[index].index_of_point]; - int ir = tripletListB.size(); - double delta_x; - double delta_y; - double delta_z; - - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - md += sqrt((p_s - p_t).transpose() * infm * (p_s - p_t)); - md_count += 1.0; - - if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::tait_bryan_xyz) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_tait_bryan_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_tait_bryan_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 6; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 6; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::rodrigues) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_rodrigues_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_rodrigues_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_rodrigues_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_rodrigues_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 6; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 6; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::quaternion) - { - Eigen::Matrix jacobian; - //----------------------- - if (pose_convention == NDT::PoseConvention::wc) - { - QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose]); - - point_to_point_source_to_target_quaternion_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_quaternion_wc_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z()); - } - else - { - QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose].inverse()); - - point_to_point_source_to_target_quaternion_cw(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - - point_to_point_source_to_target_quaternion_cw_jacobian(jacobian, - pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, - point_local.x(), point_local.y(), point_local.z()); - } - //----------------------- - int c = p.index_pose * 7; - for (int row = 0; row < 3; row++) - { - for (int col = 0; col < 7; col++) - { - if (jacobian(row, col) != 0.0) - { - tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); - } - } - } - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_left_jacobian) - { - Eigen::Matrix jacobian; - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - - Eigen::Matrix3d R = m_pose.rotation(); - Eigen::Vector3d Rp = R * p_s; - Eigen::Matrix3d Rpx; - Rpx(0, 0) = 0; - Rpx(0, 1) = -Rp.z(); - Rpx(0, 2) = Rp.y(); - Rpx(1, 0) = Rp.z(); - Rpx(1, 1) = 0; - Rpx(1, 2) = -Rp.x(); - Rpx(2, 0) = -Rp.y(); - Rpx(2, 1) = Rp.x(); - Rpx(2, 2) = 0; - - int ic = p.index_pose * 6; - - tripletListA.emplace_back(ir, ic + 0, 1); - // tripletListA.emplace_back(ir, ic + 1, 0); - // tripletListA.emplace_back(ir, ic + 2, 0); - tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - - // tripletListA.emplace_back(ir + 1, ic + 0, 0); - tripletListA.emplace_back(ir + 1, ic + 1, 1); - // tripletListA.emplace_back(ir + 1, ic + 2, 0); - tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - - // tripletListA.emplace_back(ir + 2, ic + 0, 0); - // tripletListA.emplace_back(ir + 2, ic + 1, 0); - tripletListA.emplace_back(ir + 2, ic + 2, 1); - tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - - Eigen::Vector3d target = p_t; - Eigen::Vector3d source = m_pose * p_s; - - delta_x = target.x() - source.x(); - delta_y = target.y() - source.y(); - delta_z = target.z() - source.z(); - } - else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_right_jacobian) - { - Eigen::Matrix jacobian; - Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - - Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); - Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - - Eigen::Matrix3d px; - px(0, 0) = 0; - px(0, 1) = -p_s.z(); - px(0, 2) = p_s.y(); - px(1, 0) = p_s.z(); - px(1, 1) = 0; - px(1, 2) = -p_s.x(); - px(2, 0) = -p_s.y(); - px(2, 1) = p_s.x(); - px(2, 2) = 0; - Eigen::Matrix3d R = m_pose.rotation(); - Eigen::Matrix3d Rpx = R * px; - - int ic = p.index_pose * 6; - - tripletListA.emplace_back(ir, ic + 0, R(0, 0)); - tripletListA.emplace_back(ir, ic + 1, R(0, 1)); - tripletListA.emplace_back(ir, ic + 2, R(0, 2)); - tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - - tripletListA.emplace_back(ir + 1, ic + 0, R(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 1, R(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 2, R(1, 2)); - tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); - tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); - tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - - tripletListA.emplace_back(ir + 2, ic + 0, R(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 1, R(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 2, R(2, 2)); - tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); - tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); - tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - - Eigen::Vector3d target = p_t; - Eigen::Vector3d source = m_pose * p_s; - - delta_x = target.x() - source.x(); - delta_y = target.y() - source.y(); - delta_z = target.z() - source.z(); - } - - tripletListB.emplace_back(ir, 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - - tripletListP.emplace_back(ir, ir, infm(0, 0)); - tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); - tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); - tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); - tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); - tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); - tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); - tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); - tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); - - ssr += delta_x * delta_x; - ssr += delta_y * delta_y; - ssr += delta_z * delta_z; - sum_obs += 3; - } - - if (tripletListB.size() > 100000) - { - Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPAt = AtP * matA; - AtPBt = AtP * matB; - - if (init) - { - AtPAt_tmp = AtPAt; - AtPBt_tmp = AtPBt; - init = false; - } - else - { - AtPAt_tmp += AtPAt; - AtPBt_tmp += AtPBt; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - } - } -#endif - } - -#if 0 - (*sumssr) = ssr; - (*sums_obs) = sum_obs; - (*md_out) = md; - (*md_count_out) = md_count; - - // std::cout << md << " "; - - if (tripletListB.size() > 0) - { - Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); - Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPAt = AtP * matA; - AtPBt = AtP * matB; - - if (init) - { - (*AtPA) = AtPAt; - (*AtPB) = AtPBt; - } - else - { - AtPAt_tmp += AtPAt; - AtPBt_tmp += AtPBt; - - (*AtPA) = AtPAt_tmp; - (*AtPB) = AtPBt_tmp; - } - // std::cout << AtPAt << std::endl; - } - } - else - { - (*AtPA) = AtPAt_tmp; - (*AtPB) = AtPBt_tmp; - } -#endif -} - -bool NDT::optimize(std::vector &point_clouds, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) -{ - auto start = std::chrono::system_clock::now(); - - OptimizationAlgorithm optimization_algorithm; - if (is_gauss_newton) - { - optimization_algorithm = OptimizationAlgorithm::gauss_newton; - } - if (is_levenberg_marguardt) - { - optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; - } - - PoseConvention pose_convention; - if (is_wc) - { - pose_convention = PoseConvention::wc; - } - if (is_cw) - { - pose_convention = PoseConvention::cw; - } - - RotationMatrixParametrization rotation_matrix_parametrization; - if (is_tait_bryan_angles) - { - rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; - } - else if (is_rodrigues) - { - rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; - } - else if (is_quaternion) - { - rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; - } - else if (is_lie_algebra_left_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; - } - else if (is_lie_algebra_right_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; - } - - if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - for (size_t i = 0; i < point_clouds.size(); i++) - { - TaitBryanPose pose; - pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); - point_clouds[i].m_pose = point_clouds[i].m_pose * m; - } - } - - int number_of_unknowns; - if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - number_of_unknowns = 6; - } - if (is_quaternion) - { - number_of_unknowns = 7; - } - - double lm_lambda = 0.0001; - double previous_rms = std::numeric_limits::max(); - int number_of_lm_iterations = 0; - - std::vector m_poses_tmp; - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - for (size_t i = 0; i < point_clouds.size(); i++) - { - m_poses_tmp.push_back(point_clouds[i].m_pose); - } - } - - for (int iter = 0; iter < number_of_iterations; iter++) - { - -#if 1 - //////rgd external/////// - // double min_x = std::numeric_limits::max(); - // double max_x = std::numeric_limits::lowest(); - - // double min_y = std::numeric_limits::max(); - // double max_y = std::numeric_limits::lowest(); - - // double min_z = std::numeric_limits::max(); - // double max_z = std::numeric_limits::lowest(); - - std::cout << "building points_global_external begin" << std::endl; - std::vector points_global_external; - size_t num_total_points = 0; - for (int i = 0; i < point_clouds.size(); i++) - { - num_total_points += point_clouds[i].points_local.size(); - } - points_global_external.reserve(num_total_points); - Eigen::Vector3d vt; - Point3D p; - for (int i = 0; i < point_clouds.size(); i++) - { - std::cout << "processing point_cloud [" << i + 1 << "] of " << point_clouds.size() << std::endl; - - for (int j = 0; j < point_clouds[i].points_local.size(); j++) - { - vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = i; - points_global_external.emplace_back(p); - } - } - std::cout << "building points_global_external end" << std::endl; - - std::vector index_pair_external; - std::vector buckets_external; - - GridParameters rgd_params_external; - rgd_params_external.resolution_X = this->bucket_size_external[0]; - rgd_params_external.resolution_Y = this->bucket_size_external[1]; - rgd_params_external.resolution_Z = this->bucket_size_external[2]; - - int bbext = this->bucket_size_external[0]; - if (this->bucket_size_external[1] > bbext) - bbext = this->bucket_size_external[1]; - if (this->bucket_size_external[2] > bbext) - bbext = this->bucket_size_external[2]; - rgd_params_external.bounding_box_extension = bbext; - - std::cout << "building external grid begin" << std::endl; - grid_calculate_params(points_global_external, rgd_params_external); - int num_threads = 1; - if (buckets_external.size() > this->number_of_threads) - { - num_threads = this->number_of_threads; - } - build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); - std::vector buckets_external_reduced; - - for (const auto &b : buckets_external) - { - // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; - if (b.number_of_points > 1000) - { - buckets_external_reduced.push_back(b); - } - } - buckets_external = buckets_external_reduced; - buckets_external_reduced.clear(); - - std::sort(buckets_external.begin(), buckets_external.end(), [](const Bucket &a, const Bucket &b) - { return (a.number_of_points > b.number_of_points); }); - - std::cout << "building external grid end" << std::endl; - std::cout << "number active buckets external: " << buckets_external.size() << std::endl; - - bool init = false; - Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - Eigen::SparseMatrix AtPB_ndt(point_clouds.size() * number_of_unknowns, 1); - double rms = 0.0; - int sum = 0; - double md = 0.0; - double md_sum = 0.0; - - for (int bi = 0; bi < buckets_external.size(); bi++) - { - if (compute_only_mahalanobis_distance) - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; - } - else - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; - } - std::vector points_global; - - for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) - { - points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); - } - - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; - - std::vector index_pair; - std::vector buckets; - - std::cout << "building rgd begin" << std::endl; - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); - std::cout << "building rgd end" << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - for (size_t i = 0; i < point_clouds.size(); i++) - { - mposes.push_back(point_clouds[i].m_pose); - mposes_inv.push_back(point_clouds[i].m_pose.inverse()); - } - - std::cout << "computing AtPA AtPB start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points_global, &mposes, &mposes_inv, point_clouds.size(), pose_convention, rotation_matrix_parametrization, - number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, num_extended_points, &(md_out[k]), &(md_count_out[k]), false, compute_mean_and_cov_for_bucket)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing AtPA AtPB finished" << std::endl; - - for (size_t k = 0; k < jobs.size(); k++) - { - rms += sumrmss[k]; - sum += sums[k]; - md += md_out[k]; - md_sum += md_count_out[k]; - } - - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt = AtPAtmp[k]; - AtPB_ndt = AtPBtmp[k]; - init = true; - } - } - else - { - if (AtPBtmp[k].size() > 0) - { - - AtPA_ndt += AtPAtmp[k]; - AtPB_ndt += AtPBtmp[k]; - } - } - } - } - std::cout << "cleaning start" << std::endl; - points_global_external.clear(); - index_pair_external.clear(); - buckets_external.clear(); - std::cout << "cleaning finished" << std::endl; - - rms /= sum; - std::cout << "rms " << rms << std::endl; - - md /= md_sum; - std::cout << "mean mahalanobis distance: " << md << std::endl; - - if (compute_only_mahalanobis_distance) - { - return true; - } -#else - - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; - - std::vector points_global; - for (size_t i = 0; i < point_clouds.size(); i++) - { - for (size_t j = 0; j < point_clouds[i].points_local.size(); j++) - { - Eigen::Vector3d vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; - Point3D p; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = i; - if (p.x > -250 && p.x < 250 && p.y > -250 && p.y < 250 && p.z > -50 && p.z < 50) - points_global.push_back(p); - } - } - - std::vector index_pair; - std::vector buckets; - - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - } - - std::vector mposes; - std::vector mposes_inv; - for (size_t i = 0; i < point_clouds.size(); i++) - { - mposes.push_back(point_clouds[i].m_pose); - mposes_inv.push_back(point_clouds[i].m_pose.inverse()); - } - - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points_global, &mposes, &mposes_inv, point_clouds.size(), pose_convention, rotation_matrix_parametrization, number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, num_extended_points)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - - double rms = 0.0; - int sum = 0; - for (size_t k = 0; k < jobs.size(); k++) - { - rms += sumrmss[k]; - sum += sums[k]; - } - rms /= sum; - std::cout << "rms " << rms << std::endl; - - bool init = false; - Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - Eigen::SparseMatrix AtPB_ndt(point_clouds.size() * number_of_unknowns, 1); - - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt = AtPAtmp[k]; - AtPB_ndt = AtPBtmp[k]; - init = true; - } - } - else - { - if (AtPBtmp[k].size() > 0) - { - - AtPA_ndt += AtPAtmp[k]; - AtPB_ndt += AtPBtmp[k]; - } - } - } -#endif - ////////////////////////////////////////////////////////////////// - - if (is_fix_first_node) - { - Eigen::SparseMatrix I(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - for (int ii = 0; ii < number_of_unknowns; ii++) - { - I.coeffRef(ii, ii) = 1000000; - } - AtPA_ndt += I; - } - - std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; - if (is_levenberg_marguardt) - { - - if (rms < previous_rms) - { - if (lm_lambda < 1000000) - { - lm_lambda *= 10.0; - } - previous_rms = rms; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - } - else - { - lm_lambda /= 10.0; - number_of_lm_iterations++; - iter--; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - for (size_t i = 0; i < point_clouds.size(); i++) - { - point_clouds[i].m_pose = m_poses_tmp[i]; - } - previous_rms = std::numeric_limits::max(); - continue; - } - } - else - { - previous_rms = rms; - } - - if (is_quaternion) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - for (size_t i = 0; i < point_clouds.size(); i++) - { - int ic = i * 7; - int ir = 0; - QuaternionPose pose; - if (is_wc) - { - pose = pose_quaternion_from_affine_matrix(point_clouds[i].m_pose); - } - else - { - pose = pose_quaternion_from_affine_matrix(point_clouds[i].m_pose.inverse()); - } - double delta; - quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); - - Eigen::Matrix jacobian; - quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); - - tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); - - tripletListP.emplace_back(ir, ir, 1000000.0); - - tripletListB.emplace_back(ir, 0, delta); - } - Eigen::SparseMatrix matA(tripletListB.size(), point_clouds.size() * 7); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(point_clouds.size() * 7, point_clouds.size() * 7); - Eigen::SparseMatrix AtPB(point_clouds.size() * 7, 1); - - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = AtP * matA; - AtPB = AtP * matB; - - AtPA_ndt += AtPA; - AtPB_ndt += AtPB; - } - - if (is_levenberg_marguardt) - { - Eigen::SparseMatrix LM(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - LM.setIdentity(); - LM *= lm_lambda; - AtPA_ndt += LM; - } - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA_ndt); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB_ndt); - - std::vector h_x; - std::cout << "redult: row,col,value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - if (it.value() == it.value()) - { - h_x.push_back(it.value()); - std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; - } - } - } - - if (h_x.size() == point_clouds.size() * number_of_unknowns) - { - std::cout << "AtPA=AtPB SOLVED" << std::endl; - int counter = 0; - for (size_t i = 0; i < point_clouds.size(); i++) - { - Eigen::Affine3d m_pose; - - if (is_wc) - { - m_pose = point_clouds[i].m_pose; - } - else - { - m_pose = point_clouds[i].m_pose.inverse(); - } - - if (is_tait_bryan_angles) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else if (is_rodrigues) - { - RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.sx += h_x[counter++]; - pose.sy += h_x[counter++]; - pose.sz += h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose); - } - else if (is_quaternion) - { - QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); - - QuaternionPose poseq; - poseq.px = h_x[counter++]; - poseq.py = h_x[counter++]; - poseq.pz = h_x[counter++]; - poseq.q0 = h_x[counter++]; - poseq.q1 = h_x[counter++]; - poseq.q2 = h_x[counter++]; - poseq.q3 = h_x[counter++]; - - if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && fabs(poseq.pz) < this->bucket_size[0] && - fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && fabs(poseq.q3) < 10) - { - pose.px += poseq.px; - pose.py += poseq.py; - pose.pz += poseq.pz; - pose.q0 += poseq.q0; - pose.q1 += poseq.q1; - pose.q2 += poseq.q2; - pose.q3 += poseq.q3; - m_pose = affine_matrix_from_pose_quaternion(pose); - } - } - else if (is_lie_algebra_left_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; - } - else if (is_lie_algebra_right_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); - } - - if (is_wc) - { - // point_clouds[i].m_pose = m_pose; - } - else - { - m_pose = m_pose.inverse(); - } - - auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); - auto pose_src = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); - - if (!point_clouds[i].fixed_x) - { - pose_src.px = pose_res.px; - } - if (!point_clouds[i].fixed_y) - { - pose_src.py = pose_res.py; - } - if (!point_clouds[i].fixed_z) - { - pose_src.pz = pose_res.pz; - } - if (!point_clouds[i].fixed_om) - { - pose_src.om = pose_res.om; - } - if (!point_clouds[i].fixed_fi) - { - pose_src.fi = pose_res.fi; - } - if (!point_clouds[i].fixed_ka) - { - pose_src.ka = pose_res.ka; - } - - point_clouds[i].m_pose = affine_matrix_from_pose_tait_bryan(pose_src); - point_clouds[i].pose = pose_src; - point_clouds[i].gui_translation[0] = pose_src.px; - point_clouds[i].gui_translation[1] = pose_src.py; - point_clouds[i].gui_translation[2] = pose_src.pz; - point_clouds[i].gui_rotation[0] = rad2deg(pose_src.om); - point_clouds[i].gui_rotation[1] = rad2deg(pose_src.fi); - point_clouds[i].gui_rotation[2] = rad2deg(pose_src.ka); - - /*if (is_wc) - { - point_clouds[i].m_pose = m_pose; - } - else - { - point_clouds[i].m_pose = m_pose.inverse(); - } - if (!point_clouds[i].fixed) - { - point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); - point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; - point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; - point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; - point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); - point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); - point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); - }*/ - } - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - for (size_t i = 0; i < point_clouds.size(); i++) - { - m_poses_tmp.push_back(point_clouds[i].m_pose); - } - } - - std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - break; - } - } - - auto end = std::chrono::system_clock::now(); - auto elapsed = - std::chrono::duration_cast(end - start); - - std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; - - return true; -} - -bool NDT::optimize(std::vector &sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) -{ - std::cout << "optimize sessions" << std::endl; - - auto start = std::chrono::system_clock::now(); - - Session tmp_session; - - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - tmp_session = sessions[0]; - PointClouds point_clouds_container; - - std::vector point_clouds; - - std::vector points_local; - - for (const auto &pc : sessions[0].point_clouds_container.point_clouds) - { - for (const auto &p : pc.points_local) - { - Eigen::Vector3d pg = pc.m_pose * p; - points_local.push_back(pg); - } - } - - PointCloud pc; - pc.points_local = points_local; - pc.m_initial_pose = Eigen::Affine3d::Identity(); - pc.m_pose = Eigen::Affine3d::Identity(); - - point_clouds.push_back(pc); - point_clouds_container.point_clouds = point_clouds; - - sessions[0].point_clouds_container = point_clouds_container; - } - } - - OptimizationAlgorithm optimization_algorithm; - if (is_gauss_newton) - { - optimization_algorithm = OptimizationAlgorithm::gauss_newton; - } - if (is_levenberg_marguardt) - { - optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; - } - - PoseConvention pose_convention; - if (is_wc) - { - pose_convention = PoseConvention::wc; - } - if (is_cw) - { - pose_convention = PoseConvention::cw; - } - - RotationMatrixParametrization rotation_matrix_parametrization; - if (is_tait_bryan_angles) - { - rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; - } - else if (is_rodrigues) - { - rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; - } - else if (is_quaternion) - { - rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; - } - else if (is_lie_algebra_left_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; - } - else if (is_lie_algebra_right_jacobian) - { - rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; - } - - if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - TaitBryanPose pose; - pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); - pc.m_pose = pc.m_pose * m; - } - } - } - - int number_of_unknowns; - if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) - { - number_of_unknowns = 6; - } - if (is_quaternion) - { - number_of_unknowns = 7; - } - - double lm_lambda = 0.0001; - double previous_rms = std::numeric_limits::max(); - int number_of_lm_iterations = 0; - - std::vector m_poses_tmp; - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - // for (size_t i = 0; i < point_clouds.size(); i++) - //{ - // m_poses_tmp.push_back(point_clouds[i].m_pose); - // } - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - m_poses_tmp.push_back(pc.m_pose); - } - } - } - - for (int iter = 0; iter < number_of_iterations; iter++) - { - std::cout << "building points_global_external begin" << std::endl; - std::vector points_global_external; - size_t num_total_points = 0; - - // for (int i = 0; i < point_clouds.size(); i++) - //{ - // num_total_points += point_clouds[i].points_local.size(); - // } - - int num_point_clouds = 0; - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - num_total_points += pc.points_local.size(); - num_point_clouds++; - } - } - - points_global_external.reserve(num_total_points); - Eigen::Vector3d vt; - Point3D p; - - int index_pose = 0; - - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - // num_total_points += pc.points_local.size(); - std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; - - for (int j = 0; j < pc.points_local.size(); j++) - { - vt = pc.m_pose * pc.points_local[j]; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = index_pose; - points_global_external.emplace_back(p); - } - index_pose++; - } - } - - std::cout << "building points_global_external end" << std::endl; - - std::vector index_pair_external; - std::vector buckets_external; - - GridParameters rgd_params_external; - rgd_params_external.resolution_X = this->bucket_size_external[0]; - rgd_params_external.resolution_Y = this->bucket_size_external[1]; - rgd_params_external.resolution_Z = this->bucket_size_external[2]; - - int bbext = this->bucket_size_external[0]; - if (this->bucket_size_external[1] > bbext) - bbext = this->bucket_size_external[1]; - if (this->bucket_size_external[2] > bbext) - bbext = this->bucket_size_external[2]; - rgd_params_external.bounding_box_extension = bbext; - - std::cout << "building external grid begin" << std::endl; - grid_calculate_params(points_global_external, rgd_params_external); - int num_threads = 1; - if (buckets_external.size() > this->number_of_threads) - { - num_threads = this->number_of_threads; - } - build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); - std::vector buckets_external_reduced; - - for (const auto &b : buckets_external) - { - // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; - if (b.number_of_points > 1000) - { - buckets_external_reduced.push_back(b); - } - } - buckets_external = buckets_external_reduced; - buckets_external_reduced.clear(); - - std::sort(buckets_external.begin(), buckets_external.end(), [](const Bucket &a, const Bucket &b) - { return (a.number_of_points > b.number_of_points); }); - - std::cout << "building external grid end" << std::endl; - std::cout << "number active buckets external: " << buckets_external.size() << std::endl; - - bool init = false; - Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); - double rms = 0.0; - int sum = 0; - double md = 0.0; - double md_sum = 0.0; - - for (int bi = 0; bi < buckets_external.size(); bi++) - { - if (compute_only_mahalanobis_distance) - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; - } - else - { - std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; - } - std::vector points_global; - - for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) - { - points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); - } - - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; - - std::vector index_pair; - std::vector buckets; - - std::cout << "building rgd begin" << std::endl; - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); - std::cout << "building rgd end" << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - mposes.push_back(pc.m_pose); - mposes_inv.push_back(pc.m_pose.inverse()); - } - } - - std::cout << "computing AtPA AtPB start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points_global, &mposes, &mposes_inv, num_point_clouds, pose_convention, rotation_matrix_parametrization, - number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, num_extended_points, &(md_out[k]), &(md_count_out[k]), false, compute_mean_and_cov_for_bucket)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing AtPA AtPB finished" << std::endl; - - for (size_t k = 0; k < jobs.size(); k++) - { - rms += sumrmss[k]; - sum += sums[k]; - md += md_out[k]; - md_sum += md_count_out[k]; - } - - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) - { - if (AtPBtmp[k].size() > 0) - { - AtPA_ndt = AtPAtmp[k]; - AtPB_ndt = AtPBtmp[k]; - init = true; - } - } - else - { - if (AtPBtmp[k].size() > 0) - { - - AtPA_ndt += AtPAtmp[k]; - AtPB_ndt += AtPBtmp[k]; - } - } - } - } - std::cout << "cleaning start" << std::endl; - points_global_external.clear(); - index_pair_external.clear(); - buckets_external.clear(); - std::cout << "cleaning finished" << std::endl; - - rms /= sum; - std::cout << "rms " << rms << std::endl; - - md /= md_sum; - std::cout << "mean mahalanobis distance: " << md << std::endl; - - if (compute_only_mahalanobis_distance) - { - return true; - } - - ////////////////////////////////////////////////////////////////// - - if (is_fix_first_node) - { - Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - for (int ii = 0; ii < number_of_unknowns; ii++) - { - I.coeffRef(ii, ii) = 1000000; - } - AtPA_ndt += I; - } - - std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; - if (is_levenberg_marguardt) - { - - if (rms < previous_rms) - { - if (lm_lambda < 1000000) - { - lm_lambda *= 10.0; - } - previous_rms = rms; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - } - else - { - lm_lambda /= 10.0; - number_of_lm_iterations++; - iter--; - std::cout << " lm_lambda: " << lm_lambda << std::endl; - int index = 0; - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - pc.m_pose = m_poses_tmp[index++]; - } - } - - previous_rms = std::numeric_limits::max(); - continue; - } - } - else - { - previous_rms = rms; - } - - if (is_quaternion) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - int index = 0; - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - // pc.m_pose = m_poses_tmp[index++]; - int ic = index * 7; - int ir = 0; - QuaternionPose pose; - if (is_wc) - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose); - } - else - { - pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); - } - double delta; - quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); - - Eigen::Matrix jacobian; - quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); - - tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); - - tripletListP.emplace_back(ir, ir, 1000000.0); - - tripletListB.emplace_back(ir, 0, delta); - - index++; - } - } - - Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); - Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); - - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = AtP * matA; - AtPB = AtP * matB; - - AtPA_ndt += AtPA; - AtPB_ndt += AtPB; - } - - if (is_levenberg_marguardt) - { - Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); - LM.setIdentity(); - LM *= lm_lambda; - AtPA_ndt += LM; - } - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA_ndt); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB_ndt); - - std::vector h_x; - std::cout << "redult: row,col,value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - if (it.value() == it.value()) - { - h_x.push_back(it.value()); - std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; - } - } - } - - if (h_x.size() == num_point_clouds * number_of_unknowns) - { - std::cout << "AtPA=AtPB SOLVED" << std::endl; - int counter = 0; - - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) - { - Eigen::Affine3d m_pose; - - if (is_wc) - { - m_pose = pc.m_pose; + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z()); } else { - m_pose = pc.m_pose.inverse(); - } + TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose].inverse()); - if (is_tait_bryan_angles) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else if (is_rodrigues) - { - RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.sx += h_x[counter++]; - pose.sy += h_x[counter++]; - pose.sz += h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose); + point_to_point_source_to_target_tait_bryan_cw(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_tait_bryan_cw_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + point_local.x(), point_local.y(), point_local.z()); } - else if (is_quaternion) + //----------------------- + int c = p.index_pose * 6; + for (int row = 0; row < 3; row++) { - QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); - - QuaternionPose poseq; - poseq.px = h_x[counter++]; - poseq.py = h_x[counter++]; - poseq.pz = h_x[counter++]; - poseq.q0 = h_x[counter++]; - poseq.q1 = h_x[counter++]; - poseq.q2 = h_x[counter++]; - poseq.q3 = h_x[counter++]; - - if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && fabs(poseq.pz) < this->bucket_size[0] && - fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && fabs(poseq.q3) < 10) + for (int col = 0; col < 6; col++) { - pose.px += poseq.px; - pose.py += poseq.py; - pose.pz += poseq.pz; - pose.q0 += poseq.q0; - pose.q1 += poseq.q1; - pose.q2 += poseq.q2; - pose.q3 += poseq.q3; - m_pose = affine_matrix_from_pose_quaternion(pose); + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } } } - else if (is_lie_algebra_left_jacobian) - { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; - } - else if (is_lie_algebra_right_jacobian) + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::rodrigues) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) { - RodriguesPose pose_update; - pose_update.px = h_x[counter++]; - pose_update.py = h_x[counter++]; - pose_update.pz = h_x[counter++]; - pose_update.sx = h_x[counter++]; - pose_update.sy = h_x[counter++]; - pose_update.sz = h_x[counter++]; - m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); - } + RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose]); - if (is_wc) - { + point_to_point_source_to_target_rodrigues_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_rodrigues_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, + point_local.x(), point_local.y(), point_local.z()); } else { - m_pose = m_pose.inverse(); - } + RodriguesPose pose_s = pose_rodrigues_from_affine_matrix((*mposes)[p.index_pose].inverse()); - auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); - auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); + point_to_point_source_to_target_rodrigues_cw(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - if (!pc.fixed_x) - { - pose_src.px = pose_res.px; - } - if (!pc.fixed_y) - { - pose_src.py = pose_res.py; - } - if (!pc.fixed_z) - { - pose_src.pz = pose_res.pz; - } - if (!pc.fixed_om) - { - pose_src.om = pose_res.om; + point_to_point_source_to_target_rodrigues_cw_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, + point_local.x(), point_local.y(), point_local.z()); } - if (!pc.fixed_fi) + //----------------------- + int c = p.index_pose * 6; + for (int row = 0; row < 3; row++) { - pose_src.fi = pose_res.fi; + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } } - if (!pc.fixed_ka) + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::quaternion) + { + Eigen::Matrix jacobian; + //----------------------- + if (pose_convention == NDT::PoseConvention::wc) { - pose_src.ka = pose_res.ka; - } + QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose]); - pc.pose = pose_src; - pc.gui_translation[0] = pose_src.px; - pc.gui_translation[1] = pose_src.py; - pc.gui_translation[2] = pose_src.pz; - pc.gui_rotation[0] = rad2deg(pose_src.om); - pc.gui_rotation[1] = rad2deg(pose_src.fi); - pc.gui_rotation[2] = rad2deg(pose_src.ka); + point_to_point_source_to_target_quaternion_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); - /* - if (is_wc) - { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose; // ToDo check if !pc.fixed needed - //} + point_to_point_source_to_target_quaternion_wc_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, + point_local.x(), point_local.y(), point_local.z()); } else { - // if (!s.is_ground_truth) - //{ - pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed - //} - } + QuaternionPose pose_s = pose_quaternion_from_affine_matrix((*mposes)[p.index_pose].inverse()); - if (!pc.fixed) - { - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - }*/ - } - } - if (is_levenberg_marguardt) - { - m_poses_tmp.clear(); - for (auto &s : sessions) - { - for (auto &pc : s.point_clouds_container.point_clouds) + point_to_point_source_to_target_quaternion_cw(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, + point_local.x(), point_local.y(), point_local.z(), mean.x(), mean.y(), mean.z()); + + point_to_point_source_to_target_quaternion_cw_jacobian(jacobian, + pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, + point_local.x(), point_local.y(), point_local.z()); + } + //----------------------- + int c = p.index_pose * 7; + for (int row = 0; row < 3; row++) { - m_poses_tmp.push_back(pc.m_pose); + for (int col = 0; col < 7; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } } } - } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_left_jacobian) + { + Eigen::Matrix jacobian; + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; - std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - break; - } - } + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - ////////// + Eigen::Matrix3d R = m_pose.rotation(); + Eigen::Vector3d Rp = R * p_s; + Eigen::Matrix3d Rpx; + Rpx(0, 0) = 0; + Rpx(0, 1) = -Rp.z(); + Rpx(0, 2) = Rp.y(); + Rpx(1, 0) = Rp.z(); + Rpx(1, 1) = 0; + Rpx(1, 2) = -Rp.x(); + Rpx(2, 0) = -Rp.y(); + Rpx(2, 1) = Rp.x(); + Rpx(2, 2) = 0; - if (sessions.size() > 1) - { - if (sessions[0].is_ground_truth) - { - Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); + int ic = p.index_pose * 6; - sessions[0].point_clouds_container = tmp_session.point_clouds_container; + tripletListA.emplace_back(ir, ic + 0, 1); + // tripletListA.emplace_back(ir, ic + 1, 0); + // tripletListA.emplace_back(ir, ic + 2, 0); + tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - for (int i = 1; i < sessions.size(); i++) - { - for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) - { - sessions[i].point_clouds_container.point_clouds[j].m_pose = sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; - 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] = sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = sessions[i].point_clouds_container.point_clouds[j].pose.pz; - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); - } - } - } - } - ////////// + // tripletListA.emplace_back(ir + 1, ic + 0, 0); + tripletListA.emplace_back(ir + 1, ic + 1, 1); + // tripletListA.emplace_back(ir + 1, ic + 2, 0); + tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - auto end = std::chrono::system_clock::now(); - auto elapsed = - std::chrono::duration_cast(end - start); + // tripletListA.emplace_back(ir + 2, ic + 0, 0); + // tripletListA.emplace_back(ir + 2, ic + 1, 0); + tripletListA.emplace_back(ir + 2, ic + 2, 1); + tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; + Eigen::Vector3d target = p_t; + Eigen::Vector3d source = m_pose * p_s; - return true; -} + delta_x = target.x() - source.x(); + delta_y = target.y() - source.y(); + delta_z = target.z() - source.z(); + } + else if (rotation_matrix_parametrization == NDT::RotationMatrixParametrization::lie_algebra_right_jacobian) + { + Eigen::Matrix jacobian; + Eigen::Affine3d m_pose = (*mposes)[p.index_pose]; -std::vector> NDT::compute_covariance_matrices_and_rms(std::vector &point_clouds, double &rms) -{ + Eigen::Vector3d p_s(point_local.x(), point_local.y(), point_local.z()); + Eigen::Vector3d p_t(mean.x(), mean.y(), mean.z()); - OptimizationAlgorithm optimization_algorithm; - if (is_gauss_newton) - { - optimization_algorithm = OptimizationAlgorithm::gauss_newton; - } - if (is_levenberg_marguardt) - { - optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; - } + Eigen::Matrix3d px; + px(0, 0) = 0; + px(0, 1) = -p_s.z(); + px(0, 2) = p_s.y(); + px(1, 0) = p_s.z(); + px(1, 1) = 0; + px(1, 2) = -p_s.x(); + px(2, 0) = -p_s.y(); + px(2, 1) = p_s.x(); + px(2, 2) = 0; + Eigen::Matrix3d R = m_pose.rotation(); + Eigen::Matrix3d Rpx = R * px; - PoseConvention pose_convention; - if (is_wc) - { - pose_convention = PoseConvention::wc; - } - if (is_cw) - { - pose_convention = PoseConvention::cw; - } + int ic = p.index_pose * 6; - RotationMatrixParametrization rotation_matrix_parametrization; - if (is_tait_bryan_angles) - { - rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; - } - if (is_rodrigues) - { - rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; - } - if (is_quaternion) - { - rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; - } + tripletListA.emplace_back(ir, ic + 0, R(0, 0)); + tripletListA.emplace_back(ir, ic + 1, R(0, 1)); + tripletListA.emplace_back(ir, ic + 2, R(0, 2)); + tripletListA.emplace_back(ir, ic + 3, -Rpx(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -Rpx(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -Rpx(0, 2)); - if (is_rodrigues || is_quaternion) - { - for (size_t i = 0; i < point_clouds.size(); i++) - { - TaitBryanPose pose; - pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); - point_clouds[i].m_pose = point_clouds[i].m_pose * m; - } - } + tripletListA.emplace_back(ir + 1, ic + 0, R(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 1, R(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 2, R(1, 2)); + tripletListA.emplace_back(ir + 1, ic + 3, -Rpx(1, 0)); + tripletListA.emplace_back(ir + 1, ic + 4, -Rpx(1, 1)); + tripletListA.emplace_back(ir + 1, ic + 5, -Rpx(1, 2)); - int number_of_unknowns; - if (is_tait_bryan_angles || is_rodrigues) - { - number_of_unknowns = 6; - } - if (is_quaternion) - { - number_of_unknowns = 7; - } + tripletListA.emplace_back(ir + 2, ic + 0, R(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 1, R(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 2, R(2, 2)); + tripletListA.emplace_back(ir + 2, ic + 3, -Rpx(2, 0)); + tripletListA.emplace_back(ir + 2, ic + 4, -Rpx(2, 1)); + tripletListA.emplace_back(ir + 2, ic + 5, -Rpx(2, 2)); - std::vector> covariance_matrices; + Eigen::Vector3d target = p_t; + Eigen::Vector3d source = m_pose * p_s; - for (size_t i = 0; i < point_clouds.size(); i++) - { - Eigen::SparseMatrix covariance_matrix(number_of_unknowns, number_of_unknowns); - covariance_matrices.push_back(covariance_matrix); - } + delta_x = target.x() - source.x(); + delta_y = target.y() - source.y(); + delta_z = target.z() - source.z(); + } - GridParameters rgd_params; - rgd_params.resolution_X = this->bucket_size[0]; - rgd_params.resolution_Y = this->bucket_size[1]; - rgd_params.resolution_Z = this->bucket_size[2]; - rgd_params.bounding_box_extension = 1.0; + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); - std::vector points_global; - for (size_t i = 0; i < point_clouds.size(); i++) - { - for (size_t j = 0; j < point_clouds[i].points_local.size(); j++) - { - Eigen::Vector3d vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; - Point3D p; - p.x = vt.x(); - p.y = vt.y(); - p.z = vt.z(); - p.index_pose = i; - points_global.push_back(p); - } - } + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); - std::vector index_pair; - std::vector buckets; + ssr += delta_x * delta_x; + ssr += delta_y * delta_y; + ssr += delta_z * delta_z; + sum_obs += 3; + } - grid_calculate_params(points_global, rgd_params); - build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + if (tripletListB.size() > 100000) + { + Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - std::vector threads; + Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPAt = AtP * matA; + AtPBt = AtP * matB; - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); + if (init) + { + AtPAt_tmp = AtPAt; + AtPBt_tmp = AtPBt; + init = false; + } + else + { + AtPAt_tmp += AtPAt; + AtPBt_tmp += AtPBt; + } - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + } + } +#endif + } - std::vector mposes; - std::vector mposes_inv; - for (size_t i = 0; i < point_clouds.size(); i++) - { - // poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose)); - mposes.push_back(point_clouds[i].m_pose); - mposes_inv.push_back(point_clouds[i].m_pose.inverse()); - } - // std::cout << "XXX" << std::endl; - // std::cout << (int)pose_convention << " " << (int)rotation_matrix_parametrization << " " << (int)number_of_unknowns << std::endl; +#if 0 + (*sumssr) = ssr; + (*sums_obs) = sum_obs; + (*md_out) = md; + (*md_count_out) = md_count; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points_global, &mposes, &mposes_inv, point_clouds.size(), pose_convention, rotation_matrix_parametrization, - number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, num_extended_points, &(md_out[k]), - &(md_count_out[k]), false, false)); - } + // std::cout << md << " "; - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - double ssr = 0.0; - int num_obs = 0; - for (size_t k = 0; k < jobs.size(); k++) + if (tripletListB.size() > 0) { - ssr += sumrmss[k]; - num_obs += sums[k]; - } - - double sq = ssr / ((double)num_obs - point_clouds.size() * number_of_unknowns); - rms = ssr / (double)num_obs; + Eigen::SparseMatrix matA(tripletListB.size(), trajectory_size * number_of_unknowns); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); - // std::cout << "sq " << sq << " num_obs " << num_obs << std::endl; + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - bool init = false; - Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + Eigen::SparseMatrix AtPAt(trajectory_size * number_of_unknowns, trajectory_size * number_of_unknowns); + Eigen::SparseMatrix AtPBt(trajectory_size * number_of_unknowns, 1); - for (size_t k = 0; k < jobs.size(); k++) - { - if (!init) { - if (AtPBtmp[k].size() > 0) + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPAt = AtP * matA; + AtPBt = AtP * matB; + + if (init) { - // - AtPA_ndt = AtPAtmp[k]; - init = true; + (*AtPA) = AtPAt; + (*AtPB) = AtPBt; } - } - else - { - if (AtPBtmp[k].size() > 0) + else { - AtPA_ndt += AtPAtmp[k]; + AtPAt_tmp += AtPAt; + AtPBt_tmp += AtPBt; + + (*AtPA) = AtPAt_tmp; + (*AtPB) = AtPBt_tmp; } + // std::cout << AtPAt << std::endl; } } - - AtPA_ndt = 0.5 * AtPA_ndt; - - Eigen::SimplicialCholesky> solver(AtPA_ndt); - Eigen::SparseMatrix I(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - I.setIdentity(); - - Eigen::SparseMatrix AtAinv(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); - AtAinv = solver.solve(I); - - AtAinv = AtAinv * sq; - - for (int i = 0; i < point_clouds.size(); i++) + else { - Eigen::SparseMatrix cm(number_of_unknowns, number_of_unknowns); - for (int r = 0; r < number_of_unknowns; r++) - { - for (int c = 0; c < number_of_unknowns; c++) - { - cm.coeffRef(r, c) = AtAinv.coeff(i * number_of_unknowns + r, i * number_of_unknowns + c); - } - } - covariance_matrices.push_back(cm); - // std::cout << "cm" << std::endl; - // std::cout << cm << std::endl; + (*AtPA) = AtPAt_tmp; + (*AtPB) = AtPBt_tmp; } - return covariance_matrices; +#endif } -bool NDT::optimize(std::vector &point_clouds, double &rms_initial, double &rms_final, double &mui, bool compute_mean_and_cov_for_bucket) +bool NDT::optimize(std::vector& point_clouds, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) { - // double rms; - // std::vector> cm_before = compute_covariance_matrices_and_rms(point_clouds, rms); - // rms_initial = rms; - //-- - bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); - //-- - // std::vector> cm_after = compute_covariance_matrices_and_rms(point_clouds, rms); - // rms_final = rms; - // mui = get_mean_uncertainty_xyz_impact(cm_before, cm_after); - return res; + auto start = std::chrono::system_clock::now(); + + OptimizationAlgorithm optimization_algorithm; + if (is_gauss_newton) + { + optimization_algorithm = OptimizationAlgorithm::gauss_newton; + } + if (is_levenberg_marguardt) + { + optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; + } + + PoseConvention pose_convention; + if (is_wc) + { + pose_convention = PoseConvention::wc; + } + if (is_cw) + { + pose_convention = PoseConvention::cw; + } + + RotationMatrixParametrization rotation_matrix_parametrization; + if (is_tait_bryan_angles) + { + rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; + } + else if (is_rodrigues) + { + rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; + } + else if (is_quaternion) + { + rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; + } + else if (is_lie_algebra_left_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; + } + else if (is_lie_algebra_right_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; + } + + if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + for (size_t i = 0; i < point_clouds.size(); i++) + { + TaitBryanPose pose; + pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); + point_clouds[i].m_pose = point_clouds[i].m_pose * m; + } + } + + int number_of_unknowns; + if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + number_of_unknowns = 6; + } + if (is_quaternion) + { + number_of_unknowns = 7; + } + + double lm_lambda = 0.0001; + double previous_rms = std::numeric_limits::max(); + int number_of_lm_iterations = 0; + + std::vector m_poses_tmp; + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + for (size_t i = 0; i < point_clouds.size(); i++) + { + m_poses_tmp.push_back(point_clouds[i].m_pose); + } + } + + for (int iter = 0; iter < number_of_iterations; iter++) + { +#if 1 + //////rgd external/////// + // double min_x = std::numeric_limits::max(); + // double max_x = std::numeric_limits::lowest(); + + // double min_y = std::numeric_limits::max(); + // double max_y = std::numeric_limits::lowest(); + + // double min_z = std::numeric_limits::max(); + // double max_z = std::numeric_limits::lowest(); + + std::cout << "building points_global_external begin" << std::endl; + std::vector points_global_external; + size_t num_total_points = 0; + for (int i = 0; i < point_clouds.size(); i++) + { + num_total_points += point_clouds[i].points_local.size(); + } + points_global_external.reserve(num_total_points); + Eigen::Vector3d vt; + Point3D p; + for (int i = 0; i < point_clouds.size(); i++) + { + std::cout << "processing point_cloud [" << i + 1 << "] of " << point_clouds.size() << std::endl; + + for (int j = 0; j < point_clouds[i].points_local.size(); j++) + { + vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = i; + points_global_external.emplace_back(p); + } + } + std::cout << "building points_global_external end" << std::endl; + + std::vector index_pair_external; + std::vector buckets_external; + + GridParameters rgd_params_external; + rgd_params_external.resolution_X = this->bucket_size_external[0]; + rgd_params_external.resolution_Y = this->bucket_size_external[1]; + rgd_params_external.resolution_Z = this->bucket_size_external[2]; + + int bbext = this->bucket_size_external[0]; + if (this->bucket_size_external[1] > bbext) + bbext = this->bucket_size_external[1]; + if (this->bucket_size_external[2] > bbext) + bbext = this->bucket_size_external[2]; + rgd_params_external.bounding_box_extension = bbext; + + std::cout << "building external grid begin" << std::endl; + grid_calculate_params(points_global_external, rgd_params_external); + int num_threads = 1; + if (buckets_external.size() > this->number_of_threads) + { + num_threads = this->number_of_threads; + } + build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); + std::vector buckets_external_reduced; + + for (const auto& b : buckets_external) + { + // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; + if (b.number_of_points > 1000) + { + buckets_external_reduced.push_back(b); + } + } + buckets_external = buckets_external_reduced; + buckets_external_reduced.clear(); + + std::sort( + buckets_external.begin(), + buckets_external.end(), + [](const Bucket& a, const Bucket& b) + { + return (a.number_of_points > b.number_of_points); + }); + + std::cout << "building external grid end" << std::endl; + std::cout << "number active buckets external: " << buckets_external.size() << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + Eigen::SparseMatrix AtPB_ndt(point_clouds.size() * number_of_unknowns, 1); + double rms = 0.0; + int sum = 0; + double md = 0.0; + double md_sum = 0.0; + + for (int bi = 0; bi < buckets_external.size(); bi++) + { + if (compute_only_mahalanobis_distance) + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; + } + else + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " + << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; + } + std::vector points_global; + + for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) + { + points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); + } + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector index_pair; + std::vector buckets; + + std::cout << "building rgd begin" << std::endl; + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + std::cout << "building rgd end" << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = + Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + for (size_t i = 0; i < point_clouds.size(); i++) + { + mposes.push_back(point_clouds[i].m_pose); + mposes_inv.push_back(point_clouds[i].m_pose.inverse()); + } + + std::cout << "computing AtPA AtPB start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + point_clouds.size(), + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + compute_mean_and_cov_for_bucket)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing AtPA AtPB finished" << std::endl; + + for (size_t k = 0; k < jobs.size(); k++) + { + rms += sumrmss[k]; + sum += sums[k]; + md += md_out[k]; + md_sum += md_count_out[k]; + } + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt = AtPAtmp[k]; + AtPB_ndt = AtPBtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + AtPB_ndt += AtPBtmp[k]; + } + } + } + } + std::cout << "cleaning start" << std::endl; + points_global_external.clear(); + index_pair_external.clear(); + buckets_external.clear(); + std::cout << "cleaning finished" << std::endl; + + rms /= sum; + std::cout << "rms " << rms << std::endl; + + md /= md_sum; + std::cout << "mean mahalanobis distance: " << md << std::endl; + + if (compute_only_mahalanobis_distance) + { + return true; + } +#else + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector points_global; + for (size_t i = 0; i < point_clouds.size(); i++) + { + for (size_t j = 0; j < point_clouds[i].points_local.size(); j++) + { + Eigen::Vector3d vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; + Point3D p; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = i; + if (p.x > -250 && p.x < 250 && p.y > -250 && p.y < 250 && p.z > -50 && p.z < 50) + points_global.push_back(p); + } + } + + std::vector index_pair; + std::vector buckets; + + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + } + + std::vector mposes; + std::vector mposes_inv; + for (size_t i = 0; i < point_clouds.size(); i++) + { + mposes.push_back(point_clouds[i].m_pose); + mposes_inv.push_back(point_clouds[i].m_pose.inverse()); + } + + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + point_clouds.size(), + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + + double rms = 0.0; + int sum = 0; + for (size_t k = 0; k < jobs.size(); k++) + { + rms += sumrmss[k]; + sum += sums[k]; + } + rms /= sum; + std::cout << "rms " << rms << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + Eigen::SparseMatrix AtPB_ndt(point_clouds.size() * number_of_unknowns, 1); + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt = AtPAtmp[k]; + AtPB_ndt = AtPBtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + AtPB_ndt += AtPBtmp[k]; + } + } + } +#endif + ////////////////////////////////////////////////////////////////// + + if (is_fix_first_node) + { + Eigen::SparseMatrix I(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + for (int ii = 0; ii < number_of_unknowns; ii++) + { + I.coeffRef(ii, ii) = 1000000; + } + AtPA_ndt += I; + } + + std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; + if (is_levenberg_marguardt) + { + if (rms < previous_rms) + { + if (lm_lambda < 1000000) + { + lm_lambda *= 10.0; + } + previous_rms = rms; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + } + else + { + lm_lambda /= 10.0; + number_of_lm_iterations++; + iter--; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + for (size_t i = 0; i < point_clouds.size(); i++) + { + point_clouds[i].m_pose = m_poses_tmp[i]; + } + previous_rms = std::numeric_limits::max(); + continue; + } + } + else + { + previous_rms = rms; + } + + if (is_quaternion) + { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + for (size_t i = 0; i < point_clouds.size(); i++) + { + int ic = i * 7; + int ir = 0; + QuaternionPose pose; + if (is_wc) + { + pose = pose_quaternion_from_affine_matrix(point_clouds[i].m_pose); + } + else + { + pose = pose_quaternion_from_affine_matrix(point_clouds[i].m_pose.inverse()); + } + double delta; + quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); + + Eigen::Matrix jacobian; + quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); + + tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); + tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); + + tripletListP.emplace_back(ir, ir, 1000000.0); + + tripletListB.emplace_back(ir, 0, delta); + } + Eigen::SparseMatrix matA(tripletListB.size(), point_clouds.size() * 7); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(point_clouds.size() * 7, point_clouds.size() * 7); + Eigen::SparseMatrix AtPB(point_clouds.size() * 7, 1); + + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = AtP * matA; + AtPB = AtP * matB; + + AtPA_ndt += AtPA; + AtPB_ndt += AtPB; + } + + if (is_levenberg_marguardt) + { + Eigen::SparseMatrix LM(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + LM.setIdentity(); + LM *= lm_lambda; + AtPA_ndt += LM; + } + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA_ndt); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB_ndt); + + std::vector h_x; + std::cout << "redult: row,col,value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + if (it.value() == it.value()) + { + h_x.push_back(it.value()); + std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; + } + } + } + + if (h_x.size() == point_clouds.size() * number_of_unknowns) + { + std::cout << "AtPA=AtPB SOLVED" << std::endl; + int counter = 0; + for (size_t i = 0; i < point_clouds.size(); i++) + { + Eigen::Affine3d m_pose; + + if (is_wc) + { + m_pose = point_clouds[i].m_pose; + } + else + { + m_pose = point_clouds[i].m_pose.inverse(); + } + + if (is_tait_bryan_angles) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + else if (is_rodrigues) + { + RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.sx += h_x[counter++]; + pose.sy += h_x[counter++]; + pose.sz += h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose); + } + else if (is_quaternion) + { + QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); + + QuaternionPose poseq; + poseq.px = h_x[counter++]; + poseq.py = h_x[counter++]; + poseq.pz = h_x[counter++]; + poseq.q0 = h_x[counter++]; + poseq.q1 = h_x[counter++]; + poseq.q2 = h_x[counter++]; + poseq.q3 = h_x[counter++]; + + if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && + fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && + fabs(poseq.q3) < 10) + { + pose.px += poseq.px; + pose.py += poseq.py; + pose.pz += poseq.pz; + pose.q0 += poseq.q0; + pose.q1 += poseq.q1; + pose.q2 += poseq.q2; + pose.q3 += poseq.q3; + m_pose = affine_matrix_from_pose_quaternion(pose); + } + } + else if (is_lie_algebra_left_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; + } + else if (is_lie_algebra_right_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); + } + + if (is_wc) + { + // point_clouds[i].m_pose = m_pose; + } + else + { + m_pose = m_pose.inverse(); + } + + auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); + auto pose_src = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); + + if (!point_clouds[i].fixed_x) + { + pose_src.px = pose_res.px; + } + if (!point_clouds[i].fixed_y) + { + pose_src.py = pose_res.py; + } + if (!point_clouds[i].fixed_z) + { + pose_src.pz = pose_res.pz; + } + if (!point_clouds[i].fixed_om) + { + pose_src.om = pose_res.om; + } + if (!point_clouds[i].fixed_fi) + { + pose_src.fi = pose_res.fi; + } + if (!point_clouds[i].fixed_ka) + { + pose_src.ka = pose_res.ka; + } + + point_clouds[i].m_pose = affine_matrix_from_pose_tait_bryan(pose_src); + point_clouds[i].pose = pose_src; + point_clouds[i].gui_translation[0] = pose_src.px; + point_clouds[i].gui_translation[1] = pose_src.py; + point_clouds[i].gui_translation[2] = pose_src.pz; + point_clouds[i].gui_rotation[0] = rad2deg(pose_src.om); + point_clouds[i].gui_rotation[1] = rad2deg(pose_src.fi); + point_clouds[i].gui_rotation[2] = rad2deg(pose_src.ka); + + /*if (is_wc) + { + point_clouds[i].m_pose = m_pose; + } + else + { + point_clouds[i].m_pose = m_pose.inverse(); + } + if (!point_clouds[i].fixed) + { + point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); + point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; + point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; + point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; + point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); + point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); + point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); + }*/ + } + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + for (size_t i = 0; i < point_clouds.size(); i++) + { + m_poses_tmp.push_back(point_clouds[i].m_pose); + } + } + + std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; + } + else + { + std::cout << "AtPA=AtPB FAILED" << std::endl; + break; + } + } + + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; + + return true; } -bool NDT::optimize_lie_algebra_left_jacobian(std::vector &point_clouds, bool compute_mean_and_cov_for_bucket) +bool NDT::optimize(std::vector& sessions, bool compute_only_mahalanobis_distance, bool compute_mean_and_cov_for_bucket) { - is_tait_bryan_angles = false; - is_quaternion = false; - is_rodrigues = false; - is_lie_algebra_left_jacobian = true; - is_lie_algebra_right_jacobian = false; - - bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); - - is_tait_bryan_angles = true; - is_quaternion = false; - is_rodrigues = false; - is_lie_algebra_left_jacobian = false; - is_lie_algebra_right_jacobian = false; - return true; + std::cout << "optimize sessions" << std::endl; + + auto start = std::chrono::system_clock::now(); + + Session tmp_session; + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + tmp_session = sessions[0]; + PointClouds point_clouds_container; + + std::vector point_clouds; + + std::vector points_local; + + for (const auto& pc : sessions[0].point_clouds_container.point_clouds) + { + for (const auto& p : pc.points_local) + { + Eigen::Vector3d pg = pc.m_pose * p; + points_local.push_back(pg); + } + } + + PointCloud pc; + pc.points_local = points_local; + pc.m_initial_pose = Eigen::Affine3d::Identity(); + pc.m_pose = Eigen::Affine3d::Identity(); + + point_clouds.push_back(pc); + point_clouds_container.point_clouds = point_clouds; + + sessions[0].point_clouds_container = point_clouds_container; + } + } + + OptimizationAlgorithm optimization_algorithm; + if (is_gauss_newton) + { + optimization_algorithm = OptimizationAlgorithm::gauss_newton; + } + if (is_levenberg_marguardt) + { + optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; + } + + PoseConvention pose_convention; + if (is_wc) + { + pose_convention = PoseConvention::wc; + } + if (is_cw) + { + pose_convention = PoseConvention::cw; + } + + RotationMatrixParametrization rotation_matrix_parametrization; + if (is_tait_bryan_angles) + { + rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; + } + else if (is_rodrigues) + { + rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; + } + else if (is_quaternion) + { + rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; + } + else if (is_lie_algebra_left_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_left_jacobian; + } + else if (is_lie_algebra_right_jacobian) + { + rotation_matrix_parametrization = RotationMatrixParametrization::lie_algebra_right_jacobian; + } + + if (is_rodrigues || is_quaternion || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + TaitBryanPose pose; + pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); + pc.m_pose = pc.m_pose * m; + } + } + } + + int number_of_unknowns; + if (is_tait_bryan_angles || is_rodrigues || is_lie_algebra_left_jacobian || is_lie_algebra_right_jacobian) + { + number_of_unknowns = 6; + } + if (is_quaternion) + { + number_of_unknowns = 7; + } + + double lm_lambda = 0.0001; + double previous_rms = std::numeric_limits::max(); + int number_of_lm_iterations = 0; + + std::vector m_poses_tmp; + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + // for (size_t i = 0; i < point_clouds.size(); i++) + //{ + // m_poses_tmp.push_back(point_clouds[i].m_pose); + // } + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + for (int iter = 0; iter < number_of_iterations; iter++) + { + std::cout << "building points_global_external begin" << std::endl; + std::vector points_global_external; + size_t num_total_points = 0; + + // for (int i = 0; i < point_clouds.size(); i++) + //{ + // num_total_points += point_clouds[i].points_local.size(); + // } + + int num_point_clouds = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + num_total_points += pc.points_local.size(); + num_point_clouds++; + } + } + + points_global_external.reserve(num_total_points); + Eigen::Vector3d vt; + Point3D p; + + int index_pose = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // num_total_points += pc.points_local.size(); + std::cout << "processing point_cloud [" << index_pose + 1 << "] of " << num_point_clouds << std::endl; + + for (int j = 0; j < pc.points_local.size(); j++) + { + vt = pc.m_pose * pc.points_local[j]; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = index_pose; + points_global_external.emplace_back(p); + } + index_pose++; + } + } + + std::cout << "building points_global_external end" << std::endl; + + std::vector index_pair_external; + std::vector buckets_external; + + GridParameters rgd_params_external; + rgd_params_external.resolution_X = this->bucket_size_external[0]; + rgd_params_external.resolution_Y = this->bucket_size_external[1]; + rgd_params_external.resolution_Z = this->bucket_size_external[2]; + + int bbext = this->bucket_size_external[0]; + if (this->bucket_size_external[1] > bbext) + bbext = this->bucket_size_external[1]; + if (this->bucket_size_external[2] > bbext) + bbext = this->bucket_size_external[2]; + rgd_params_external.bounding_box_extension = bbext; + + std::cout << "building external grid begin" << std::endl; + grid_calculate_params(points_global_external, rgd_params_external); + int num_threads = 1; + if (buckets_external.size() > this->number_of_threads) + { + num_threads = this->number_of_threads; + } + build_rgd(points_global_external, index_pair_external, buckets_external, rgd_params_external, num_threads); + std::vector buckets_external_reduced; + + for (const auto& b : buckets_external) + { + // std::cout << "number of points inside bucket: " << b.number_of_points << std::endl; + if (b.number_of_points > 1000) + { + buckets_external_reduced.push_back(b); + } + } + buckets_external = buckets_external_reduced; + buckets_external_reduced.clear(); + + std::sort( + buckets_external.begin(), + buckets_external.end(), + [](const Bucket& a, const Bucket& b) + { + return (a.number_of_points > b.number_of_points); + }); + + std::cout << "building external grid end" << std::endl; + std::cout << "number active buckets external: " << buckets_external.size() << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + Eigen::SparseMatrix AtPB_ndt(num_point_clouds * number_of_unknowns, 1); + double rms = 0.0; + int sum = 0; + double md = 0.0; + double md_sum = 0.0; + + for (int bi = 0; bi < buckets_external.size(); bi++) + { + if (compute_only_mahalanobis_distance) + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << std::endl; + } + else + { + std::cout << "bucket [" << bi + 1 << "] of " << buckets_external.size() << " | iteration [" << iter + 1 << "] of " + << number_of_iterations << " | number of points: " << buckets_external[bi].number_of_points << std::endl; + } + std::vector points_global; + + for (size_t index = buckets_external[bi].index_begin; index < buckets_external[bi].index_end; index++) + { + points_global.push_back(points_global_external[index_pair_external[index].index_of_point]); + } + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector index_pair; + std::vector buckets; + + std::cout << "building rgd begin" << std::endl; + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + std::cout << "building rgd end" << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(num_point_clouds * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + mposes.push_back(pc.m_pose); + mposes_inv.push_back(pc.m_pose.inverse()); + } + } + + std::cout << "computing AtPA AtPB start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + num_point_clouds, + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + compute_mean_and_cov_for_bucket)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing AtPA AtPB finished" << std::endl; + + for (size_t k = 0; k < jobs.size(); k++) + { + rms += sumrmss[k]; + sum += sums[k]; + md += md_out[k]; + md_sum += md_count_out[k]; + } + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt = AtPAtmp[k]; + AtPB_ndt = AtPBtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + AtPB_ndt += AtPBtmp[k]; + } + } + } + } + std::cout << "cleaning start" << std::endl; + points_global_external.clear(); + index_pair_external.clear(); + buckets_external.clear(); + std::cout << "cleaning finished" << std::endl; + + rms /= sum; + std::cout << "rms " << rms << std::endl; + + md /= md_sum; + std::cout << "mean mahalanobis distance: " << md << std::endl; + + if (compute_only_mahalanobis_distance) + { + return true; + } + + ////////////////////////////////////////////////////////////////// + + if (is_fix_first_node) + { + Eigen::SparseMatrix I(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + for (int ii = 0; ii < number_of_unknowns; ii++) + { + I.coeffRef(ii, ii) = 1000000; + } + AtPA_ndt += I; + } + + std::cout << "previous_rms: " << previous_rms << " rms: " << rms << std::endl; + if (is_levenberg_marguardt) + { + if (rms < previous_rms) + { + if (lm_lambda < 1000000) + { + lm_lambda *= 10.0; + } + previous_rms = rms; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + } + else + { + lm_lambda /= 10.0; + number_of_lm_iterations++; + iter--; + std::cout << " lm_lambda: " << lm_lambda << std::endl; + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + pc.m_pose = m_poses_tmp[index++]; + } + } + + previous_rms = std::numeric_limits::max(); + continue; + } + } + else + { + previous_rms = rms; + } + + if (is_quaternion) + { + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + int index = 0; + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + // pc.m_pose = m_poses_tmp[index++]; + int ic = index * 7; + int ir = 0; + QuaternionPose pose; + if (is_wc) + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose); + } + else + { + pose = pose_quaternion_from_affine_matrix(pc.m_pose.inverse()); + } + double delta; + quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3); + + Eigen::Matrix jacobian; + quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3); + + tripletListA.emplace_back(ir, ic + 3, -jacobian(0, 0)); + tripletListA.emplace_back(ir, ic + 4, -jacobian(0, 1)); + tripletListA.emplace_back(ir, ic + 5, -jacobian(0, 2)); + tripletListA.emplace_back(ir, ic + 6, -jacobian(0, 3)); + + tripletListP.emplace_back(ir, ir, 1000000.0); + + tripletListB.emplace_back(ir, 0, delta); + + index++; + } + } + + Eigen::SparseMatrix matA(tripletListB.size(), num_point_clouds * 7); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(num_point_clouds * 7, num_point_clouds * 7); + Eigen::SparseMatrix AtPB(num_point_clouds * 7, 1); + + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = AtP * matA; + AtPB = AtP * matB; + + AtPA_ndt += AtPA; + AtPB_ndt += AtPB; + } + + if (is_levenberg_marguardt) + { + Eigen::SparseMatrix LM(num_point_clouds * number_of_unknowns, num_point_clouds * number_of_unknowns); + LM.setIdentity(); + LM *= lm_lambda; + AtPA_ndt += LM; + } + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA_ndt); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB_ndt); + + std::vector h_x; + std::cout << "redult: row,col,value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + if (it.value() == it.value()) + { + h_x.push_back(it.value()); + std::cout << it.row() << "," << it.col() << "," << it.value() << std::endl; + } + } + } + + if (h_x.size() == num_point_clouds * number_of_unknowns) + { + std::cout << "AtPA=AtPB SOLVED" << std::endl; + int counter = 0; + + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + Eigen::Affine3d m_pose; + + if (is_wc) + { + m_pose = pc.m_pose; + } + else + { + m_pose = pc.m_pose.inverse(); + } + + if (is_tait_bryan_angles) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + else if (is_rodrigues) + { + RodriguesPose pose = pose_rodrigues_from_affine_matrix(m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.sx += h_x[counter++]; + pose.sy += h_x[counter++]; + pose.sz += h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose); + } + else if (is_quaternion) + { + QuaternionPose pose = pose_quaternion_from_affine_matrix(m_pose); + + QuaternionPose poseq; + poseq.px = h_x[counter++]; + poseq.py = h_x[counter++]; + poseq.pz = h_x[counter++]; + poseq.q0 = h_x[counter++]; + poseq.q1 = h_x[counter++]; + poseq.q2 = h_x[counter++]; + poseq.q3 = h_x[counter++]; + + if (fabs(poseq.px) < this->bucket_size[0] && fabs(poseq.py) < this->bucket_size[0] && + fabs(poseq.pz) < this->bucket_size[0] && fabs(poseq.q0) < 10 && fabs(poseq.q1) < 10 && fabs(poseq.q2) < 10 && + fabs(poseq.q3) < 10) + { + pose.px += poseq.px; + pose.py += poseq.py; + pose.pz += poseq.pz; + pose.q0 += poseq.q0; + pose.q1 += poseq.q1; + pose.q2 += poseq.q2; + pose.q3 += poseq.q3; + m_pose = affine_matrix_from_pose_quaternion(pose); + } + } + else if (is_lie_algebra_left_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = affine_matrix_from_pose_rodrigues(pose_update) * m_pose; + } + else if (is_lie_algebra_right_jacobian) + { + RodriguesPose pose_update; + pose_update.px = h_x[counter++]; + pose_update.py = h_x[counter++]; + pose_update.pz = h_x[counter++]; + pose_update.sx = h_x[counter++]; + pose_update.sy = h_x[counter++]; + pose_update.sz = h_x[counter++]; + m_pose = m_pose * affine_matrix_from_pose_rodrigues(pose_update); + } + + if (is_wc) + { + } + else + { + m_pose = m_pose.inverse(); + } + + auto pose_res = pose_tait_bryan_from_affine_matrix(m_pose); + auto pose_src = pose_tait_bryan_from_affine_matrix(pc.m_pose); + + if (!pc.fixed_x) + { + pose_src.px = pose_res.px; + } + if (!pc.fixed_y) + { + pose_src.py = pose_res.py; + } + if (!pc.fixed_z) + { + pose_src.pz = pose_res.pz; + } + if (!pc.fixed_om) + { + pose_src.om = pose_res.om; + } + if (!pc.fixed_fi) + { + pose_src.fi = pose_res.fi; + } + if (!pc.fixed_ka) + { + pose_src.ka = pose_res.ka; + } + + pc.pose = pose_src; + pc.gui_translation[0] = pose_src.px; + pc.gui_translation[1] = pose_src.py; + pc.gui_translation[2] = pose_src.pz; + pc.gui_rotation[0] = rad2deg(pose_src.om); + pc.gui_rotation[1] = rad2deg(pose_src.fi); + pc.gui_rotation[2] = rad2deg(pose_src.ka); + + /* + if (is_wc) + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose; // ToDo check if !pc.fixed needed + //} + } + else + { + // if (!s.is_ground_truth) + //{ + pc.m_pose = m_pose.inverse(); // ToDo check if !pc.fixed needed + //} + } + + if (!pc.fixed) + { + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + }*/ + } + } + if (is_levenberg_marguardt) + { + m_poses_tmp.clear(); + for (auto& s : sessions) + { + for (auto& pc : s.point_clouds_container.point_clouds) + { + m_poses_tmp.push_back(pc.m_pose); + } + } + } + + std::cout << "iteration: " << iter + 1 << " of " << number_of_iterations << std::endl; + } + else + { + std::cout << "AtPA=AtPB FAILED" << std::endl; + break; + } + } + + ////////// + + if (sessions.size() > 1) + { + if (sessions[0].is_ground_truth) + { + Eigen::Affine3d pose_inv0 = sessions[0].point_clouds_container.point_clouds[0].m_pose.inverse(); + + sessions[0].point_clouds_container = tmp_session.point_clouds_container; + + for (int i = 1; i < sessions.size(); i++) + { + for (int j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + { + sessions[i].point_clouds_container.point_clouds[j].m_pose = + sessions[i].point_clouds_container.point_clouds[j].m_pose * pose_inv0; + 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] = + sessions[i].point_clouds_container.point_clouds[j].pose.px; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = + sessions[i].point_clouds_container.point_clouds[j].pose.py; + sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = + sessions[i].point_clouds_container.point_clouds[j].pose.pz; + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.om); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.fi); + sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = + rad2deg(sessions[i].point_clouds_container.point_clouds[j].pose.ka); + } + } + } + } + ////////// + + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + std::cout << "ndt execution time [ms]: " << elapsed.count() << std::endl; + + return true; } -bool NDT::optimize_lie_algebra_right_jacobian(std::vector &point_clouds, bool compute_mean_and_cov_for_bucket) +std::vector> NDT::compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms) { - is_tait_bryan_angles = false; - is_quaternion = false; - is_rodrigues = false; - is_lie_algebra_left_jacobian = false; - is_lie_algebra_right_jacobian = true; - - bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); - - is_tait_bryan_angles = true; - is_quaternion = false; - is_rodrigues = false; - is_lie_algebra_left_jacobian = false; - is_lie_algebra_right_jacobian = false; - return true; + OptimizationAlgorithm optimization_algorithm; + if (is_gauss_newton) + { + optimization_algorithm = OptimizationAlgorithm::gauss_newton; + } + if (is_levenberg_marguardt) + { + optimization_algorithm = OptimizationAlgorithm::levenberg_marguardt; + } + + PoseConvention pose_convention; + if (is_wc) + { + pose_convention = PoseConvention::wc; + } + if (is_cw) + { + pose_convention = PoseConvention::cw; + } + + RotationMatrixParametrization rotation_matrix_parametrization; + if (is_tait_bryan_angles) + { + rotation_matrix_parametrization = RotationMatrixParametrization::tait_bryan_xyz; + } + if (is_rodrigues) + { + rotation_matrix_parametrization = RotationMatrixParametrization::rodrigues; + } + if (is_quaternion) + { + rotation_matrix_parametrization = RotationMatrixParametrization::quaternion; + } + + if (is_rodrigues || is_quaternion) + { + for (size_t i = 0; i < point_clouds.size(); i++) + { + TaitBryanPose pose; + pose.px = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.py = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.pz = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.om = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.fi = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + pose.ka = (((rand() % 1000000000) / 1000000000.0) - 0.5) * 2.0 * 0.000001; + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(pose); + point_clouds[i].m_pose = point_clouds[i].m_pose * m; + } + } + + int number_of_unknowns; + if (is_tait_bryan_angles || is_rodrigues) + { + number_of_unknowns = 6; + } + if (is_quaternion) + { + number_of_unknowns = 7; + } + + std::vector> covariance_matrices; + + for (size_t i = 0; i < point_clouds.size(); i++) + { + Eigen::SparseMatrix covariance_matrix(number_of_unknowns, number_of_unknowns); + covariance_matrices.push_back(covariance_matrix); + } + + GridParameters rgd_params; + rgd_params.resolution_X = this->bucket_size[0]; + rgd_params.resolution_Y = this->bucket_size[1]; + rgd_params.resolution_Z = this->bucket_size[2]; + rgd_params.bounding_box_extension = 1.0; + + std::vector points_global; + for (size_t i = 0; i < point_clouds.size(); i++) + { + for (size_t j = 0; j < point_clouds[i].points_local.size(); j++) + { + Eigen::Vector3d vt = point_clouds[i].m_pose * point_clouds[i].points_local[j]; + Point3D p; + p.x = vt.x(); + p.y = vt.y(); + p.z = vt.z(); + p.index_pose = i; + points_global.push_back(p); + } + } + + std::vector index_pair; + std::vector buckets; + + grid_calculate_params(points_global, rgd_params); + build_rgd(points_global, index_pair, buckets, rgd_params, this->number_of_threads); + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(point_clouds.size() * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + for (size_t i = 0; i < point_clouds.size(); i++) + { + // poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose)); + mposes.push_back(point_clouds[i].m_pose); + mposes_inv.push_back(point_clouds[i].m_pose.inverse()); + } + // std::cout << "XXX" << std::endl; + // std::cout << (int)pose_convention << " " << (int)rotation_matrix_parametrization << " " << (int)number_of_unknowns << std::endl; + + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points_global, + &mposes, + &mposes_inv, + point_clouds.size(), + pose_convention, + rotation_matrix_parametrization, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + false, + false)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + double ssr = 0.0; + int num_obs = 0; + for (size_t k = 0; k < jobs.size(); k++) + { + ssr += sumrmss[k]; + num_obs += sums[k]; + } + + double sq = ssr / ((double)num_obs - point_clouds.size() * number_of_unknowns); + rms = ssr / (double)num_obs; + + // std::cout << "sq " << sq << " num_obs " << num_obs << std::endl; + + bool init = false; + Eigen::SparseMatrix AtPA_ndt(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + + for (size_t k = 0; k < jobs.size(); k++) + { + if (!init) + { + if (AtPBtmp[k].size() > 0) + { + // + AtPA_ndt = AtPAtmp[k]; + init = true; + } + } + else + { + if (AtPBtmp[k].size() > 0) + { + AtPA_ndt += AtPAtmp[k]; + } + } + } + + AtPA_ndt = 0.5 * AtPA_ndt; + + Eigen::SimplicialCholesky> solver(AtPA_ndt); + Eigen::SparseMatrix I(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + I.setIdentity(); + + Eigen::SparseMatrix AtAinv(point_clouds.size() * number_of_unknowns, point_clouds.size() * number_of_unknowns); + AtAinv = solver.solve(I); + + AtAinv = AtAinv * sq; + + for (int i = 0; i < point_clouds.size(); i++) + { + Eigen::SparseMatrix cm(number_of_unknowns, number_of_unknowns); + for (int r = 0; r < number_of_unknowns; r++) + { + for (int c = 0; c < number_of_unknowns; c++) + { + cm.coeffRef(r, c) = AtAinv.coeff(i * number_of_unknowns + r, i * number_of_unknowns + c); + } + } + covariance_matrices.push_back(cm); + // std::cout << "cm" << std::endl; + // std::cout << cm << std::endl; + } + return covariance_matrices; } -bool NDT::compute_cov_mean(std::vector &points, std::vector &index_pair, std::vector &buckets, GridParameters &rgd_params, - double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, int num_threads) +bool NDT::optimize( + std::vector& point_clouds, double& rms_initial, double& rms_final, double& mui, bool compute_mean_and_cov_for_bucket) { - int number_of_unknowns = 6; - - grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); - build_rgd(points, index_pair, buckets, rgd_params); - std::cout << "buckets.size() " << buckets.size() << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, points.size() * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - mposes.push_back(Eigen::Affine3d::Identity()); - mposes_inv.push_back(Eigen::Affine3d::Identity()); - - // for (size_t i = 0; i < points.size(); i++) - //{ - // mposes.push_back(points[i].m_pose); - // mposes_inv.push_back(points[i].m_pose.inverse()); - // } - - std::cout << "computing cov mean start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_job, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points, &mposes, &mposes_inv, points.size(), - PoseConvention::wc, RotationMatrixParametrization::tait_bryan_xyz, - number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, - num_extended_points, &(md_out[k]), &(md_count_out[k]), true, false)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing cov mean finished" << std::endl; - - // for(int i = 0; i < buckets.size(); i++){ - // if(buckets[i].number_of_points > 5){ - // std::cout << i << " " << buckets[i].cov << std::endl; - // } - // } - buckets[0].number_of_points = 0; - for (auto &b : buckets) - { - if (b.number_of_points < 5) - { - b.number_of_points = 0; - } - } - return true; + // double rms; + // std::vector> cm_before = compute_covariance_matrices_and_rms(point_clouds, rms); + // rms_initial = rms; + //-- + bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); + //-- + // std::vector> cm_after = compute_covariance_matrices_and_rms(point_clouds, rms); + // rms_final = rms; + // mui = get_mean_uncertainty_xyz_impact(cm_before, cm_after); + return res; } -bool NDT::compute_cov_mean(std::vector &points, - std::vector &index_pair, - std::vector &buckets, GridParameters &rgd_params, - int num_threads) +bool NDT::optimize_lie_algebra_left_jacobian(std::vector& point_clouds, bool compute_mean_and_cov_for_bucket) { - int number_of_unknowns = 6; - - std::cout << "grid_calculate_params start" << std::endl; - grid_calculate_params(points, rgd_params); - // for(auto &p:points){ - // std::cout << p.point; - // } - std::cout << "grid_calculate_params finished" << std::endl; - - // grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); - std::cout << "build_rgd start" << std::endl; - build_rgd(points, index_pair, buckets, rgd_params); - std::cout << "build_rgd finished" << std::endl; - - std::cout << "check" << std::endl; - int counter_active_buckets = 0; - for (int i = 0; i < buckets.size(); i++) - { - if (buckets[i].number_of_points > 5) - { - counter_active_buckets++; - } - } - std::cout << "check: counter_active_buckets: " << counter_active_buckets << std::endl; - - std::cout << "buckets.size() " << buckets.size() << std::endl; - - std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); - - std::vector threads; - - std::vector> AtPAtmp(jobs.size()); - std::vector> AtPBtmp(jobs.size()); - std::vector sumrmss(jobs.size()); - std::vector sums(jobs.size()); - - std::vector md_out(jobs.size()); - std::vector md_count_out(jobs.size()); - // double *md_out, double *md_count_out - - for (size_t i = 0; i < jobs.size(); i++) - { - AtPAtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, points.size() * number_of_unknowns); - AtPBtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, 1); - sumrmss[i] = 0; - sums[i] = 0; - md_out[i] = 0.0; - md_count_out[i] = 0.0; - } - - std::vector mposes; - std::vector mposes_inv; - mposes.push_back(Eigen::Affine3d::Identity()); - mposes_inv.push_back(Eigen::Affine3d::Identity()); - - // for (size_t i = 0; i < points.size(); i++) - //{ - // mposes.push_back(points[i].m_pose); - // mposes_inv.push_back(points[i].m_pose.inverse()); - // } - - std::cout << "computing cov mean ndt_jobi start" << std::endl; - for (size_t k = 0; k < jobs.size(); k++) - { - threads.push_back(std::thread(ndt_jobi, k, &jobs[k], &buckets, &(AtPAtmp[k]), &(AtPBtmp[k]), - &index_pair, &points, &mposes, &mposes_inv, points.size(), - PoseConvention::wc, RotationMatrixParametrization::tait_bryan_xyz, - number_of_unknowns, &(sumrmss[k]), &(sums[k]), - is_generalized, sigma_r, sigma_polar_angle, sigma_azimuthal_angle, - num_extended_points, &(md_out[k]), &(md_count_out[k]), true)); - } + is_tait_bryan_angles = false; + is_quaternion = false; + is_rodrigues = false; + is_lie_algebra_left_jacobian = true; + is_lie_algebra_right_jacobian = false; + + bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); + + is_tait_bryan_angles = true; + is_quaternion = false; + is_rodrigues = false; + is_lie_algebra_left_jacobian = false; + is_lie_algebra_right_jacobian = false; + return true; +} - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - std::cout << "computing cov mean ndt_jobi finished" << std::endl; +bool NDT::optimize_lie_algebra_right_jacobian(std::vector& point_clouds, bool compute_mean_and_cov_for_bucket) +{ + is_tait_bryan_angles = false; + is_quaternion = false; + is_rodrigues = false; + is_lie_algebra_left_jacobian = false; + is_lie_algebra_right_jacobian = true; + + bool res = optimize(point_clouds, false, compute_mean_and_cov_for_bucket); + + is_tait_bryan_angles = true; + is_quaternion = false; + is_rodrigues = false; + is_lie_algebra_left_jacobian = false; + is_lie_algebra_right_jacobian = false; + return true; +} - // for(int i = 0; i < buckets.size(); i++){ - // if(buckets[i].number_of_points > 5){ - // std::cout << i << " " << buckets[i].cov << std::endl; - // } - // } - buckets[0].number_of_points = 0; +bool NDT::compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + double min_x, + double max_x, + double min_y, + double max_y, + double min_z, + double max_z, + int num_threads) +{ + int number_of_unknowns = 6; + + grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); + build_rgd(points, index_pair, buckets, rgd_params); + std::cout << "buckets.size() " << buckets.size() << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, points.size() * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + mposes.push_back(Eigen::Affine3d::Identity()); + mposes_inv.push_back(Eigen::Affine3d::Identity()); + + // for (size_t i = 0; i < points.size(); i++) + //{ + // mposes.push_back(points[i].m_pose); + // mposes_inv.push_back(points[i].m_pose.inverse()); + // } + + std::cout << "computing cov mean start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_job, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points, + &mposes, + &mposes_inv, + points.size(), + PoseConvention::wc, + RotationMatrixParametrization::tait_bryan_xyz, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + true, + false)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing cov mean finished" << std::endl; + + // for(int i = 0; i < buckets.size(); i++){ + // if(buckets[i].number_of_points > 5){ + // std::cout << i << " " << buckets[i].cov << std::endl; + // } + // } + buckets[0].number_of_points = 0; + for (auto& b : buckets) + { + if (b.number_of_points < 5) + { + b.number_of_points = 0; + } + } + return true; +} - // int counter_active_buckets = 0; - for (auto &b : buckets) - { - if (b.number_of_points < 5) - { - b.number_of_points = 0; - } - else - { - // counter_active_buckets ++; - } - } - // std::cout << "counter_active_buckets: " << counter_active_buckets << std::endl; - return true; +bool NDT::compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::vector& buckets, + GridParameters& rgd_params, + int num_threads) +{ + int number_of_unknowns = 6; + + std::cout << "grid_calculate_params start" << std::endl; + grid_calculate_params(points, rgd_params); + // for(auto &p:points){ + // std::cout << p.point; + // } + std::cout << "grid_calculate_params finished" << std::endl; + + // grid_calculate_params(rgd_params, min_x, max_x, min_y, max_y, min_z, max_z); + std::cout << "build_rgd start" << std::endl; + build_rgd(points, index_pair, buckets, rgd_params); + std::cout << "build_rgd finished" << std::endl; + + std::cout << "check" << std::endl; + int counter_active_buckets = 0; + for (int i = 0; i < buckets.size(); i++) + { + if (buckets[i].number_of_points > 5) + { + counter_active_buckets++; + } + } + std::cout << "check: counter_active_buckets: " << counter_active_buckets << std::endl; + + std::cout << "buckets.size() " << buckets.size() << std::endl; + + std::vector jobs = get_jobs(buckets.size(), this->number_of_threads); + + std::vector threads; + + std::vector> AtPAtmp(jobs.size()); + std::vector> AtPBtmp(jobs.size()); + std::vector sumrmss(jobs.size()); + std::vector sums(jobs.size()); + + std::vector md_out(jobs.size()); + std::vector md_count_out(jobs.size()); + // double *md_out, double *md_count_out + + for (size_t i = 0; i < jobs.size(); i++) + { + AtPAtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, points.size() * number_of_unknowns); + AtPBtmp[i] = Eigen::SparseMatrix(points.size() * number_of_unknowns, 1); + sumrmss[i] = 0; + sums[i] = 0; + md_out[i] = 0.0; + md_count_out[i] = 0.0; + } + + std::vector mposes; + std::vector mposes_inv; + mposes.push_back(Eigen::Affine3d::Identity()); + mposes_inv.push_back(Eigen::Affine3d::Identity()); + + // for (size_t i = 0; i < points.size(); i++) + //{ + // mposes.push_back(points[i].m_pose); + // mposes_inv.push_back(points[i].m_pose.inverse()); + // } + + std::cout << "computing cov mean ndt_jobi start" << std::endl; + for (size_t k = 0; k < jobs.size(); k++) + { + threads.push_back(std::thread( + ndt_jobi, + k, + &jobs[k], + &buckets, + &(AtPAtmp[k]), + &(AtPBtmp[k]), + &index_pair, + &points, + &mposes, + &mposes_inv, + points.size(), + PoseConvention::wc, + RotationMatrixParametrization::tait_bryan_xyz, + number_of_unknowns, + &(sumrmss[k]), + &(sums[k]), + is_generalized, + sigma_r, + sigma_polar_angle, + sigma_azimuthal_angle, + num_extended_points, + &(md_out[k]), + &(md_count_out[k]), + true)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + std::cout << "computing cov mean ndt_jobi finished" << std::endl; + + // for(int i = 0; i < buckets.size(); i++){ + // if(buckets[i].number_of_points > 5){ + // std::cout << i << " " << buckets[i].cov << std::endl; + // } + // } + buckets[0].number_of_points = 0; + + // int counter_active_buckets = 0; + for (auto& b : buckets) + { + if (b.number_of_points < 5) + { + b.number_of_points = 0; + } + else + { + // counter_active_buckets ++; + } + } + // std::cout << "counter_active_buckets: " << counter_active_buckets << std::endl; + return true; } -bool NDT::compute_cov_mean(std::vector &points, - std::vector &index_pair, - std::map &buckets, GridParameters &rgd_params, - int num_threads) +bool NDT::compute_cov_mean( + std::vector& points, + std::vector& index_pair, + std::map& buckets, + GridParameters& rgd_params, + int num_threads) { #if 0 int number_of_unknowns = 6; @@ -3801,67 +4133,72 @@ bool NDT::compute_cov_mean(std::vector &points, //std::cout << "counter_active_buckets: " << counter_active_buckets << std::endl; #endif - return true; + return true; } -void NDT::build_rgd(std::vector &points, std::vector &index_pair, std::map &buckets, NDT::GridParameters &rgd_params, int num_threads) +void NDT::build_rgd( + std::vector& points, + std::vector& index_pair, + std::map& buckets, + NDT::GridParameters& rgd_params, + int num_threads) { - if (num_threads < 1) - num_threads = 1; - - index_pair.resize(points.size()); - std::cout << "reindex start" << std::endl; - reindex(points, index_pair, rgd_params, num_threads); - std::cout << "reindex finished" << std::endl; - - // buckets.resize(rgd_params.number_of_buckets); - - // std::vector jobs = get_jobs(buckets.size(), num_threads); - // std::vector threads; - - // std::cout << "build_rgd_init_jobs start" << std::endl; - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); - // } - - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_init_jobs finished" << std::endl; - - // jobs = get_jobs(points.size(), num_threads); - - // std::cout << "build_rgd_jobs start jobs.size():" << jobs.size() << std::endl; - // std::cout << "points.size() " << points.size() << std::endl; - // std::cout << "index_pair.size() " << index_pair.size() << std::endl; - // std::cout << "buckets.size() " << buckets.size() << std::endl; - - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); - // } - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_jobs finished" << std::endl; - - // jobs = get_jobs(buckets.size(), num_threads); - - // std::cout << "build_rgd_final_jobs start" << std::endl; - // for (size_t i = 0; i < jobs.size(); i++) - //{ - // threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); - // } - - // for (size_t j = 0; j < threads.size(); j++) - //{ - // threads[j].join(); - // } - // threads.clear(); - // std::cout << "build_rgd_final_jobs finished" << std::endl; + if (num_threads < 1) + num_threads = 1; + + index_pair.resize(points.size()); + std::cout << "reindex start" << std::endl; + reindex(points, index_pair, rgd_params, num_threads); + std::cout << "reindex finished" << std::endl; + + // buckets.resize(rgd_params.number_of_buckets); + + // std::vector jobs = get_jobs(buckets.size(), num_threads); + // std::vector threads; + + // std::cout << "build_rgd_init_jobs start" << std::endl; + // for (size_t i = 0; i < jobs.size(); i++) + //{ + // threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); + // } + + // for (size_t j = 0; j < threads.size(); j++) + //{ + // threads[j].join(); + // } + // threads.clear(); + // std::cout << "build_rgd_init_jobs finished" << std::endl; + + // jobs = get_jobs(points.size(), num_threads); + + // std::cout << "build_rgd_jobs start jobs.size():" << jobs.size() << std::endl; + // std::cout << "points.size() " << points.size() << std::endl; + // std::cout << "index_pair.size() " << index_pair.size() << std::endl; + // std::cout << "buckets.size() " << buckets.size() << std::endl; + + // for (size_t i = 0; i < jobs.size(); i++) + //{ + // threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pair, &buckets)); + // } + // for (size_t j = 0; j < threads.size(); j++) + //{ + // threads[j].join(); + // } + // threads.clear(); + // std::cout << "build_rgd_jobs finished" << std::endl; + + // jobs = get_jobs(buckets.size(), num_threads); + + // std::cout << "build_rgd_final_jobs start" << std::endl; + // for (size_t i = 0; i < jobs.size(); i++) + //{ + // threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); + // } + + // for (size_t j = 0; j < threads.size(); j++) + //{ + // threads[j].join(); + // } + // threads.clear(); + // std::cout << "build_rgd_final_jobs finished" << std::endl; } \ No newline at end of file diff --git a/core/src/nmea.cpp b/core/src/nmea.cpp index ae36085a..91ea3f3d 100644 --- a/core/src/nmea.cpp +++ b/core/src/nmea.cpp @@ -1,120 +1,128 @@ +#include + #include "nmea.h" -#include - -namespace hd_mapping::nmea { - -double dm_to_dd(const std::string& dm, char direction, bool is_latitude) { - if (dm.empty()) return 0.0; - - size_t deg_len = is_latitude ? 2 : 3; - - double degrees = std::stod(dm.substr(0, deg_len)); - double minutes = std::stod(dm.substr(deg_len)); - - double dd = degrees + minutes / 60.0; - if (direction == 'S' || direction == 'W') dd *= -1.0; - - return dd; -} - -std::tuple -BreakLineFromNMEAFile(const std::string &line) { - std::istringstream iss(line); - uint64_t timestampLidar; - uint64_t timestampUnix; - std::string nmeaSentence; - iss >> timestampLidar >> timestampUnix; - char data; - iss.read(&data, 1); - std::getline(iss, nmeaSentence); - return std::make_tuple(timestampLidar, timestampUnix, - nmeaSentence); -} - -bool validateNMEAChecksum(const std::string &nmea) { - if (nmea.empty() || nmea[0] != '$') { - return false; - } - auto asterisk = nmea.find('*'); - if (asterisk == std::string::npos || asterisk + 3 > nmea.size()) - return false; - unsigned char checksum = 0; - for (size_t i = 1; i < asterisk; ++i) { - checksum ^= nmea[i]; - } - unsigned int expected; - std::istringstream iss(nmea.substr(asterisk + 1, 2)); - iss >> std::hex >> expected; - return checksum == expected; -} - -std::optional parseGNRMC(const std::string &nmea) { - if (nmea.find("$GNRMC") != 0 && nmea.find("$GPRMC") != 0) - return std::nullopt; - - std::vector fields; - std::stringstream ss(nmea); - std::string item; - while (std::getline(ss, item, ',')) { - fields.push_back(item); - } - if (fields.size() < 12) - return std::nullopt; - - const auto& latStr = fields[3]; - const auto& lonStr = fields[5]; - const char latDir = fields[4].empty() ? 'N' : fields[4][0]; - const char lonDir = fields[6].empty() ? 'E' : fields[6][0]; - - GNRMCData data; - data.time_utc = fields[1]; - data.status = fields[2].empty() ? 'V' : fields[2][0]; - data.latitude = dm_to_dd(latStr, latDir, true); - data.lat_dir = latDir; - data.longitude = dm_to_dd(lonStr, lonDir, false); - data.lon_dir = lonDir; - data.speed_knots = fields[7].empty() ? 0.0 : std::stod(fields[7]); - data.track_angle = fields[8].empty() ? 0.0 : std::stod(fields[8]); - data.date = fields[9]; - data.magnetic_variation = fields[10].empty() ? 0.0 : std::stod(fields[10]); - data.mag_var_dir = fields[11].empty() ? 'E' : fields[11][0]; - - return data; -} - -std::optional parseGNGGA(const std::string &nmea) { - if (nmea.find("$GNGGA") != 0 && nmea.find("$GPGGA") != 0) - return std::nullopt; - - std::vector fields; - std::stringstream ss(nmea); - std::string item; - while (std::getline(ss, item, ',')) { - fields.push_back(item); - } - if (fields.size() < 15) - return std::nullopt; - - const auto& latStr = fields[2]; - const auto& lonStr = fields[4]; - const char latDir = fields[3].empty() ? 'N' : fields[3][0]; - const char lonDir = fields[5].empty() ? 'E' : fields[5][0]; - - - GNGGAData data; - data.latitude = dm_to_dd(latStr, latDir, true); - data.lat_dir = latDir; - data.longitude = dm_to_dd(lonStr, lonDir, false); - data.lon_dir = lonDir; - data.altitude = fields[9].empty() ? 0.0 : std::stod(fields[9]); - //std::cout << "data.altitude " << data.altitude << std::endl; - data.hdop = fields[8].empty() ? 0.0 : std::stod(fields[8]); - data.fix_quality = fields[6].empty() ? 0 : std::stoi(fields[6]); - data.satellites_tracked = fields[7].empty() ? 0 : std::stoi(fields[7]); - data.age_of_data = - fields.size() > 13 && !fields[13].empty() ? std::stod(fields[13]) : -1.0; - - return data; -} + +namespace hd_mapping::nmea +{ + double dm_to_dd(const std::string& dm, char direction, bool is_latitude) + { + if (dm.empty()) + return 0.0; + + size_t deg_len = is_latitude ? 2 : 3; + + double degrees = std::stod(dm.substr(0, deg_len)); + double minutes = std::stod(dm.substr(deg_len)); + + double dd = degrees + minutes / 60.0; + if (direction == 'S' || direction == 'W') + dd *= -1.0; + + return dd; + } + + std::tuple BreakLineFromNMEAFile(const std::string& line) + { + std::istringstream iss(line); + uint64_t timestampLidar; + uint64_t timestampUnix; + std::string nmeaSentence; + iss >> timestampLidar >> timestampUnix; + char data; + iss.read(&data, 1); + std::getline(iss, nmeaSentence); + return std::make_tuple(timestampLidar, timestampUnix, nmeaSentence); + } + + bool validateNMEAChecksum(const std::string& nmea) + { + if (nmea.empty() || nmea[0] != '$') + { + return false; + } + auto asterisk = nmea.find('*'); + if (asterisk == std::string::npos || asterisk + 3 > nmea.size()) + return false; + unsigned char checksum = 0; + for (size_t i = 1; i < asterisk; ++i) + { + checksum ^= nmea[i]; + } + unsigned int expected; + std::istringstream iss(nmea.substr(asterisk + 1, 2)); + iss >> std::hex >> expected; + return checksum == expected; + } + + std::optional parseGNRMC(const std::string& nmea) + { + if (nmea.find("$GNRMC") != 0 && nmea.find("$GPRMC") != 0) + return std::nullopt; + + std::vector fields; + std::stringstream ss(nmea); + std::string item; + while (std::getline(ss, item, ',')) + { + fields.push_back(item); + } + if (fields.size() < 12) + return std::nullopt; + + const auto& latStr = fields[3]; + const auto& lonStr = fields[5]; + const char latDir = fields[4].empty() ? 'N' : fields[4][0]; + const char lonDir = fields[6].empty() ? 'E' : fields[6][0]; + + GNRMCData data; + data.time_utc = fields[1]; + data.status = fields[2].empty() ? 'V' : fields[2][0]; + data.latitude = dm_to_dd(latStr, latDir, true); + data.lat_dir = latDir; + data.longitude = dm_to_dd(lonStr, lonDir, false); + data.lon_dir = lonDir; + data.speed_knots = fields[7].empty() ? 0.0 : std::stod(fields[7]); + data.track_angle = fields[8].empty() ? 0.0 : std::stod(fields[8]); + data.date = fields[9]; + data.magnetic_variation = fields[10].empty() ? 0.0 : std::stod(fields[10]); + data.mag_var_dir = fields[11].empty() ? 'E' : fields[11][0]; + + return data; + } + + std::optional parseGNGGA(const std::string& nmea) + { + if (nmea.find("$GNGGA") != 0 && nmea.find("$GPGGA") != 0) + return std::nullopt; + + std::vector fields; + std::stringstream ss(nmea); + std::string item; + while (std::getline(ss, item, ',')) + { + fields.push_back(item); + } + if (fields.size() < 15) + return std::nullopt; + + const auto& latStr = fields[2]; + const auto& lonStr = fields[4]; + const char latDir = fields[3].empty() ? 'N' : fields[3][0]; + const char lonDir = fields[5].empty() ? 'E' : fields[5][0]; + + GNGGAData data; + data.latitude = dm_to_dd(latStr, latDir, true); + data.lat_dir = latDir; + data.longitude = dm_to_dd(lonStr, lonDir, false); + data.lon_dir = lonDir; + data.altitude = fields[9].empty() ? 0.0 : std::stod(fields[9]); + // std::cout << "data.altitude " << data.altitude << std::endl; + data.hdop = fields[8].empty() ? 0.0 : std::stod(fields[8]); + data.fix_quality = fields[6].empty() ? 0 : std::stoi(fields[6]); + data.satellites_tracked = fields[7].empty() ? 0 : std::stoi(fields[7]); + data.age_of_data = fields.size() > 13 && !fields[13].empty() ? std::stod(fields[13]) : -1.0; + + return data; + } } // namespace hd_mapping::nmea diff --git a/core/src/nns.cpp b/core/src/nns.cpp deleted file mode 100644 index ab727572..00000000 --- a/core/src/nns.cpp +++ /dev/null @@ -1 +0,0 @@ -#include \ No newline at end of file diff --git a/core/src/observation_picking.cpp b/core/src/observation_picking.cpp index dfa4782c..b91bd984 100644 --- a/core/src/observation_picking.cpp +++ b/core/src/observation_picking.cpp @@ -1,9 +1,8 @@ +#include + #include #include -#include -#include - #include //void renderBitmapString(float x, float y, float z, char* string) { diff --git a/core/src/optimization_point_to_point_source_to_target.cpp b/core/src/optimization_point_to_point_source_to_target.cpp index 2ed4cb9f..3a58d561 100644 --- a/core/src/optimization_point_to_point_source_to_target.cpp +++ b/core/src/optimization_point_to_point_source_to_target.cpp @@ -1,7 +1,6 @@ +#include + #include -#include -#include -#include #include #include diff --git a/core/src/optimize_distance_point_to_plane_source_to_target.cpp b/core/src/optimize_distance_point_to_plane_source_to_target.cpp index fa64c9fd..aaeea927 100644 --- a/core/src/optimize_distance_point_to_plane_source_to_target.cpp +++ b/core/src/optimize_distance_point_to_plane_source_to_target.cpp @@ -1,5 +1,4 @@ -#include -#include +#include #include #include diff --git a/core/src/optimize_plane_to_plane_source_to_target.cpp b/core/src/optimize_plane_to_plane_source_to_target.cpp index afb1be44..a4533aad 100644 --- a/core/src/optimize_plane_to_plane_source_to_target.cpp +++ b/core/src/optimize_plane_to_plane_source_to_target.cpp @@ -1,8 +1,7 @@ +#include + #include #include -#include -#include -#include #include #include diff --git a/core/src/optimize_point_to_plane_source_to_target.cpp b/core/src/optimize_point_to_plane_source_to_target.cpp index 80986cae..3cd0a85b 100644 --- a/core/src/optimize_point_to_plane_source_to_target.cpp +++ b/core/src/optimize_point_to_plane_source_to_target.cpp @@ -1,11 +1,9 @@ -#include -#include +#include #include #include #include - #include #include diff --git a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp index 50c28e38..c8b1c6d0 100644 --- a/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp +++ b/core/src/optimize_point_to_projection_onto_plane_source_to_target.cpp @@ -1,8 +1,7 @@ +#include + #include #include -#include -#include -#include #include #include diff --git a/core/src/pair_wise_iterative_closest_point.cpp b/core/src/pair_wise_iterative_closest_point.cpp index 959ed35a..8fadc8bb 100644 --- a/core/src/pair_wise_iterative_closest_point.cpp +++ b/core/src/pair_wise_iterative_closest_point.cpp @@ -1,13 +1,22 @@ -#include +#include + #include -#include +#include +#include #include #include -#include - -#include -inline void point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_4(Eigen::Matrix &AtPA, const double &tx, const double &ty, const double &tz, const double &om, const double &fi, const double &ka, const double &x_s, const double &y_s, const double &z_s) +inline void point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_4( + Eigen::Matrix& AtPA, + const double& tx, + const double& ty, + const double& tz, + const double& om, + const double& fi, + const double& ka, + const double& x_s, + const double& y_s, + const double& z_s) { double sin_om = sin(om); double cos_om = cos(om); @@ -78,7 +87,20 @@ inline void point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_4(Eige AtPA.coeffRef(5, 4) = x25; AtPA.coeffRef(5, 5) = pow(cos_fi, 2) * pow(x4, 2) + pow(x18, 2) + pow(x21, 2); } -inline void point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_4(Eigen::Matrix &AtPB, const double &tx, const double &ty, const double &tz, const double &om, const double &fi, const double &ka, const double &x_s, const double &y_s, const double &z_s, const double &x_t, const double &y_t, const double &z_t) +inline void point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_4( + Eigen::Matrix& AtPB, + const double& tx, + const double& ty, + const double& tz, + const double& om, + const double& fi, + const double& ka, + const double& x_s, + const double& y_s, + const double& z_s, + const double& x_t, + const double& y_t, + const double& z_t) { double sin_om = sin(om); double cos_om = cos(om); @@ -111,7 +133,12 @@ inline void point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_4(Eige AtPB.coeffRef(5) = -cos_fi * x3 * (cos_ka * y_s + sin_ka * x_s) + x12 * (-x6 * y_s + x9 * x_s) + x16 * (x13 * x_s + x14 * y_s); } -bool PairWiseICP::compute(const std::vector &source, const std::vector &target, double search_radious, int number_of_iterations, Eigen::Affine3d &m_pose_result) +bool PairWiseICP::compute( + const std::vector& source, + const std::vector& target, + double search_radious, + int number_of_iterations, + Eigen::Affine3d& m_pose_result) { std::cout << "PairWiseICP::compute" << std::endl; bool multithread = true; @@ -122,13 +149,17 @@ bool PairWiseICP::compute(const std::vector &source, const std: for (int i = 0; i < target.size(); i++) { - unsigned long long int index = get_rgd_index(target[i], {search_radious, search_radious, search_radious}); + unsigned long long int index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); indexes.emplace_back(index, i); } - std::sort(indexes.begin(), indexes.end(), - [](const std::pair &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -145,7 +176,7 @@ bool PairWiseICP::compute(const std::vector &source, const std: buckets[index_of_bucket].second = i; } } - + std::cout << "iteration: " << iter + 1 << " of: " << number_of_iterations << std::endl; Eigen::MatrixXd AtPA(6, 6); AtPA.setZero(); @@ -155,8 +186,8 @@ bool PairWiseICP::compute(const std::vector &source, const std: std::mutex mutex; int counter_nn = 0; - - const auto hessian_fun = [&](const Eigen::Vector3d &source_i) + + const auto hessian_fun = [&](const Eigen::Vector3d& source_i) { if (source_i.norm() < 0.1) { @@ -176,7 +207,8 @@ bool PairWiseICP::compute(const std::vector &source, const std: for (double z = -search_radious; z <= search_radious; z += search_radious) { Eigen::Vector3d source_point_global_ext = source_point_global + Eigen::Vector3d(x, y, z); - unsigned long long int index_of_bucket = get_rgd_index(source_point_global_ext, {search_radious, search_radious, search_radious}); + unsigned long long int index_of_bucket = + get_rgd_index(source_point_global_ext, { search_radious, search_radious, search_radious }); if (buckets.contains(index_of_bucket)) { @@ -204,28 +236,60 @@ bool PairWiseICP::compute(const std::vector &source, const std: if (nn_found) { - const Eigen::Vector3d &p_s = source_i; + const Eigen::Vector3d& p_s = source_i; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose_result); Eigen::Matrix AtPA_; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( AtPA_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - 1, 0, 0, 0, 1, 0, 0, 0, 1); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + 1, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 1); Eigen::Matrix AtPB_; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( AtPB_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - 1, 0, 0, 0, 1, 0, 0, 0, 1, - target_i_nn.x(), target_i_nn.y(), target_i_nn.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + 1, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 1, + target_i_nn.x(), + target_i_nn.y(), + target_i_nn.z()); std::unique_lock lck(mutex); AtPA.block<6, 6>(0, 0) += AtPA_; AtPB.block<6, 1>(0, 0) -= AtPB_; - counter_nn ++; + counter_nn++; } }; @@ -240,13 +304,18 @@ bool PairWiseICP::compute(const std::vector &source, const std: if (AtPB(0, 0) == 0.0 && AtPB(1, 0) == 0.0 && AtPB(2, 0) == 0.0 && AtPB(3, 0) == 0.0 && AtPB(4, 0) == 0.0 && AtPB(5, 0) == 0.0) { - std::cout << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" << std::endl; + std::cout + << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" + << std::endl; return false; } - if (counter_nn < 100){ + if (counter_nn < 100) + { std::cout << "not suficient number of observations" << std::endl; - std::cout << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" << std::endl; + std::cout + << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" + << std::endl; return false; } @@ -291,7 +360,9 @@ bool PairWiseICP::compute(const std::vector &source, const std: } else { - std::cout << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" << std::endl; + std::cout + << "PairWiseICP::compute FAILED (Relative pose is not changed/solved/optimized --> please consider removing this edge)" + << std::endl; return false; } } @@ -299,7 +370,13 @@ bool PairWiseICP::compute(const std::vector &source, const std: return true; } -bool PairWiseICP::compute_fast(const std::vector &source, const std::vector &target, double search_radious, int number_of_iterations, Eigen::Affine3d &m_pose_result, int dec) +bool PairWiseICP::compute_fast( + const std::vector& source, + const std::vector& target, + double search_radious, + int number_of_iterations, + Eigen::Affine3d& m_pose_result, + int dec) { // std::cout << "PairWiseICP::compute" << std::endl; bool multithread = true; @@ -308,13 +385,17 @@ bool PairWiseICP::compute_fast(const std::vector &source, const for (int i = 0; i < target.size(); i++) { - unsigned long long int index = get_rgd_index(target[i], {search_radious, search_radious, search_radious}); + unsigned long long int index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); indexes.emplace_back(index, i); } - std::sort(indexes.begin(), indexes.end(), - [](const std::pair &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -343,7 +424,7 @@ bool PairWiseICP::compute_fast(const std::vector &source, const std::mutex mutex; - const auto hessian_fun = [&](const Eigen::Vector3d &source_i) + const auto hessian_fun = [&](const Eigen::Vector3d& source_i) { if (source_i.norm() < 0.1) { @@ -363,7 +444,8 @@ bool PairWiseICP::compute_fast(const std::vector &source, const for (double z = -search_radious; z <= search_radious; z += search_radious) { Eigen::Vector3d source_point_global_ext = source_point_global + Eigen::Vector3d(x, y, z); - unsigned long long int index_of_bucket = get_rgd_index(source_point_global_ext, {search_radious, search_radious, search_radious}); + unsigned long long int index_of_bucket = + get_rgd_index(source_point_global_ext, { search_radious, search_radious, search_radious }); if (buckets.contains(index_of_bucket)) { @@ -391,21 +473,28 @@ bool PairWiseICP::compute_fast(const std::vector &source, const if (nn_found) { - const Eigen::Vector3d &p_s = source_i; + const Eigen::Vector3d& p_s = source_i; const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose_result); Eigen::Matrix AtPA_; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_4( - AtPA_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + AtPA_, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); Eigen::Matrix AtPB_; point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_4( AtPB_, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), - target_i_nn.x(), target_i_nn.y(), target_i_nn.z()); + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + target_i_nn.x(), + target_i_nn.y(), + target_i_nn.z()); std::unique_lock lck(mutex); AtPA.block<6, 6>(0, 0) += AtPA_; @@ -469,4 +558,4 @@ bool PairWiseICP::compute_fast(const std::vector &source, const } return true; -} \ No newline at end of file +} diff --git a/core/src/pcl_wrapper.cpp b/core/src/pcl_wrapper.cpp deleted file mode 100644 index fd3b9dcb..00000000 --- a/core/src/pcl_wrapper.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "pcl_wrapper.h" -#include -#ifdef WITH_PCL -#include -#include - -#endif //WITH_PCL diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index 385cbc13..b07e4b5a 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -1,23 +1,24 @@ -#include "pfd_wrapper.hpp" +#include + +#include #include -#include -#include -namespace mandeye::fd{ - std::string OpenFileDialogOneFile(const std::string& title, const std::vector&filter) - { - auto sel = OpenFileDialog(title, filter, false); - if (sel.empty()) - return ""; +namespace mandeye::fd +{ + std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter) + { + auto sel = OpenFileDialog(title, filter, false); + if (sel.empty()) + return ""; - return std::filesystem::path(sel.back()).lexically_normal().string(); - } + return std::filesystem::path(sel.back()).lexically_normal().string(); + } - std::vector OpenFileDialog(const std::string& title, const std::vector&filter, bool multiselect) + std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect) { std::vector files; static std::shared_ptr open_file; - + files = pfd::open_file(title, internal::lastLocationHint, filter, multiselect).result(); for (auto& f : files) @@ -36,40 +37,45 @@ namespace mandeye::fd{ return files; } - std::string SaveFileDialog(const std::string& title, const std::vector& filter, const std::string& defaultExtension, const std::string& defaultFileName) - { - std::string file; - static std::shared_ptr save_file; + std::string SaveFileDialog( + const std::string& title, + const std::vector& filter, + const std::string& defaultExtension, + const std::string& defaultFileName) + { + std::string file; + static std::shared_ptr save_file; - // build default path (directory + suggested filename) - std::string defaultPath = internal::lastLocationHint; - if (!defaultFileName.empty()) { - defaultPath = (std::filesystem::path(internal::lastLocationHint) / defaultFileName).string(); - } + // build default path (directory + suggested filename) + std::string defaultPath = internal::lastLocationHint; + if (!defaultFileName.empty()) + { + defaultPath = (std::filesystem::path(internal::lastLocationHint) / defaultFileName).string(); + } - file = pfd::save_file(title, defaultPath, filter).result(); + file = pfd::save_file(title, defaultPath, filter).result(); - if (file.empty()) - return file; + if (file.empty()) + return file; - std::filesystem::path pfile(file); - if (!pfile.has_extension()) - file += defaultExtension; + std::filesystem::path pfile(file); + if (!pfile.has_extension()) + file += defaultExtension; - if (pfile.has_parent_path()) - internal::lastLocationHint = pfile.parent_path().string(); + if (pfile.has_parent_path()) + internal::lastLocationHint = pfile.parent_path().string(); - return file; - } + return file; + } std::string SelectFolder(const std::string& title) { - std::string output_folder_name = ""; + std::string output_folder_name = ""; - output_folder_name = pfd::select_folder(title, internal::lastLocationHint).result(); - std::cout << "folder: '" << output_folder_name << "'" << std::endl; + output_folder_name = pfd::select_folder(title, internal::lastLocationHint).result(); + std::cout << "folder: '" << output_folder_name << "'" << std::endl; - return output_folder_name; + return output_folder_name; } void OutOfMemMessage() @@ -85,7 +91,8 @@ namespace mandeye::fd{ "Please follow guidlines available here : " "https://github.com/MapsHD/HDMapping/tree/main/doc/" "virtual_memory.md", - pfd::choice::ok, pfd::icon::error); + pfd::choice::ok, + pfd::icon::error); message.result(); - } -} \ No newline at end of file + } +} // namespace mandeye::fd \ No newline at end of file diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 69c1d2a7..7041055d 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -1,76 +1,66 @@ +#include + #include #include -#include -#include -#include -#include -#include -#include -#include -#include - #include #include -bool PointCloud::load(const std::string &file_name) +bool PointCloud::load(const std::string& file_name) { - points_local.clear(); - - // try - //{ - std::cout << "Loading PLY data..." << std::endl; - plycpp::PLYData data; - - plycpp::load(file_name, data); - - // Listing PLY content - { - std::cout << "List of elements and properties:\n" - << "===========================" << std::endl; - for (const auto &element : data) - { - std::cout << "* " << element.key << " -- size: " << element.data->size() << std::endl; - for (const auto &prop : element.data->properties) - { - std::cout << " - " << prop.key - << " -- type: " - << (prop.data->isList ? "list of " : "") - << prop.data->type.name() - << " -- size: " << prop.data->size() << std::endl; - } - } - std::cout << "\n"; - } - - // Example of direct access - { - auto xData = data["vertex"]->properties["x"]; - // std::cout << "x value of the first vertex element:\n" << xData->at(0) << std::endl; - // std::cout << "\n"; - auto yData = data["vertex"]->properties["y"]; - auto zData = data["vertex"]->properties["z"]; - for (size_t i = 0; i < xData->size(); i++) - { - Eigen::Vector3d point(xData->at(i), yData->at(i), zData->at(i)); - points_local.push_back(point); - timestamps.push_back(0); // no timestamps available in ETH ply - } - if (data["vertex"]->properties.has_key("intensity")) - { - auto iData = data["vertex"]->properties["intensity"]; - for (size_t i = 0; i < xData->size(); i++) - { - intensities.push_back(static_cast(iData->at(i) * 65535.0f)); - } - } - } - //} - // catch (const plycpp::Exception& e) - //{ - // std::cout << "An exception happened:\n" << e.what() << std::endl; - //} + points_local.clear(); + + // try + //{ + std::cout << "Loading PLY data..." << std::endl; + plycpp::PLYData data; + + plycpp::load(file_name, data); + + // Listing PLY content + { + std::cout << "List of elements and properties:\n" + << "===========================" << std::endl; + for (const auto& element : data) + { + std::cout << "* " << element.key << " -- size: " << element.data->size() << std::endl; + for (const auto& prop : element.data->properties) + { + std::cout << " - " << prop.key << " -- type: " << (prop.data->isList ? "list of " : "") << prop.data->type.name() + << " -- size: " << prop.data->size() << std::endl; + } + } + std::cout << "\n"; + } + + // Example of direct access + { + auto xData = data["vertex"]->properties["x"]; + // std::cout << "x value of the first vertex element:\n" << xData->at(0) << std::endl; + // std::cout << "\n"; + auto yData = data["vertex"]->properties["y"]; + auto zData = data["vertex"]->properties["z"]; + for (size_t i = 0; i < xData->size(); i++) + { + Eigen::Vector3d point(xData->at(i), yData->at(i), zData->at(i)); + points_local.push_back(point); + timestamps.push_back(0); // no timestamps available in ETH ply + } + if (data["vertex"]->properties.has_key("intensity")) + { + auto iData = data["vertex"]->properties["intensity"]; + for (size_t i = 0; i < xData->size(); i++) + { + intensities.push_back(static_cast(iData->at(i) * 65535.0f)); + } + } + } + //} + // catch (const plycpp::Exception& e) + //{ + // std::cout << "An exception happened:\n" << e.what() << std::endl; + //} #if 0 std::ifstream infile(file_name); @@ -96,1812 +86,1827 @@ bool PointCloud::load(const std::string &file_name) } #endif - std::cout << "File: '" << file_name << "' loaded" << std::endl; - return true; + std::cout << "File: '" << file_name << "' loaded" << std::endl; + return true; } bool PointCloud::load_pc(std::string input_file_name, bool load_cache_mode) { - double min_ts = std::numeric_limits::max(); - double max_ts = std::numeric_limits::lowest(); - int number_of_points_with_timestamp_eq_0 = 0; - - laszip_POINTER laszip_reader; - if (laszip_create(&laszip_reader)) - { - fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ - return false; - } - - laszip_BOOL is_compressed = 0; - if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) - { - fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ - return false; - } - laszip_header *header; - - if (laszip_get_header_pointer(laszip_reader, &header)) - { - fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); - /*PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc);*/ - return false; - } - - // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.c_str(), header->number_of_point_records); - - laszip_point *point; - if (laszip_get_point_pointer(laszip_reader, &point)) - { - fprintf(stderr, ":DLL ERROR: getting point pointer from laszip reader\n"); - return false; - } - - if(!load_cache_mode){ - this->m_pose = Eigen::Affine3d::Identity(); - this->m_initial_pose = this->m_pose; - this->pose = pose_tait_bryan_from_affine_matrix(this->m_pose); - this->gui_translation[0] = static_cast(this->pose.px); - this->gui_translation[1] = static_cast(this->pose.py); - this->gui_translation[2] = static_cast(this->pose.pz); - this->gui_rotation[0] = rad2deg(this->pose.om); - this->gui_rotation[1] = rad2deg(this->pose.fi); - this->gui_rotation[2] = rad2deg(this->pose.ka); - } - - /*for (int j = 0; j < header->number_of_point_records; j++) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, ":DLL ERROR: reading point %u\n", j); - laszip_close_reader(laszip_reader); - return true; - // continue; - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - - Eigen::Vector3d pp(p.x, p.y, p.z); - pc.points_local.push_back(pp); - pc.intensities.push_back(point->intensity); - pc.timestamps.push_back(p.timestamp); - }*/ - - laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); - - std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) << "' contains " << npoints << " points" << std::endl; - - laszip_I64 p_count = 0; - - this->points_local.clear(); - this->intensities.clear(); - this->timestamps.clear(); - - this->points_local.resize(npoints); - this->intensities.resize(npoints); - this->timestamps.resize(npoints); - - while (p_count < npoints) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); - laszip_close_reader(laszip_reader); - return false; - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - p.timestamp = point->gps_time; - - if (point->gps_time < min_ts) - { - min_ts = point->gps_time; - } - - if (point->gps_time > max_ts) - { - max_ts = point->gps_time; - } - - if (point->gps_time == 0.0) - { - number_of_points_with_timestamp_eq_0++; - } - - //std::cout << "p.timestamp: " << p.timestamp << std::endl; - - Eigen::Vector3d pp(p.x, p.y, p.z); - this->points_local.emplace_back(pp); - this->intensities.emplace_back(point->intensity); - this->timestamps.emplace_back(p.timestamp); - - // Eigen::Vector3d color( - // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) : 1.0f)) / 256.0, - // static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / static_cast(0xFFFFU) : 1.0f)) / 256.0); - - Eigen::Vector3d color( - static_cast(point->rgb[0]) / 256.0, - static_cast(point->rgb[1]) / 256.0, - static_cast(point->rgb[2]) / 256.0); - - // std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl; - - this->colors.push_back(color); - - p_count++; - } - - laszip_close_reader(laszip_reader); - // laszip_clean(laszip_reader); - // laszip_destroy(laszip_reader); - - std::cout << std::setprecision(20); - std::cout << "min_ts " << min_ts << std::endl; - std::cout << "max_ts " << max_ts << std::endl; - std::cout << "number_of_points_with_timestamp_eq_0: " << number_of_points_with_timestamp_eq_0 << std::endl; - return true; + double min_ts = std::numeric_limits::max(); + double max_ts = std::numeric_limits::lowest(); + int number_of_points_with_timestamp_eq_0 = 0; + + laszip_POINTER laszip_reader; + if (laszip_create(&laszip_reader)) + { + fprintf(stderr, ":DLL ERROR: creating laszip reader\n"); + /*PointCloud pc; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_initial_pose = pc.m_pose; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + pc.file_name = input_file_names[i]; + point_clouds.push_back(pc);*/ + return false; + } + + laszip_BOOL is_compressed = 0; + if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed)) + { + fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str()); + /*PointCloud pc; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_initial_pose = pc.m_pose; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + pc.file_name = input_file_names[i]; + point_clouds.push_back(pc);*/ + return false; + } + laszip_header* header; + + if (laszip_get_header_pointer(laszip_reader, &header)) + { + fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n"); + /*PointCloud pc; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_initial_pose = pc.m_pose; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + pc.file_name = input_file_names[i]; + point_clouds.push_back(pc);*/ + return false; + } + + // fprintf(stderr, "file '%s' contains %u points\n", input_file_name.c_str(), header->number_of_point_records); + + laszip_point* point; + if (laszip_get_point_pointer(laszip_reader, &point)) + { + fprintf(stderr, ":DLL ERROR: getting point pointer from laszip reader\n"); + return false; + } + + if (!load_cache_mode) + { + this->m_pose = Eigen::Affine3d::Identity(); + this->m_initial_pose = this->m_pose; + this->pose = pose_tait_bryan_from_affine_matrix(this->m_pose); + this->gui_translation[0] = static_cast(this->pose.px); + this->gui_translation[1] = static_cast(this->pose.py); + this->gui_translation[2] = static_cast(this->pose.pz); + this->gui_rotation[0] = rad2deg(this->pose.om); + this->gui_rotation[1] = rad2deg(this->pose.fi); + this->gui_rotation[2] = rad2deg(this->pose.ka); + } + + /*for (int j = 0; j < header->number_of_point_records; j++) + { + if (laszip_read_point(laszip_reader)) + { + fprintf(stderr, ":DLL ERROR: reading point %u\n", j); + laszip_close_reader(laszip_reader); + return true; + // continue; + } + + LAZPoint p; + p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); + p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); + p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); + p.timestamp = point->gps_time; + + Eigen::Vector3d pp(p.x, p.y, p.z); + pc.points_local.push_back(pp); + pc.intensities.push_back(point->intensity); + pc.timestamps.push_back(p.timestamp); + }*/ + + laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records); + + std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string()) + << "' contains " << npoints << " points" << std::endl; + + laszip_I64 p_count = 0; + + this->points_local.clear(); + this->intensities.clear(); + this->timestamps.clear(); + + this->points_local.resize(npoints); + this->intensities.resize(npoints); + this->timestamps.resize(npoints); + + while (p_count < npoints) + { + if (laszip_read_point(laszip_reader)) + { + fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count); + laszip_close_reader(laszip_reader); + return false; + } + + LAZPoint p; + p.x = header->x_offset + header->x_scale_factor * static_cast(point->X); + p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y); + p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); + p.timestamp = point->gps_time; + + if (point->gps_time < min_ts) + { + min_ts = point->gps_time; + } + + if (point->gps_time > max_ts) + { + max_ts = point->gps_time; + } + + if (point->gps_time == 0.0) + { + number_of_points_with_timestamp_eq_0++; + } + + // std::cout << "p.timestamp: " << p.timestamp << std::endl; + + Eigen::Vector3d pp(p.x, p.y, p.z); + this->points_local.emplace_back(pp); + this->intensities.emplace_back(point->intensity); + this->timestamps.emplace_back(p.timestamp); + + // Eigen::Vector3d color( + // static_cast(0xFFU * ((point->rgb[0] > 0) ? static_cast(point->rgb[0]) / static_cast(0xFFFFU) : 1.0f)) + /// 256.0, static_cast(0xFFU * ((point->rgb[1] > 0) ? static_cast(point->rgb[1]) / static_cast(0xFFFFU) + //: 1.0f)) / 256.0, static_cast(0xFFU * ((point->rgb[2] > 0) ? static_cast(point->rgb[2]) / + // static_cast(0xFFFFU) : 1.0f)) / 256.0); + + Eigen::Vector3d color( + static_cast(point->rgb[0]) / 256.0, + static_cast(point->rgb[1]) / 256.0, + static_cast(point->rgb[2]) / 256.0); + + // std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl; + + this->colors.push_back(color); + + p_count++; + } + + laszip_close_reader(laszip_reader); + // laszip_clean(laszip_reader); + // laszip_destroy(laszip_reader); + + std::cout << std::setprecision(20); + std::cout << "min_ts " << min_ts << std::endl; + std::cout << "max_ts " << max_ts << std::endl; + std::cout << "number_of_points_with_timestamp_eq_0: " << number_of_points_with_timestamp_eq_0 << std::endl; + return true; } void PointCloud::update_from_gui() { - pose.px = gui_translation[0]; - pose.py = gui_translation[1]; - pose.pz = gui_translation[2]; + pose.px = gui_translation[0]; + pose.py = gui_translation[1]; + pose.pz = gui_translation[2]; - pose.om = deg2rad(gui_rotation[0]); - pose.fi = deg2rad(gui_rotation[1]); - pose.ka = deg2rad(gui_rotation[2]); + pose.om = deg2rad(gui_rotation[0]); + pose.fi = deg2rad(gui_rotation[1]); + pose.ka = deg2rad(gui_rotation[2]); - m_pose = affine_matrix_from_pose_tait_bryan(pose); + m_pose = affine_matrix_from_pose_tait_bryan(pose); } bool PointCloud::save_as_global(std::string file_name) { - std::ofstream outfile; - outfile.open(file_name); - if (!outfile.good()) - { - std::cout << "Problem with saving to file: '" << file_name << "'" << std::endl; - return false; - } - - for (size_t i = 0; i < this->points_local.size(); i++) - { - Eigen::Vector3d vt = this->m_pose * this->points_local[i]; - outfile << vt.x() << ";" << vt.y() << ";" << vt.z() << std::endl; - } - - std::cout << "File: '" << file_name << "' saved" << std::endl; - outfile.close(); - return true; + std::ofstream outfile; + outfile.open(file_name); + if (!outfile.good()) + { + std::cout << "Problem with saving to file: '" << file_name << "'" << std::endl; + return false; + } + + for (size_t i = 0; i < this->points_local.size(); i++) + { + Eigen::Vector3d vt = this->m_pose * this->points_local[i]; + outfile << vt.x() << ";" << vt.y() << ";" << vt.z() << std::endl; + } + + std::cout << "File: '" << file_name << "' saved" << std::endl; + outfile.close(); + return true; } -void build_rgd_init_job(int i, PointCloud::Job *job, std::vector *buckets) +void build_rgd_init_job(int i, PointCloud::Job* job, std::vector* buckets) { - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - (*buckets)[ii].index_begin = -1; - (*buckets)[ii].index_end = -1; - (*buckets)[ii].number_of_points = 0; - } + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + (*buckets)[ii].index_begin = -1; + (*buckets)[ii].index_end = -1; + (*buckets)[ii].number_of_points = 0; + } } -void build_rgd_job(int i, PointCloud::Job *job, std::vector *index_pair, std::vector *buckets) +void build_rgd_job( + int i, PointCloud::Job* job, std::vector* index_pair, std::vector* buckets) { - // std::cout << "build_rgd_job:[" << i << "]" << std::endl; - - for (long long unsigned int ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - long long unsigned int ind = ii; - if (ind == 0) - { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; - - (*buckets)[index_of_bucket].index_begin = ind; - if (index_of_bucket != index_of_bucket_1) - { - (*buckets)[index_of_bucket].index_end = ind + 1; - (*buckets)[index_of_bucket_1].index_end = ind + 1; - } - } - else if (ind == (*buckets).size() - 1) - { - if ((*index_pair)[ind].index_of_bucket < (*buckets).size()) - { - (*buckets)[(*index_pair)[ind].index_of_bucket].index_end = ind + 1; - } - } - else if (ind + 1 < (*index_pair).size()) - { - - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; - - if (index_of_bucket != index_of_bucket_1) - { - (*buckets)[index_of_bucket].index_end = ind + 1; - (*buckets)[index_of_bucket_1].index_begin = ind + 1; - } - } - } + // std::cout << "build_rgd_job:[" << i << "]" << std::endl; + + for (long long unsigned int ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + long long unsigned int ind = ii; + if (ind == 0) + { + long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; + long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + + (*buckets)[index_of_bucket].index_begin = ind; + if (index_of_bucket != index_of_bucket_1) + { + (*buckets)[index_of_bucket].index_end = ind + 1; + (*buckets)[index_of_bucket_1].index_end = ind + 1; + } + } + else if (ind == (*buckets).size() - 1) + { + if ((*index_pair)[ind].index_of_bucket < (*buckets).size()) + { + (*buckets)[(*index_pair)[ind].index_of_bucket].index_end = ind + 1; + } + } + else if (ind + 1 < (*index_pair).size()) + { + long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; + long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + + if (index_of_bucket != index_of_bucket_1) + { + (*buckets)[index_of_bucket].index_end = ind + 1; + (*buckets)[index_of_bucket_1].index_begin = ind + 1; + } + } + } } -void build_rgd_final_job(int i, PointCloud::Job *job, std::vector *buckets) +void build_rgd_final_job(int i, PointCloud::Job* job, std::vector* buckets) { - // std::cout << "build_rgd_init_job:[" << i << "]" << std::endl; - - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - - // std::cout << job->index_begin_inclusive << " " << job->index_end_exclusive << " " << (*buckets).size() << std::endl; - - long long unsigned int index_begin = (*buckets)[ii].index_begin; - long long unsigned int index_end = (*buckets)[ii].index_end; - if (index_begin != -1 && index_end != -1) - { - (*buckets)[ii].number_of_points = index_end - index_begin; - } - } + // std::cout << "build_rgd_init_job:[" << i << "]" << std::endl; + + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + // std::cout << job->index_begin_inclusive << " " << job->index_end_exclusive << " " << (*buckets).size() << std::endl; + + long long unsigned int index_begin = (*buckets)[ii].index_begin; + long long unsigned int index_end = (*buckets)[ii].index_end; + if (index_begin != -1 && index_end != -1) + { + (*buckets)[ii].number_of_points = index_end - index_begin; + } + } } bool PointCloud::build_rgd() { - // std::cout << "build_rgd()" << std::endl; - buckets.clear(); - index_pairs.clear(); + // std::cout << "build_rgd()" << std::endl; + buckets.clear(); + index_pairs.clear(); - grid_calculate_params(this->points_local, this->rgd_params); - cout_rgd(); + grid_calculate_params(this->points_local, this->rgd_params); + cout_rgd(); - // std::cout << "grid_calculate_params done" << std::endl; + // std::cout << "grid_calculate_params done" << std::endl; - reindex(this->index_pairs, this->points_local, this->rgd_params); - // std::cout << "reindex done" << std::endl; + reindex(this->index_pairs, this->points_local, this->rgd_params); + // std::cout << "reindex done" << std::endl; - // std::cout << "rgd_params.number_of_buckets: " << rgd_params.number_of_buckets << std::endl; + // std::cout << "rgd_params.number_of_buckets: " << rgd_params.number_of_buckets << std::endl; - buckets.resize(rgd_params.number_of_buckets); + buckets.resize(rgd_params.number_of_buckets); - std::vector jobs = get_jobs(buckets.size(), num_threads); + std::vector jobs = get_jobs(buckets.size(), num_threads); - std::vector threads; + std::vector threads; - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); - } + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_init_job, i, &jobs[i], &buckets)); + } - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); - jobs = get_jobs(points_local.size(), num_threads); + jobs = get_jobs(points_local.size(), num_threads); - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pairs, &buckets)); - } + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_job, i, &jobs[i], &index_pairs, &buckets)); + } - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); - jobs = get_jobs(buckets.size(), num_threads); + jobs = get_jobs(buckets.size(), num_threads); - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); - } + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(build_rgd_final_job, i, &jobs[i], &buckets)); + } - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); - return true; + return true; } -void PointCloud::grid_calculate_params(std::vector &points, GridParameters ¶ms) +void PointCloud::grid_calculate_params(std::vector& points, GridParameters& params) { - double min_x = std::numeric_limits::max(); - double max_x = std::numeric_limits::lowest(); - - double min_y = std::numeric_limits::max(); - double max_y = std::numeric_limits::lowest(); - - double min_z = std::numeric_limits::max(); - double max_z = std::numeric_limits::lowest(); - - for (size_t i = 0; i < points.size(); i++) - { - if (points[i].x() < min_x) - min_x = points[i].x(); - if (points[i].x() > max_x) - max_x = points[i].x(); - - if (points[i].y() < min_y) - min_y = points[i].y(); - if (points[i].y() > max_y) - max_y = points[i].y(); - - if (points[i].z() < min_z) - min_z = points[i].z(); - if (points[i].z() > max_z) - max_z = points[i].z(); - } - - min_x -= params.bounding_box_extension; - max_x += params.bounding_box_extension; - min_y -= params.bounding_box_extension; - max_y += params.bounding_box_extension; - min_z -= params.bounding_box_extension; - max_z += params.bounding_box_extension; - - long long unsigned int number_of_buckets_X = ((max_x - min_x) / params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / params.resolution_Z) + 1; - - params.number_of_buckets_X = number_of_buckets_X; - params.number_of_buckets_Y = number_of_buckets_Y; - params.number_of_buckets_Z = number_of_buckets_Z; - params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); - - params.bounding_box_max_X = max_x; - params.bounding_box_min_X = min_x; - params.bounding_box_max_Y = max_y; - params.bounding_box_min_Y = min_y; - params.bounding_box_max_Z = max_z; - params.bounding_box_min_Z = min_z; + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::lowest(); + + double min_z = std::numeric_limits::max(); + double max_z = std::numeric_limits::lowest(); + + for (size_t i = 0; i < points.size(); i++) + { + if (points[i].x() < min_x) + min_x = points[i].x(); + if (points[i].x() > max_x) + max_x = points[i].x(); + + if (points[i].y() < min_y) + min_y = points[i].y(); + if (points[i].y() > max_y) + max_y = points[i].y(); + + if (points[i].z() < min_z) + min_z = points[i].z(); + if (points[i].z() > max_z) + max_z = points[i].z(); + } + + min_x -= params.bounding_box_extension; + max_x += params.bounding_box_extension; + min_y -= params.bounding_box_extension; + max_y += params.bounding_box_extension; + min_z -= params.bounding_box_extension; + max_z += params.bounding_box_extension; + + long long unsigned int number_of_buckets_X = ((max_x - min_x) / params.resolution_X) + 1; + long long unsigned int number_of_buckets_Y = ((max_y - min_y) / params.resolution_Y) + 1; + long long unsigned int number_of_buckets_Z = ((max_z - min_z) / params.resolution_Z) + 1; + + params.number_of_buckets_X = number_of_buckets_X; + params.number_of_buckets_Y = number_of_buckets_Y; + params.number_of_buckets_Z = number_of_buckets_Z; + params.number_of_buckets = static_cast(number_of_buckets_X) * + static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + + params.bounding_box_max_X = max_x; + params.bounding_box_min_X = min_x; + params.bounding_box_max_Y = max_y; + params.bounding_box_min_Y = min_y; + params.bounding_box_max_Z = max_z; + params.bounding_box_min_Z = min_z; } -void reindex_job(int i, PointCloud::Job *job, std::vector *points, std::vector *pairs, - PointCloud::GridParameters rgd_params) +void reindex_job( + int i, + PointCloud::Job* job, + std::vector* points, + std::vector* pairs, + PointCloud::GridParameters rgd_params) { + // std::cout << "reindex_job:[" << i << "]" << std::endl; - // std::cout << "reindex_job:[" << i << "]" << std::endl; - - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - - Eigen::Vector3d &p = (*points)[ii]; + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + Eigen::Vector3d& p = (*points)[ii]; - (*pairs)[ii].index_of_point = ii; - (*pairs)[ii].index_of_bucket = 0; - //(*pairs)[ii].index_pose = p.index_pose; + (*pairs)[ii].index_of_point = ii; + (*pairs)[ii].index_of_bucket = 0; + //(*pairs)[ii].index_pose = p.index_pose; - long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; - } + (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * + static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + } } -void PointCloud::reindex(std::vector &ip, std::vector &points, GridParameters params) +void PointCloud::reindex(std::vector& ip, std::vector& points, GridParameters params) { - ip.resize(points.size()); - - std::vector jobs = get_jobs(ip.size(), num_threads); - - std::vector threads; - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(reindex_job, i, &jobs[i], &points, &ip, params)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - - // std::sort(index_pairs.begin(), index_pairs.end(), [](const PointBucketIndexPair& a, const PointBucketIndexPair& b) { return ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); }); - std::sort(ip.begin(), ip.end(), [](const PointBucketIndexPair &a, const PointBucketIndexPair &b) - { return a.index_of_bucket < b.index_of_bucket; }); + ip.resize(points.size()); + + std::vector jobs = get_jobs(ip.size(), num_threads); + + std::vector threads; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread(reindex_job, i, &jobs[i], &points, &ip, params)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + // std::sort(index_pairs.begin(), index_pairs.end(), [](const PointBucketIndexPair& a, const PointBucketIndexPair& b) { return + // ((a.index_of_bucket == b.index_of_bucket) ? (a.index_pose < b.index_pose) : (a.index_of_bucket < b.index_of_bucket)); }); + std::sort( + ip.begin(), + ip.end(), + [](const PointBucketIndexPair& a, const PointBucketIndexPair& b) + { + return a.index_of_bucket < b.index_of_bucket; + }); } std::vector PointCloud::get_jobs(long long unsigned int size, int num_threads) { - - int hc = size / num_threads; - if (hc < 1) - hc = 1; - - std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) - { - long long unsigned int sequence_length = hc; - if (i + hc >= size) - { - sequence_length = size - i; - } - if (sequence_length == 0) - break; - - PointCloud::Job j; - j.index_begin_inclusive = i; - j.index_end_exclusive = i + sequence_length; - jobs.push_back(j); - } - - // std::cout << jobs.size() << " jobs; chunks: "; - // for(size_t i = 0; i < jobs.size(); i++){ - // std::cout << "("< jobs; + for (long long unsigned int i = 0; i < size; i += hc) + { + long long unsigned int sequence_length = hc; + if (i + hc >= size) + { + sequence_length = size - i; + } + if (sequence_length == 0) + break; + + PointCloud::Job j; + j.index_begin_inclusive = i; + j.index_end_exclusive = i + sequence_length; + jobs.push_back(j); + } + + // std::cout << jobs.size() << " jobs; chunks: "; + // for(size_t i = 0; i < jobs.size(); i++){ + // std::cout << "("<> *nn_segments, - std::vector *points_local_source, - std::vector *points_local_target, - std::vector *points_type_source, - std::vector *points_type_target, - PointCloud::GridParameters rgd_params, - std::vector *buckets, - std::vector *index_pairs, - Eigen::Affine3d m_pose_source, - Eigen::Affine3d m_pose_target, - double search_radious) + // int i, + PointCloud::Job* job, + std::vector>* nn_segments, + std::vector* points_local_source, + std::vector* points_local_target, + std::vector* points_type_source, + std::vector* points_type_target, + PointCloud::GridParameters rgd_params, + std::vector* buckets, + std::vector* index_pairs, + Eigen::Affine3d m_pose_source, + Eigen::Affine3d m_pose_target, + double search_radious) { - - int threshold_inner = 100; - int threshold_outer = 100; - - Eigen::Affine3d m_pose_target_inv = m_pose_target.inverse(); - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - Eigen::Vector3d point_source_local = (*points_local_source)[ii]; - Eigen::Vector3d p = m_pose_target_inv * (m_pose_source * point_source_local); - int point_source_type = (*points_type_source)[ii]; - - if (p.x() < rgd_params.bounding_box_min_X || p.x() > rgd_params.bounding_box_max_X) - continue; - if (p.y() < rgd_params.bounding_box_min_Y || p.y() > rgd_params.bounding_box_max_Y) - continue; - if (p.z() < rgd_params.bounding_box_min_Z || p.z() > rgd_params.bounding_box_max_Z) - continue; - - long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - - long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; - - if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) - { - int sx, sy, sz, stx, sty, stz; - if (ix == 0) - sx = 0; - else - sx = -1; - if (iy == 0) - sy = 0; - else - sy = -1; - if (iz == 0) - sz = 0; - else - sz = -1; - - if (ix == rgd_params.number_of_buckets_X - 1) - stx = 1; - else - stx = 2; - if (iy == rgd_params.number_of_buckets_Y - 1) - sty = 1; - else - sty = 2; - if (iz == rgd_params.number_of_buckets_Z - 1) - stz = 1; - else - stz = 2; - - long long unsigned int index_next_bucket; - int iter; - int number_of_points_in_bucket; - int l_begin; - int l_end; - float dist = 0.0f; - - /////////////////////////////////////////////////////////////////////////////////////////// - double min_dist = std::numeric_limits::max(); - PointCloud::NearestNeighbour nn; - nn.found = false; - - // Point pTargetGlobal; - // Point pTargetGlobalOut; - - for (int i = sx; i < stx; i++) - { - for (int j = sy; j < sty; j++) - { - for (int k = sz; k < stz; k++) - { - index_next_bucket = index_bucket + - i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + - j * rgd_params.number_of_buckets_Z + k; - - if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) - { - - PointCloud::Bucket b = (*buckets)[index_next_bucket]; - - number_of_points_in_bucket = b.number_of_points; - - if (number_of_points_in_bucket <= 0) - continue; - - int max_number_considered_in_bucket; - if (index_next_bucket == index_bucket) - { - max_number_considered_in_bucket = threshold_inner; - } - else - { - max_number_considered_in_bucket = threshold_outer; - } - if (max_number_considered_in_bucket <= 0) - continue; - - if (max_number_considered_in_bucket >= number_of_points_in_bucket) - { - iter = 1; - } - else - { - iter = number_of_points_in_bucket / max_number_considered_in_bucket; - if (iter <= 0) - iter = 1; - } - - l_begin = b.index_begin; - l_end = b.index_end; - - for (int l = l_begin; l < l_end; l += iter) - { - if (l >= 0 && l < (*index_pairs).size()) - { - int hashed_index_of_point = (*index_pairs)[l].index_of_point; - - if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points_local_target).size()) - { - const auto &p_target = (*points_local_target)[hashed_index_of_point]; - int point_target_type = (*points_type_target)[hashed_index_of_point]; - - double dist = (p_target - p).norm(); - - if (dist <= search_radious && (point_source_type == point_target_type)) - { - if (dist < min_dist) - { - min_dist = dist; - nn.index_source = ii; - nn.index_target = hashed_index_of_point; - nn.found = true; - } - } - } - } - } - } - } - } - } - if (nn.found) - { - (*nn_segments).emplace_back(nn.index_source, nn.index_target); - } - } - } + int threshold_inner = 100; + int threshold_outer = 100; + + Eigen::Affine3d m_pose_target_inv = m_pose_target.inverse(); + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + Eigen::Vector3d point_source_local = (*points_local_source)[ii]; + Eigen::Vector3d p = m_pose_target_inv * (m_pose_source * point_source_local); + int point_source_type = (*points_type_source)[ii]; + + if (p.x() < rgd_params.bounding_box_min_X || p.x() > rgd_params.bounding_box_max_X) + continue; + if (p.y() < rgd_params.bounding_box_min_Y || p.y() > rgd_params.bounding_box_max_Y) + continue; + if (p.z() < rgd_params.bounding_box_min_Z || p.z() > rgd_params.bounding_box_max_Z) + continue; + + long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + + long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * + static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + + if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) + { + int sx, sy, sz, stx, sty, stz; + if (ix == 0) + sx = 0; + else + sx = -1; + if (iy == 0) + sy = 0; + else + sy = -1; + if (iz == 0) + sz = 0; + else + sz = -1; + + if (ix == rgd_params.number_of_buckets_X - 1) + stx = 1; + else + stx = 2; + if (iy == rgd_params.number_of_buckets_Y - 1) + sty = 1; + else + sty = 2; + if (iz == rgd_params.number_of_buckets_Z - 1) + stz = 1; + else + stz = 2; + + long long unsigned int index_next_bucket; + int iter; + int number_of_points_in_bucket; + int l_begin; + int l_end; + float dist = 0.0f; + + /////////////////////////////////////////////////////////////////////////////////////////// + double min_dist = std::numeric_limits::max(); + PointCloud::NearestNeighbour nn; + nn.found = false; + + // Point pTargetGlobal; + // Point pTargetGlobalOut; + + for (int i = sx; i < stx; i++) + { + for (int j = sy; j < sty; j++) + { + for (int k = sz; k < stz; k++) + { + index_next_bucket = index_bucket + i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + + j * rgd_params.number_of_buckets_Z + k; + + if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) + { + PointCloud::Bucket b = (*buckets)[index_next_bucket]; + + number_of_points_in_bucket = b.number_of_points; + + if (number_of_points_in_bucket <= 0) + continue; + + int max_number_considered_in_bucket; + if (index_next_bucket == index_bucket) + { + max_number_considered_in_bucket = threshold_inner; + } + else + { + max_number_considered_in_bucket = threshold_outer; + } + if (max_number_considered_in_bucket <= 0) + continue; + + if (max_number_considered_in_bucket >= number_of_points_in_bucket) + { + iter = 1; + } + else + { + iter = number_of_points_in_bucket / max_number_considered_in_bucket; + if (iter <= 0) + iter = 1; + } + + l_begin = b.index_begin; + l_end = b.index_end; + + for (int l = l_begin; l < l_end; l += iter) + { + if (l >= 0 && l < (*index_pairs).size()) + { + int hashed_index_of_point = (*index_pairs)[l].index_of_point; + + if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points_local_target).size()) + { + const auto& p_target = (*points_local_target)[hashed_index_of_point]; + int point_target_type = (*points_type_target)[hashed_index_of_point]; + + double dist = (p_target - p).norm(); + + if (dist <= search_radious && (point_source_type == point_target_type)) + { + if (dist < min_dist) + { + min_dist = dist; + nn.index_source = ii; + nn.index_target = hashed_index_of_point; + nn.found = true; + } + } + } + } + } + } + } + } + } + if (nn.found) + { + (*nn_segments).emplace_back(nn.index_source, nn.index_target); + } + } + } } -std::vector> PointCloud::nns(PointCloud &pc_target, double search_radious) +std::vector> PointCloud::nns(PointCloud& pc_target, double search_radious) { - std::vector> nerest_neighbours; - - std::vector jobs = get_jobs(points_local.size(), num_threads); - - std::vector>> nn_segments; - nn_segments.resize(jobs.size()); - - std::vector threads; - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back( - std::thread( - nearest_neighbours_job, - // i, - &jobs[i], - &nn_segments[i], - &this->points_local, - &pc_target.points_local, - &this->points_type, - &pc_target.points_type, - pc_target.rgd_params, - &pc_target.buckets, - &pc_target.index_pairs, - this->m_pose, - pc_target.m_pose, - search_radious)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - - for (size_t i = 0; i < nn_segments.size(); i++) - { - if (nn_segments[i].size() > 0) - { - nerest_neighbours.insert(nerest_neighbours.end(), nn_segments[i].begin(), nn_segments[i].end()); - } - }; - - return nerest_neighbours; + std::vector> nerest_neighbours; + + std::vector jobs = get_jobs(points_local.size(), num_threads); + + std::vector>> nn_segments; + nn_segments.resize(jobs.size()); + + std::vector threads; + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread( + nearest_neighbours_job, + // i, + &jobs[i], + &nn_segments[i], + &this->points_local, + &pc_target.points_local, + &this->points_type, + &pc_target.points_type, + pc_target.rgd_params, + &pc_target.buckets, + &pc_target.index_pairs, + this->m_pose, + pc_target.m_pose, + search_radious)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + for (size_t i = 0; i < nn_segments.size(); i++) + { + if (nn_segments[i].size() > 0) + { + nerest_neighbours.insert(nerest_neighbours.end(), nn_segments[i].begin(), nn_segments[i].end()); + } + }; + + return nerest_neighbours; } void PointCloud::clean() { - index_pairs.clear(); - buckets.clear(); + index_pairs.clear(); + buckets.clear(); } -inline void gpuata3(double *AA, double *A) +inline void gpuata3(double* AA, double* A) { - AA[3 * 0 + 0] = A[3 * 0 + 0] * A[3 * 0 + 0] + A[3 * 0 + 1] * A[3 * 0 + 1] + A[3 * 0 + 2] * A[3 * 0 + 2]; - AA[3 * 1 + 0] = A[3 * 0 + 0] * A[3 * 1 + 0] + A[3 * 0 + 1] * A[3 * 1 + 1] + A[3 * 0 + 2] * A[3 * 1 + 2]; - AA[3 * 2 + 0] = A[3 * 0 + 0] * A[3 * 2 + 0] + A[3 * 0 + 1] * A[3 * 2 + 1] + A[3 * 0 + 2] * A[3 * 2 + 2]; + AA[3 * 0 + 0] = A[3 * 0 + 0] * A[3 * 0 + 0] + A[3 * 0 + 1] * A[3 * 0 + 1] + A[3 * 0 + 2] * A[3 * 0 + 2]; + AA[3 * 1 + 0] = A[3 * 0 + 0] * A[3 * 1 + 0] + A[3 * 0 + 1] * A[3 * 1 + 1] + A[3 * 0 + 2] * A[3 * 1 + 2]; + AA[3 * 2 + 0] = A[3 * 0 + 0] * A[3 * 2 + 0] + A[3 * 0 + 1] * A[3 * 2 + 1] + A[3 * 0 + 2] * A[3 * 2 + 2]; - AA[3 * 0 + 1] = AA[3 * 1 + 0]; - AA[3 * 1 + 1] = A[3 * 1 + 0] * A[3 * 1 + 0] + A[3 * 1 + 1] * A[3 * 1 + 1] + A[3 * 1 + 2] * A[3 * 1 + 2]; - AA[3 * 2 + 1] = A[3 * 1 + 0] * A[3 * 2 + 0] + A[3 * 1 + 1] * A[3 * 2 + 1] + A[3 * 1 + 2] * A[3 * 2 + 2]; + AA[3 * 0 + 1] = AA[3 * 1 + 0]; + AA[3 * 1 + 1] = A[3 * 1 + 0] * A[3 * 1 + 0] + A[3 * 1 + 1] * A[3 * 1 + 1] + A[3 * 1 + 2] * A[3 * 1 + 2]; + AA[3 * 2 + 1] = A[3 * 1 + 0] * A[3 * 2 + 0] + A[3 * 1 + 1] * A[3 * 2 + 1] + A[3 * 1 + 2] * A[3 * 2 + 2]; - AA[3 * 0 + 2] = AA[3 * 2 + 0]; - AA[3 * 1 + 2] = AA[3 * 2 + 1]; - AA[3 * 2 + 2] = A[3 * 2 + 0] * A[3 * 2 + 0] + A[3 * 2 + 1] * A[3 * 2 + 1] + A[3 * 2 + 2] * A[3 * 2 + 2]; + AA[3 * 0 + 2] = AA[3 * 2 + 0]; + AA[3 * 1 + 2] = AA[3 * 2 + 1]; + AA[3 * 2 + 2] = A[3 * 2 + 0] * A[3 * 2 + 0] + A[3 * 2 + 1] * A[3 * 2 + 1] + A[3 * 2 + 2] * A[3 * 2 + 2]; } -inline void gpusolvecubic(double *c) +inline void gpusolvecubic(double* c) { - double sq3d2 = 0.86602540378443864676, c2d3 = c[2] / 3, - c2sq = c[2] * c[2], Q = (3 * c[1] - c2sq) / 9, - R = (c[2] * (9 * c[1] - 2 * c2sq) - 27 * c[0]) / 54; - double tmp, t, sint, cost; - - if (Q < 0) - { - /* - * Instead of computing - * c_0 = A cos(t) - B - * c_1 = A cos(t + 2 pi/3) - B - * c_2 = A cos(t + 4 pi/3) - B - * Use cos(a+b) = cos(a) cos(b) - sin(a) sin(b) - * Keeps t small and eliminates 1 function call. - * cos(2 pi/3) = cos(4 pi/3) = -0.5 - * sin(2 pi/3) = sqrtf(3.0f)/2 - * sin(4 pi/3) = -sqrtf(3.0f)/2 - */ - - tmp = 2 * sqrt(-Q); - t = acos(R / sqrt(-Q * Q * Q)) / 3; - cost = tmp * cos(t); - sint = tmp * sin(t); - - c[0] = cost - c2d3; - - cost = -0.5 * cost - c2d3; - sint = sq3d2 * sint; - - c[1] = cost - sint; - c[2] = cost + sint; - } - else - { - tmp = cbrt(R); - c[0] = -c2d3 + 2 * tmp; - c[1] = c[2] = -c2d3 - tmp; - } + double sq3d2 = 0.86602540378443864676, c2d3 = c[2] / 3, c2sq = c[2] * c[2], Q = (3 * c[1] - c2sq) / 9, + R = (c[2] * (9 * c[1] - 2 * c2sq) - 27 * c[0]) / 54; + double tmp, t, sint, cost; + + if (Q < 0) + { + /* + * Instead of computing + * c_0 = A cos(t) - B + * c_1 = A cos(t + 2 pi/3) - B + * c_2 = A cos(t + 4 pi/3) - B + * Use cos(a+b) = cos(a) cos(b) - sin(a) sin(b) + * Keeps t small and eliminates 1 function call. + * cos(2 pi/3) = cos(4 pi/3) = -0.5 + * sin(2 pi/3) = sqrtf(3.0f)/2 + * sin(4 pi/3) = -sqrtf(3.0f)/2 + */ + + tmp = 2 * sqrt(-Q); + t = acos(R / sqrt(-Q * Q * Q)) / 3; + cost = tmp * cos(t); + sint = tmp * sin(t); + + c[0] = cost - c2d3; + + cost = -0.5 * cost - c2d3; + sint = sq3d2 * sint; + + c[1] = cost - sint; + c[2] = cost + sint; + } + else + { + tmp = cbrt(R); + c[0] = -c2d3 + 2 * tmp; + c[1] = c[2] = -c2d3 - tmp; + } } -inline void gpusort3(double *x) +inline void gpusort3(double* x) { - double tmp; - - if (x[0] < x[1]) - { - tmp = x[0]; - x[0] = x[1]; - x[1] = tmp; - } - if (x[1] < x[2]) - { - if (x[0] < x[2]) - { - tmp = x[2]; - x[2] = x[1]; - x[1] = x[0]; - x[0] = tmp; - } - else - { - tmp = x[1]; - x[1] = x[2]; - x[2] = tmp; - } - } + double tmp; + + if (x[0] < x[1]) + { + tmp = x[0]; + x[0] = x[1]; + x[1] = tmp; + } + if (x[1] < x[2]) + { + if (x[0] < x[2]) + { + tmp = x[2]; + x[2] = x[1]; + x[1] = x[0]; + x[0] = tmp; + } + else + { + tmp = x[1]; + x[1] = x[2]; + x[2] = tmp; + } + } } -inline void gpuldu3(double *A, int *P) +inline void gpuldu3(double* A, int* P) { - int tmp; - - P[1] = 1; - P[2] = 2; - - P[0] = abs(A[3 * 1 + 0]) > abs(A[3 * 0 + 0]) ? (abs(A[3 * 2 + 0]) > abs(A[3 * 1 + 0]) ? 2 : 1) : (abs(A[3 * 2 + 0]) > abs(A[3 * 0 + 0]) ? 2 : 0); - P[P[0]] = 0; - - if (abs(A[3 * P[2] + 1]) > abs(A[3 * P[1] + 1])) - { - tmp = P[1]; - P[1] = P[2]; - P[2] = tmp; - } - - if (A[3 * P[0] + 0] != 0) - { - A[3 * P[1] + 0] = A[3 * P[1] + 0] / A[3 * P[0] + 0]; - A[3 * P[2] + 0] = A[3 * P[2] + 0] / A[3 * P[0] + 0]; - A[3 * P[0] + 1] = A[3 * P[0] + 1] / A[3 * P[0] + 0]; - A[3 * P[0] + 2] = A[3 * P[0] + 2] / A[3 * P[0] + 0]; - } - - A[3 * P[1] + 1] = A[3 * P[1] + 1] - A[3 * P[0] + 1] * A[3 * P[1] + 0] * A[3 * P[0] + 0]; - - if (A[3 * P[1] + 1] != 0) - { - A[3 * P[2] + 1] = (A[3 * P[2] + 1] - A[3 * P[0] + 1] * A[3 * P[2] + 0] * A[3 * P[0] + 0]) / A[3 * P[1] + 1]; - A[3 * P[1] + 2] = (A[3 * P[1] + 2] - A[3 * P[0] + 2] * A[3 * P[1] + 0] * A[3 * P[0] + 0]) / A[3 * P[1] + 1]; - } - - A[3 * P[2] + 2] = A[3 * P[2] + 2] - A[3 * P[0] + 2] * A[3 * P[2] + 0] * A[3 * P[0] + 0] - A[3 * P[1] + 2] * A[3 * P[2] + 1] * A[3 * P[1] + 1]; + int tmp; + + P[1] = 1; + P[2] = 2; + + P[0] = abs(A[3 * 1 + 0]) > abs(A[3 * 0 + 0]) ? (abs(A[3 * 2 + 0]) > abs(A[3 * 1 + 0]) ? 2 : 1) + : (abs(A[3 * 2 + 0]) > abs(A[3 * 0 + 0]) ? 2 : 0); + P[P[0]] = 0; + + if (abs(A[3 * P[2] + 1]) > abs(A[3 * P[1] + 1])) + { + tmp = P[1]; + P[1] = P[2]; + P[2] = tmp; + } + + if (A[3 * P[0] + 0] != 0) + { + A[3 * P[1] + 0] = A[3 * P[1] + 0] / A[3 * P[0] + 0]; + A[3 * P[2] + 0] = A[3 * P[2] + 0] / A[3 * P[0] + 0]; + A[3 * P[0] + 1] = A[3 * P[0] + 1] / A[3 * P[0] + 0]; + A[3 * P[0] + 2] = A[3 * P[0] + 2] / A[3 * P[0] + 0]; + } + + A[3 * P[1] + 1] = A[3 * P[1] + 1] - A[3 * P[0] + 1] * A[3 * P[1] + 0] * A[3 * P[0] + 0]; + + if (A[3 * P[1] + 1] != 0) + { + A[3 * P[2] + 1] = (A[3 * P[2] + 1] - A[3 * P[0] + 1] * A[3 * P[2] + 0] * A[3 * P[0] + 0]) / A[3 * P[1] + 1]; + A[3 * P[1] + 2] = (A[3 * P[1] + 2] - A[3 * P[0] + 2] * A[3 * P[1] + 0] * A[3 * P[0] + 0]) / A[3 * P[1] + 1]; + } + + A[3 * P[2] + 2] = + A[3 * P[2] + 2] - A[3 * P[0] + 2] * A[3 * P[2] + 0] * A[3 * P[0] + 0] - A[3 * P[1] + 2] * A[3 * P[2] + 1] * A[3 * P[1] + 1]; } -inline void gpuldubsolve3(double *x, double *y, double *LDU, const int *P) +inline void gpuldubsolve3(double* x, double* y, double* LDU, const int* P) { - x[P[2]] = y[2]; - x[P[1]] = y[1] - LDU[3 * P[2] + 1] * x[P[2]]; - x[P[0]] = y[0] - LDU[3 * P[2] + 0] * x[P[2]] - LDU[3 * P[1] + 0] * x[P[1]]; + x[P[2]] = y[2]; + x[P[1]] = y[1] - LDU[3 * P[2] + 1] * x[P[2]]; + x[P[0]] = y[0] - LDU[3 * P[2] + 0] * x[P[2]] - LDU[3 * P[1] + 0] * x[P[1]]; } -inline void gpucross(double *z, double *x, double *y) +inline void gpucross(double* z, double* x, double* y) { - z[0] = x[1] * y[2] - x[2] * y[1]; - z[1] = -(x[0] * y[2] - x[2] * y[0]); - z[2] = x[0] * y[1] - x[1] * y[0]; + z[0] = x[1] * y[2] - x[2] * y[1]; + z[1] = -(x[0] * y[2] - x[2] * y[0]); + z[2] = x[0] * y[1] - x[1] * y[0]; } -inline void gpumatvec3(double *y, double *A, double *x) +inline void gpumatvec3(double* y, double* A, double* x) { - y[0] = A[3 * 0 + 0] * x[0] + A[3 * 1 + 0] * x[1] + A[3 * 2 + 0] * x[2]; - y[1] = A[3 * 0 + 1] * x[0] + A[3 * 1 + 1] * x[1] + A[3 * 2 + 1] * x[2]; - y[2] = A[3 * 0 + 2] * x[0] + A[3 * 1 + 2] * x[1] + A[3 * 2 + 2] * x[2]; + y[0] = A[3 * 0 + 0] * x[0] + A[3 * 1 + 0] * x[1] + A[3 * 2 + 0] * x[2]; + y[1] = A[3 * 0 + 1] * x[0] + A[3 * 1 + 1] * x[1] + A[3 * 2 + 1] * x[2]; + y[2] = A[3 * 0 + 2] * x[0] + A[3 * 1 + 2] * x[1] + A[3 * 2 + 2] * x[2]; } -inline void gpumatmul3(double *C, double *A, double *B) +inline void gpumatmul3(double* C, double* A, double* B) { - C[3 * 0 + 0] = A[3 * 0 + 0] * B[3 * 0 + 0] + A[3 * 1 + 0] * B[3 * 0 + 1] + A[3 * 2 + 0] * B[3 * 0 + 2]; - C[3 * 1 + 0] = A[3 * 0 + 0] * B[3 * 1 + 0] + A[3 * 1 + 0] * B[3 * 1 + 1] + A[3 * 2 + 0] * B[3 * 1 + 2]; - C[3 * 2 + 0] = A[3 * 0 + 0] * B[3 * 2 + 0] + A[3 * 1 + 0] * B[3 * 2 + 1] + A[3 * 2 + 0] * B[3 * 2 + 2]; + C[3 * 0 + 0] = A[3 * 0 + 0] * B[3 * 0 + 0] + A[3 * 1 + 0] * B[3 * 0 + 1] + A[3 * 2 + 0] * B[3 * 0 + 2]; + C[3 * 1 + 0] = A[3 * 0 + 0] * B[3 * 1 + 0] + A[3 * 1 + 0] * B[3 * 1 + 1] + A[3 * 2 + 0] * B[3 * 1 + 2]; + C[3 * 2 + 0] = A[3 * 0 + 0] * B[3 * 2 + 0] + A[3 * 1 + 0] * B[3 * 2 + 1] + A[3 * 2 + 0] * B[3 * 2 + 2]; - C[3 * 0 + 1] = A[3 * 0 + 1] * B[3 * 0 + 0] + A[3 * 1 + 1] * B[3 * 0 + 1] + A[3 * 2 + 1] * B[3 * 0 + 2]; - C[3 * 1 + 1] = A[3 * 0 + 1] * B[3 * 1 + 0] + A[3 * 1 + 1] * B[3 * 1 + 1] + A[3 * 2 + 1] * B[3 * 1 + 2]; - C[3 * 2 + 1] = A[3 * 0 + 1] * B[3 * 2 + 0] + A[3 * 1 + 1] * B[3 * 2 + 1] + A[3 * 2 + 1] * B[3 * 2 + 2]; + C[3 * 0 + 1] = A[3 * 0 + 1] * B[3 * 0 + 0] + A[3 * 1 + 1] * B[3 * 0 + 1] + A[3 * 2 + 1] * B[3 * 0 + 2]; + C[3 * 1 + 1] = A[3 * 0 + 1] * B[3 * 1 + 0] + A[3 * 1 + 1] * B[3 * 1 + 1] + A[3 * 2 + 1] * B[3 * 1 + 2]; + C[3 * 2 + 1] = A[3 * 0 + 1] * B[3 * 2 + 0] + A[3 * 1 + 1] * B[3 * 2 + 1] + A[3 * 2 + 1] * B[3 * 2 + 2]; - C[3 * 0 + 2] = A[3 * 0 + 2] * B[3 * 0 + 0] + A[3 * 1 + 2] * B[3 * 0 + 1] + A[3 * 2 + 2] * B[3 * 0 + 2]; - C[3 * 1 + 2] = A[3 * 0 + 2] * B[3 * 1 + 0] + A[3 * 1 + 2] * B[3 * 1 + 1] + A[3 * 2 + 2] * B[3 * 1 + 2]; - C[3 * 2 + 2] = A[3 * 0 + 2] * B[3 * 2 + 0] + A[3 * 1 + 2] * B[3 * 2 + 1] + A[3 * 2 + 2] * B[3 * 2 + 2]; + C[3 * 0 + 2] = A[3 * 0 + 2] * B[3 * 0 + 0] + A[3 * 1 + 2] * B[3 * 0 + 1] + A[3 * 2 + 2] * B[3 * 0 + 2]; + C[3 * 1 + 2] = A[3 * 0 + 2] * B[3 * 1 + 0] + A[3 * 1 + 2] * B[3 * 1 + 1] + A[3 * 2 + 2] * B[3 * 1 + 2]; + C[3 * 2 + 2] = A[3 * 0 + 2] * B[3 * 2 + 0] + A[3 * 1 + 2] * B[3 * 2 + 1] + A[3 * 2 + 2] * B[3 * 2 + 2]; } -inline void gpuunit3(double *x) +inline void gpuunit3(double* x) { - double tmp = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); - x[0] /= tmp; - x[1] /= tmp; - x[2] /= tmp; + double tmp = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); + x[0] /= tmp; + x[1] /= tmp; + x[2] /= tmp; } -inline void gpuSVD(double *_A, double *U, double *_S, double *V) +inline void gpuSVD(double* _A, double* U, double* _S, double* V) { - double A[9]; - for (int i = 0; i < 9; i++) - A[i] = (double)_A[i]; - - double thr = 1e-10; - int P[3], k; - double y[3], AA[3][3], LDU[3][3]; - - double S[3]; - /* - * Steps: - * 1) Use eigendecomposition on A^T A to compute V. - * Since A = U S V^T then A^T A = V S^T S V^T with D = S^T S and V the - * eigenvalues and eigenvectors respectively (V is orthogonal). - * 2) Compute U from A and V. - * 3) Normalize columns of U and V and root the eigenvalues to obtain - * the singular values. - */ - - /* Compute AA = A^T A */ - gpuata3((double *)AA, A); - - /* Form the monic characteristic polynomial */ - S[2] = -AA[0][0] - AA[1][1] - AA[2][2]; - S[1] = AA[0][0] * AA[1][1] + AA[2][2] * AA[0][0] + AA[2][2] * AA[1][1] - - AA[2][1] * AA[1][2] - AA[2][0] * AA[0][2] - AA[1][0] * AA[0][1]; - S[0] = AA[2][1] * AA[1][2] * AA[0][0] + AA[2][0] * AA[0][2] * AA[1][1] + AA[1][0] * AA[0][1] * AA[2][2] - - AA[0][0] * AA[1][1] * AA[2][2] - AA[1][0] * AA[2][1] * AA[0][2] - AA[2][0] * AA[0][1] * AA[1][2]; - - /* Solve the cubic equation. */ - gpusolvecubic(S); - - /* All roots should be positive */ - if (S[0] < 0) - S[0] = 0; - if (S[1] < 0) - S[1] = 0; - if (S[2] < 0) - S[2] = 0; - - /* Sort from greatest to least */ - gpusort3(S); - - /* Form the eigenvector system for the first (largest) eigenvalue */ - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - { - LDU[i][j] = AA[i][j]; - } - - LDU[0][0] -= S[0]; - LDU[1][1] -= S[0]; - LDU[2][2] -= S[0]; - - /* Perform LDUP decomposition */ - gpuldu3((double *)LDU, P); - - /* - * Write LDU = AA-I*lambda. Then an eigenvector can be - * found by solving LDU x = LD y = L z = 0 - * L is invertible, so L z = 0 implies z = 0 - * D is singular since det(AA-I*lambda) = 0 and so - * D y = z = 0 has a non-unique solution. - * Pick k so that D_kk = 0 and set y = e_k, the k'th column - * of the identity matrix. - * U is invertible so U x = y has a unique solution for a given y. - * The solution for U x = y is an eigenvector. - */ - - /* Pick the component of D nearest to 0 */ - y[0] = y[1] = y[2] = 0; - k = abs(LDU[P[1]][1]) < abs(LDU[P[0]][0]) ? (abs(LDU[P[2]][2]) < abs(LDU[P[1]][1]) ? 2 : 1) : (abs(LDU[P[2]][2]) < abs(LDU[P[0]][0]) ? 2 : 0); - y[k] = 1; - - /* Do a backward solve for the eigenvector */ - gpuldubsolve3(V + (3 * 0 + 0), y, (double *)LDU, P); - - /* Form the eigenvector system for the last (smallest) eigenvalue */ - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - { - LDU[i][j] = AA[i][j]; - } - - LDU[0][0] -= S[2]; - LDU[1][1] -= S[2]; - LDU[2][2] -= S[2]; - - /* Perform LDUP decomposition */ - gpuldu3((double *)LDU, P); - - /* - * NOTE: The arrangement of the ternary operator output is IMPORTANT! - * It ensures a different system is solved if there are 3 repeat eigenvalues. - */ - - /* Pick the component of D nearest to 0 */ - y[0] = y[1] = y[2] = 0; - k = abs(LDU[P[0]][0]) < abs(LDU[P[2]][2]) ? (abs(LDU[P[0]][0]) < abs(LDU[P[1]][1]) ? 0 : 1) : (abs(LDU[P[1]][1]) < abs(LDU[P[2]][2]) ? 1 : 2); - y[k] = 1; - - /* Do a backward solve for the eigenvector */ - gpuldubsolve3(V + (3 * 2 + 0), y, (double *)LDU, P); - - /* The remaining column must be orthogonal (AA is symmetric) */ - gpucross(V + (3 * 1 + 0), V + (3 * 2 + 0), V + (3 * 0 + 0)); - - /* Count the rank */ - k = (S[0] > thr) + (S[1] > thr) + (S[2] > thr); - - switch (k) - { - case 0: - /* - * Zero matrix. - * Since V is already orthogonal, just copy it into U. - */ - for (int i = 0; i < 9; i++) - U[i] = V[i]; - - break; - case 1: - /* - * The first singular value is non-zero. - * Since A = U S V^T, then A V = U S. - * A V_1 = S_11 U_1 is non-zero. Here V_1 and U_1 are - * column vectors. Since V_1 is known, we may compute - * U_1 = A V_1. The S_11 factor is not important as - * U_1 will be normalized later. - */ - gpumatvec3(U + (3 * 0 + 0), A, V + (3 * 0 + 0)); - - /* - * The other columns of U do not contribute to the expansion - * and we may arbitrarily choose them (but they do need to be - * orthogonal). To ensure the first cross product does not fail, - * pick k so that U_k1 is nearest 0 and then cross with e_k to - * obtain an orthogonal vector to U_1. - */ - y[0] = y[1] = y[2] = 0; - k = abs(U[3 * 0 + 0]) < abs(U[3 * 0 + 2]) ? (abs(U[3 * 0 + 0]) < abs(U[3 * 0 + 1]) ? 0 : 1) : (abs(U[3 * 0 + 1]) < abs(U[3 * 0 + 2]) ? 1 : 2); - y[k] = 1; - - gpucross(U + (3 * 1 + 0), y, U + (3 * 0 + 0)); - - /* Cross the first two to obtain the remaining column */ - gpucross(U + (3 * 2 + 0), U + (3 * 0 + 0), U + (3 * 1 + 0)); - break; - case 2: - /* - * The first two singular values are non-zero. - * Compute U_1 = A V_1 and U_2 = A V_2. See case 1 - * for more information. - */ - gpumatvec3(U + (3 * 0 + 0), A, V + (3 * 0 + 0)); - gpumatvec3(U + (3 * 1 + 0), A, V + (3 * 1 + 0)); - - /* Cross the first two to obtain the remaining column */ - gpucross(U + (3 * 2 + 0), U + (3 * 0 + 0), U + (3 * 1 + 0)); - break; - case 3: - /* - * All singular values are non-zero. - * We may compute U = A V. See case 1 for more information. - */ - gpumatmul3(U, A, V); - break; - } - - /* Normalize the columns of U and V */ - gpuunit3(V + (3 * 0 + 0)); - gpuunit3(V + (3 * 1 + 0)); - gpuunit3(V + (3 * 2 + 0)); - - gpuunit3(U + (3 * 0 + 0)); - gpuunit3(U + (3 * 1 + 0)); - gpuunit3(U + (3 * 2 + 0)); - - /* S was initially the eigenvalues of A^T A = V S^T S V^T which are squared. */ - S[0] = sqrt(S[0]); - S[1] = sqrt(S[1]); - S[2] = sqrt(S[2]); - - for (int i = 0; i < 9; i++) - _S[i] = 0.0; - - _S[0] = S[0]; - _S[4] = S[1]; - _S[8] = S[2]; + double A[9]; + for (int i = 0; i < 9; i++) + A[i] = (double)_A[i]; + + double thr = 1e-10; + int P[3], k; + double y[3], AA[3][3], LDU[3][3]; + + double S[3]; + /* + * Steps: + * 1) Use eigendecomposition on A^T A to compute V. + * Since A = U S V^T then A^T A = V S^T S V^T with D = S^T S and V the + * eigenvalues and eigenvectors respectively (V is orthogonal). + * 2) Compute U from A and V. + * 3) Normalize columns of U and V and root the eigenvalues to obtain + * the singular values. + */ + + /* Compute AA = A^T A */ + gpuata3((double*)AA, A); + + /* Form the monic characteristic polynomial */ + S[2] = -AA[0][0] - AA[1][1] - AA[2][2]; + S[1] = + AA[0][0] * AA[1][1] + AA[2][2] * AA[0][0] + AA[2][2] * AA[1][1] - AA[2][1] * AA[1][2] - AA[2][0] * AA[0][2] - AA[1][0] * AA[0][1]; + S[0] = AA[2][1] * AA[1][2] * AA[0][0] + AA[2][0] * AA[0][2] * AA[1][1] + AA[1][0] * AA[0][1] * AA[2][2] - + AA[0][0] * AA[1][1] * AA[2][2] - AA[1][0] * AA[2][1] * AA[0][2] - AA[2][0] * AA[0][1] * AA[1][2]; + + /* Solve the cubic equation. */ + gpusolvecubic(S); + + /* All roots should be positive */ + if (S[0] < 0) + S[0] = 0; + if (S[1] < 0) + S[1] = 0; + if (S[2] < 0) + S[2] = 0; + + /* Sort from greatest to least */ + gpusort3(S); + + /* Form the eigenvector system for the first (largest) eigenvalue */ + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + { + LDU[i][j] = AA[i][j]; + } + + LDU[0][0] -= S[0]; + LDU[1][1] -= S[0]; + LDU[2][2] -= S[0]; + + /* Perform LDUP decomposition */ + gpuldu3((double*)LDU, P); + + /* + * Write LDU = AA-I*lambda. Then an eigenvector can be + * found by solving LDU x = LD y = L z = 0 + * L is invertible, so L z = 0 implies z = 0 + * D is singular since det(AA-I*lambda) = 0 and so + * D y = z = 0 has a non-unique solution. + * Pick k so that D_kk = 0 and set y = e_k, the k'th column + * of the identity matrix. + * U is invertible so U x = y has a unique solution for a given y. + * The solution for U x = y is an eigenvector. + */ + + /* Pick the component of D nearest to 0 */ + y[0] = y[1] = y[2] = 0; + k = abs(LDU[P[1]][1]) < abs(LDU[P[0]][0]) ? (abs(LDU[P[2]][2]) < abs(LDU[P[1]][1]) ? 2 : 1) + : (abs(LDU[P[2]][2]) < abs(LDU[P[0]][0]) ? 2 : 0); + y[k] = 1; + + /* Do a backward solve for the eigenvector */ + gpuldubsolve3(V + (3 * 0 + 0), y, (double*)LDU, P); + + /* Form the eigenvector system for the last (smallest) eigenvalue */ + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + { + LDU[i][j] = AA[i][j]; + } + + LDU[0][0] -= S[2]; + LDU[1][1] -= S[2]; + LDU[2][2] -= S[2]; + + /* Perform LDUP decomposition */ + gpuldu3((double*)LDU, P); + + /* + * NOTE: The arrangement of the ternary operator output is IMPORTANT! + * It ensures a different system is solved if there are 3 repeat eigenvalues. + */ + + /* Pick the component of D nearest to 0 */ + y[0] = y[1] = y[2] = 0; + k = abs(LDU[P[0]][0]) < abs(LDU[P[2]][2]) ? (abs(LDU[P[0]][0]) < abs(LDU[P[1]][1]) ? 0 : 1) + : (abs(LDU[P[1]][1]) < abs(LDU[P[2]][2]) ? 1 : 2); + y[k] = 1; + + /* Do a backward solve for the eigenvector */ + gpuldubsolve3(V + (3 * 2 + 0), y, (double*)LDU, P); + + /* The remaining column must be orthogonal (AA is symmetric) */ + gpucross(V + (3 * 1 + 0), V + (3 * 2 + 0), V + (3 * 0 + 0)); + + /* Count the rank */ + k = (S[0] > thr) + (S[1] > thr) + (S[2] > thr); + + switch (k) + { + case 0: + /* + * Zero matrix. + * Since V is already orthogonal, just copy it into U. + */ + for (int i = 0; i < 9; i++) + U[i] = V[i]; + + break; + case 1: + /* + * The first singular value is non-zero. + * Since A = U S V^T, then A V = U S. + * A V_1 = S_11 U_1 is non-zero. Here V_1 and U_1 are + * column vectors. Since V_1 is known, we may compute + * U_1 = A V_1. The S_11 factor is not important as + * U_1 will be normalized later. + */ + gpumatvec3(U + (3 * 0 + 0), A, V + (3 * 0 + 0)); + + /* + * The other columns of U do not contribute to the expansion + * and we may arbitrarily choose them (but they do need to be + * orthogonal). To ensure the first cross product does not fail, + * pick k so that U_k1 is nearest 0 and then cross with e_k to + * obtain an orthogonal vector to U_1. + */ + y[0] = y[1] = y[2] = 0; + k = abs(U[3 * 0 + 0]) < abs(U[3 * 0 + 2]) ? (abs(U[3 * 0 + 0]) < abs(U[3 * 0 + 1]) ? 0 : 1) + : (abs(U[3 * 0 + 1]) < abs(U[3 * 0 + 2]) ? 1 : 2); + y[k] = 1; + + gpucross(U + (3 * 1 + 0), y, U + (3 * 0 + 0)); + + /* Cross the first two to obtain the remaining column */ + gpucross(U + (3 * 2 + 0), U + (3 * 0 + 0), U + (3 * 1 + 0)); + break; + case 2: + /* + * The first two singular values are non-zero. + * Compute U_1 = A V_1 and U_2 = A V_2. See case 1 + * for more information. + */ + gpumatvec3(U + (3 * 0 + 0), A, V + (3 * 0 + 0)); + gpumatvec3(U + (3 * 1 + 0), A, V + (3 * 1 + 0)); + + /* Cross the first two to obtain the remaining column */ + gpucross(U + (3 * 2 + 0), U + (3 * 0 + 0), U + (3 * 1 + 0)); + break; + case 3: + /* + * All singular values are non-zero. + * We may compute U = A V. See case 1 for more information. + */ + gpumatmul3(U, A, V); + break; + } + + /* Normalize the columns of U and V */ + gpuunit3(V + (3 * 0 + 0)); + gpuunit3(V + (3 * 1 + 0)); + gpuunit3(V + (3 * 2 + 0)); + + gpuunit3(U + (3 * 0 + 0)); + gpuunit3(U + (3 * 1 + 0)); + gpuunit3(U + (3 * 2 + 0)); + + /* S was initially the eigenvalues of A^T A = V S^T S V^T which are squared. */ + S[0] = sqrt(S[0]); + S[1] = sqrt(S[1]); + S[2] = sqrt(S[2]); + + for (int i = 0; i < 9; i++) + _S[i] = 0.0; + + _S[0] = S[0]; + _S[4] = S[1]; + _S[8] = S[2]; } void compute_normal_vectors_job( - int i, - PointCloud::Job *job, - std::vector *points, - PointCloud::GridParameters rgd_params, - std::vector *buckets, - std::vector *index_pairs, - std::vector *normal_vectors, - double search_radious) + int i, + PointCloud::Job* job, + std::vector* points, + PointCloud::GridParameters rgd_params, + std::vector* buckets, + std::vector* index_pairs, + std::vector* normal_vectors, + double search_radious) { - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) - { - Eigen::Vector3d &point = (*points)[ii]; - Eigen::Vector3d &normal_vector = (*normal_vectors)[ii]; - - if (point.x() < rgd_params.bounding_box_min_X || point.x() > rgd_params.bounding_box_max_X) - continue; - if (point.y() < rgd_params.bounding_box_min_Y || point.y() > rgd_params.bounding_box_max_Y) - continue; - if (point.z() < rgd_params.bounding_box_min_Z || point.z() > rgd_params.bounding_box_max_Z) - continue; - - normal_vector.x() = 0; - normal_vector.y() = 0; - normal_vector.z() = 0; - // point.valid_nv = false; - - Eigen::Vector3d mean(0.0, 0.0, 0.0); - int number_of_points_nn = 0; - - long long unsigned int ix = (point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - - long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; - - /////////////////////////////////// - if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) - { - int sx, sy, sz, stx, sty, stz; - if (ix == 0) - sx = 0; - else - sx = -1; - if (iy == 0) - sy = 0; - else - sy = -1; - if (iz == 0) - sz = 0; - else - sz = -1; - - if (ix == rgd_params.number_of_buckets_X - 1) - stx = 1; - else - stx = 2; - if (iy == rgd_params.number_of_buckets_Y - 1) - sty = 1; - else - sty = 2; - if (iz == rgd_params.number_of_buckets_Z - 1) - stz = 1; - else - stz = 2; - - long long unsigned int index_next_bucket; - int iter; - int number_of_points_in_bucket; - int l_begin; - int l_end; - float dist = 0.0f; - - /////////////////////////////////////////////////////////////////////////////////////////// - for (int i = sx; i < stx; i++) - { - for (int j = sy; j < sty; j++) - { - for (int k = sz; k < stz; k++) - { - index_next_bucket = index_bucket + - i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + - j * rgd_params.number_of_buckets_Z + k; - - if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) - { - - PointCloud::Bucket &b = (*buckets)[index_next_bucket]; - - number_of_points_in_bucket = b.number_of_points; - - if (number_of_points_in_bucket <= 0) - continue; - - int max_number_considered_in_bucket; - if (index_next_bucket == index_bucket) - { - max_number_considered_in_bucket = 100; - } - else - { - max_number_considered_in_bucket = 100; - } - if (max_number_considered_in_bucket <= 0) - continue; - - if (max_number_considered_in_bucket >= number_of_points_in_bucket) - { - iter = 1; - } - else - { - iter = number_of_points_in_bucket / max_number_considered_in_bucket; - if (iter <= 0) - iter = 1; - } - - l_begin = b.index_begin; - l_end = b.index_end; - - for (int l = l_begin; l < l_end; l += iter) - { - if (l >= 0 && l < (*index_pairs).size()) - { - int hashed_index_of_point = (*index_pairs)[l].index_of_point; - - if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points).size()) - { - Eigen::Vector3d &point_nn = (*points)[hashed_index_of_point]; - - double dist = (point_nn - point).norm(); - - if (dist < search_radious) - { - mean += point_nn; - number_of_points_nn++; - } - } - } - } - } - } - } - } - } - - if (number_of_points_nn >= 3) - { - mean /= number_of_points_nn; - - double cov[3][3]; - cov[0][0] = cov[0][1] = cov[0][2] = cov[1][0] = cov[1][1] = cov[1][2] = cov[2][0] = cov[2][1] = cov[2][2] = 0; - number_of_points_nn = 0; - - if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) - { - int sx, sy, sz, stx, sty, stz; - if (ix == 0) - sx = 0; - else - sx = -1; - if (iy == 0) - sy = 0; - else - sy = -1; - if (iz == 0) - sz = 0; - else - sz = -1; - - if (ix == rgd_params.number_of_buckets_X - 1) - stx = 1; - else - stx = 2; - if (iy == rgd_params.number_of_buckets_Y - 1) - sty = 1; - else - sty = 2; - if (iz == rgd_params.number_of_buckets_Z - 1) - stz = 1; - else - stz = 2; - - long long unsigned int index_next_bucket; - int iter; - int number_of_points_in_bucket; - int l_begin; - int l_end; - float dist = 0.0f; - - /////////////////////////////////////////////////////////////////////////////////////////// - for (int i = sx; i < stx; i++) - { - for (int j = sy; j < sty; j++) - { - for (int k = sz; k < stz; k++) - { - index_next_bucket = index_bucket + - i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + - j * rgd_params.number_of_buckets_Z + k; - - if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) - { - - PointCloud::Bucket &b = (*buckets)[index_next_bucket]; - - number_of_points_in_bucket = b.number_of_points; - - if (number_of_points_in_bucket <= 0) - continue; - - int max_number_considered_in_bucket; - if (index_next_bucket == index_bucket) - { - max_number_considered_in_bucket = 100; - } - else - { - max_number_considered_in_bucket = 100; - } - if (max_number_considered_in_bucket <= 0) - continue; - - if (max_number_considered_in_bucket >= number_of_points_in_bucket) - { - iter = 1; - } - else - { - iter = number_of_points_in_bucket / max_number_considered_in_bucket; - if (iter <= 0) - iter = 1; - } - - l_begin = b.index_begin; - l_end = b.index_end; - - for (int l = l_begin; l < l_end; l += iter) - { - if (l >= 0 && l < (*index_pairs).size()) - { - int hashed_index_of_point = (*index_pairs)[l].index_of_point; - - if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points).size()) - { - Eigen::Vector3d &point_nn = (*points)[hashed_index_of_point]; - - double dist = (point_nn - point).norm(); - - if (dist < search_radious) - { - cov[0][0] += (mean.x() - point_nn.x()) * (mean.x() - point_nn.x()); - cov[0][1] += (mean.x() - point_nn.x()) * (mean.y() - point_nn.y()); - cov[0][2] += (mean.x() - point_nn.x()) * (mean.z() - point_nn.z()); - cov[1][0] += (mean.y() - point_nn.y()) * (mean.x() - point_nn.x()); - cov[1][1] += (mean.y() - point_nn.y()) * (mean.y() - point_nn.y()); - cov[1][2] += (mean.y() - point_nn.y()) * (mean.z() - point_nn.z()); - cov[2][0] += (mean.z() - point_nn.z()) * (mean.x() - point_nn.x()); - cov[2][1] += (mean.z() - point_nn.z()) * (mean.y() - point_nn.y()); - cov[2][2] += (mean.z() - point_nn.z()) * (mean.z() - point_nn.z()); - number_of_points_nn++; - } - } - } - } // for (int l = l_begin; l < l_end; l += iter){ - } - } - } - } // for (int i = sx; i < stx; i++) - } // if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) - - if (number_of_points_nn >= 3) - { - cov[0][0] /= number_of_points_nn; - cov[0][1] /= number_of_points_nn; - cov[0][2] /= number_of_points_nn; - cov[1][0] /= number_of_points_nn; - cov[1][1] /= number_of_points_nn; - cov[1][2] /= number_of_points_nn; - cov[2][0] /= number_of_points_nn; - cov[2][1] /= number_of_points_nn; - cov[2][2] /= number_of_points_nn; - - double U[3][3], V[3][3]; - double SS[9]; - gpuSVD((double *)cov, (double *)U, (double *)SS, (double *)V); - double _nx = (float)(U[0][1] * U[1][2] - U[0][2] * U[1][1]); - double _ny = (float)(-(U[0][0] * U[1][2] - U[0][2] * U[1][0])); - double _nz = (float)(U[0][0] * U[1][1] - U[0][1] * U[1][0]); - - double lenght = sqrt(_nx * _nx + _ny * _ny + _nz * _nz); - if (lenght == 0) - { - normal_vector.x() = 0.0f; - normal_vector.y() = 0.0f; - normal_vector.z() = 0.0f; - } - else - { - normal_vector.x() = _nx / lenght; - normal_vector.y() = _ny / lenght; - normal_vector.z() = _nz / lenght; - } - } - } // if(number_of_points_nn >= 3) - } + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { + Eigen::Vector3d& point = (*points)[ii]; + Eigen::Vector3d& normal_vector = (*normal_vectors)[ii]; + + if (point.x() < rgd_params.bounding_box_min_X || point.x() > rgd_params.bounding_box_max_X) + continue; + if (point.y() < rgd_params.bounding_box_min_Y || point.y() > rgd_params.bounding_box_max_Y) + continue; + if (point.z() < rgd_params.bounding_box_min_Z || point.z() > rgd_params.bounding_box_max_Z) + continue; + + normal_vector.x() = 0; + normal_vector.y() = 0; + normal_vector.z() = 0; + // point.valid_nv = false; + + Eigen::Vector3d mean(0.0, 0.0, 0.0); + int number_of_points_nn = 0; + + long long unsigned int ix = (point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + long long unsigned int iy = (point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + long long unsigned int iz = (point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + + long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * + static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + + /////////////////////////////////// + if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) + { + int sx, sy, sz, stx, sty, stz; + if (ix == 0) + sx = 0; + else + sx = -1; + if (iy == 0) + sy = 0; + else + sy = -1; + if (iz == 0) + sz = 0; + else + sz = -1; + + if (ix == rgd_params.number_of_buckets_X - 1) + stx = 1; + else + stx = 2; + if (iy == rgd_params.number_of_buckets_Y - 1) + sty = 1; + else + sty = 2; + if (iz == rgd_params.number_of_buckets_Z - 1) + stz = 1; + else + stz = 2; + + long long unsigned int index_next_bucket; + int iter; + int number_of_points_in_bucket; + int l_begin; + int l_end; + float dist = 0.0f; + + /////////////////////////////////////////////////////////////////////////////////////////// + for (int i = sx; i < stx; i++) + { + for (int j = sy; j < sty; j++) + { + for (int k = sz; k < stz; k++) + { + index_next_bucket = index_bucket + i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + + j * rgd_params.number_of_buckets_Z + k; + + if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) + { + PointCloud::Bucket& b = (*buckets)[index_next_bucket]; + + number_of_points_in_bucket = b.number_of_points; + + if (number_of_points_in_bucket <= 0) + continue; + + int max_number_considered_in_bucket; + if (index_next_bucket == index_bucket) + { + max_number_considered_in_bucket = 100; + } + else + { + max_number_considered_in_bucket = 100; + } + if (max_number_considered_in_bucket <= 0) + continue; + + if (max_number_considered_in_bucket >= number_of_points_in_bucket) + { + iter = 1; + } + else + { + iter = number_of_points_in_bucket / max_number_considered_in_bucket; + if (iter <= 0) + iter = 1; + } + + l_begin = b.index_begin; + l_end = b.index_end; + + for (int l = l_begin; l < l_end; l += iter) + { + if (l >= 0 && l < (*index_pairs).size()) + { + int hashed_index_of_point = (*index_pairs)[l].index_of_point; + + if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points).size()) + { + Eigen::Vector3d& point_nn = (*points)[hashed_index_of_point]; + + double dist = (point_nn - point).norm(); + + if (dist < search_radious) + { + mean += point_nn; + number_of_points_nn++; + } + } + } + } + } + } + } + } + } + + if (number_of_points_nn >= 3) + { + mean /= number_of_points_nn; + + double cov[3][3]; + cov[0][0] = cov[0][1] = cov[0][2] = cov[1][0] = cov[1][1] = cov[1][2] = cov[2][0] = cov[2][1] = cov[2][2] = 0; + number_of_points_nn = 0; + + if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) + { + int sx, sy, sz, stx, sty, stz; + if (ix == 0) + sx = 0; + else + sx = -1; + if (iy == 0) + sy = 0; + else + sy = -1; + if (iz == 0) + sz = 0; + else + sz = -1; + + if (ix == rgd_params.number_of_buckets_X - 1) + stx = 1; + else + stx = 2; + if (iy == rgd_params.number_of_buckets_Y - 1) + sty = 1; + else + sty = 2; + if (iz == rgd_params.number_of_buckets_Z - 1) + stz = 1; + else + stz = 2; + + long long unsigned int index_next_bucket; + int iter; + int number_of_points_in_bucket; + int l_begin; + int l_end; + float dist = 0.0f; + + /////////////////////////////////////////////////////////////////////////////////////////// + for (int i = sx; i < stx; i++) + { + for (int j = sy; j < sty; j++) + { + for (int k = sz; k < stz; k++) + { + index_next_bucket = index_bucket + i * rgd_params.number_of_buckets_Y * rgd_params.number_of_buckets_Z + + j * rgd_params.number_of_buckets_Z + k; + + if (index_next_bucket >= 0 && index_next_bucket < (*buckets).size()) + { + PointCloud::Bucket& b = (*buckets)[index_next_bucket]; + + number_of_points_in_bucket = b.number_of_points; + + if (number_of_points_in_bucket <= 0) + continue; + + int max_number_considered_in_bucket; + if (index_next_bucket == index_bucket) + { + max_number_considered_in_bucket = 100; + } + else + { + max_number_considered_in_bucket = 100; + } + if (max_number_considered_in_bucket <= 0) + continue; + + if (max_number_considered_in_bucket >= number_of_points_in_bucket) + { + iter = 1; + } + else + { + iter = number_of_points_in_bucket / max_number_considered_in_bucket; + if (iter <= 0) + iter = 1; + } + + l_begin = b.index_begin; + l_end = b.index_end; + + for (int l = l_begin; l < l_end; l += iter) + { + if (l >= 0 && l < (*index_pairs).size()) + { + int hashed_index_of_point = (*index_pairs)[l].index_of_point; + + if (hashed_index_of_point >= 0 && hashed_index_of_point < (*points).size()) + { + Eigen::Vector3d& point_nn = (*points)[hashed_index_of_point]; + + double dist = (point_nn - point).norm(); + + if (dist < search_radious) + { + cov[0][0] += (mean.x() - point_nn.x()) * (mean.x() - point_nn.x()); + cov[0][1] += (mean.x() - point_nn.x()) * (mean.y() - point_nn.y()); + cov[0][2] += (mean.x() - point_nn.x()) * (mean.z() - point_nn.z()); + cov[1][0] += (mean.y() - point_nn.y()) * (mean.x() - point_nn.x()); + cov[1][1] += (mean.y() - point_nn.y()) * (mean.y() - point_nn.y()); + cov[1][2] += (mean.y() - point_nn.y()) * (mean.z() - point_nn.z()); + cov[2][0] += (mean.z() - point_nn.z()) * (mean.x() - point_nn.x()); + cov[2][1] += (mean.z() - point_nn.z()) * (mean.y() - point_nn.y()); + cov[2][2] += (mean.z() - point_nn.z()) * (mean.z() - point_nn.z()); + number_of_points_nn++; + } + } + } + } // for (int l = l_begin; l < l_end; l += iter){ + } + } + } + } // for (int i = sx; i < stx; i++) + } // if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) + + if (number_of_points_nn >= 3) + { + cov[0][0] /= number_of_points_nn; + cov[0][1] /= number_of_points_nn; + cov[0][2] /= number_of_points_nn; + cov[1][0] /= number_of_points_nn; + cov[1][1] /= number_of_points_nn; + cov[1][2] /= number_of_points_nn; + cov[2][0] /= number_of_points_nn; + cov[2][1] /= number_of_points_nn; + cov[2][2] /= number_of_points_nn; + + double U[3][3], V[3][3]; + double SS[9]; + gpuSVD((double*)cov, (double*)U, (double*)SS, (double*)V); + double _nx = (float)(U[0][1] * U[1][2] - U[0][2] * U[1][1]); + double _ny = (float)(-(U[0][0] * U[1][2] - U[0][2] * U[1][0])); + double _nz = (float)(U[0][0] * U[1][1] - U[0][1] * U[1][0]); + + double lenght = sqrt(_nx * _nx + _ny * _ny + _nz * _nz); + if (lenght == 0) + { + normal_vector.x() = 0.0f; + normal_vector.y() = 0.0f; + normal_vector.z() = 0.0f; + } + else + { + normal_vector.x() = _nx / lenght; + normal_vector.y() = _ny / lenght; + normal_vector.z() = _nz / lenght; + } + } + } // if(number_of_points_nn >= 3) + } } void PointCloud::compute_normal_vectors(double search_radious) { - number_points_vertical = 0; - number_points_horizontal = 0; - - normal_vectors_local.clear(); - normal_vectors_local.resize(points_local.size()); - points_type.resize(points_local.size()); - std::vector threads; - std::vector jobs = get_jobs(points_local.size(), num_threads); - - for (size_t i = 0; i < jobs.size(); i++) - { - threads.push_back(std::thread(compute_normal_vectors_job, - i, - &jobs[i], - &points_local, - rgd_params, - &buckets, - &index_pairs, - &normal_vectors_local, - search_radious)); - } - - for (size_t j = 0; j < threads.size(); j++) - { - threads[j].join(); - } - threads.clear(); - - for (size_t i = 0; i < normal_vectors_local.size(); i++) - { - auto nv = normal_vectors_local[i]; - nv = this->m_pose.rotation() * nv; - - if (fabs(nv.z()) > 0.8) - { - number_points_horizontal++; - points_type[i] = 0; - } - else - { - number_points_vertical++; - points_type[i] = 1; - } - } - std::cout << "number_points_horizontal: " << number_points_horizontal << " number_points_vertical: " << number_points_vertical << std::endl; + number_points_vertical = 0; + number_points_horizontal = 0; + + normal_vectors_local.clear(); + normal_vectors_local.resize(points_local.size()); + points_type.resize(points_local.size()); + std::vector threads; + std::vector jobs = get_jobs(points_local.size(), num_threads); + + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread( + compute_normal_vectors_job, + i, + &jobs[i], + &points_local, + rgd_params, + &buckets, + &index_pairs, + &normal_vectors_local, + search_radious)); + } + + for (size_t j = 0; j < threads.size(); j++) + { + threads[j].join(); + } + threads.clear(); + + for (size_t i = 0; i < normal_vectors_local.size(); i++) + { + auto nv = normal_vectors_local[i]; + nv = this->m_pose.rotation() * nv; + + if (fabs(nv.z()) > 0.8) + { + number_points_horizontal++; + points_type[i] = 0; + } + else + { + number_points_vertical++; + points_type[i] = 1; + } + } + std::cout << "number_points_horizontal: " << number_points_horizontal << " number_points_vertical: " << number_points_vertical + << std::endl; } void PointCloud::decimate(double bucket_x, double bucket_y, double bucket_z) { - auto params = rgd_params; - - params.resolution_X = bucket_x; - params.resolution_Y = bucket_y; - params.resolution_Z = bucket_z; - params.bounding_box_extension = 1.0; - - grid_calculate_params(this->points_local, params); - - std::vector ip; - reindex(ip, this->points_local, params); - - std::vector n_points_local; - std::vector n_normal_vectors_local; - std::vector n_points_type; - std::vector n_intensities; - std::vector n_timestamps; - std::vector n_colors; - - for (int i = 1; i < ip.size(); i++) - { - if (ip[i - 1].index_of_bucket != ip[i].index_of_bucket) - { - // std::cout << index_pairs[i].index_of_bucket << std::endl; - n_points_local.emplace_back(points_local[ip[i].index_of_point]); - if (normal_vectors_local.size() == points_local.size()) - n_normal_vectors_local.emplace_back(normal_vectors_local[ip[i].index_of_point]); - if (points_type.size() == points_local.size()) - n_points_type.emplace_back(points_type[ip[i].index_of_point]); - if (intensities.size() == points_local.size()) - n_intensities.emplace_back(intensities[ip[i].index_of_point]); - if (timestamps.size() == points_local.size()) - n_timestamps.emplace_back(timestamps[ip[i].index_of_point]); - if (colors.size() == points_local.size()) - n_colors.emplace_back(colors[ip[i].index_of_point]); - } - } - - std::cout << "colors.size(): " << colors.size() << std::endl; - std::cout << "n_colors.size(): " << n_colors.size() << std::endl; - - points_local = n_points_local; - normal_vectors_local = n_normal_vectors_local; - points_type = n_points_type; - intensities = n_intensities; - timestamps = n_timestamps; - colors = n_colors; + auto params = rgd_params; + + params.resolution_X = bucket_x; + params.resolution_Y = bucket_y; + params.resolution_Z = bucket_z; + params.bounding_box_extension = 1.0; + + grid_calculate_params(this->points_local, params); + + std::vector ip; + reindex(ip, this->points_local, params); + + std::vector n_points_local; + std::vector n_normal_vectors_local; + std::vector n_points_type; + std::vector n_intensities; + std::vector n_timestamps; + std::vector n_colors; + + for (int i = 1; i < ip.size(); i++) + { + if (ip[i - 1].index_of_bucket != ip[i].index_of_bucket) + { + // std::cout << index_pairs[i].index_of_bucket << std::endl; + n_points_local.emplace_back(points_local[ip[i].index_of_point]); + if (normal_vectors_local.size() == points_local.size()) + n_normal_vectors_local.emplace_back(normal_vectors_local[ip[i].index_of_point]); + if (points_type.size() == points_local.size()) + n_points_type.emplace_back(points_type[ip[i].index_of_point]); + if (intensities.size() == points_local.size()) + n_intensities.emplace_back(intensities[ip[i].index_of_point]); + if (timestamps.size() == points_local.size()) + n_timestamps.emplace_back(timestamps[ip[i].index_of_point]); + if (colors.size() == points_local.size()) + n_colors.emplace_back(colors[ip[i].index_of_point]); + } + } + + std::cout << "colors.size(): " << colors.size() << std::endl; + std::cout << "n_colors.size(): " << n_colors.size() << std::endl; + + points_local = n_points_local; + normal_vectors_local = n_normal_vectors_local; + points_type = n_points_type; + intensities = n_intensities; + timestamps = n_timestamps; + colors = n_colors; } void PointCloud::shift_to_center() { - Eigen::Vector3d mean(0, 0, 0); - double sum = 0.0; - - for (int i = 0; i < this->points_local.size(); i += 1000) - { - mean += this->points_local[i]; - sum++; - } - if (sum > 0) - { - mean /= sum; - for (int i = 0; i < this->points_local.size(); i++) - { - this->points_local[i] -= mean; - } - } + Eigen::Vector3d mean(0, 0, 0); + double sum = 0.0; + + for (int i = 0; i < this->points_local.size(); i += 1000) + { + mean += this->points_local[i]; + sum++; + } + if (sum > 0) + { + mean /= sum; + for (int i = 0; i < this->points_local.size(); i++) + { + this->points_local[i] -= mean; + } + } } #if WITH_GUI == 1 -void PointCloud::render(bool show_with_initial_pose, const ObservationPicking &observation_picking, int viewer_decimate_point_cloud, bool xz_intersection, bool yz_intersection, - bool xy_intersection, double intersection_width, bool visible_imu_diff) +void PointCloud::render( + bool show_with_initial_pose, + const ObservationPicking& observation_picking, + int viewer_decimate_point_cloud, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width, + bool visible_imu_diff) { - // std::cout << (int)xz_grid_10x10 << std::endl; - - glPointSize(observation_picking.point_size); - - if (this->visible) - { - glColor3f(render_color[0], render_color[1], render_color[2]); - glPointSize(point_size); - - // glColor3f(render_color[0], render_color[1], render_color[2]); - if (visible_imu_diff) - { - glBegin(GL_LINES); - for (int i = 1; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - - glColor3f(1, 0, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10, - m(1, 3), - m(2, 3)); - - glColor3f(0, 1, 0); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), - m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10, - m(2, 3)); - - glColor3f(0, 0, 1); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), - m(1, 3), - m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); - - // std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << std::endl; - } - glEnd(); - } - - // glBegin(GL_POINTS); - // for (const auto& p : this->points_local) { - - // std::cout << this->colors.size() << " " << this->points_local.size() << std::endl; - - glBegin(GL_POINTS); - for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) - { - const auto &p = this->points_local[i]; - Eigen::Vector3d vp; - if (show_with_initial_pose) - { - vp = this->m_initial_pose * p; - } - else - { - vp = this->m_pose * p; - } - if (this->show_color) - { - if (this->colors.size() == this->points_local.size()) - { - glColor3f(this->colors[i].x(), this->colors[i].y(), this->colors[i].z()); - } - } - // - - if (observation_picking.is_observation_picking_mode) - { - if (fabs(vp.z() - observation_picking.picking_plane_height) <= observation_picking.picking_plane_threshold) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - else - { - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - - if (!xz_intersection && !yz_intersection && !xy_intersection) - { - glVertex3d(vp.x(), vp.y(), vp.z()); - } - } - // glEnd(); - } - glEnd(); - glPointSize(1); - - if (!xz_intersection && !yz_intersection && !xy_intersection) - { - glColor3f(render_color[0], render_color[1], render_color[2]); - glBegin(GL_LINE_STRIP); - for (int i = 0; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - } - glEnd(); - - if (this->local_trajectory.size() > 0 && this->fuse_inclination_from_IMU) - { - // double om = local_trajectory[0].imu_om_fi_ka.x() * 180.0 / M_PI; - // double fi = local_trajectory[0].imu_om_fi_ka.y() * 180.0 / M_PI; - - // if (fabs(om) > 5 || fabs(fi) > 5) - //{ - // } - // else - //{ - - Eigen::Vector3d a1(-0.2, -0.2, 0); - Eigen::Vector3d a2(0.2, -0.2, 0); - Eigen::Vector3d a3(0.2, 0.2, 0); - Eigen::Vector3d a4(-0.2, 0.2, 0); - - Eigen::Vector3d a1t = this->m_pose * a1; - Eigen::Vector3d a2t = this->m_pose * a2; - Eigen::Vector3d a3t = this->m_pose * a3; - Eigen::Vector3d a4t = this->m_pose * a4; - - glColor3f(0, 1, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - - TaitBryanPose tb; - tb.px = this->m_pose(0, 3); - tb.py = this->m_pose(1, 3); - tb.pz = this->m_pose(2, 3); - tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); - tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); - tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); - - auto m = affine_matrix_from_pose_tait_bryan(tb); - a1t = m * a1; - a2t = m * a2; - a3t = m * a3; - a4t = m * a4; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - //} - } - - if (this->fixed_om && this->fixed_fi) - { - for (double x = -1.0; x <= 1.0; x += 0.1) - { - if (fabs(x) < 0.3) - { - continue; - } - - Eigen::Vector3d a1(-x, -x, 0); - Eigen::Vector3d a2(x, -x, 0); - Eigen::Vector3d a3(x, x, 0); - Eigen::Vector3d a4(-x, x, 0); - - Eigen::Vector3d a1t = this->m_pose * a1; - Eigen::Vector3d a2t = this->m_pose * a2; - Eigen::Vector3d a3t = this->m_pose * a3; - Eigen::Vector3d a4t = this->m_pose * a4; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glVertex3f(a2t.x(), a2t.y(), a2t.z()); - glVertex3f(a3t.x(), a3t.y(), a3t.z()); - glVertex3f(a4t.x(), a4t.y(), a4t.z()); - glVertex3f(a1t.x(), a1t.y(), a1t.z()); - glEnd(); - } - } - - if (show_IMU) - { - TaitBryanPose tb; - tb.px = this->m_pose(0, 3); - tb.py = this->m_pose(1, 3); - tb.pz = this->m_pose(2, 3); - tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); - tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); - tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); - - Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(tb); - - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - } - - if (show_pose) - { - Eigen::Affine3d m = this->m_pose; - - glLineWidth(2); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - glLineWidth(1); - } - } - } + // std::cout << (int)xz_grid_10x10 << std::endl; + + glPointSize(observation_picking.point_size); + + if (this->visible) + { + glColor3f(render_color[0], render_color[1], render_color[2]); + glPointSize(point_size); + + // glColor3f(render_color[0], render_color[1], render_color[2]); + if (visible_imu_diff) + { + glBegin(GL_LINES); + for (int i = 1; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + + glColor3f(1, 0, 0); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10, m(1, 3), m(2, 3)); + + glColor3f(0, 1, 0); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3), m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10, m(2, 3)); + + glColor3f(0, 0, 1); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3), m(1, 3), m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); + + // std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << + // this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << + // this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << std::endl; + } + glEnd(); + } + + // glBegin(GL_POINTS); + // for (const auto& p : this->points_local) { + + // std::cout << this->colors.size() << " " << this->points_local.size() << std::endl; + + glBegin(GL_POINTS); + for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) + { + const auto& p = this->points_local[i]; + Eigen::Vector3d vp; + if (show_with_initial_pose) + { + vp = this->m_initial_pose * p; + } + else + { + vp = this->m_pose * p; + } + if (this->show_color) + { + if (this->colors.size() == this->points_local.size()) + { + glColor3f(this->colors[i].x(), this->colors[i].y(), this->colors[i].z()); + } + } + // + + if (observation_picking.is_observation_picking_mode) + { + if (fabs(vp.z() - observation_picking.picking_plane_height) <= observation_picking.picking_plane_threshold) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + else + { + if (xz_intersection) + { + if (fabs(vp.y()) < intersection_width) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + if (yz_intersection) + { + if (fabs(vp.x()) < intersection_width) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + if (xy_intersection) + { + if (fabs(vp.z()) < intersection_width) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + + if (!xz_intersection && !yz_intersection && !xy_intersection) + { + glVertex3d(vp.x(), vp.y(), vp.z()); + } + } + // glEnd(); + } + glEnd(); + glPointSize(1); + + if (!xz_intersection && !yz_intersection && !xy_intersection) + { + glColor3f(render_color[0], render_color[1], render_color[2]); + glBegin(GL_LINE_STRIP); + for (int i = 0; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + } + glEnd(); + + if (this->local_trajectory.size() > 0 && this->fuse_inclination_from_IMU) + { + // double om = local_trajectory[0].imu_om_fi_ka.x() * 180.0 / M_PI; + // double fi = local_trajectory[0].imu_om_fi_ka.y() * 180.0 / M_PI; + + // if (fabs(om) > 5 || fabs(fi) > 5) + //{ + // } + // else + //{ + + Eigen::Vector3d a1(-0.2, -0.2, 0); + Eigen::Vector3d a2(0.2, -0.2, 0); + Eigen::Vector3d a3(0.2, 0.2, 0); + Eigen::Vector3d a4(-0.2, 0.2, 0); + + Eigen::Vector3d a1t = this->m_pose * a1; + Eigen::Vector3d a2t = this->m_pose * a2; + Eigen::Vector3d a3t = this->m_pose * a3; + Eigen::Vector3d a4t = this->m_pose * a4; + + glColor3f(0, 1, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glEnd(); + + TaitBryanPose tb; + tb.px = this->m_pose(0, 3); + tb.py = this->m_pose(1, 3); + tb.pz = this->m_pose(2, 3); + tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); + + auto m = affine_matrix_from_pose_tait_bryan(tb); + a1t = m * a1; + a2t = m * a2; + a3t = m * a3; + a4t = m * a4; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glEnd(); + //} + } + + if (this->fixed_om && this->fixed_fi) + { + for (double x = -1.0; x <= 1.0; x += 0.1) + { + if (fabs(x) < 0.3) + { + continue; + } + + Eigen::Vector3d a1(-x, -x, 0); + Eigen::Vector3d a2(x, -x, 0); + Eigen::Vector3d a3(x, x, 0); + Eigen::Vector3d a4(-x, x, 0); + + Eigen::Vector3d a1t = this->m_pose * a1; + Eigen::Vector3d a2t = this->m_pose * a2; + Eigen::Vector3d a3t = this->m_pose * a3; + Eigen::Vector3d a4t = this->m_pose * a4; + + glColor3f(1, 0, 0); + glBegin(GL_LINE_STRIP); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glVertex3f(a2t.x(), a2t.y(), a2t.z()); + glVertex3f(a3t.x(), a3t.y(), a3t.z()); + glVertex3f(a4t.x(), a4t.y(), a4t.z()); + glVertex3f(a1t.x(), a1t.y(), a1t.z()); + glEnd(); + } + } + + if (show_IMU) + { + TaitBryanPose tb; + tb.px = this->m_pose(0, 3); + tb.py = this->m_pose(1, 3); + tb.pz = this->m_pose(2, 3); + tb.om = this->local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = this->local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = this->local_trajectory[0].imu_om_fi_ka.z(); + + Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(tb); + + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + } + + if (show_pose) + { + Eigen::Affine3d m = this->m_pose; + + glLineWidth(2); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + glLineWidth(1); + } + } + } } void PointCloud::render(Eigen::Affine3d pose, int viewer_decimate_point_cloud) { - if (this->visible) - { - glColor3f(render_color[0], render_color[1], render_color[2]); - glPointSize(point_size); - glBegin(GL_POINTS); - - for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) - { - const auto &p = this->points_local[i]; - Eigen::Vector3d vp; - vp = pose * p; - glVertex3d(vp.x(), vp.y(), vp.z()); - } - glEnd(); - glPointSize(1); - - glColor3f(render_color[0], render_color[1], render_color[2]); - glBegin(GL_LINE_STRIP); - for (int i = 0; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - } - glEnd(); - - /*glColor3f(render_color[0], render_color[1], render_color[2]); - glBegin(GL_LINES); - for (int i = 1; i < this->local_trajectory.size(); i++) - { - auto m = this->m_pose * this->local_trajectory[i].m_pose; - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - - glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10000, - m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10000, - m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10000); - - std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << std::endl; - } - glEnd();*/ - } + if (this->visible) + { + glColor3f(render_color[0], render_color[1], render_color[2]); + glPointSize(point_size); + glBegin(GL_POINTS); + + for (int i = 0; i < this->points_local.size(); i += viewer_decimate_point_cloud) + { + const auto& p = this->points_local[i]; + Eigen::Vector3d vp; + vp = pose * p; + glVertex3d(vp.x(), vp.y(), vp.z()); + } + glEnd(); + glPointSize(1); + + glColor3f(render_color[0], render_color[1], render_color[2]); + glBegin(GL_LINE_STRIP); + for (int i = 0; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + } + glEnd(); + + /*glColor3f(render_color[0], render_color[1], render_color[2]); + glBegin(GL_LINES); + for (int i = 1; i < this->local_trajectory.size(); i++) + { + auto m = this->m_pose * this->local_trajectory[i].m_pose; + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + + glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10000, + m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10000, + m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10000); + + std::cout << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() << " " << + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() << " " << this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() << + std::endl; + } + glEnd();*/ + } } -#endif \ No newline at end of file +#endif diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index 681bfd57..b6d4bf96 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -1,1035 +1,1087 @@ -#include -#include +#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include +#include +#include // #include // #include -inline void split(std::string &str, char delim, std::vector &out) +inline void split(std::string& str, char delim, std::vector& out) { - size_t start; - size_t end = 0; - - while ((start = str.find_first_not_of(delim, end)) != std::string::npos) - { - end = str.find(delim, start); - out.push_back(str.substr(start, end - start)); - } + size_t start; + size_t end = 0; + + while ((start = str.find_first_not_of(delim, end)) != std::string::npos) + { + end = str.find(delim, start); + out.push_back(str.substr(start, end - start)); + } } -bool PointClouds::load(const std::string &folder_with_point_clouds, const std::string &poses_file_name, bool decimation, - double bucket_x, double bucket_y, double bucket_z, bool load_cache_mode) +bool PointClouds::load( + const std::string& folder_with_point_clouds, + const std::string& poses_file_name, + bool decimation, + double bucket_x, + double bucket_y, + double bucket_z, + bool load_cache_mode) { - point_clouds.clear(); - - std::ifstream infile(poses_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; - return false; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - size_t sum_points_before_decimation = 0; - size_t sum_points_after_decimation = 0; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - PointCloud pc; - pc.file_name = point_cloud_file_name; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_pose(0, 0) = r11; - pc.m_pose(0, 1) = r12; - pc.m_pose(0, 2) = r13; - pc.m_pose(1, 0) = r21; - pc.m_pose(1, 1) = r22; - pc.m_pose(1, 2) = r23; - pc.m_pose(2, 0) = r31; - pc.m_pose(2, 1) = r32; - pc.m_pose(2, 2) = r33; - pc.m_pose(0, 3) = t14; - pc.m_pose(1, 3) = t24; - pc.m_pose(2, 3) = t34; - - pc.m_initial_pose = pc.m_pose; - - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - - if (!load_cache_mode) - { - if (!pc.load(folder_with_point_clouds + "/" + pc.file_name)) - { - point_clouds.clear(); - std::cout << "problem with file '" << folder_with_point_clouds + "/" + pc.file_name << "'" << std::endl; - return false; - } - if (decimation) - { - sum_points_before_decimation += pc.points_local.size(); - pc.decimate(bucket_x, bucket_y, bucket_z); - sum_points_after_decimation += pc.points_local.size(); - } - } - - point_clouds.push_back(pc); - } - infile.close(); - folder_name = folder_with_point_clouds; - - std::cout << "all scans, sum_points_before_decimation: " << sum_points_before_decimation << std::endl; - std::cout << "all scans, sum_points_after_decimation: " << sum_points_after_decimation << std::endl; - print_point_cloud_dimension(); - return true; + point_clouds.clear(); + + std::ifstream infile(poses_file_name); + if (!infile.good()) + { + std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; + return false; + } + std::string line; + std::getline(infile, line); + std::istringstream iss(line); + + int num_scans; + iss >> num_scans; + + std::cout << "number of scans: " << num_scans << std::endl; + size_t sum_points_before_decimation = 0; + size_t sum_points_after_decimation = 0; + + for (size_t i = 0; i < num_scans; i++) + { + std::getline(infile, line); + std::istringstream iss(line); + std::string point_cloud_file_name; + iss >> point_cloud_file_name; + + double r11, r12, r13, r21, r22, r23, r31, r32, r33; + double t14, t24, t34; + + std::getline(infile, line); + std::istringstream iss1(line); + iss1 >> r11 >> r12 >> r13 >> t14; + + std::getline(infile, line); + std::istringstream iss2(line); + iss2 >> r21 >> r22 >> r23 >> t24; + + std::getline(infile, line); + std::istringstream iss3(line); + iss3 >> r31 >> r32 >> r33 >> t34; + + std::getline(infile, line); + + PointCloud pc; + pc.file_name = point_cloud_file_name; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_pose(0, 0) = r11; + pc.m_pose(0, 1) = r12; + pc.m_pose(0, 2) = r13; + pc.m_pose(1, 0) = r21; + pc.m_pose(1, 1) = r22; + pc.m_pose(1, 2) = r23; + pc.m_pose(2, 0) = r31; + pc.m_pose(2, 1) = r32; + pc.m_pose(2, 2) = r33; + pc.m_pose(0, 3) = t14; + pc.m_pose(1, 3) = t24; + pc.m_pose(2, 3) = t34; + + pc.m_initial_pose = pc.m_pose; + + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + + if (!load_cache_mode) + { + if (!pc.load(folder_with_point_clouds + "/" + pc.file_name)) + { + point_clouds.clear(); + std::cout << "problem with file '" << folder_with_point_clouds + "/" + pc.file_name << "'" << std::endl; + return false; + } + if (decimation) + { + sum_points_before_decimation += pc.points_local.size(); + pc.decimate(bucket_x, bucket_y, bucket_z); + sum_points_after_decimation += pc.points_local.size(); + } + } + + point_clouds.push_back(pc); + } + infile.close(); + folder_name = folder_with_point_clouds; + + std::cout << "all scans, sum_points_before_decimation: " << sum_points_before_decimation << std::endl; + std::cout << "all scans, sum_points_after_decimation: " << sum_points_after_decimation << std::endl; + print_point_cloud_dimension(); + return true; } -bool PointClouds::update_poses_from_RESSO(const std::string &folder_with_point_clouds, const std::string &poses_file_name) +bool PointClouds::update_poses_from_RESSO(const std::string& folder_with_point_clouds, const std::string& poses_file_name) { - std::ifstream infile(poses_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << poses_file_name << "' (!infile.good())" << std::endl; - // return false; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - - std::vector pcs; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - PointCloud pc; - pc.file_name = point_cloud_file_name; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_pose(0, 0) = r11; - pc.m_pose(0, 1) = r12; - pc.m_pose(0, 2) = r13; - pc.m_pose(1, 0) = r21; - pc.m_pose(1, 1) = r22; - pc.m_pose(1, 2) = r23; - pc.m_pose(2, 0) = r31; - pc.m_pose(2, 1) = r32; - pc.m_pose(2, 2) = r33; - pc.m_pose(0, 3) = t14; - pc.m_pose(1, 3) = t24; - pc.m_pose(2, 3) = t34; - // pc.m_initial_pose = pc.m_pose; - - // std::cout << "update pose: " << std::endl; - // std::cout << pc.m_pose.matrix() << std::endl; - - pcs.push_back(pc); - } - infile.close(); - - if (point_clouds.size() == pcs.size()) - { - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < pcs.size(); j++) - { - if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) - { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_pose.matrix() << std::endl; - - point_clouds[i].m_pose = pcs[j].m_pose; - point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); - point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; - point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; - point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; - point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); - point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); - point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); - } // else{ - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << std::endl; - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; - // std::cout << "pcs[j].file_name: "<< pcs[j].file_name << std::endl; - // std::cout << "j: " << j << std::endl; - // return false; - //} - } - - /**/ - - // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; - } - } - else - { - std::cout << "PROBLEM point_clouds.size() != pcs.size()" << std::endl; - std::cout << "point_clouds.size() " << point_clouds.size() << std::endl; - std::cout << "poses.size() " << pcs.size() << std::endl; - return false; - } - return true; + std::ifstream infile(poses_file_name); + if (!infile.good()) + { + std::cout << "problem with file: '" << poses_file_name << "' (!infile.good())" << std::endl; + // return false; + } + std::string line; + std::getline(infile, line); + std::istringstream iss(line); + + int num_scans; + iss >> num_scans; + + std::cout << "number of scans: " << num_scans << std::endl; + + std::vector pcs; + + for (size_t i = 0; i < num_scans; i++) + { + std::getline(infile, line); + std::istringstream iss(line); + std::string point_cloud_file_name; + iss >> point_cloud_file_name; + + double r11, r12, r13, r21, r22, r23, r31, r32, r33; + double t14, t24, t34; + + std::getline(infile, line); + std::istringstream iss1(line); + iss1 >> r11 >> r12 >> r13 >> t14; + + std::getline(infile, line); + std::istringstream iss2(line); + iss2 >> r21 >> r22 >> r23 >> t24; + + std::getline(infile, line); + std::istringstream iss3(line); + iss3 >> r31 >> r32 >> r33 >> t34; + + std::getline(infile, line); + + PointCloud pc; + pc.file_name = point_cloud_file_name; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_pose(0, 0) = r11; + pc.m_pose(0, 1) = r12; + pc.m_pose(0, 2) = r13; + pc.m_pose(1, 0) = r21; + pc.m_pose(1, 1) = r22; + pc.m_pose(1, 2) = r23; + pc.m_pose(2, 0) = r31; + pc.m_pose(2, 1) = r32; + pc.m_pose(2, 2) = r33; + pc.m_pose(0, 3) = t14; + pc.m_pose(1, 3) = t24; + pc.m_pose(2, 3) = t34; + // pc.m_initial_pose = pc.m_pose; + + // std::cout << "update pose: " << std::endl; + // std::cout << pc.m_pose.matrix() << std::endl; + + pcs.push_back(pc); + } + infile.close(); + + if (point_clouds.size() == pcs.size()) + { + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < pcs.size(); j++) + { + if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) + { + // std::cout << "-------------------------" << std::endl; + // std::cout << "update pose: " << i << std::endl; + // std::cout << "previous pose: " << std::endl + // << point_clouds[i].m_pose.matrix() << std::endl; + // std::cout << "current pose: " << std::endl + // << pcs[j].m_pose.matrix() << std::endl; + + point_clouds[i].m_pose = pcs[j].m_pose; + point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); + point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; + point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; + point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; + point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); + point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); + point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); + } // else{ + // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << + // std::endl; std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< + // std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; std::cout << "pcs[j].file_name: + // "<< pcs[j].file_name << std::endl; std::cout << "j: " << j << std::endl; + // return false; + //} + } + + /**/ + + // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; + } + } + else + { + std::cout << "PROBLEM point_clouds.size() != pcs.size()" << std::endl; + std::cout << "point_clouds.size() " << point_clouds.size() << std::endl; + std::cout << "poses.size() " << pcs.size() << std::endl; + return false; + } + return true; } -bool PointClouds::update_poses_from_RESSO_inverse(const std::string &folder_with_point_clouds, const std::string &poses_file_name) +bool PointClouds::update_poses_from_RESSO_inverse(const std::string& folder_with_point_clouds, const std::string& poses_file_name) { - std::ifstream infile(poses_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << poses_file_name << "' (!infile.good())" << std::endl; - // return false; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - - std::vector pcs; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - PointCloud pc; - pc.file_name = point_cloud_file_name; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_pose(0, 0) = r11; - pc.m_pose(0, 1) = r12; - pc.m_pose(0, 2) = r13; - pc.m_pose(1, 0) = r21; - pc.m_pose(1, 1) = r22; - pc.m_pose(1, 2) = r23; - pc.m_pose(2, 0) = r31; - pc.m_pose(2, 1) = r32; - pc.m_pose(2, 2) = r33; - pc.m_pose(0, 3) = t14; - pc.m_pose(1, 3) = t24; - pc.m_pose(2, 3) = t34; - // pc.m_initial_pose = pc.m_pose; - - pc.m_pose = pc.m_pose.inverse(); - std::cout << "update pose: " << std::endl; - std::cout << pc.m_pose.matrix() << std::endl; - - pcs.push_back(pc); - } - infile.close(); - - if (point_clouds.size() == pcs.size()) - { - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < pcs.size(); j++) - { - if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) - { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_pose.matrix() << std::endl; - - point_clouds[i].m_pose = pcs[j].m_pose; - point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); - point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; - point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; - point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; - point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); - point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); - point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); - } // else{ - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << std::endl; - // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; - // std::cout << "pcs[j].file_name: "<< pcs[j].file_name << std::endl; - // std::cout << "j: " << j << std::endl; - // return false; - //} - } - - /**/ - - // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; - } - } - else - { - std::cout << "PROBLEM point_clouds.size() != pcs.size()" << std::endl; - std::cout << "point_clouds.size() " << point_clouds.size() << std::endl; - std::cout << "poses.size() " << pcs.size() << std::endl; - return false; - } - return true; + std::ifstream infile(poses_file_name); + if (!infile.good()) + { + std::cout << "problem with file: '" << poses_file_name << "' (!infile.good())" << std::endl; + // return false; + } + std::string line; + std::getline(infile, line); + std::istringstream iss(line); + + int num_scans; + iss >> num_scans; + + std::cout << "number of scans: " << num_scans << std::endl; + + std::vector pcs; + + for (size_t i = 0; i < num_scans; i++) + { + std::getline(infile, line); + std::istringstream iss(line); + std::string point_cloud_file_name; + iss >> point_cloud_file_name; + + double r11, r12, r13, r21, r22, r23, r31, r32, r33; + double t14, t24, t34; + + std::getline(infile, line); + std::istringstream iss1(line); + iss1 >> r11 >> r12 >> r13 >> t14; + + std::getline(infile, line); + std::istringstream iss2(line); + iss2 >> r21 >> r22 >> r23 >> t24; + + std::getline(infile, line); + std::istringstream iss3(line); + iss3 >> r31 >> r32 >> r33 >> t34; + + std::getline(infile, line); + + PointCloud pc; + pc.file_name = point_cloud_file_name; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_pose(0, 0) = r11; + pc.m_pose(0, 1) = r12; + pc.m_pose(0, 2) = r13; + pc.m_pose(1, 0) = r21; + pc.m_pose(1, 1) = r22; + pc.m_pose(1, 2) = r23; + pc.m_pose(2, 0) = r31; + pc.m_pose(2, 1) = r32; + pc.m_pose(2, 2) = r33; + pc.m_pose(0, 3) = t14; + pc.m_pose(1, 3) = t24; + pc.m_pose(2, 3) = t34; + // pc.m_initial_pose = pc.m_pose; + + pc.m_pose = pc.m_pose.inverse(); + std::cout << "update pose: " << std::endl; + std::cout << pc.m_pose.matrix() << std::endl; + + pcs.push_back(pc); + } + infile.close(); + + if (point_clouds.size() == pcs.size()) + { + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < pcs.size(); j++) + { + if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) + { + // std::cout << "-------------------------" << std::endl; + // std::cout << "update pose: " << i << std::endl; + // std::cout << "previous pose: " << std::endl + // << point_clouds[i].m_pose.matrix() << std::endl; + // std::cout << "current pose: " << std::endl + // << pcs[j].m_pose.matrix() << std::endl; + + point_clouds[i].m_pose = pcs[j].m_pose; + point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose); + point_clouds[i].gui_translation[0] = point_clouds[i].pose.px; + point_clouds[i].gui_translation[1] = point_clouds[i].pose.py; + point_clouds[i].gui_translation[2] = point_clouds[i].pose.pz; + point_clouds[i].gui_rotation[0] = rad2deg(point_clouds[i].pose.om); + point_clouds[i].gui_rotation[1] = rad2deg(point_clouds[i].pose.fi); + point_clouds[i].gui_rotation[2] = rad2deg(point_clouds[i].pose.ka); + } // else{ + // std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string() != pcs[j].file_name" << + // std::endl; std::cout << "std::filesystem::path(point_clouds[i].file_name).filename().string(): "<< + // std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; std::cout << "pcs[j].file_name: + // "<< pcs[j].file_name << std::endl; std::cout << "j: " << j << std::endl; + // return false; + //} + } + + /**/ + + // point_clouds[i].m_initial_pose = point_clouds[i].m_pose; + } + } + else + { + std::cout << "PROBLEM point_clouds.size() != pcs.size()" << std::endl; + std::cout << "point_clouds.size() " << point_clouds.size() << std::endl; + std::cout << "poses.size() " << pcs.size() << std::endl; + return false; + } + return true; } -bool PointClouds::update_initial_poses_from_RESSO(const std::string &folder_with_point_clouds, const std::string &poses_file_name) +bool PointClouds::update_initial_poses_from_RESSO(const std::string& folder_with_point_clouds, const std::string& poses_file_name) { - std::ifstream infile(poses_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; - return false; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - - std::vector pcs; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - PointCloud pc; - pc.file_name = point_cloud_file_name; - pc.m_initial_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose(0, 0) = r11; - pc.m_initial_pose(0, 1) = r12; - pc.m_initial_pose(0, 2) = r13; - pc.m_initial_pose(1, 0) = r21; - pc.m_initial_pose(1, 1) = r22; - pc.m_initial_pose(1, 2) = r23; - pc.m_initial_pose(2, 0) = r31; - pc.m_initial_pose(2, 1) = r32; - pc.m_initial_pose(2, 2) = r33; - pc.m_initial_pose(0, 3) = t14; - pc.m_initial_pose(1, 3) = t24; - pc.m_initial_pose(2, 3) = t34; - // pc.m_initial_pose = pc.m_pose; - pcs.push_back(pc); - } - infile.close(); - - if (point_clouds.size() == pcs.size()) - { - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < pcs.size(); j++) - { - if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) - { - // std::cout << "-------------------------" << std::endl; - // std::cout << "update pose: " << i << std::endl; - // std::cout << "previous pose: " << std::endl - // << point_clouds[i].m_initial_pose.matrix() << std::endl; - // std::cout << "current pose: " << std::endl - // << pcs[j].m_initial_pose.matrix() << std::endl; - - point_clouds[i].m_initial_pose = pcs[j].m_initial_pose; - } - } - } - } - return true; + std::ifstream infile(poses_file_name); + if (!infile.good()) + { + std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; + return false; + } + std::string line; + std::getline(infile, line); + std::istringstream iss(line); + + int num_scans; + iss >> num_scans; + + std::cout << "number of scans: " << num_scans << std::endl; + + std::vector pcs; + + for (size_t i = 0; i < num_scans; i++) + { + std::getline(infile, line); + std::istringstream iss(line); + std::string point_cloud_file_name; + iss >> point_cloud_file_name; + + double r11, r12, r13, r21, r22, r23, r31, r32, r33; + double t14, t24, t34; + + std::getline(infile, line); + std::istringstream iss1(line); + iss1 >> r11 >> r12 >> r13 >> t14; + + std::getline(infile, line); + std::istringstream iss2(line); + iss2 >> r21 >> r22 >> r23 >> t24; + + std::getline(infile, line); + std::istringstream iss3(line); + iss3 >> r31 >> r32 >> r33 >> t34; + + std::getline(infile, line); + + PointCloud pc; + pc.file_name = point_cloud_file_name; + pc.m_initial_pose = Eigen::Affine3d::Identity(); + pc.m_initial_pose(0, 0) = r11; + pc.m_initial_pose(0, 1) = r12; + pc.m_initial_pose(0, 2) = r13; + pc.m_initial_pose(1, 0) = r21; + pc.m_initial_pose(1, 1) = r22; + pc.m_initial_pose(1, 2) = r23; + pc.m_initial_pose(2, 0) = r31; + pc.m_initial_pose(2, 1) = r32; + pc.m_initial_pose(2, 2) = r33; + pc.m_initial_pose(0, 3) = t14; + pc.m_initial_pose(1, 3) = t24; + pc.m_initial_pose(2, 3) = t34; + // pc.m_initial_pose = pc.m_pose; + pcs.push_back(pc); + } + infile.close(); + + if (point_clouds.size() == pcs.size()) + { + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < pcs.size(); j++) + { + if (std::filesystem::path(point_clouds[i].file_name).filename().string() == pcs[j].file_name) + { + // std::cout << "-------------------------" << std::endl; + // std::cout << "update pose: " << i << std::endl; + // std::cout << "previous pose: " << std::endl + // << point_clouds[i].m_initial_pose.matrix() << std::endl; + // std::cout << "current pose: " << std::endl + // << pcs[j].m_initial_pose.matrix() << std::endl; + + point_clouds[i].m_initial_pose = pcs[j].m_initial_pose; + } + } + } + } + return true; } -bool PointClouds::load_eth(const std::string &folder_with_point_clouds, const std::string &poses_file_name, bool decimation, double bucket_x, double bucket_y, double bucket_z) +bool PointClouds::load_eth( + const std::string& folder_with_point_clouds, + const std::string& poses_file_name, + bool decimation, + double bucket_x, + double bucket_y, + double bucket_z) { - point_clouds.clear(); - - std::cout << "Loading from file: " << poses_file_name << std::endl; - std::cout << "ply files are located at: " << folder_with_point_clouds << std::endl; - - std::vector> pairs; - - std::ifstream infile(poses_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; - return false; - } - std::string line; - // std::getline(infile, line); - while (std::getline(infile, line)) - { - std::istringstream iss(line); - std::pair pair; - iss >> pair.first >> pair.second; - pairs.push_back(pair); - } - - infile.close(); - - std::cout << "pairs" << std::endl; - for (size_t i = 0; i < pairs.size(); i++) - { - std::cout << pairs[i].first << " " << pairs[i].second << std::endl; - } - - std::set names_set; - for (size_t i = 0; i < pairs.size(); i++) - { - names_set.insert(pairs[i].first); - names_set.insert(pairs[i].second); - } - - std::vector v(names_set.begin(), names_set.end()); - std::cout << "file names:" << std::endl; - for (int i = 0; i < v.size(); i++) - { - std::cout << v[i] << std::endl; - } - - std::cout << "ground truth files with poses" << std::endl; - for (int i = 1; i < v.size(); i++) - { - std::filesystem::path path = folder_with_point_clouds; - path /= std::string("groundtruth"); - std::string filename = v[i - 1] + "-" + v[i] + ".tfm"; - path /= filename; - std::cout << path.string() << std::endl; - } - - size_t sum_points_before_decimation = 0; - size_t sum_points_after_decimation = 0; - - Eigen::Affine3d m_incremental = Eigen::Affine3d::Identity(); - for (int i = 0; i < v.size(); i++) - { - PointCloud pc; - pc.file_name = v[i] + ".ply"; - // pc.m_pose = m_incremental; - - if (i > 0) - { - std::filesystem::path path = folder_with_point_clouds; - path /= std::string("groundtruth"); - std::string filename = v[i] + "-" + v[i - 1] + ".tfm"; - path /= filename; - Eigen::Affine3d m_increment; - if (load_pose_ETH(path.string(), m_increment)) - { - std::cout << "matrix from file: " << path.string() << std::endl; - std::cout << m_increment.matrix() << std::endl; - m_incremental = m_incremental * m_increment; - } - } - pc.m_pose = m_incremental; - - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - - if (!pc.load(folder_with_point_clouds + "/" + pc.file_name)) - { - point_clouds.clear(); - std::cout << "problem with file '" << folder_with_point_clouds + "/" + pc.file_name << "'" << std::endl; - return false; - } - if (decimation) - { - std::cout << "point cloud size before decimation: " << pc.points_local.size() << std::endl; - sum_points_before_decimation += pc.points_local.size(); - pc.decimate(bucket_x, bucket_y, bucket_z); - sum_points_after_decimation += pc.points_local.size(); - std::cout << "point cloud size after decimation: " << pc.points_local.size() << std::endl; - } - point_clouds.push_back(pc); - } - infile.close(); - folder_name = folder_with_point_clouds; - - std::cout << "sum_points before/after decimation: " - << sum_points_before_decimation << " / " - << sum_points_after_decimation << std::endl; - print_point_cloud_dimension(); - return true; + point_clouds.clear(); + + std::cout << "Loading from file: " << poses_file_name << std::endl; + std::cout << "ply files are located at: " << folder_with_point_clouds << std::endl; + + std::vector> pairs; + + std::ifstream infile(poses_file_name); + if (!infile.good()) + { + std::cout << "problem with file: '" << poses_file_name << "'" << std::endl; + return false; + } + std::string line; + // std::getline(infile, line); + while (std::getline(infile, line)) + { + std::istringstream iss(line); + std::pair pair; + iss >> pair.first >> pair.second; + pairs.push_back(pair); + } + + infile.close(); + + std::cout << "pairs" << std::endl; + for (size_t i = 0; i < pairs.size(); i++) + { + std::cout << pairs[i].first << " " << pairs[i].second << std::endl; + } + + std::set names_set; + for (size_t i = 0; i < pairs.size(); i++) + { + names_set.insert(pairs[i].first); + names_set.insert(pairs[i].second); + } + + std::vector v(names_set.begin(), names_set.end()); + std::cout << "file names:" << std::endl; + for (int i = 0; i < v.size(); i++) + { + std::cout << v[i] << std::endl; + } + + std::cout << "ground truth files with poses" << std::endl; + for (int i = 1; i < v.size(); i++) + { + std::filesystem::path path = folder_with_point_clouds; + path /= std::string("groundtruth"); + std::string filename = v[i - 1] + "-" + v[i] + ".tfm"; + path /= filename; + std::cout << path.string() << std::endl; + } + + size_t sum_points_before_decimation = 0; + size_t sum_points_after_decimation = 0; + + Eigen::Affine3d m_incremental = Eigen::Affine3d::Identity(); + for (int i = 0; i < v.size(); i++) + { + PointCloud pc; + pc.file_name = v[i] + ".ply"; + // pc.m_pose = m_incremental; + + if (i > 0) + { + std::filesystem::path path = folder_with_point_clouds; + path /= std::string("groundtruth"); + std::string filename = v[i] + "-" + v[i - 1] + ".tfm"; + path /= filename; + Eigen::Affine3d m_increment; + if (load_pose_ETH(path.string(), m_increment)) + { + std::cout << "matrix from file: " << path.string() << std::endl; + std::cout << m_increment.matrix() << std::endl; + m_incremental = m_incremental * m_increment; + } + } + pc.m_pose = m_incremental; + + pc.m_initial_pose = pc.m_pose; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + + if (!pc.load(folder_with_point_clouds + "/" + pc.file_name)) + { + point_clouds.clear(); + std::cout << "problem with file '" << folder_with_point_clouds + "/" + pc.file_name << "'" << std::endl; + return false; + } + if (decimation) + { + std::cout << "point cloud size before decimation: " << pc.points_local.size() << std::endl; + sum_points_before_decimation += pc.points_local.size(); + pc.decimate(bucket_x, bucket_y, bucket_z); + sum_points_after_decimation += pc.points_local.size(); + std::cout << "point cloud size after decimation: " << pc.points_local.size() << std::endl; + } + point_clouds.push_back(pc); + } + infile.close(); + folder_name = folder_with_point_clouds; + + std::cout << "sum_points before/after decimation: " << sum_points_before_decimation << " / " << sum_points_after_decimation + << std::endl; + print_point_cloud_dimension(); + return true; } #if WITH_GUI == 1 -void PointClouds::draw_grids(bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, - bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, - bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, PointClouds::PointCloudDimensions dims) +void PointClouds::draw_grids( + bool xz_grid_10x10, + bool xz_grid_1x1, + bool xz_grid_01x01, + bool yz_grid_10x10, + bool yz_grid_1x1, + bool yz_grid_01x01, + bool xy_grid_10x10, + bool xy_grid_1x1, + bool xy_grid_01x01, + PointClouds::PointCloudDimensions dims) { - float step; - - if (xz_grid_10x10) - { - step = 10.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - - glColor3f(0.7, 0.7, 0.7); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, 0.0f, z_min); - glVertex3f(x, 0.0f, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(x_min, 0, z); - glVertex3f(x_max, 0, z); - } - glEnd(); - } - - if (xz_grid_1x1) - { - step = 1.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, 0.0f, z_min); - glVertex3f(x, 0.0f, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(x_min, 0, z); - glVertex3f(x_max, 0, z); - } - glEnd(); - } - - if (xz_grid_01x01) - { - step = 0.1f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, 0.0f, z_min); - glVertex3f(x, 0.0f, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(x_min, 0, z); - glVertex3f(x_max, 0, z); - } - glEnd(); - } - - if (yz_grid_10x10) - { - step = 10.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.7, 0.7, 0.7); - glBegin(GL_LINES); - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(0.0f, y, z_min); - glVertex3f(0.0f, y, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(0, y_min, z); - glVertex3f(0, y_max, z); - } - glEnd(); - } - - if (yz_grid_1x1) - { - step = 1.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(0.0f, y, z_min); - glVertex3f(0.0f, y, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(0, y_min, z); - glVertex3f(0, y_max, z); - } - glEnd(); - } - - if (yz_grid_01x01) - { - step = 0.1f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(0.0f, y, z_min); - glVertex3f(0.0f, y, z_max); - } - - for (float z = z_min; z <= z_max; z += step) - { - glVertex3f(0, y_min, z); - glVertex3f(0, y_max, z); - } - glEnd(); - } - - if (xy_grid_10x10) - { - step = 10.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.7, 0.7, 0.7); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, y_min, 0.0); - glVertex3f(x, y_max, 0.0); - } - - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(x_min, y, 0.0); - glVertex3f(x_max, y, 0.0); - } - glEnd(); - } - - if (xy_grid_1x1) - { - step = 1.0f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, y_min, 0.0); - glVertex3f(x, y_max, 0.0); - } - - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(x_min, y, 0.0); - glVertex3f(x_max, y, 0.0); - } - glEnd(); - } - - if (xy_grid_01x01) - { - step = 0.1f; - float x_min = std::floor(dims.x_min / step) * step; - float y_min = std::floor(dims.y_min / step) * step; - float z_min = std::floor(dims.z_min / step) * step; - float x_max = std::ceil(dims.x_max / step) * step; - float y_max = std::ceil(dims.y_max / step) * step; - float z_max = std::ceil(dims.z_max / step) * step; - glColor3f(0.3, 0.3, 0.3); - glBegin(GL_LINES); - for (float x = x_min; x <= x_max; x += step) - { - glVertex3f(x, y_min, 0.0); - glVertex3f(x, y_max, 0.0); - } - - for (float y = y_min; y <= y_max; y += step) - { - glVertex3f(x_min, y, 0.0); - glVertex3f(x_max, y, 0.0); - } - glEnd(); - } + float step; + + if (xz_grid_10x10) + { + step = 10.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + + glColor3f(0.7, 0.7, 0.7); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, 0.0f, z_min); + glVertex3f(x, 0.0f, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(x_min, 0, z); + glVertex3f(x_max, 0, z); + } + glEnd(); + } + + if (xz_grid_1x1) + { + step = 1.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, 0.0f, z_min); + glVertex3f(x, 0.0f, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(x_min, 0, z); + glVertex3f(x_max, 0, z); + } + glEnd(); + } + + if (xz_grid_01x01) + { + step = 0.1f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, 0.0f, z_min); + glVertex3f(x, 0.0f, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(x_min, 0, z); + glVertex3f(x_max, 0, z); + } + glEnd(); + } + + if (yz_grid_10x10) + { + step = 10.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.7, 0.7, 0.7); + glBegin(GL_LINES); + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(0.0f, y, z_min); + glVertex3f(0.0f, y, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(0, y_min, z); + glVertex3f(0, y_max, z); + } + glEnd(); + } + + if (yz_grid_1x1) + { + step = 1.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(0.0f, y, z_min); + glVertex3f(0.0f, y, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(0, y_min, z); + glVertex3f(0, y_max, z); + } + glEnd(); + } + + if (yz_grid_01x01) + { + step = 0.1f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(0.0f, y, z_min); + glVertex3f(0.0f, y, z_max); + } + + for (float z = z_min; z <= z_max; z += step) + { + glVertex3f(0, y_min, z); + glVertex3f(0, y_max, z); + } + glEnd(); + } + + if (xy_grid_10x10) + { + step = 10.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.7, 0.7, 0.7); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, y_min, 0.0); + glVertex3f(x, y_max, 0.0); + } + + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(x_min, y, 0.0); + glVertex3f(x_max, y, 0.0); + } + glEnd(); + } + + if (xy_grid_1x1) + { + step = 1.0f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, y_min, 0.0); + glVertex3f(x, y_max, 0.0); + } + + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(x_min, y, 0.0); + glVertex3f(x_max, y, 0.0); + } + glEnd(); + } + + if (xy_grid_01x01) + { + step = 0.1f; + float x_min = std::floor(dims.x_min / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + glColor3f(0.3, 0.3, 0.3); + glBegin(GL_LINES); + for (float x = x_min; x <= x_max; x += step) + { + glVertex3f(x, y_min, 0.0); + glVertex3f(x, y_max, 0.0); + } + + for (float y = y_min; y <= y_max; y += step) + { + glVertex3f(x_min, y, 0.0); + glVertex3f(x_max, y, 0.0); + } + glEnd(); + } } -void PointClouds::render(const ObservationPicking &observation_picking, int viewer_decmiate_point_cloud, bool xz_intersection, bool yz_intersection, bool xy_intersection, - bool xz_grid_10x10, bool xz_grid_1x1, bool xz_grid_01x01, - bool yz_grid_10x10, bool yz_grid_1x1, bool yz_grid_01x01, - bool xy_grid_10x10, bool xy_grid_1x1, bool xy_grid_01x01, - double intersection_width, PointClouds::PointCloudDimensions dims) +void PointClouds::render( + const ObservationPicking& observation_picking, + int viewer_decmiate_point_cloud, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + bool xz_grid_10x10, + bool xz_grid_1x1, + bool xz_grid_01x01, + bool yz_grid_10x10, + bool yz_grid_1x1, + bool yz_grid_01x01, + bool xy_grid_10x10, + bool xy_grid_1x1, + bool xy_grid_01x01, + double intersection_width, + PointClouds::PointCloudDimensions dims) { - // Draw grids once for the scene - if (xz_grid_10x10 || xz_grid_1x1 || xz_grid_01x01 || - yz_grid_10x10 || yz_grid_1x1 || yz_grid_01x01 || - xy_grid_10x10 || xy_grid_1x1 || xy_grid_01x01) - draw_grids(xz_grid_10x10, xz_grid_1x1, xz_grid_01x01, - yz_grid_10x10, yz_grid_1x1, yz_grid_01x01, - xy_grid_10x10, xy_grid_1x1, xy_grid_01x01, dims); - - // Render each point cloud (points + trajectories) - for (auto &p : point_clouds) - { - p.render(this->show_with_initial_pose, observation_picking, viewer_decmiate_point_cloud, - xz_intersection, yz_intersection, xy_intersection, intersection_width, show_imu_to_lio_diff); - } + // Draw grids once for the scene + if (xz_grid_10x10 || xz_grid_1x1 || xz_grid_01x01 || yz_grid_10x10 || yz_grid_1x1 || yz_grid_01x01 || xy_grid_10x10 || xy_grid_1x1 || + xy_grid_01x01) + draw_grids( + xz_grid_10x10, + xz_grid_1x1, + xz_grid_01x01, + yz_grid_10x10, + yz_grid_1x1, + yz_grid_01x01, + xy_grid_10x10, + xy_grid_1x1, + xy_grid_01x01, + dims); + + // Render each point cloud (points + trajectories) + for (auto& p : point_clouds) + { + p.render( + this->show_with_initial_pose, + observation_picking, + viewer_decmiate_point_cloud, + xz_intersection, + yz_intersection, + xy_intersection, + intersection_width, + show_imu_to_lio_diff); + } } #endif bool PointClouds::save_poses(const std::string file_name, bool is_subsession) { - int number_pc = 0; - if (!is_subsession) - { - number_pc = this->point_clouds.size(); - } - else - { - for (size_t i = 0; i < this->point_clouds.size(); i++) - { - if (this->point_clouds[i].visible) - { - number_pc++; - } - } - } - - std::ofstream outfile; - outfile.open(file_name); - if (!outfile.good()) - { - std::cout << "can not save file: " << file_name << std::endl; - std::cout << "if you can see only '' it means there is no filename assigned to poses, please read manual or ask for support" << std::endl; - std::cout << "To assign filename to poses please use following two buttons in multi_view_tls_registration_step_2" << std::endl; - std::cout << "1: update initial poses from RESSO file" << std::endl; - std::cout << "2: update poses from RESSO file" << std::endl; - return false; - } - - outfile << number_pc << std::endl; - for (size_t i = 0; i < this->point_clouds.size(); i++) - { - if (!is_subsession) - { - outfile << std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; - outfile << this->point_clouds[i].m_pose(0, 0) << " " << this->point_clouds[i].m_pose(0, 1) << " " << this->point_clouds[i].m_pose(0, 2) << " " << this->point_clouds[i].m_pose(0, 3) << std::endl; - outfile << this->point_clouds[i].m_pose(1, 0) << " " << this->point_clouds[i].m_pose(1, 1) << " " << this->point_clouds[i].m_pose(1, 2) << " " << this->point_clouds[i].m_pose(1, 3) << std::endl; - outfile << this->point_clouds[i].m_pose(2, 0) << " " << this->point_clouds[i].m_pose(2, 1) << " " << this->point_clouds[i].m_pose(2, 2) << " " << this->point_clouds[i].m_pose(2, 3) << std::endl; - outfile << "0 0 0 1" << std::endl; - } - else - { - if (this->point_clouds[i].visible) - { - outfile << std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; - outfile << this->point_clouds[i].m_pose(0, 0) << " " << this->point_clouds[i].m_pose(0, 1) << " " << this->point_clouds[i].m_pose(0, 2) << " " << this->point_clouds[i].m_pose(0, 3) << std::endl; - outfile << this->point_clouds[i].m_pose(1, 0) << " " << this->point_clouds[i].m_pose(1, 1) << " " << this->point_clouds[i].m_pose(1, 2) << " " << this->point_clouds[i].m_pose(1, 3) << std::endl; - outfile << this->point_clouds[i].m_pose(2, 0) << " " << this->point_clouds[i].m_pose(2, 1) << " " << this->point_clouds[i].m_pose(2, 2) << " " << this->point_clouds[i].m_pose(2, 3) << std::endl; - outfile << "0 0 0 1" << std::endl; - } - } - } - outfile.close(); - - return true; + int number_pc = 0; + if (!is_subsession) + { + number_pc = this->point_clouds.size(); + } + else + { + for (size_t i = 0; i < this->point_clouds.size(); i++) + { + if (this->point_clouds[i].visible) + { + number_pc++; + } + } + } + + std::ofstream outfile; + outfile.open(file_name); + if (!outfile.good()) + { + std::cout << "can not save file: " << file_name << std::endl; + std::cout << "if you can see only '' it means there is no filename assigned to poses, please read manual or ask for support" + << std::endl; + std::cout << "To assign filename to poses please use following two buttons in multi_view_tls_registration_step_2" << std::endl; + std::cout << "1: update initial poses from RESSO file" << std::endl; + std::cout << "2: update poses from RESSO file" << std::endl; + return false; + } + + outfile << number_pc << std::endl; + for (size_t i = 0; i < this->point_clouds.size(); i++) + { + if (!is_subsession) + { + outfile << std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; + outfile << this->point_clouds[i].m_pose(0, 0) << " " << this->point_clouds[i].m_pose(0, 1) << " " + << this->point_clouds[i].m_pose(0, 2) << " " << this->point_clouds[i].m_pose(0, 3) << std::endl; + outfile << this->point_clouds[i].m_pose(1, 0) << " " << this->point_clouds[i].m_pose(1, 1) << " " + << this->point_clouds[i].m_pose(1, 2) << " " << this->point_clouds[i].m_pose(1, 3) << std::endl; + outfile << this->point_clouds[i].m_pose(2, 0) << " " << this->point_clouds[i].m_pose(2, 1) << " " + << this->point_clouds[i].m_pose(2, 2) << " " << this->point_clouds[i].m_pose(2, 3) << std::endl; + outfile << "0 0 0 1" << std::endl; + } + else + { + if (this->point_clouds[i].visible) + { + outfile << std::filesystem::path(point_clouds[i].file_name).filename().string() << std::endl; + outfile << this->point_clouds[i].m_pose(0, 0) << " " << this->point_clouds[i].m_pose(0, 1) << " " + << this->point_clouds[i].m_pose(0, 2) << " " << this->point_clouds[i].m_pose(0, 3) << std::endl; + outfile << this->point_clouds[i].m_pose(1, 0) << " " << this->point_clouds[i].m_pose(1, 1) << " " + << this->point_clouds[i].m_pose(1, 2) << " " << this->point_clouds[i].m_pose(1, 3) << std::endl; + outfile << this->point_clouds[i].m_pose(2, 0) << " " << this->point_clouds[i].m_pose(2, 1) << " " + << this->point_clouds[i].m_pose(2, 2) << " " << this->point_clouds[i].m_pose(2, 3) << std::endl; + outfile << "0 0 0 1" << std::endl; + } + } + } + outfile.close(); + + return true; } bool PointClouds::save_scans() { - for (size_t i = 0; i < this->point_clouds.size(); i++) - { - if (!this->point_clouds[i].save_as_global(this->out_folder_name)) - { - std::cout << "problem with saving to folder: " << this->out_folder_name << std::endl; - } - } - return true; + for (size_t i = 0; i < this->point_clouds.size(); i++) + { + if (!this->point_clouds[i].save_as_global(this->out_folder_name)) + { + std::cout << "problem with saving to folder: " << this->out_folder_name << std::endl; + } + } + return true; } void PointClouds::show_all() { - for (auto &pc : point_clouds) - { - pc.visible = true; - } + for (auto& pc : point_clouds) + { + pc.visible = true; + } } void PointClouds::show_all_from_range(int index_begin, int index_end) { - if (index_end > index_begin) - { - for (int i = 0; i < point_clouds.size(); i++) - { - point_clouds[i].visible = false; - } - for (int i = 0; i < point_clouds.size(); i++) - { - if (i >= index_begin && i <= index_end) - { - point_clouds[i].visible = true; - } - } - } + if (index_end > index_begin) + { + for (int i = 0; i < point_clouds.size(); i++) + { + point_clouds[i].visible = false; + } + for (int i = 0; i < point_clouds.size(); i++) + { + if (i >= index_begin && i <= index_end) + { + point_clouds[i].visible = true; + } + } + } } void PointClouds::hide_all() { - for (auto &pc : point_clouds) - { - pc.visible = false; - } + for (auto& pc : point_clouds) + { + pc.visible = false; + } } -double get_mean_uncertainty_xyz_impact6x6(std::vector> &uncertainty_before, - std::vector> &uncertainty_after) +double get_mean_uncertainty_xyz_impact6x6( + std::vector>& uncertainty_before, + std::vector>& uncertainty_after) { - double mui = 0.0; + double mui = 0.0; - double sum_before = 0.0; - double sum_after = 0.0; + double sum_before = 0.0; + double sum_after = 0.0; - for (size_t i = 0; i < uncertainty_before.size(); i++) - { - sum_before += ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); - sum_after += ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); - } - sum_before /= uncertainty_before.size(); - sum_after /= uncertainty_before.size(); + for (size_t i = 0; i < uncertainty_before.size(); i++) + { + sum_before += + ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); + sum_after += + ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); + } + sum_before /= uncertainty_before.size(); + sum_after /= uncertainty_before.size(); - mui = sum_before / sum_after; + mui = sum_before / sum_after; - return mui; + return mui; } -double get_mean_uncertainty_xyz_impact7x7(std::vector> &uncertainty_before, - std::vector> &uncertainty_after) +double get_mean_uncertainty_xyz_impact7x7( + std::vector>& uncertainty_before, + std::vector>& uncertainty_after) { - double mui = 0.0; + double mui = 0.0; - double sum_before = 0.0; - double sum_after = 0.0; + double sum_before = 0.0; + double sum_after = 0.0; - for (size_t i = 0; i < uncertainty_before.size(); i++) - { - sum_before += ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); - sum_after += ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); - } - sum_before /= uncertainty_before.size(); - sum_after /= uncertainty_before.size(); + for (size_t i = 0; i < uncertainty_before.size(); i++) + { + sum_before += + ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); + sum_after += + ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); + } + sum_before /= uncertainty_before.size(); + sum_after /= uncertainty_before.size(); - mui = sum_before / sum_after; + mui = sum_before / sum_after; - return mui; + return mui; } -double get_mean_uncertainty_xyz_impact(std::vector> &uncertainty_before, std::vector> &uncertainty_after) +double get_mean_uncertainty_xyz_impact( + std::vector>& uncertainty_before, std::vector>& uncertainty_after) { - double mui = 0.0; + double mui = 0.0; - double sum_before = 0.0; - double sum_after = 0.0; + double sum_before = 0.0; + double sum_after = 0.0; - for (size_t i = 0; i < uncertainty_before.size(); i++) - { - sum_before += ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); - sum_after += ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); - } - sum_before /= uncertainty_before.size(); - sum_after /= uncertainty_before.size(); + for (size_t i = 0; i < uncertainty_before.size(); i++) + { + sum_before += + ((uncertainty_before[i].coeffRef(0, 0) + uncertainty_before[i].coeffRef(1, 1) + uncertainty_before[i].coeffRef(2, 2)) / 3.0); + sum_after += + ((uncertainty_after[i].coeffRef(0, 0) + uncertainty_after[i].coeffRef(1, 1) + uncertainty_after[i].coeffRef(2, 2)) / 3.0); + } + sum_before /= uncertainty_before.size(); + sum_after /= uncertainty_before.size(); - mui = sum_before / sum_after; + mui = sum_before / sum_after; - return mui; + return mui; } -bool PointClouds::load_pose_ETH(const std::string &fn, Eigen::Affine3d &m_increment) +bool PointClouds::load_pose_ETH(const std::string& fn, Eigen::Affine3d& m_increment) { - m_increment = Eigen::Affine3d::Identity(); - - std::ifstream infile(fn); - if (!infile.good()) - { - std::cout << "problem with file: '" << fn << "'" << std::endl; - return false; - } - - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - iss >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r31 >> r32 >> r33 >> t34; - - m_increment(0, 0) = r11; - m_increment(0, 1) = r12; - m_increment(0, 2) = r13; - m_increment(0, 3) = t14; - - m_increment(1, 0) = r21; - m_increment(1, 1) = r22; - m_increment(1, 2) = r23; - m_increment(1, 3) = t24; - - m_increment(2, 0) = r31; - m_increment(2, 1) = r32; - m_increment(2, 2) = r33; - m_increment(2, 3) = t34; - - infile.close(); - return true; + m_increment = Eigen::Affine3d::Identity(); + + std::ifstream infile(fn); + if (!infile.good()) + { + std::cout << "problem with file: '" << fn << "'" << std::endl; + return false; + } + + std::string line; + std::getline(infile, line); + std::istringstream iss(line); + double r11, r12, r13, r21, r22, r23, r31, r32, r33; + double t14, t24, t34; + iss >> r11 >> r12 >> r13 >> t14; + + std::getline(infile, line); + std::istringstream iss1(line); + iss1 >> r21 >> r22 >> r23 >> t24; + + std::getline(infile, line); + std::istringstream iss2(line); + iss2 >> r31 >> r32 >> r33 >> t34; + + m_increment(0, 0) = r11; + m_increment(0, 1) = r12; + m_increment(0, 2) = r13; + m_increment(0, 3) = t14; + + m_increment(1, 0) = r21; + m_increment(1, 1) = r22; + m_increment(1, 2) = r23; + m_increment(1, 3) = t24; + + m_increment(2, 0) = r31; + m_increment(2, 1) = r32; + m_increment(2, 2) = r33; + m_increment(2, 3) = t34; + + infile.close(); + return true; } -bool PointClouds::load_pc(PointCloud &pc, std::string input_file_name, bool load_cache_mode) +bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load_cache_mode) { - return pc.load_pc(input_file_name, load_cache_mode); + return pc.load_pc(input_file_name, load_cache_mode); #if 0 laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) @@ -1178,360 +1230,377 @@ bool PointClouds::load_pc(PointCloud &pc, std::string input_file_name, bool load return true; - #endif +#endif } -bool PointClouds::load_whu_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset, bool load_cache_mode) +bool PointClouds::load_whu_tls( + std::vector input_file_names, + bool is_decimate, + double bucket_x, + double bucket_y, + double bucket_z, + bool calculate_offset, + bool load_cache_mode) { - const auto start = std::chrono::system_clock::now(); - std::vector point_clouds_nodata; - - point_clouds_nodata.resize(input_file_names.size()); // pre-allocate - - for (size_t i = 0; i < input_file_names.size(); i++) - { - std::cout << "Loading file " << i + 1 << "/" << input_file_names.size() << " (" << (std::filesystem::path(input_file_names[i]).filename().string()) << "). "; - - auto &pc = point_clouds_nodata[i]; // reference directly to vector slot - - pc.file_name = input_file_names[i]; - //--- - std::filesystem::path path = std::filesystem::path(pc.file_name); - - auto only_fn = (path.filename()).stem(); - - std::vector strs; - std::string line = only_fn.string(); - split(line, '_', strs); - - //-- - - auto trj_path = std::filesystem::path(input_file_names[i]).parent_path(); - std::filesystem::path trajectorypath(pc.file_name); - // std::string fn = (trajectorypath.filename().stem()).string(); - - trajectorypath.remove_filename(); - - // std::string trj_fn = "trajectory_lio_" + std::to_string(i) + ".csv"; - std::string trj_fn = "trajectory_lio_" + strs[strs.size() - 1] + ".csv"; - - // fn.replace(0, 9, "trajectory_lio_"); - // std::string trajectory_filename = (fn + ".csv"); - // trj_path /= trajectory_filename; - - trj_path /= trj_fn; - - std::cout << "From trajectory file (" << (std::filesystem::path(trj_path).filename().string()) << ")"; - - if (std::filesystem::exists(trj_path)) - { - std::cout << " loading.. "; - - std::vector local_trajectory; - // - std::ifstream infile(trj_path.string()); - if (!infile.good()) - { - std::cout << "problem with file: '" << trj_path.string() << "'" << std::endl; - return false; - } - - std::string s; - getline(infile, s); // csv header - while (!infile.eof()) - { - getline(infile, s); - std::vector strs; - split(s, ' ', strs); - - if (strs.size() == 13) - { - PointCloud::LocalTrajectoryNode ltn; - std::istringstream(strs[0]) >> ltn.timestamps.first; - std::istringstream(strs[1]) >> ltn.m_pose(0, 0); - std::istringstream(strs[2]) >> ltn.m_pose(0, 1); - std::istringstream(strs[3]) >> ltn.m_pose(0, 2); - std::istringstream(strs[4]) >> ltn.m_pose(0, 3); - std::istringstream(strs[5]) >> ltn.m_pose(1, 0); - std::istringstream(strs[6]) >> ltn.m_pose(1, 1); - std::istringstream(strs[7]) >> ltn.m_pose(1, 2); - std::istringstream(strs[8]) >> ltn.m_pose(1, 3); - std::istringstream(strs[9]) >> ltn.m_pose(2, 0); - std::istringstream(strs[10]) >> ltn.m_pose(2, 1); - std::istringstream(strs[11]) >> ltn.m_pose(2, 2); - std::istringstream(strs[12]) >> ltn.m_pose(2, 3); - - ltn.timestamps.second = 0.0; - ltn.imu_om_fi_ka = {0, 0, 0}; - local_trajectory.push_back(ltn); - } - - if (strs.size() == 14) - { - PointCloud::LocalTrajectoryNode ltn; - std::istringstream(strs[0]) >> ltn.timestamps.first; - std::istringstream(strs[1]) >> ltn.m_pose(0, 0); - std::istringstream(strs[2]) >> ltn.m_pose(0, 1); - std::istringstream(strs[3]) >> ltn.m_pose(0, 2); - std::istringstream(strs[4]) >> ltn.m_pose(0, 3); - std::istringstream(strs[5]) >> ltn.m_pose(1, 0); - std::istringstream(strs[6]) >> ltn.m_pose(1, 1); - std::istringstream(strs[7]) >> ltn.m_pose(1, 2); - std::istringstream(strs[8]) >> ltn.m_pose(1, 3); - std::istringstream(strs[9]) >> ltn.m_pose(2, 0); - std::istringstream(strs[10]) >> ltn.m_pose(2, 1); - std::istringstream(strs[11]) >> ltn.m_pose(2, 2); - std::istringstream(strs[12]) >> ltn.m_pose(2, 3); - std::istringstream(strs[13]) >> ltn.timestamps.second; - ltn.imu_om_fi_ka = {0, 0, 0}; - local_trajectory.push_back(ltn); - } - - if (strs.size() == 17) - { - PointCloud::LocalTrajectoryNode ltn; - std::istringstream(strs[0]) >> ltn.timestamps.first; - std::istringstream(strs[1]) >> ltn.m_pose(0, 0); - std::istringstream(strs[2]) >> ltn.m_pose(0, 1); - std::istringstream(strs[3]) >> ltn.m_pose(0, 2); - std::istringstream(strs[4]) >> ltn.m_pose(0, 3); - std::istringstream(strs[5]) >> ltn.m_pose(1, 0); - std::istringstream(strs[6]) >> ltn.m_pose(1, 1); - std::istringstream(strs[7]) >> ltn.m_pose(1, 2); - std::istringstream(strs[8]) >> ltn.m_pose(1, 3); - std::istringstream(strs[9]) >> ltn.m_pose(2, 0); - std::istringstream(strs[10]) >> ltn.m_pose(2, 1); - std::istringstream(strs[11]) >> ltn.m_pose(2, 2); - std::istringstream(strs[12]) >> ltn.m_pose(2, 3); - std::istringstream(strs[13]) >> ltn.timestamps.second; - std::istringstream(strs[14]) >> ltn.imu_om_fi_ka.x(); - std::istringstream(strs[15]) >> ltn.imu_om_fi_ka.y(); - std::istringstream(strs[16]) >> ltn.imu_om_fi_ka.z(); - - local_trajectory.push_back(ltn); - } - } - - std::cout << local_trajectory.size() << " local nodes" << std::endl; - infile.close(); - - pc.local_trajectory = local_trajectory; - } - else - std::cout << "trajectory path: '" << trj_path.string() << "' does not exist" << std::endl; - } - - //// load actual pointclouds - point_clouds.resize(point_clouds_nodata.size()); - - std::transform(std::execution::par_unseq, std::begin(point_clouds_nodata), std::end(point_clouds_nodata), std::begin(point_clouds), [&](auto &pc) - { - if(!load_cache_mode) - { - if (load_pc(pc, pc.file_name, load_cache_mode)) - { - if (is_decimate && pc.points_local.size() > 0) - { - std::cout << "start downsampling.." << std::endl; - - size_t sum_points_before_decimation = pc.points_local.size(); - pc.decimate(bucket_x, bucket_y, bucket_z); - size_t sum_points_after_decimation = pc.points_local.size(); - - std::cout << "downsampling finished. sum_points before/after decimation: " - << sum_points_before_decimation << " / " - << sum_points_after_decimation << std::endl; - } - } - } - - return pc; - }); - - //calculate average position of a subset of points from all clouds to center the point clouds around the origin - if (calculate_offset) - { - int num = 0; - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < point_clouds[i].points_local.size(); j += 1000) - { - this->offset += point_clouds[i].points_local[j]; - num++; - } - } - this->offset /= num; - } - - //recenter point clouds around calculated offset - if (!this->offset.isZero()) - { - for (int i = 0; i < point_clouds.size(); i++) - for (int j = 0; j < point_clouds[i].points_local.size(); j++) - point_clouds[i].points_local[j] -= this->offset; - } - - print_point_cloud_dimension(); - const auto end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - std::cout << "Load time: " << std::fixed << std::setprecision(1) << elapsed_seconds.count() << " [s]" << std::endl; - return true; + const auto start = std::chrono::system_clock::now(); + std::vector point_clouds_nodata; + + point_clouds_nodata.resize(input_file_names.size()); // pre-allocate + + for (size_t i = 0; i < input_file_names.size(); i++) + { + std::cout << "Loading file " << i + 1 << "/" << input_file_names.size() << " (" + << (std::filesystem::path(input_file_names[i]).filename().string()) << "). "; + + auto& pc = point_clouds_nodata[i]; // reference directly to vector slot + + pc.file_name = input_file_names[i]; + //--- + std::filesystem::path path = std::filesystem::path(pc.file_name); + + auto only_fn = (path.filename()).stem(); + + std::vector strs; + std::string line = only_fn.string(); + split(line, '_', strs); + + //-- + + auto trj_path = std::filesystem::path(input_file_names[i]).parent_path(); + std::filesystem::path trajectorypath(pc.file_name); + // std::string fn = (trajectorypath.filename().stem()).string(); + + trajectorypath.remove_filename(); + + // std::string trj_fn = "trajectory_lio_" + std::to_string(i) + ".csv"; + std::string trj_fn = "trajectory_lio_" + strs[strs.size() - 1] + ".csv"; + + // fn.replace(0, 9, "trajectory_lio_"); + // std::string trajectory_filename = (fn + ".csv"); + // trj_path /= trajectory_filename; + + trj_path /= trj_fn; + + std::cout << "From trajectory file (" << (std::filesystem::path(trj_path).filename().string()) << ")"; + + if (std::filesystem::exists(trj_path)) + { + std::cout << " loading.. "; + + std::vector local_trajectory; + // + std::ifstream infile(trj_path.string()); + if (!infile.good()) + { + std::cout << "problem with file: '" << trj_path.string() << "'" << std::endl; + return false; + } + + std::string s; + getline(infile, s); // csv header + while (!infile.eof()) + { + getline(infile, s); + std::vector strs; + split(s, ' ', strs); + + if (strs.size() == 13) + { + PointCloud::LocalTrajectoryNode ltn; + std::istringstream(strs[0]) >> ltn.timestamps.first; + std::istringstream(strs[1]) >> ltn.m_pose(0, 0); + std::istringstream(strs[2]) >> ltn.m_pose(0, 1); + std::istringstream(strs[3]) >> ltn.m_pose(0, 2); + std::istringstream(strs[4]) >> ltn.m_pose(0, 3); + std::istringstream(strs[5]) >> ltn.m_pose(1, 0); + std::istringstream(strs[6]) >> ltn.m_pose(1, 1); + std::istringstream(strs[7]) >> ltn.m_pose(1, 2); + std::istringstream(strs[8]) >> ltn.m_pose(1, 3); + std::istringstream(strs[9]) >> ltn.m_pose(2, 0); + std::istringstream(strs[10]) >> ltn.m_pose(2, 1); + std::istringstream(strs[11]) >> ltn.m_pose(2, 2); + std::istringstream(strs[12]) >> ltn.m_pose(2, 3); + + ltn.timestamps.second = 0.0; + ltn.imu_om_fi_ka = { 0, 0, 0 }; + local_trajectory.push_back(ltn); + } + + if (strs.size() == 14) + { + PointCloud::LocalTrajectoryNode ltn; + std::istringstream(strs[0]) >> ltn.timestamps.first; + std::istringstream(strs[1]) >> ltn.m_pose(0, 0); + std::istringstream(strs[2]) >> ltn.m_pose(0, 1); + std::istringstream(strs[3]) >> ltn.m_pose(0, 2); + std::istringstream(strs[4]) >> ltn.m_pose(0, 3); + std::istringstream(strs[5]) >> ltn.m_pose(1, 0); + std::istringstream(strs[6]) >> ltn.m_pose(1, 1); + std::istringstream(strs[7]) >> ltn.m_pose(1, 2); + std::istringstream(strs[8]) >> ltn.m_pose(1, 3); + std::istringstream(strs[9]) >> ltn.m_pose(2, 0); + std::istringstream(strs[10]) >> ltn.m_pose(2, 1); + std::istringstream(strs[11]) >> ltn.m_pose(2, 2); + std::istringstream(strs[12]) >> ltn.m_pose(2, 3); + std::istringstream(strs[13]) >> ltn.timestamps.second; + ltn.imu_om_fi_ka = { 0, 0, 0 }; + local_trajectory.push_back(ltn); + } + + if (strs.size() == 17) + { + PointCloud::LocalTrajectoryNode ltn; + std::istringstream(strs[0]) >> ltn.timestamps.first; + std::istringstream(strs[1]) >> ltn.m_pose(0, 0); + std::istringstream(strs[2]) >> ltn.m_pose(0, 1); + std::istringstream(strs[3]) >> ltn.m_pose(0, 2); + std::istringstream(strs[4]) >> ltn.m_pose(0, 3); + std::istringstream(strs[5]) >> ltn.m_pose(1, 0); + std::istringstream(strs[6]) >> ltn.m_pose(1, 1); + std::istringstream(strs[7]) >> ltn.m_pose(1, 2); + std::istringstream(strs[8]) >> ltn.m_pose(1, 3); + std::istringstream(strs[9]) >> ltn.m_pose(2, 0); + std::istringstream(strs[10]) >> ltn.m_pose(2, 1); + std::istringstream(strs[11]) >> ltn.m_pose(2, 2); + std::istringstream(strs[12]) >> ltn.m_pose(2, 3); + std::istringstream(strs[13]) >> ltn.timestamps.second; + std::istringstream(strs[14]) >> ltn.imu_om_fi_ka.x(); + std::istringstream(strs[15]) >> ltn.imu_om_fi_ka.y(); + std::istringstream(strs[16]) >> ltn.imu_om_fi_ka.z(); + + local_trajectory.push_back(ltn); + } + } + + std::cout << local_trajectory.size() << " local nodes" << std::endl; + infile.close(); + + pc.local_trajectory = local_trajectory; + } + else + std::cout << "trajectory path: '" << trj_path.string() << "' does not exist" << std::endl; + } + + //// load actual pointclouds + point_clouds.resize(point_clouds_nodata.size()); + + std::transform( + std::execution::par_unseq, + std::begin(point_clouds_nodata), + std::end(point_clouds_nodata), + std::begin(point_clouds), + [&](auto& pc) + { + if (!load_cache_mode) + { + if (load_pc(pc, pc.file_name, load_cache_mode)) + { + if (is_decimate && pc.points_local.size() > 0) + { + std::cout << "start downsampling.." << std::endl; + + size_t sum_points_before_decimation = pc.points_local.size(); + pc.decimate(bucket_x, bucket_y, bucket_z); + size_t sum_points_after_decimation = pc.points_local.size(); + + std::cout << "downsampling finished. sum_points before/after decimation: " << sum_points_before_decimation << " / " + << sum_points_after_decimation << std::endl; + } + } + } + + return pc; + }); + + // calculate average position of a subset of points from all clouds to center the point clouds around the origin + if (calculate_offset) + { + int num = 0; + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < point_clouds[i].points_local.size(); j += 1000) + { + this->offset += point_clouds[i].points_local[j]; + num++; + } + } + this->offset /= num; + } + + // recenter point clouds around calculated offset + if (!this->offset.isZero()) + { + for (int i = 0; i < point_clouds.size(); i++) + for (int j = 0; j < point_clouds[i].points_local.size(); j++) + point_clouds[i].points_local[j] -= this->offset; + } + + print_point_cloud_dimension(); + const auto end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::cout << "Load time: " << std::fixed << std::setprecision(1) << elapsed_seconds.count() << " [s]" << std::endl; + return true; } PointClouds::PointCloudDimensions PointClouds::compute_point_cloud_dimension() const { - PointCloudDimensions dim{ - std::numeric_limits::max(), std::numeric_limits::lowest(), - std::numeric_limits::max(), std::numeric_limits::lowest(), - std::numeric_limits::max(), std::numeric_limits::lowest(), - 0, 0, 0}; - - int sum = 0; - for (const auto &pc : this->point_clouds) - { - for (const auto &p : pc.points_local) - { - auto pt = pc.m_initial_pose * p; - dim.x_min = std::min(dim.x_min, pt.x()); - dim.x_max = std::max(dim.x_max, pt.x()); - dim.y_min = std::min(dim.y_min, pt.y()); - dim.y_max = std::max(dim.y_max, pt.y()); - dim.z_min = std::min(dim.z_min, pt.z()); - dim.z_max = std::max(dim.z_max, pt.z()); - - sum ++; - } - } - - if (sum == 0){ - dim.length = 0.0; - dim.width = 0.0; - dim.height = 0.0; - - dim.x_min = 0.0; - dim.y_min = 0.0; - dim.z_min = 0.0; - - dim.x_max = 0.0; - dim.y_max = 0.0; - dim.z_max = 0.0; - }else{ - dim.length = dim.x_max - dim.x_min; - dim.width = dim.y_max - dim.y_min; - dim.height = dim.z_max - dim.z_min; - } - return dim; + PointCloudDimensions dim{ std::numeric_limits::max(), + std::numeric_limits::lowest(), + std::numeric_limits::max(), + std::numeric_limits::lowest(), + std::numeric_limits::max(), + std::numeric_limits::lowest(), + 0, + 0, + 0 }; + + int sum = 0; + for (const auto& pc : this->point_clouds) + { + for (const auto& p : pc.points_local) + { + auto pt = pc.m_initial_pose * p; + dim.x_min = std::min(dim.x_min, pt.x()); + dim.x_max = std::max(dim.x_max, pt.x()); + dim.y_min = std::min(dim.y_min, pt.y()); + dim.y_max = std::max(dim.y_max, pt.y()); + dim.z_min = std::min(dim.z_min, pt.z()); + dim.z_max = std::max(dim.z_max, pt.z()); + + sum++; + } + } + + if (sum == 0) + { + dim.length = 0.0; + dim.width = 0.0; + dim.height = 0.0; + + dim.x_min = 0.0; + dim.y_min = 0.0; + dim.z_min = 0.0; + + dim.x_max = 0.0; + dim.y_max = 0.0; + dim.z_max = 0.0; + } + else + { + dim.length = dim.x_max - dim.x_min; + dim.width = dim.y_max - dim.y_min; + dim.height = dim.z_max - dim.z_min; + } + return dim; } void PointClouds::print_point_cloud_dimension() { - auto dims = compute_point_cloud_dimension(); - - std::cout << "------------------------" << std::endl; - - std::cout << "Coordinates: min / max / size [m]" << std::endl; - std::cout << "X (length): " << std::fixed << std::setprecision(3) << dims.x_min - << " / " << std::fixed << std::setprecision(3) << dims.x_max - << " / " << std::fixed << std::setprecision(3) << dims.length << std::endl; - std::cout << "Y (width) : " << std::fixed << std::setprecision(3) << dims.y_min - << " / " << std::fixed << std::setprecision(3) << dims.y_max - << " / " << std::fixed << std::setprecision(3) << dims.width << std::endl; - std::cout << "Z (height): " << std::setprecision(3) << dims.z_min - << " / " << std::fixed << std::setprecision(3) << dims.z_max - << " / " << std::fixed << std::setprecision(3) << dims.height << std::endl; - - std::cout << "------------------------" << std::endl; -} + auto dims = compute_point_cloud_dimension(); -bool PointClouds::load_3DTK_tls(std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z) -{ - this->offset = Eigen::Vector3d(0, 0, 0); - size_t sum_points_before_decimation = 0; - size_t sum_points_after_decimation = 0; - - for (size_t i = 0; i < input_file_names.size(); i++) - { - std::cout << "loading file " << i + 1 << "/" << input_file_names.size() << " (" << (std::filesystem::path(input_file_names[i]).filename().string()) << ")" << std::endl; - PointCloud pc; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.m_initial_pose = pc.m_pose; - pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); - pc.gui_translation[0] = pc.pose.px; - pc.gui_translation[1] = pc.pose.py; - pc.gui_translation[2] = pc.pose.pz; - pc.gui_rotation[0] = rad2deg(pc.pose.om); - pc.gui_rotation[1] = rad2deg(pc.pose.fi); - pc.gui_rotation[2] = rad2deg(pc.pose.ka); - - std::ifstream f; - f.open(input_file_names[i]); - if (f.good()) - { - std::string s; - getline(f, s); - while (!f.eof()) - { - getline(f, s); - std::vector strs; - split(s, ' ', strs); - - if (strs.size() >= 3) - { - Eigen::Vector3d pp; - std::istringstream(strs[0]) >> pp.x(); - std::istringstream(strs[1]) >> pp.y(); - std::istringstream(strs[2]) >> pp.z(); - pc.points_local.push_back(pp); - pc.intensities.push_back(0); - pc.timestamps.push_back(0); - sum_points_before_decimation++; - } - } - } - else - { - std::cout << "problem opening file: " << input_file_names[i] << std::endl; - } - pc.file_name = input_file_names[i]; - point_clouds.push_back(pc); - } + std::cout << "------------------------" << std::endl; - int num = 0; - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < point_clouds[i].points_local.size(); j++) - { - this->offset += point_clouds[i].points_local[j]; - num++; - } - } - this->offset /= num; + std::cout << "Coordinates: min / max / size [m]" << std::endl; + std::cout << "X (length): " << std::fixed << std::setprecision(3) << dims.x_min << " / " << std::fixed << std::setprecision(3) + << dims.x_max << " / " << std::fixed << std::setprecision(3) << dims.length << std::endl; + std::cout << "Y (width) : " << std::fixed << std::setprecision(3) << dims.y_min << " / " << std::fixed << std::setprecision(3) + << dims.y_max << " / " << std::fixed << std::setprecision(3) << dims.width << std::endl; + std::cout << "Z (height): " << std::setprecision(3) << dims.z_min << " / " << std::fixed << std::setprecision(3) << dims.z_max << " / " + << std::fixed << std::setprecision(3) << dims.height << std::endl; - for (int i = 0; i < point_clouds.size(); i++) - { - for (int j = 0; j < point_clouds[i].points_local.size(); j++) - { - point_clouds[i].points_local[j] -= this->offset; - } - } + std::cout << "------------------------" << std::endl; +} - std::cout << "start downsampling.." << std::endl; - if (is_decimate) - { - for (int i = 0; i < point_clouds.size(); i++) - { - sum_points_before_decimation += point_clouds[i].points_local.size(); - point_clouds[i].decimate(bucket_x, bucket_y, bucket_z); - sum_points_after_decimation += point_clouds[i].points_local.size(); - } - } - else - { - sum_points_after_decimation = sum_points_before_decimation; - } - std::cout << "downsampling finished. sum_points before/after decimation: " - << sum_points_before_decimation << " / " - << sum_points_after_decimation << std::endl; - print_point_cloud_dimension(); - return true; -} \ No newline at end of file +bool PointClouds::load_3DTK_tls( + std::vector input_file_names, bool is_decimate, double bucket_x, double bucket_y, double bucket_z) +{ + this->offset = Eigen::Vector3d(0, 0, 0); + size_t sum_points_before_decimation = 0; + size_t sum_points_after_decimation = 0; + + for (size_t i = 0; i < input_file_names.size(); i++) + { + std::cout << "loading file " << i + 1 << "/" << input_file_names.size() << " (" + << (std::filesystem::path(input_file_names[i]).filename().string()) << ")" << std::endl; + PointCloud pc; + pc.m_pose = Eigen::Affine3d::Identity(); + pc.m_initial_pose = pc.m_pose; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = pc.pose.px; + pc.gui_translation[1] = pc.pose.py; + pc.gui_translation[2] = pc.pose.pz; + pc.gui_rotation[0] = rad2deg(pc.pose.om); + pc.gui_rotation[1] = rad2deg(pc.pose.fi); + pc.gui_rotation[2] = rad2deg(pc.pose.ka); + + std::ifstream f; + f.open(input_file_names[i]); + if (f.good()) + { + std::string s; + getline(f, s); + while (!f.eof()) + { + getline(f, s); + std::vector strs; + split(s, ' ', strs); + + if (strs.size() >= 3) + { + Eigen::Vector3d pp; + std::istringstream(strs[0]) >> pp.x(); + std::istringstream(strs[1]) >> pp.y(); + std::istringstream(strs[2]) >> pp.z(); + pc.points_local.push_back(pp); + pc.intensities.push_back(0); + pc.timestamps.push_back(0); + sum_points_before_decimation++; + } + } + } + else + { + std::cout << "problem opening file: " << input_file_names[i] << std::endl; + } + pc.file_name = input_file_names[i]; + point_clouds.push_back(pc); + } + + int num = 0; + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < point_clouds[i].points_local.size(); j++) + { + this->offset += point_clouds[i].points_local[j]; + num++; + } + } + this->offset /= num; + + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = 0; j < point_clouds[i].points_local.size(); j++) + { + point_clouds[i].points_local[j] -= this->offset; + } + } + + std::cout << "start downsampling.." << std::endl; + if (is_decimate) + { + for (int i = 0; i < point_clouds.size(); i++) + { + sum_points_before_decimation += point_clouds[i].points_local.size(); + point_clouds[i].decimate(bucket_x, bucket_y, bucket_z); + sum_points_after_decimation += point_clouds[i].points_local.size(); + } + } + else + { + sum_points_after_decimation = sum_points_before_decimation; + } + std::cout << "downsampling finished. sum_points before/after decimation: " << sum_points_before_decimation << " / " + << sum_points_after_decimation << std::endl; + print_point_cloud_dimension(); + return true; +} diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp index 4e4fe738..aa015bf2 100644 --- a/core/src/pose_graph_loop_closure.cpp +++ b/core/src/pose_graph_loop_closure.cpp @@ -1,20 +1,21 @@ -#include -#include -#include -#include +#include + #include +#include #include -#include +#include +#include #include -#include +#include +#include + #include +#include #include -#include - #include +#include -void PoseGraphLoopClosure::add_edge(PointClouds &point_clouds_container, - int index_loop_closure_source, int index_loop_closure_target) +void PoseGraphLoopClosure::add_edge(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target) { Edge edge; edge.index_from = index_loop_closure_source; @@ -35,27 +36,27 @@ void PoseGraphLoopClosure::add_edge(PointClouds &point_clouds_container, edges.push_back(edge); } -void PoseGraphLoopClosure::set_initial_poses_as_motion_model(PointClouds &point_clouds_container) +void PoseGraphLoopClosure::set_initial_poses_as_motion_model(PointClouds& point_clouds_container) { poses_motion_model.clear(); - for (auto &pc : point_clouds_container.point_clouds) + for (auto& pc : point_clouds_container.point_clouds) { poses_motion_model.push_back(pc.m_initial_pose); } std::cout << "Set initial poses as motion model DONE" << std::endl; } -void PoseGraphLoopClosure::set_current_poses_as_motion_model(PointClouds &point_clouds_container) +void PoseGraphLoopClosure::set_current_poses_as_motion_model(PointClouds& point_clouds_container) { poses_motion_model.clear(); - for (auto &pc : point_clouds_container.point_clouds) + for (auto& pc : point_clouds_container.point_clouds) { poses_motion_model.push_back(pc.m_pose); } std::cout << "Set current result as motion model DONE" << std::endl; } -void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS &gnss, GroundControlPoints &gcps, ControlPoints &cps) +void PoseGraphLoopClosure::graph_slam(PointClouds& point_clouds_container, GNSS& gnss, GroundControlPoints& gcps, ControlPoints& cps) { std::cout << "Compute Pose Graph SLAM" << std::endl; @@ -153,19 +154,20 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS normalize_angle(all_edges[i].relative_pose_tb.om), normalize_angle(all_edges[i].relative_pose_tb.fi), normalize_angle(all_edges[i].relative_pose_tb.ka)); - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[all_edges[i].index_from].px, - poses[all_edges[i].index_from].py, - poses[all_edges[i].index_from].pz, - normalize_angle(poses[all_edges[i].index_from].om), - normalize_angle(poses[all_edges[i].index_from].fi), - normalize_angle(poses[all_edges[i].index_from].ka), - poses[all_edges[i].index_to].px, - poses[all_edges[i].index_to].py, - poses[all_edges[i].index_to].pz, - normalize_angle(poses[all_edges[i].index_to].om), - normalize_angle(poses[all_edges[i].index_to].fi), - normalize_angle(poses[all_edges[i].index_to].ka)); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); } else if (is_cw) { @@ -189,19 +191,20 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS normalize_angle(all_edges[i].relative_pose_tb.om), normalize_angle(all_edges[i].relative_pose_tb.fi), normalize_angle(all_edges[i].relative_pose_tb.ka)); - relative_pose_obs_eq_tait_bryan_cw_case1_jacobian(jacobian, - poses[all_edges[i].index_from].px, - poses[all_edges[i].index_from].py, - poses[all_edges[i].index_from].pz, - normalize_angle(poses[all_edges[i].index_from].om), - normalize_angle(poses[all_edges[i].index_from].fi), - normalize_angle(poses[all_edges[i].index_from].ka), - poses[all_edges[i].index_to].px, - poses[all_edges[i].index_to].py, - poses[all_edges[i].index_to].pz, - normalize_angle(poses[all_edges[i].index_to].om), - normalize_angle(poses[all_edges[i].index_to].fi), - normalize_angle(poses[all_edges[i].index_to].ka)); + relative_pose_obs_eq_tait_bryan_cw_case1_jacobian( + jacobian, + poses[all_edges[i].index_from].px, + poses[all_edges[i].index_from].py, + poses[all_edges[i].index_from].pz, + normalize_angle(poses[all_edges[i].index_from].om), + normalize_angle(poses[all_edges[i].index_from].fi), + normalize_angle(poses[all_edges[i].index_from].ka), + poses[all_edges[i].index_to].px, + poses[all_edges[i].index_to].py, + poses[all_edges[i].index_to].pz, + normalize_angle(poses[all_edges[i].index_to].om), + normalize_angle(poses[all_edges[i].index_to].fi), + normalize_angle(poses[all_edges[i].index_to].ka)); } int ir = tripletListB.size(); @@ -273,39 +276,59 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS // for (const auto &pc : point_clouds_container.point_clouds) for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) { - const auto &pc = point_clouds_container.point_clouds[index_pose]; + const auto& pc = point_clouds_container.point_clouds[index_pose]; for (int i = 0; i < gnss.gnss_poses.size(); i++) { double time_stamp = gnss.gnss_poses[i].timestamp; - auto it = std::lower_bound(pc.local_trajectory.begin(), pc.local_trajectory.end(), - time_stamp, [](const PointCloud::LocalTrajectoryNode &lhs, const double &time) -> bool - { return lhs.timestamps.first < time; }); + auto it = std::lower_bound( + pc.local_trajectory.begin(), + pc.local_trajectory.end(), + time_stamp, + [](const PointCloud::LocalTrajectoryNode& lhs, const double& time) -> bool + { + return lhs.timestamps.first < time; + }); int index = it - pc.local_trajectory.begin(); if (index > 0 && index < pc.local_trajectory.size()) { - if (fabs(time_stamp - pc.local_trajectory[index].timestamps.first) < 10e12) { - Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(m_poses[index_pose]); Eigen::Vector3d p_s = pc.local_trajectory[index].m_pose.translation(); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - // Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, gnss.gnss_poses[i].alt - gnss.offset_alt); - Eigen::Vector3d p_t(gnss.gnss_poses[i].x - point_clouds_container.offset.x(), gnss.gnss_poses[i].y - point_clouds_container.offset.y(), gnss.gnss_poses[i].alt - point_clouds_container.offset.z()); - - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + // Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, + // gnss.gnss_poses[i].alt - gnss.offset_alt); + Eigen::Vector3d p_t( + gnss.gnss_poses[i].x - point_clouds_container.offset.x(), + gnss.gnss_poses[i].y - point_clouds_container.offset.y(), + gnss.gnss_poses[i].alt - point_clouds_container.offset.z()); + + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); // std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; @@ -337,22 +360,37 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS // GCPs for (int i = 0; i < gcps.gpcs.size(); i++) { - Eigen::Vector3d p_s = point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner].local_trajectory[gcps.gpcs[i].index_to_node_outer].m_pose.translation(); + Eigen::Vector3d p_s = point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner] + .local_trajectory[gcps.gpcs[i].index_to_node_outer] + .m_pose.translation(); Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; Eigen::Vector3d p_t(gcps.gpcs[i].x, gcps.gpcs[i].y, gcps.gpcs[i].z + gcps.gpcs[i].lidar_height_above_ground); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = gcps.gpcs[i].index_to_node_inner * 6; @@ -383,24 +421,35 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS { if (!cps.cps[i].is_z_0) { - Eigen::Vector3d p_s(cps.cps[i].x_source_local, - cps.cps[i].y_source_local, cps.cps[i].z_source_local); + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(cps.cps[i].x_target_global, - cps.cps[i].y_target_global, cps.cps[i].z_target_global); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, cps.cps[i].z_target_global); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = cps.cps[i].index_to_pose * 6; @@ -427,25 +476,35 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS } else { - - Eigen::Vector3d p_s(cps.cps[i].x_source_local, - cps.cps[i].y_source_local, cps.cps[i].z_source_local); + Eigen::Vector3d p_s(cps.cps[i].x_source_local, cps.cps[i].y_source_local, cps.cps[i].z_source_local); Eigen::Matrix jacobian; TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[cps.cps[i].index_to_pose].m_pose); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(cps.cps[i].x_target_global, - cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t(cps.cps[i].x_target_global, cps.cps[i].y_target_global, 0.0 /*cps.cps[i].z_target_global*/); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); int ir = tripletListB.size(); int ic = cps.cps[i].index_to_pose * 6; @@ -473,13 +532,13 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS } // fuse_inclination_from_imu - + double error_imu = 0; double error_imu_sum = 0; for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) { - const auto &pc = point_clouds_container.point_clouds[index_pose]; + const auto& pc = point_clouds_container.point_clouds[index_pose]; if (!pc.fuse_inclination_from_IMU) { continue; @@ -509,16 +568,42 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS TaitBryanPose current_pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); Eigen::Matrix delta; - point_to_plane_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 1, 0, 0, // add 0,1,0 - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, // add 0,1,0 + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); Eigen::Matrix delta_jacobian; - point_to_plane_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 1, 0, 0, - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 1, + 0, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); int ir = tripletListB.size(); int ic = index_pose * 6; @@ -530,15 +615,41 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS tripletListB.emplace_back(ir, 0, delta(0, 0)); /////////////////////////////// - point_to_plane_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 0, 1, 0, - xtg, ytg, ztg, vzx, vzy, vzz); - - point_to_plane_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - 0, 1, 0, - xtg, ytg, ztg, vzx, vzy, vzz); + point_to_plane_tait_bryan_wc( + delta, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); + + point_to_plane_tait_bryan_wc_jacobian( + delta_jacobian, + current_pose.px, + current_pose.py, + current_pose.pz, + current_pose.om, + current_pose.fi, + current_pose.ka, + 0, + 1, + 0, + xtg, + ytg, + ztg, + vzx, + vzy, + vzz); ir = tripletListB.size(); ic = index_pose * 6; @@ -576,17 +687,15 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS Eigen::Matrix delta; point_to_line_tait_bryan_wc(delta, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - point_source_local.x(), point_source_local.y(), point_source_local.z(), - point_on_target_line.x(), point_on_target_line.y(), point_on_target_line.z(), - vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); + current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, + current_pose.ka, point_source_local.x(), point_source_local.y(), point_source_local.z(), point_on_target_line.x(), + point_on_target_line.y(), point_on_target_line.z(), vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); Eigen::Matrix delta_jacobian; point_to_line_tait_bryan_wc_jacobian(delta_jacobian, - current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, current_pose.ka, - point_source_local.x(), point_source_local.y(), point_source_local.z(), - point_on_target_line.x(), point_on_target_line.y(), point_on_target_line.z(), - vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); + current_pose.px, current_pose.py, current_pose.pz, current_pose.om, current_pose.fi, + current_pose.ka, point_source_local.x(), point_source_local.y(), point_source_local.z(), point_on_target_line.x(), + point_on_target_line.y(), point_on_target_line.z(), vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z()); int ir = tripletListB.size(); int ic = index_pose * 6; @@ -674,8 +783,7 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS } } - Eigen::SparseMatrix - matA(tripletListB.size(), point_clouds_container.point_clouds.size() * 6); + Eigen::SparseMatrix matA(tripletListB.size(), point_clouds_container.point_clouds.size() * 6); Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); Eigen::SparseMatrix matB(tripletListB.size(), 1); @@ -788,7 +896,8 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { point_clouds_container.point_clouds[i].m_pose = m_poses[i]; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -807,7 +916,9 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS { std::cout << "--" << std::endl; - Eigen::Vector3d p_s = point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner].local_trajectory[gcps.gpcs[i].index_to_node_outer].m_pose.translation(); + Eigen::Vector3d p_s = point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner] + .local_trajectory[gcps.gpcs[i].index_to_node_outer] + .m_pose.translation(); TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner].m_pose); @@ -815,28 +926,42 @@ void PoseGraphLoopClosure::graph_slam(PointClouds &point_clouds_container, GNSS double delta_y; double delta_z; Eigen::Vector3d p_t(gcps.gpcs[i].x, gcps.gpcs[i].y, gcps.gpcs[i].z + gcps.gpcs[i].lidar_height_above_ground); - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); std::cout << "GCP[" << i << "] name: '" << gcps.gpcs[i].name << "'" << std::endl; std::cout << "lidar_height_above_ground: " << gcps.gpcs[i].lidar_height_above_ground << " " << std::endl; std::cout << "delta_x: " << delta_x << " [m], delta_y: " << delta_y << " [m], delta_z: " << delta_z << " [m]" << std::endl; - std::cout << "GCP-> x:" << gcps.gpcs[i].x << " [m], y:" << gcps.gpcs[i].y << " [m], z:" << gcps.gpcs[i].z << " [m]" << std::endl; + std::cout << "GCP-> x:" << gcps.gpcs[i].x << " [m], y:" << gcps.gpcs[i].y + << " [m], z:" << gcps.gpcs[i].z << " [m]" << std::endl; Eigen::Vector3d pp = point_clouds_container.point_clouds[gcps.gpcs[i].index_to_node_inner].m_pose * p_s; - std::cout << "TrajectoryNode (center of LiDAR)-> x:" << pp.x() << " [m], y:" << pp.y() << " [m], z:" << pp.z() << " [m]" << std::endl; + std::cout << "TrajectoryNode (center of LiDAR)-> x:" << pp.x() << " [m], y:" << pp.y() << " [m], z:" << pp.z() << " [m]" + << std::endl; std::cout << "--" << std::endl; } std::cout << "Compute Pose Graph SLAM FINISHED" << std::endl; } -void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_container, GNSS &gnss) +void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds& point_clouds_container, GNSS& gnss) { for (int iter = 0; iter < 30; iter++) { - std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; @@ -845,13 +970,18 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_cont for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) { - const auto &pc = point_clouds_container.point_clouds[index_pose]; + const auto& pc = point_clouds_container.point_clouds[index_pose]; double time_stamp = pc.timestamps[0]; - auto it = std::lower_bound(gnss.gnss_poses.begin(), gnss.gnss_poses.end(), - time_stamp, [](const GNSS::GlobalPose &lhs, const double &time) -> bool - { return lhs.timestamp < time; }); + auto it = std::lower_bound( + gnss.gnss_poses.begin(), + gnss.gnss_poses.end(), + time_stamp, + [](const GNSS::GlobalPose& lhs, const double& time) -> bool + { + return lhs.timestamp < time; + }); int index = it - gnss.gnss_poses.begin() - 1; @@ -863,17 +993,33 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_cont TaitBryanPose pose_s; pose_s = pose_tait_bryan_from_affine_matrix(m_pose); Eigen::Vector3d p_s = pc.m_pose.translation(); - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); double delta_x; double delta_y; double delta_z; - Eigen::Vector3d p_t(gnss.gnss_poses[index].x - point_clouds_container.offset.x(), gnss.gnss_poses[index].y - point_clouds_container.offset.y(), gnss.gnss_poses[index].alt - point_clouds_container.offset.z()); - - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + Eigen::Vector3d p_t( + gnss.gnss_poses[index].x - point_clouds_container.offset.x(), + gnss.gnss_poses[index].y - point_clouds_container.offset.y(), + gnss.gnss_poses[index].alt - point_clouds_container.offset.z()); + + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); // std::cout << " delta_x " << delta_x << " delta_y " << delta_y << " delta_z " << delta_z << std::endl; @@ -901,8 +1047,7 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_cont } } - Eigen::SparseMatrix - matA(tripletListB.size(), 6); + Eigen::SparseMatrix matA(tripletListB.size(), 6); Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); Eigen::SparseMatrix matB(tripletListB.size(), 1); @@ -974,7 +1119,8 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_cont for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { point_clouds_container.point_clouds[i].m_pose = m_pose * point_clouds_container.point_clouds[i].m_pose; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; @@ -986,7 +1132,13 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds &point_clouds_cont } } -void PoseGraphLoopClosure::run_icp(PointClouds &point_clouds_container, int index_active_edge, float search_radius, int number_of_iterations, int num_edge_extended_before, int num_edge_extended_after) +void PoseGraphLoopClosure::run_icp( + PointClouds& point_clouds_container, + int index_active_edge, + float search_radius, + int number_of_iterations, + int num_edge_extended_before, + int num_edge_extended_after) { PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); @@ -995,7 +1147,7 @@ void PoseGraphLoopClosure::run_icp(PointClouds &point_clouds_container, int inde //////////////////////////////// std::vector source; - auto &e = edges[index_active_edge]; + auto& e = edges[index_active_edge]; for (int i = -num_edge_extended_before; i <= num_edge_extended_after; i++) { int index_src = e.index_to + i; @@ -1029,14 +1181,14 @@ void PoseGraphLoopClosure::run_icp(PointClouds &point_clouds_container, int inde Eigen::Affine3d m_src_inv = point_clouds_container.point_clouds[e.index_to].m_pose.inverse(); - for (auto &p : source) + for (auto& p : source) { p = m_src_inv * p; } Eigen::Affine3d m_trg_inv = point_clouds_container.point_clouds[e.index_from].m_pose.inverse(); - for (auto &p : target) + for (auto& p : target) { p = m_trg_inv * p; } diff --git a/core/src/pose_graph_slam.cpp b/core/src/pose_graph_slam.cpp index 278ea56e..48b7fb79 100644 --- a/core/src/pose_graph_slam.cpp +++ b/core/src/pose_graph_slam.cpp @@ -1,14 +1,12 @@ -#include -#include -#include -#include +#include -#include -//#include - -#include -#include +#include #include +#include +#include +#include +#include +#include #include @@ -16,22 +14,24 @@ #include #ifdef WITH_PCL +#include #include #include -#include #include -#include +#include + #endif #if WITH_GTSAM #include #include -#include -#include -#include #include #include +#include #include +#include +#include + #endif #if WITH_MANIF @@ -49,10 +49,12 @@ inline double random(double low, double high) return dist(gen); } -void PoseGraphSLAM::add_random_noise(PointClouds& point_clouds_container) { - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { +void PoseGraphSLAM::add_random_noise(PointClouds& point_clouds_container) +{ + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { TaitBryanPose pose; - pose.px = random(-0.000001, 0.000001); + pose.px = random(-0.000001, 0.000001); pose.py = random(-0.000001, 0.000001); pose.pz = random(-0.000001, 0.000001); pose.om = random(-0.000001, 0.000001); @@ -63,8 +65,10 @@ void PoseGraphSLAM::add_random_noise(PointClouds& point_clouds_container) { } } -void add_noise_to_poses(std::vector& poses) { - for (size_t i = 0; i < poses.size(); i++) { +void add_noise_to_poses(std::vector& poses) +{ + for (size_t i = 0; i < poses.size(); i++) + { poses[i].px += random(-0.000001, 0.000001); poses[i].py += random(-0.000001, 0.000001); poses[i].pz += random(-0.000001, 0.000001); @@ -74,8 +78,10 @@ void add_noise_to_poses(std::vector& poses) { } } -void add_noise_to_poses(std::vector& poses) { - for (size_t i = 0; i < poses.size(); i++) { +void add_noise_to_poses(std::vector& poses) +{ + for (size_t i = 0; i < poses.size(); i++) + { TaitBryanPose pose; pose.px = random(-0.000001, 0.000001); pose.py = random(-0.000001, 0.000001); @@ -91,8 +97,10 @@ void add_noise_to_poses(std::vector& poses) { } } -void add_noise_to_poses(std::vector& poses) { - for (size_t i = 0; i < poses.size(); i++) { +void add_noise_to_poses(std::vector& poses) +{ + for (size_t i = 0; i < poses.size(); i++) + { TaitBryanPose pose; pose.px = random(-0.000001, 0.000001); pose.py = random(-0.000001, 0.000001); @@ -114,35 +122,42 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) edges.clear(); int number_of_unknowns = 6; - if (is_quaternion)number_of_unknowns = 7; + if (is_quaternion) + number_of_unknowns = 7; - for (auto& pc : point_clouds_container.point_clouds) { + for (auto& pc : point_clouds_container.point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } - //get edges based on overlap + // get edges based on overlap auto min_overlap = std::numeric_limits::max(); auto max_overlap = std::numeric_limits::lowest(); - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) { + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) + { std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], 0.5); double overlap = double(nns.size()) / double(point_clouds_container.point_clouds[i].points_local.size()); std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; - if (overlap > overlap_threshold) { + if (overlap > overlap_threshold) + { Edge edge; edge.index_from = i; edge.index_to = j; edges.push_back(edge); } - if (overlap < min_overlap) { + if (overlap < min_overlap) + { min_overlap = overlap; } - if (overlap > max_overlap) { + if (overlap > max_overlap) + { max_overlap = overlap; } } @@ -151,41 +166,47 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "min_overlap: " << min_overlap * 100 << "[%]" << std::endl; std::cout << "max_overlap: " << max_overlap * 100 << "[%]" << std::endl; - calculate_edges(point_clouds_container.point_clouds); - //graph slam + // graph slam bool is_ok = true; std::vector m_poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { m_poses.push_back(point_clouds_container.point_clouds[i].m_pose); } - if (is_tait_bryan_angles) { + if (is_tait_bryan_angles) + { std::vector poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { - + for (int iter = 0; iter < iterations; iter++) + { add_noise_to_poses(poses); std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_tait_bryan_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { + if (is_wc) + { relative_pose_obs_eq_tait_bryan_wc_case1( delta, poses[edges[i].index_from].px, @@ -206,7 +227,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) normalize_angle(relative_pose.om), normalize_angle(relative_pose.fi), normalize_angle(relative_pose.ka)); - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -220,7 +242,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) normalize_angle(poses[edges[i].index_to].fi), normalize_angle(poses[edges[i].index_to].ka)); } - else if (is_cw) { + else if (is_cw) + { relative_pose_obs_eq_tait_bryan_cw_case1( delta, poses[edges[i].index_from].px, @@ -241,7 +264,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) normalize_angle(relative_pose.om), normalize_angle(relative_pose.fi), normalize_angle(relative_pose.ka)); - relative_pose_obs_eq_tait_bryan_cw_case1_jacobian(jacobian, + relative_pose_obs_eq_tait_bryan_cw_case1_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -261,7 +285,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) int ic_1 = edges[i].index_from * 6; int ic_2 = edges[i].index_to * 6; - for (size_t row = 0; row < 6; row++) { + for (size_t row = 0; row < 6; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -291,29 +316,30 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "normalize_angle(delta(4, 0)): " << normalize_angle(delta(4, 0)) << std::endl; std::cout << "normalize_angle(delta(5, 0)): " << normalize_angle(delta(5, 0)) << std::endl; - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } //} - tripletListP.emplace_back(ir , ir , get_cauchy_w(delta(0, 0), 10)); + tripletListP.emplace_back(ir, ir, get_cauchy_w(delta(0, 0), 10)); tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta(1, 0), 10)); tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta(2, 0), 10)); tripletListP.emplace_back(ir + 3, ir + 3, get_cauchy_w(delta(3, 0), 10)); tripletListP.emplace_back(ir + 4, ir + 4, get_cauchy_w(delta(4, 0), 10)); tripletListP.emplace_back(ir + 5, ir + 5, get_cauchy_w(delta(5, 0), 10)); } - if (is_fix_first_node) { + if (is_fix_first_node) + { int ir = tripletListB.size(); - tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); tripletListA.emplace_back(ir + 2, 2, 1); tripletListA.emplace_back(ir + 3, 3, 1); tripletListA.emplace_back(ir + 4, 4, 1); tripletListA.emplace_back(ir + 5, 5, 1); - tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir, ir, 1); tripletListP.emplace_back(ir + 1, ir + 1, 1); tripletListP.emplace_back(ir + 2, ir + 2, 1); tripletListP.emplace_back(ir + 3, ir + 3, 1); @@ -336,8 +362,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(point_clouds_container.point_clouds.size() * 6, point_clouds_container.point_clouds.size() * 6); + Eigen::SparseMatrix AtPA( + point_clouds_container.point_clouds.size() * 6, point_clouds_container.point_clouds.size() * 6); Eigen::SparseMatrix AtPB(point_clouds_container.point_clouds.size() * 6, 1); { @@ -361,8 +387,10 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); } } @@ -371,14 +399,18 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "AtPA=AtPB SOLVED" << std::endl; std::cout << "updates:" << std::endl; - for (size_t i = 0; i < h_x.size(); i += 6) { - std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + for (size_t i = 0; i < h_x.size(); i += 6) + { + std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + << std::endl; } - if (h_x.size() == 6 * point_clouds_container.point_clouds.size()) { + if (h_x.size() == 6 * point_clouds_container.point_clouds.size()) + { int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { TaitBryanPose pose = poses[i]; poses[i].px += h_x[counter++]; poses[i].py += h_x[counter++]; @@ -387,56 +419,68 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) poses[i].fi += h_x[counter++]; poses[i].ka += h_x[counter++]; - if (i == 0 && is_fix_first_node) { + if (i == 0 && is_fix_first_node) + { poses[i] = pose; } } std::cout << "optimizing with tait bryan finished" << std::endl; } - else { + else + { std::cout << "optimizing with tait bryan FAILED" << std::endl; std::cout << "h_x.size(): " << h_x.size() << " should be: " << 6 * point_clouds_container.point_clouds.size() << std::endl; is_ok = false; break; } } - if (is_ok) { - for (size_t i = 0; i < m_poses.size(); i++) { - if (is_wc) { + if (is_ok) + { + for (size_t i = 0; i < m_poses.size(); i++) + { + if (is_wc) + { m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]); } - else if (is_cw) { + else if (is_cw) + { m_poses[i] = affine_matrix_from_pose_tait_bryan(poses[i]).inverse(); } } } } - if (is_rodrigues) { + if (is_rodrigues) + { std::vector poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_rodrigues_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_rodrigues_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { + for (int iter = 0; iter < iterations; iter++) + { add_noise_to_poses(poses); std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_rodrigues_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { - + if (is_wc) + { std::cout << "relative_pose.px " << relative_pose.px << std::endl; std::cout << "relative_pose.py " << relative_pose.py << std::endl; std::cout << "relative_pose.py " << relative_pose.pz << std::endl; @@ -464,7 +508,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) relative_pose.sx, relative_pose.sy, relative_pose.sz); - relative_pose_obs_eq_rodrigues_wc_jacobian(jacobian, + relative_pose_obs_eq_rodrigues_wc_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -478,7 +523,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) poses[edges[i].index_to].sy, poses[edges[i].index_to].sz); } - else if (is_cw) { + else if (is_cw) + { /*relative_pose_obs_eq_rodrigues_cw( delta, poses[edges[i].index_from].px, @@ -520,7 +566,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) int ic_1 = edges[i].index_from * 6; int ic_2 = edges[i].index_to * 6; - for (size_t row = 0; row < 6; row++) { + for (size_t row = 0; row < 6; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -550,13 +597,13 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "delta(4, 0): " << delta(4, 0) << std::endl; std::cout << "delta(5, 0): " << delta(5, 0) << std::endl; - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } //} - tripletListP.emplace_back(ir, ir, get_cauchy_w(delta(0, 0), 10)); + tripletListP.emplace_back(ir, ir, get_cauchy_w(delta(0, 0), 10)); tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta(1, 0), 10)); tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta(2, 0), 10)); tripletListP.emplace_back(ir + 3, ir + 3, get_cauchy_w(delta(3, 0), 10)); @@ -564,16 +611,17 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) tripletListP.emplace_back(ir + 5, ir + 5, get_cauchy_w(delta(5, 0), 10)); } - if (is_fix_first_node) { + if (is_fix_first_node) + { int ir = tripletListB.size(); - tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); tripletListA.emplace_back(ir + 2, 2, 1); tripletListA.emplace_back(ir + 3, 3, 1); tripletListA.emplace_back(ir + 4, 4, 1); tripletListA.emplace_back(ir + 5, 5, 1); - tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir, ir, 1); tripletListP.emplace_back(ir + 1, ir + 1, 1); tripletListP.emplace_back(ir + 2, ir + 2, 1); tripletListP.emplace_back(ir + 3, ir + 3, 1); @@ -596,8 +644,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(point_clouds_container.point_clouds.size() * 6, point_clouds_container.point_clouds.size() * 6); + Eigen::SparseMatrix AtPA( + point_clouds_container.point_clouds.size() * 6, point_clouds_container.point_clouds.size() * 6); Eigen::SparseMatrix AtPB(point_clouds_container.point_clouds.size() * 6, 1); { @@ -622,8 +670,10 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::vector h_x; std::cout << "solution [row,col,value]" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); std::cout << "[" << it.row() << "," << it.col() << "," << it.value() << "]" << std::endl; } @@ -633,14 +683,18 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "AtPA=AtPB SOLVED" << std::endl; std::cout << "updates:" << std::endl; - for (size_t i = 0; i < h_x.size(); i += 6) { - std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << std::endl; + for (size_t i = 0; i < h_x.size(); i += 6) + { + std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] + << std::endl; } - if (h_x.size() == 6 * point_clouds_container.point_clouds.size()) { + if (h_x.size() == 6 * point_clouds_container.point_clouds.size()) + { int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { RodriguesPose pose = poses[i]; poses[i].px += h_x[counter++]; poses[i].py += h_x[counter++]; @@ -649,55 +703,68 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) poses[i].sy += h_x[counter++]; poses[i].sz += h_x[counter++]; - if (i == 0 && is_fix_first_node) { + if (i == 0 && is_fix_first_node) + { poses[i] = pose; } } std::cout << "optimizing with rodrigues finished" << std::endl; } - else { + else + { std::cout << "optimizing with rodrigues FAILED" << std::endl; std::cout << "h_x.size(): " << h_x.size() << " should be: " << 6 * point_clouds_container.point_clouds.size() << std::endl; is_ok = false; break; } } - if (is_ok) { - for (size_t i = 0; i < m_poses.size(); i++) { - if (is_wc) { + if (is_ok) + { + for (size_t i = 0; i < m_poses.size(); i++) + { + if (is_wc) + { m_poses[i] = affine_matrix_from_pose_rodrigues(poses[i]); } - else if (is_cw) { + else if (is_cw) + { m_poses[i] = affine_matrix_from_pose_rodrigues(poses[i]).inverse(); } } } } - if (is_quaternion) { + if (is_quaternion) + { std::vector poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_quaternion_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_quaternion_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { + for (int iter = 0; iter < iterations; iter++) + { add_noise_to_poses(poses); std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_quaternion_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { + if (is_wc) + { relative_pose_obs_eq_quaternion_wc( delta, poses[edges[i].index_from].px, @@ -721,7 +788,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) relative_pose.q1, relative_pose.q2, relative_pose.q3); - relative_pose_obs_eq_quaternion_wc_jacobian(jacobian, + relative_pose_obs_eq_quaternion_wc_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -737,7 +805,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) poses[edges[i].index_to].q2, poses[edges[i].index_to].q3); } - else if (is_cw) { + else if (is_cw) + { relative_pose_obs_eq_quaternion_cw( delta, poses[edges[i].index_from].px, @@ -761,7 +830,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) relative_pose.q1, relative_pose.q2, relative_pose.q3); - relative_pose_obs_eq_quaternion_cw_jacobian(jacobian, + relative_pose_obs_eq_quaternion_cw_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -783,7 +853,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) int ic_1 = edges[i].index_from * 7; int ic_2 = edges[i].index_to * 7; - for (size_t row = 0; row < 7; row++) { + for (size_t row = 0; row < 7; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -809,7 +880,7 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); tripletListB.emplace_back(ir + 6, 0, delta(6, 0)); - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } @@ -824,7 +895,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) tripletListP.emplace_back(ir + 6, ir + 6, get_cauchy_w(delta(6, 0), 10)); } - if (is_fix_first_node) { + if (is_fix_first_node) + { int ir = tripletListB.size(); tripletListA.emplace_back(ir, 0, 1); tripletListA.emplace_back(ir + 1, 1, 1); @@ -834,7 +906,7 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) tripletListA.emplace_back(ir + 5, 5, 1); tripletListA.emplace_back(ir + 6, 6, 1); - tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir, ir, 1); tripletListP.emplace_back(ir + 1, ir + 1, 1); tripletListP.emplace_back(ir + 2, ir + 2, 1); tripletListP.emplace_back(ir + 3, ir + 3, 1); @@ -851,7 +923,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) tripletListB.emplace_back(ir + 6, 0, 0); } - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { int ic = i * 7; int ir = tripletListB.size(); QuaternionPose pose; @@ -881,8 +954,8 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(point_clouds_container.point_clouds.size() * 7, point_clouds_container.point_clouds.size() * 7); + Eigen::SparseMatrix AtPA( + point_clouds_container.point_clouds.size() * 7, point_clouds_container.point_clouds.size() * 7); Eigen::SparseMatrix AtPB(point_clouds_container.point_clouds.size() * 7, 1); { @@ -906,8 +979,10 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::vector h_x; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { h_x.push_back(it.value()); } } @@ -915,15 +990,18 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) std::cout << "h_x.size(): " << h_x.size() << std::endl; std::cout << "AtPA=AtPB SOLVED" << std::endl; - //std::cout << "updates:" << std::endl; - //for (size_t i = 0; i < h_x.size(); i += 7) { - // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + 5] << "," << h_x[i + 6] << std::endl; + // std::cout << "updates:" << std::endl; + // for (size_t i = 0; i < h_x.size(); i += 7) { + // std::cout << h_x[i] << "," << h_x[i + 1] << "," << h_x[i + 2] << "," << h_x[i + 3] << "," << h_x[i + 4] << "," << h_x[i + + // 5] << "," << h_x[i + 6] << std::endl; //} - if (h_x.size() == 7 * point_clouds_container.point_clouds.size()) { + if (h_x.size() == 7 * point_clouds_container.point_clouds.size()) + { int counter = 0; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { QuaternionPose pose = poses[i]; poses[i].px += h_x[counter++]; poses[i].py += h_x[counter++]; @@ -933,32 +1011,40 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) poses[i].q2 += h_x[counter++]; poses[i].q3 += h_x[counter++]; - if (i == 0 && is_fix_first_node) { + if (i == 0 && is_fix_first_node) + { poses[i] = pose; } } std::cout << "optimizing with quaternion finished" << std::endl; } - else { + else + { std::cout << "optimizing with quaternion FAILED" << std::endl; is_ok = false; break; } } - if (is_ok) { - for (size_t i = 0; i < m_poses.size(); i++) { - if (is_wc) { + if (is_ok) + { + for (size_t i = 0; i < m_poses.size(); i++) + { + if (is_wc) + { m_poses[i] = affine_matrix_from_pose_quaternion(poses[i]); } - else if (is_cw) { + else if (is_cw) + { m_poses[i] = affine_matrix_from_pose_quaternion(poses[i]).inverse(); } } } } - if (is_ok) { - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { + if (is_ok) + { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { point_clouds_container.point_clouds[i].m_pose = m_poses[i]; point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; @@ -970,22 +1056,24 @@ bool PoseGraphSLAM::optimize(PointClouds& point_clouds_container) } } - //clean - for (auto& pc : point_clouds_container.point_clouds) { + // clean + for (auto& pc : point_clouds_container.point_clouds) + { pc.clean(); } edges.clear(); //- - //covariance_matrices_after6x6 = compute_covariance_matrices_wc(point_clouds_container); + // covariance_matrices_after6x6 = compute_covariance_matrices_wc(point_clouds_container); - //double mui = get_mean_uncertainty_xyz_impact6x6(covariance_matrices_before6x6, covariance_matrices_after6x6); - //std::cout << "mean uncertainty_xyz impact: " << mui << std::endl; + // double mui = get_mean_uncertainty_xyz_impact6x6(covariance_matrices_before6x6, covariance_matrices_after6x6); + // std::cout << "mean uncertainty_xyz impact: " << mui << std::endl; //-- return true; } -std::vector> PoseGraphSLAM::compute_covariance_matrices_and_rms(std::vector& point_clouds, double& rms) +std::vector> PoseGraphSLAM::compute_covariance_matrices_and_rms( + std::vector& point_clouds, double& rms) { std::vector> covariance_matrices; @@ -993,24 +1081,28 @@ std::vector> PoseGraphSLAM::compute_covariance_matri rms = 0.0; int num_obs = 0; int number_of_unknowns = 6; - if (is_quaternion)number_of_unknowns = 7; + if (is_quaternion) + number_of_unknowns = 7; Eigen::SparseMatrix AtPA(point_clouds.size() * 7, point_clouds.size() * 7); - - for (auto& pc : point_clouds) { + for (auto& pc : point_clouds) + { pc.build_rgd(); pc.cout_rgd(); pc.compute_normal_vectors(0.5); } - //get edges based on overlap - for (int i = 0; i < point_clouds.size(); i++) { - for (int j = i + 1; j < point_clouds.size(); j++) { + // get edges based on overlap + for (int i = 0; i < point_clouds.size(); i++) + { + for (int j = i + 1; j < point_clouds.size(); j++) + { std::vector> nns = point_clouds[i].nns(point_clouds[j], 0.5); float overlap = float(nns.size()) / float(point_clouds[i].points_local.size()); std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; - if (overlap > overlap_threshold) { + if (overlap > overlap_threshold) + { Edge edge; edge.index_from = i; edge.index_to = j; @@ -1021,36 +1113,43 @@ std::vector> PoseGraphSLAM::compute_covariance_matri calculate_edges(point_clouds); - - //graph slam + // graph slam bool is_ok = true; std::vector m_poses; - for (size_t i = 0; i < point_clouds.size(); i++) { + for (size_t i = 0; i < point_clouds.size(); i++) + { m_poses.push_back(point_clouds[i].m_pose); } - if (is_tait_bryan_angles) { + if (is_tait_bryan_angles) + { std::vector poses; - for (size_t i = 0; i < point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_tait_bryan_from_affine_matrix(point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { + for (int iter = 0; iter < iterations; iter++) + { std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_tait_bryan_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { + if (is_wc) + { relative_pose_obs_eq_tait_bryan_wc_case1( delta, poses[edges[i].index_from].px, @@ -1071,7 +1170,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri normalize_angle(relative_pose.om), normalize_angle(relative_pose.fi), normalize_angle(relative_pose.ka)); - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -1085,7 +1185,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri normalize_angle(poses[edges[i].index_to].fi), normalize_angle(poses[edges[i].index_to].ka)); } - else if (is_cw) { + else if (is_cw) + { relative_pose_obs_eq_tait_bryan_cw_case1( delta, poses[edges[i].index_from].px, @@ -1106,7 +1207,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri normalize_angle(relative_pose.om), normalize_angle(relative_pose.fi), normalize_angle(relative_pose.ka)); - relative_pose_obs_eq_tait_bryan_cw_case1_jacobian(jacobian, + relative_pose_obs_eq_tait_bryan_cw_case1_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -1126,7 +1228,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri int ic_1 = edges[i].index_from * 6; int ic_2 = edges[i].index_to * 6; - for (size_t row = 0; row < 6; row++) { + for (size_t row = 0; row < 6; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -1149,7 +1252,6 @@ std::vector> PoseGraphSLAM::compute_covariance_matri tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); - ssr += delta(0, 0) * delta(0, 0); ssr += delta(1, 0) * delta(1, 0); ssr += delta(2, 0) * delta(2, 0); @@ -1157,9 +1259,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri ssr += normalize_angle(delta(4, 0) * delta(4, 0)); ssr += normalize_angle(delta(5, 0) * delta(5, 0)); num_obs += 6; - - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } @@ -1174,26 +1275,26 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } int ir = tripletListB.size(); - tripletListA.emplace_back(ir , 0, 1); - tripletListA.emplace_back(ir + 1 , 1, 1); - tripletListA.emplace_back(ir + 2 , 2, 1); - tripletListA.emplace_back(ir + 3 , 3, 1); - tripletListA.emplace_back(ir + 4 , 4, 1); - tripletListA.emplace_back(ir + 5 , 5, 1); - - tripletListP.emplace_back(ir , ir, 1); - tripletListP.emplace_back(ir + 1 , ir + 1, 1); - tripletListP.emplace_back(ir + 2 , ir + 2, 1); - tripletListP.emplace_back(ir + 3 , ir + 3, 1); - tripletListP.emplace_back(ir + 4 , ir + 4, 1); - tripletListP.emplace_back(ir + 5 , ir + 5, 1); - - tripletListB.emplace_back(ir , 0, 0); - tripletListB.emplace_back(ir + 1 , 0, 0); - tripletListB.emplace_back(ir + 2 , 0, 0); - tripletListB.emplace_back(ir + 3 , 0, 0); - tripletListB.emplace_back(ir + 4 , 0, 0); - tripletListB.emplace_back(ir + 5 , 0, 0); + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); Eigen::SparseMatrix matA(tripletListB.size(), point_clouds.size() * 6); Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); @@ -1203,9 +1304,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - Eigen::SparseMatrix AtPA(point_clouds.size() * 6, point_clouds.size() * 6); - + { Eigen::SparseMatrix AtP = matA.transpose() * matP; AtPA = (AtP)*matA; @@ -1217,28 +1317,35 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } } - if (is_rodrigues) { + if (is_rodrigues) + { std::vector poses; - for (size_t i = 0; i < point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_rodrigues_from_affine_matrix(point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_rodrigues_from_affine_matrix(point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { + for (int iter = 0; iter < iterations; iter++) + { std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_rodrigues_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { + if (is_wc) + { relative_pose_obs_eq_rodrigues_wc( delta, poses[edges[i].index_from].px, @@ -1259,7 +1366,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri relative_pose.sx, relative_pose.sy, relative_pose.sz); - relative_pose_obs_eq_rodrigues_wc_jacobian(jacobian, + relative_pose_obs_eq_rodrigues_wc_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -1273,7 +1381,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri poses[edges[i].index_to].sy, poses[edges[i].index_to].sz); } - else if (is_cw) { + else if (is_cw) + { /*relative_pose_obs_eq_rodrigues_cw( delta, poses[edges[i].index_from].px, @@ -1314,7 +1423,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri int ic_1 = edges[i].index_from * 6; int ic_2 = edges[i].index_to * 6; - for (size_t row = 0; row < 6; row++) { + for (size_t row = 0; row < 6; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -1345,7 +1455,7 @@ std::vector> PoseGraphSLAM::compute_covariance_matri ssr += delta(5, 0) * delta(5, 0); num_obs += 6; - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } @@ -1360,26 +1470,26 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } int ir = tripletListB.size(); - tripletListA.emplace_back(ir , 0, 1); - tripletListA.emplace_back(ir + 1 , 1, 1); - tripletListA.emplace_back(ir + 2 , 2, 1); - tripletListA.emplace_back(ir + 3 , 3, 1); - tripletListA.emplace_back(ir + 4 , 4, 1); - tripletListA.emplace_back(ir + 5 , 5, 1); - - tripletListP.emplace_back(ir , ir, 1); - tripletListP.emplace_back(ir + 1 , ir + 1, 1); - tripletListP.emplace_back(ir + 2 , ir + 2, 1); - tripletListP.emplace_back(ir + 3 , ir + 3, 1); - tripletListP.emplace_back(ir + 4 , ir + 4, 1); - tripletListP.emplace_back(ir + 5 , ir + 5, 1); - - tripletListB.emplace_back(ir , 0, 0); - tripletListB.emplace_back(ir + 1 , 0, 0); - tripletListB.emplace_back(ir + 2 , 0, 0); - tripletListB.emplace_back(ir + 3 , 0, 0); - tripletListB.emplace_back(ir + 4 , 0, 0); - tripletListB.emplace_back(ir + 5 , 0, 0); + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); Eigen::SparseMatrix matA(tripletListB.size(), point_clouds.size() * 6); Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); @@ -1400,28 +1510,35 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } } - if (is_quaternion) { + if (is_quaternion) + { std::vector poses; - for (size_t i = 0; i < point_clouds.size(); i++) { - if (is_wc) { + for (size_t i = 0; i < point_clouds.size(); i++) + { + if (is_wc) + { poses.push_back(pose_quaternion_from_affine_matrix(point_clouds[i].m_pose)); } - else if (is_cw) { + else if (is_cw) + { poses.push_back(pose_quaternion_from_affine_matrix(point_clouds[i].m_pose.inverse())); } } - for (int iter = 0; iter < iterations; iter++) { + for (int iter = 0; iter < iterations; iter++) + { std::vector> tripletListA; std::vector> tripletListP; std::vector> tripletListB; - for (size_t i = 0; i < edges.size(); i++) { + for (size_t i = 0; i < edges.size(); i++) + { Eigen::Matrix delta; Eigen::Matrix jacobian; auto relative_pose = pose_quaternion_from_affine_matrix(edges[i].m_relative_pose); - if (is_wc) { + if (is_wc) + { relative_pose_obs_eq_quaternion_wc( delta, poses[edges[i].index_from].px, @@ -1445,7 +1562,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri relative_pose.q1, relative_pose.q2, relative_pose.q3); - relative_pose_obs_eq_quaternion_wc_jacobian(jacobian, + relative_pose_obs_eq_quaternion_wc_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -1461,7 +1579,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri poses[edges[i].index_to].q2, poses[edges[i].index_to].q3); } - else if (is_cw) { + else if (is_cw) + { relative_pose_obs_eq_quaternion_cw( delta, poses[edges[i].index_from].px, @@ -1485,7 +1604,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri relative_pose.q1, relative_pose.q2, relative_pose.q3); - relative_pose_obs_eq_quaternion_cw_jacobian(jacobian, + relative_pose_obs_eq_quaternion_cw_jacobian( + jacobian, poses[edges[i].index_from].px, poses[edges[i].index_from].py, poses[edges[i].index_from].pz, @@ -1507,7 +1627,8 @@ std::vector> PoseGraphSLAM::compute_covariance_matri int ic_1 = edges[i].index_from * 7; int ic_2 = edges[i].index_to * 7; - for (size_t row = 0; row < 7; row++) { + for (size_t row = 0; row < 7; row++) + { tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); @@ -1542,7 +1663,7 @@ std::vector> PoseGraphSLAM::compute_covariance_matri ssr += delta(6, 0) * delta(6, 0); num_obs += 7; - //for (int r = 0; r < 6; r++) { + // for (int r = 0; r < 6; r++) { // for (int c = 0; c < 6; c++) { // tripletListP.emplace_back(ir + r, ir + c, edges[i].information_matrix.coeffRef(r, c)); // } @@ -1558,31 +1679,32 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } int ir = tripletListB.size(); - tripletListA.emplace_back(ir , 0, 1); - tripletListA.emplace_back(ir + 1 , 1, 1); - tripletListA.emplace_back(ir + 2 , 2, 1); - tripletListA.emplace_back(ir + 3 , 3, 1); - tripletListA.emplace_back(ir + 4 , 4, 1); - tripletListA.emplace_back(ir + 5 , 5, 1); - tripletListA.emplace_back(ir + 6 , 6, 1); - - tripletListP.emplace_back(ir , ir, 1); - tripletListP.emplace_back(ir + 1 , ir + 1, 1); - tripletListP.emplace_back(ir + 2 , ir + 2, 1); - tripletListP.emplace_back(ir + 3 , ir + 3, 1); - tripletListP.emplace_back(ir + 4 , ir + 4, 1); - tripletListP.emplace_back(ir + 5 , ir + 5, 1); - tripletListP.emplace_back(ir + 6 , ir + 6, 1); - - tripletListB.emplace_back(ir , 0, 0); - tripletListB.emplace_back(ir + 1 , 0, 0); - tripletListB.emplace_back(ir + 2 , 0, 0); - tripletListB.emplace_back(ir + 3 , 0, 0); - tripletListB.emplace_back(ir + 4 , 0, 0); - tripletListB.emplace_back(ir + 5 , 0, 0); - tripletListB.emplace_back(ir + 6 , 0, 0); - - for (size_t i = 0; i < point_clouds.size(); i++) { + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + tripletListA.emplace_back(ir + 6, 6, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + tripletListP.emplace_back(ir + 6, ir + 6, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + tripletListB.emplace_back(ir + 6, 0, 0); + + for (size_t i = 0; i < point_clouds.size(); i++) + { int ic = i * 7; int ir = tripletListB.size(); QuaternionPose pose; @@ -1624,7 +1746,7 @@ std::vector> PoseGraphSLAM::compute_covariance_matri } } - //double sq = ssr / ((double)num_obs - point_clouds.size() * number_of_unknowns); + // double sq = ssr / ((double)num_obs - point_clouds.size() * number_of_unknowns); double sq = ssr / ((double)num_obs); rms = ssr / (double)num_obs; @@ -1641,10 +1763,13 @@ std::vector> PoseGraphSLAM::compute_covariance_matri AtAinv = AtAinv * sq; - for (int i = 0; i < point_clouds.size(); i++) { + for (int i = 0; i < point_clouds.size(); i++) + { Eigen::SparseMatrix cm(number_of_unknowns, number_of_unknowns); - for (int r = 0; r < number_of_unknowns; r++) { - for (int c = 0; c < number_of_unknowns; c++) { + for (int r = 0; r < number_of_unknowns; r++) + { + for (int c = 0; c < number_of_unknowns; c++) + { cm.coeffRef(r, c) = AtAinv.coeff(i * number_of_unknowns + r, i * number_of_unknowns + c); } } @@ -1658,189 +1783,200 @@ std::vector> PoseGraphSLAM::compute_covariance_matri return covariance_matrices; } -void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ - if(pair_wise_matching_type == PairWiseMatchingType::general) +void PoseGraphSLAM::calculate_edges(std::vector& point_clouds) +{ + if (pair_wise_matching_type == PairWiseMatchingType::general) { - for (size_t i = 0; i < edges.size(); i++) { - std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," << edges[i].index_to << "]" << std::endl; + for (size_t i = 0; i < edges.size(); i++) + { + std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," + << edges[i].index_to << "]" << std::endl; - PointClouds pcs; - pcs.point_clouds.push_back(point_clouds[edges[i].index_from]); - pcs.point_clouds.push_back(point_clouds[edges[i].index_to]); + PointClouds pcs; + pcs.point_clouds.push_back(point_clouds[edges[i].index_from]); + pcs.point_clouds.push_back(point_clouds[edges[i].index_to]); - Eigen::Affine3d m_inv = pcs.point_clouds[0].m_pose.inverse(); + Eigen::Affine3d m_inv = pcs.point_clouds[0].m_pose.inverse(); - auto m_initial_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + auto m_initial_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - pcs.point_clouds[0].m_pose = m_inv * pcs.point_clouds[0].m_pose; - pcs.point_clouds[1].m_pose = m_inv * pcs.point_clouds[1].m_pose; + pcs.point_clouds[0].m_pose = m_inv * pcs.point_clouds[0].m_pose; + pcs.point_clouds[1].m_pose = m_inv * pcs.point_clouds[1].m_pose; - if (is_ndt) { - NDT ndt; + if (is_ndt) + { + NDT ndt; - ndt.is_fix_first_node = true; - ndt.is_gauss_newton = is_gauss_newton; - ndt.is_levenberg_marguardt = is_levenberg_marguardt; - ndt.is_wc = is_wc; - ndt.is_cw = is_cw; - ndt.is_tait_bryan_angles = is_tait_bryan_angles; - ndt.is_quaternion = is_quaternion; - ndt.is_rodrigues = is_rodrigues; + ndt.is_fix_first_node = true; + ndt.is_gauss_newton = is_gauss_newton; + ndt.is_levenberg_marguardt = is_levenberg_marguardt; + ndt.is_wc = is_wc; + ndt.is_cw = is_cw; + ndt.is_tait_bryan_angles = is_tait_bryan_angles; + ndt.is_quaternion = is_quaternion; + ndt.is_rodrigues = is_rodrigues; - ndt.bucket_size[0] = ndt_bucket_size[0]; - ndt.bucket_size[1] = ndt_bucket_size[1]; - ndt.bucket_size[2] = ndt_bucket_size[2]; + ndt.bucket_size[0] = ndt_bucket_size[0]; + ndt.bucket_size[1] = ndt_bucket_size[1]; + ndt.bucket_size[2] = ndt_bucket_size[2]; - ndt.number_of_threads = number_of_threads; - ndt.number_of_iterations = number_of_iterations_pair_wise_matching; + ndt.number_of_threads = number_of_threads; + ndt.number_of_iterations = number_of_iterations_pair_wise_matching; - ndt.optimize(pcs.point_clouds, true, false); - - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimization_point_to_point_source_to_target) { - ICP icp; - - icp.search_radius = search_radius; - icp.number_of_threads = number_of_threads; - icp.number_of_iterations = number_of_iterations_pair_wise_matching; - icp.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - - icp.is_ballanced_horizontal_vs_vertical = true; - icp.is_fix_first_node = is_fix_first_node; - icp.is_gauss_newton = is_gauss_newton; - icp.is_levenberg_marguardt = is_levenberg_marguardt; - icp.is_cw = is_cw; - icp.is_wc = is_wc; - icp.is_tait_bryan_angles = is_tait_bryan_angles; - icp.is_quaternion = is_quaternion; - icp.is_rodrigues = is_rodrigues; - - icp.optimization_point_to_point_source_to_target(pcs); - - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimize_point_to_projection_onto_plane_source_to_target) { - RegistrationPlaneFeature registration_plane_feature; + ndt.optimize(pcs.point_clouds, true, false); - registration_plane_feature.search_radius = search_radius; - registration_plane_feature.number_of_threads = number_of_threads; - registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; - registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimization_point_to_point_source_to_target) + { + ICP icp; - registration_plane_feature.is_gauss_newton = is_gauss_newton; - registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + icp.search_radius = search_radius; + icp.number_of_threads = number_of_threads; + icp.number_of_iterations = number_of_iterations_pair_wise_matching; + icp.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - registration_plane_feature.is_wc = is_wc; - registration_plane_feature.is_cw = is_cw; + icp.is_ballanced_horizontal_vs_vertical = true; + icp.is_fix_first_node = is_fix_first_node; + icp.is_gauss_newton = is_gauss_newton; + icp.is_levenberg_marguardt = is_levenberg_marguardt; + icp.is_cw = is_cw; + icp.is_wc = is_wc; + icp.is_tait_bryan_angles = is_tait_bryan_angles; + icp.is_quaternion = is_quaternion; + icp.is_rodrigues = is_rodrigues; - registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; - registration_plane_feature.is_quaternion = is_quaternion; - registration_plane_feature.is_rodrigues = is_rodrigues; + icp.optimization_point_to_point_source_to_target(pcs); - registration_plane_feature.is_fix_first_node = is_fix_first_node; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimize_point_to_projection_onto_plane_source_to_target) + { + RegistrationPlaneFeature registration_plane_feature; - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(pcs); + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; + registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimize_point_to_projection_onto_plane_source_to_target) { - RegistrationPlaneFeature registration_plane_feature; + registration_plane_feature.is_gauss_newton = is_gauss_newton; + registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; - registration_plane_feature.search_radius = search_radius; - registration_plane_feature.number_of_threads = number_of_threads; - registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; - registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + registration_plane_feature.is_wc = is_wc; + registration_plane_feature.is_cw = is_cw; - registration_plane_feature.is_gauss_newton = is_gauss_newton; - registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; + registration_plane_feature.is_quaternion = is_quaternion; + registration_plane_feature.is_rodrigues = is_rodrigues; - registration_plane_feature.is_wc = is_wc; - registration_plane_feature.is_cw = is_cw; + registration_plane_feature.is_fix_first_node = is_fix_first_node; - registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; - registration_plane_feature.is_quaternion = is_quaternion; - registration_plane_feature.is_rodrigues = is_rodrigues; + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(pcs); - registration_plane_feature.is_fix_first_node = is_fix_first_node; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimize_point_to_projection_onto_plane_source_to_target) + { + RegistrationPlaneFeature registration_plane_feature; - registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(pcs); + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; + registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimize_point_to_plane_source_to_target) { - RegistrationPlaneFeature registration_plane_feature; + registration_plane_feature.is_gauss_newton = is_gauss_newton; + registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; - registration_plane_feature.search_radius = search_radius; - registration_plane_feature.number_of_threads = number_of_threads; - registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; - registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + registration_plane_feature.is_wc = is_wc; + registration_plane_feature.is_cw = is_cw; - registration_plane_feature.is_gauss_newton = is_gauss_newton; - registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; + registration_plane_feature.is_quaternion = is_quaternion; + registration_plane_feature.is_rodrigues = is_rodrigues; - registration_plane_feature.is_wc = is_wc; - registration_plane_feature.is_cw = is_cw; + registration_plane_feature.is_fix_first_node = is_fix_first_node; - registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; - registration_plane_feature.is_quaternion = is_quaternion; - registration_plane_feature.is_rodrigues = is_rodrigues; + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(pcs); - registration_plane_feature.is_fix_first_node = is_fix_first_node; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimize_point_to_plane_source_to_target) + { + RegistrationPlaneFeature registration_plane_feature; - registration_plane_feature.optimize_point_to_plane_source_to_target(pcs); + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; + registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimize_distance_point_to_plane_source_to_target) { - RegistrationPlaneFeature registration_plane_feature; + registration_plane_feature.is_gauss_newton = is_gauss_newton; + registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; - registration_plane_feature.search_radius = search_radius; - registration_plane_feature.number_of_threads = number_of_threads; - registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; - registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + registration_plane_feature.is_wc = is_wc; + registration_plane_feature.is_cw = is_cw; - registration_plane_feature.is_gauss_newton = is_gauss_newton; - registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; + registration_plane_feature.is_quaternion = is_quaternion; + registration_plane_feature.is_rodrigues = is_rodrigues; - registration_plane_feature.is_wc = is_wc; - registration_plane_feature.is_cw = is_cw; + registration_plane_feature.is_fix_first_node = is_fix_first_node; - registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; - registration_plane_feature.is_quaternion = is_quaternion; - registration_plane_feature.is_rodrigues = is_rodrigues; + registration_plane_feature.optimize_point_to_plane_source_to_target(pcs); - registration_plane_feature.is_fix_first_node = is_fix_first_node; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimize_distance_point_to_plane_source_to_target) + { + RegistrationPlaneFeature registration_plane_feature; - registration_plane_feature.optimize_distance_point_to_plane_source_to_target(pcs); + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; + registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - } - else if (is_optimize_plane_to_plane_source_to_target) { - RegistrationPlaneFeature registration_plane_feature; + registration_plane_feature.is_gauss_newton = is_gauss_newton; + registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; - registration_plane_feature.search_radius = search_radius; - registration_plane_feature.number_of_threads = number_of_threads; - registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; - registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + registration_plane_feature.is_wc = is_wc; + registration_plane_feature.is_cw = is_cw; - registration_plane_feature.is_gauss_newton = is_gauss_newton; - registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; + registration_plane_feature.is_quaternion = is_quaternion; + registration_plane_feature.is_rodrigues = is_rodrigues; - registration_plane_feature.is_wc = is_wc; - registration_plane_feature.is_cw = is_cw; + registration_plane_feature.is_fix_first_node = is_fix_first_node; - registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; - registration_plane_feature.is_quaternion = is_quaternion; - registration_plane_feature.is_rodrigues = is_rodrigues; + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(pcs); - registration_plane_feature.is_fix_first_node = is_fix_first_node; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + } + else if (is_optimize_plane_to_plane_source_to_target) + { + RegistrationPlaneFeature registration_plane_feature; - registration_plane_feature.optimize_plane_to_plane_source_to_target(pcs); + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations_pair_wise_matching; + registration_plane_feature.is_adaptive_robust_kernel = is_adaptive_robust_kernel; + + registration_plane_feature.is_gauss_newton = is_gauss_newton; + registration_plane_feature.is_levenberg_marguardt = is_levenberg_marguardt; + + registration_plane_feature.is_wc = is_wc; + registration_plane_feature.is_cw = is_cw; + + registration_plane_feature.is_tait_bryan_angles = is_tait_bryan_angles; + registration_plane_feature.is_quaternion = is_quaternion; + registration_plane_feature.is_rodrigues = is_rodrigues; + + registration_plane_feature.is_fix_first_node = is_fix_first_node; + + registration_plane_feature.optimize_plane_to_plane_source_to_target(pcs); - edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; + edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; } - else if (is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian) { + else if (is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian) + { ICP icp; icp.search_radius = search_radius; @@ -1862,7 +1998,8 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; } - else if (is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian) { + else if (is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian) + { ICP icp; icp.search_radius = search_radius; @@ -1884,7 +2021,8 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; } - else if (is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian) { + else if (is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian) + { RegistrationPlaneFeature registration_plane_feature; registration_plane_feature.search_radius = search_radius; @@ -1908,7 +2046,8 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; } - else if (is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian) { + else if (is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian) + { RegistrationPlaneFeature registration_plane_feature; registration_plane_feature.search_radius = search_radius; @@ -1931,7 +2070,9 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian(pcs); edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; - }else if (is_ndt_lie_algebra_left_jacobian) { + } + else if (is_ndt_lie_algebra_left_jacobian) + { NDT ndt; ndt.is_fix_first_node = true; @@ -1956,7 +2097,8 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ edges[i].m_relative_pose = pcs.point_clouds[0].m_pose.inverse() * pcs.point_clouds[1].m_pose; } - else if (is_ndt_lie_algebra_right_jacobian) { + else if (is_ndt_lie_algebra_right_jacobian) + { NDT ndt; ndt.is_fix_first_node = true; @@ -1986,150 +2128,163 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ #ifdef WITH_PCL - else if(pair_wise_matching_type == PairWiseMatchingType::pcl_ndt){ - std::cout << "pair_wise_matching_type == PairWiseMatchingType::pcl_ndt" << std::endl; - for (size_t i = 0; i < edges.size(); i++) { - std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," << edges[i].index_to << "]" << std::endl; - - pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud); - for(size_t j = 0; j < point_clouds[edges[i].index_to].points_local.size(); j++){ - Eigen::Vector3d p(point_clouds[edges[i].index_to].points_local[j].x(), - point_clouds[edges[i].index_to].points_local[j].y(), - point_clouds[edges[i].index_to].points_local[j].z()); - - Eigen::Vector3d pt = point_clouds[edges[i].index_to].m_pose * p; - pcl::PointXYZ p_pcl(pt.x(), pt.y(), pt.z()); - target_cloud->push_back(p_pcl); - } - //Eigen::Affine3d m_inv = point_clouds[edges[i].index_to].m_pose.inverse(); - //auto m_initial_pose = m_inv * point_clouds[edges[i].index_from].m_pose; + else if (pair_wise_matching_type == PairWiseMatchingType::pcl_ndt) + { + std::cout << "pair_wise_matching_type == PairWiseMatchingType::pcl_ndt" << std::endl; + for (size_t i = 0; i < edges.size(); i++) + { + std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," + << edges[i].index_to << "]" << std::endl; + + pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud); + for (size_t j = 0; j < point_clouds[edges[i].index_to].points_local.size(); j++) + { + Eigen::Vector3d p( + point_clouds[edges[i].index_to].points_local[j].x(), + point_clouds[edges[i].index_to].points_local[j].y(), + point_clouds[edges[i].index_to].points_local[j].z()); + + Eigen::Vector3d pt = point_clouds[edges[i].index_to].m_pose * p; + pcl::PointXYZ p_pcl(pt.x(), pt.y(), pt.z()); + target_cloud->push_back(p_pcl); + } + // Eigen::Affine3d m_inv = point_clouds[edges[i].index_to].m_pose.inverse(); + // auto m_initial_pose = m_inv * point_clouds[edges[i].index_from].m_pose; - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); - for(size_t j = 0; j < point_clouds[edges[i].index_from].points_local.size(); j++){ - Eigen::Vector3d p(point_clouds[edges[i].index_from].points_local[j].x(), - point_clouds[edges[i].index_from].points_local[j].y(), - point_clouds[edges[i].index_from].points_local[j].z()); - //Eigen::Vector3d pt = point_clouds[edges[i].index_from].m_pose * p; - pcl::PointXYZ p_pcl(p.x(), p.y(), p.z()); - input_cloud->push_back(p_pcl); - } - //from https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html - // Filtering input scan to roughly 10% of original size to increase speed of registration. - pcl::PointCloud::Ptr filtered_cloud (new pcl::PointCloud); - pcl::ApproximateVoxelGrid approximate_voxel_filter; - approximate_voxel_filter.setLeafSize (0.05, 0.05, 0.05); - approximate_voxel_filter.setInputCloud (input_cloud); - approximate_voxel_filter.filter (*filtered_cloud); - - // Initializing Normal Distributions Transform (NDT). - pcl::NormalDistributionsTransform ndt; - // Setting scale dependent NDT parameters - // Setting minimum transformation difference for termination condition. - ndt.setTransformationEpsilon (0.01); - // Setting maximum step size for More-Thuente line search. - ndt.setStepSize (0.1); - //Setting Resolution of NDT grid structure (VoxelGridCovariance). - ndt.setResolution (ndt_bucket_size[0]); - // Setting max number of registration iterations. - ndt.setMaximumIterations (number_of_iterations_pair_wise_matching); - // Setting point cloud to be aligned. - ndt.setInputSource (filtered_cloud); - // Setting point cloud to be aligned to. - ndt.setInputTarget (target_cloud); - - Eigen::Matrix4f init_guess = point_clouds[edges[i].index_from].m_pose.matrix ().cast (); - - - // Calculating required rigid transform to align the input cloud to the target cloud. - pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); - ndt.align (*output_cloud, init_guess); - std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged () - << " score: " << ndt.getFitnessScore () << std::endl; - // Transforming unfiltered, input cloud using found transform. - // pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ()); - - std::cout << "ndt.getFinalTransformation (): " << std::endl << ndt.getFinalTransformation () << std::endl; - edges[i].m_relative_pose.matrix() = ndt.getFinalTransformation ().cast (); - edges[i].m_relative_pose = edges[i].m_relative_pose.inverse() * point_clouds[edges[i].index_to].m_pose; - - - //ndt.getFinalTransformation ().inverse().matrix().cast () * point_clouds[edges[i].index_to].m_pose; - } + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + for (size_t j = 0; j < point_clouds[edges[i].index_from].points_local.size(); j++) + { + Eigen::Vector3d p( + point_clouds[edges[i].index_from].points_local[j].x(), + point_clouds[edges[i].index_from].points_local[j].y(), + point_clouds[edges[i].index_from].points_local[j].z()); + // Eigen::Vector3d pt = point_clouds[edges[i].index_from].m_pose * p; + pcl::PointXYZ p_pcl(p.x(), p.y(), p.z()); + input_cloud->push_back(p_pcl); + } + // from https://pointclouds.org/documentation/tutorials/normal_distributions_transform.html + // Filtering input scan to roughly 10% of original size to increase speed of registration. + pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud); + pcl::ApproximateVoxelGrid approximate_voxel_filter; + approximate_voxel_filter.setLeafSize(0.05, 0.05, 0.05); + approximate_voxel_filter.setInputCloud(input_cloud); + approximate_voxel_filter.filter(*filtered_cloud); + + // Initializing Normal Distributions Transform (NDT). + pcl::NormalDistributionsTransform ndt; + // Setting scale dependent NDT parameters + // Setting minimum transformation difference for termination condition. + ndt.setTransformationEpsilon(0.01); + // Setting maximum step size for More-Thuente line search. + ndt.setStepSize(0.1); + // Setting Resolution of NDT grid structure (VoxelGridCovariance). + ndt.setResolution(ndt_bucket_size[0]); + // Setting max number of registration iterations. + ndt.setMaximumIterations(number_of_iterations_pair_wise_matching); + // Setting point cloud to be aligned. + ndt.setInputSource(filtered_cloud); + // Setting point cloud to be aligned to. + ndt.setInputTarget(target_cloud); + + Eigen::Matrix4f init_guess = point_clouds[edges[i].index_from].m_pose.matrix().cast(); + + // Calculating required rigid transform to align the input cloud to the target cloud. + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + ndt.align(*output_cloud, init_guess); + std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged() << " score: " << ndt.getFitnessScore() + << std::endl; + // Transforming unfiltered, input cloud using found transform. + // pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ()); + + std::cout << "ndt.getFinalTransformation (): " << std::endl << ndt.getFinalTransformation() << std::endl; + edges[i].m_relative_pose.matrix() = ndt.getFinalTransformation().cast(); + edges[i].m_relative_pose = edges[i].m_relative_pose.inverse() * point_clouds[edges[i].index_to].m_pose; + + // ndt.getFinalTransformation ().inverse().matrix().cast () * point_clouds[edges[i].index_to].m_pose; + } } - else if(pair_wise_matching_type == PairWiseMatchingType::pcl_icp){ - std::cout << "pair_wise_matching_type == PairWiseMatchingType::pcl_icp" << std::endl; - for (size_t i = 0; i < edges.size(); i++) { - std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," << edges[i].index_to << "]" << std::endl; - - Eigen::Affine3d m_source = point_clouds[edges[i].index_from].m_pose; - Eigen::Affine3d m_source_inv = m_source.inverse(); - Eigen::Affine3d m_target = point_clouds[edges[i].index_to].m_pose; - Eigen::Affine3d mt = m_source_inv * m_target; - - pcl::PointCloud::Ptr target_cloud (new pcl::PointCloud); - for(size_t j = 0; j < point_clouds[edges[i].index_to].points_local.size(); j++){ - Eigen::Vector3d p(point_clouds[edges[i].index_to].points_local[j].x(), - point_clouds[edges[i].index_to].points_local[j].y(), - point_clouds[edges[i].index_to].points_local[j].z()); - - Eigen::Vector3d nv(point_clouds[edges[i].index_to].normal_vectors_local[j].x(), - point_clouds[edges[i].index_to].normal_vectors_local[j].y(), - point_clouds[edges[i].index_to].normal_vectors_local[j].z()); - - Eigen::Vector3d pt = mt * p; - Eigen::Vector3d nvt = mt.rotation() * nv; - - pcl::PointXYZRGBNormal p_pcl; - p_pcl.x = pt.x(); - p_pcl.y = pt.y(); - p_pcl.z = pt.z(); - p_pcl.normal_x = nvt.x(); - p_pcl.normal_y = nvt.y(); - p_pcl.normal_z = nvt.z(); - - target_cloud->push_back(p_pcl); - } - - pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); - for(size_t j = 0; j < point_clouds[edges[i].index_from].points_local.size(); j++){ - Eigen::Vector3d p(point_clouds[edges[i].index_from].points_local[j].x(), - point_clouds[edges[i].index_from].points_local[j].y(), - point_clouds[edges[i].index_from].points_local[j].z()); - - pcl::PointXYZRGBNormal p_pcl; - Eigen::Vector3d nv(point_clouds[edges[i].index_from].normal_vectors_local[j].x(), - point_clouds[edges[i].index_from].normal_vectors_local[j].y(), - point_clouds[edges[i].index_from].normal_vectors_local[j].z()); - - p_pcl.x = p.x(); - p_pcl.y = p.y(); - p_pcl.z = p.z(); - - p_pcl.normal_x = nv.x(); - p_pcl.normal_y = nv.y(); - p_pcl.normal_z = nv.z(); + else if (pair_wise_matching_type == PairWiseMatchingType::pcl_icp) + { + std::cout << "pair_wise_matching_type == PairWiseMatchingType::pcl_icp" << std::endl; + for (size_t i = 0; i < edges.size(); i++) + { + std::cout << "PROGRESS calculating edges: " << i + 1 << " of " << edges.size() << "[" << edges[i].index_from << "," + << edges[i].index_to << "]" << std::endl; + + Eigen::Affine3d m_source = point_clouds[edges[i].index_from].m_pose; + Eigen::Affine3d m_source_inv = m_source.inverse(); + Eigen::Affine3d m_target = point_clouds[edges[i].index_to].m_pose; + Eigen::Affine3d mt = m_source_inv * m_target; + + pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud); + for (size_t j = 0; j < point_clouds[edges[i].index_to].points_local.size(); j++) + { + Eigen::Vector3d p( + point_clouds[edges[i].index_to].points_local[j].x(), + point_clouds[edges[i].index_to].points_local[j].y(), + point_clouds[edges[i].index_to].points_local[j].z()); + + Eigen::Vector3d nv( + point_clouds[edges[i].index_to].normal_vectors_local[j].x(), + point_clouds[edges[i].index_to].normal_vectors_local[j].y(), + point_clouds[edges[i].index_to].normal_vectors_local[j].z()); + + Eigen::Vector3d pt = mt * p; + Eigen::Vector3d nvt = mt.rotation() * nv; + + pcl::PointXYZRGBNormal p_pcl; + p_pcl.x = pt.x(); + p_pcl.y = pt.y(); + p_pcl.z = pt.z(); + p_pcl.normal_x = nvt.x(); + p_pcl.normal_y = nvt.y(); + p_pcl.normal_z = nvt.z(); + + target_cloud->push_back(p_pcl); + } - input_cloud->push_back(p_pcl); - } + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + for (size_t j = 0; j < point_clouds[edges[i].index_from].points_local.size(); j++) + { + Eigen::Vector3d p( + point_clouds[edges[i].index_from].points_local[j].x(), + point_clouds[edges[i].index_from].points_local[j].y(), + point_clouds[edges[i].index_from].points_local[j].z()); + + pcl::PointXYZRGBNormal p_pcl; + Eigen::Vector3d nv( + point_clouds[edges[i].index_from].normal_vectors_local[j].x(), + point_clouds[edges[i].index_from].normal_vectors_local[j].y(), + point_clouds[edges[i].index_from].normal_vectors_local[j].z()); + + p_pcl.x = p.x(); + p_pcl.y = p.y(); + p_pcl.z = p.z(); + + p_pcl.normal_x = nv.x(); + p_pcl.normal_y = nv.y(); + p_pcl.normal_z = nv.z(); + + input_cloud->push_back(p_pcl); + } - pcl::IterativeClosestPoint icp; - icp.setInputSource(input_cloud); - icp.setInputTarget(target_cloud); - icp.setMaximumIterations (number_of_iterations_pair_wise_matching) ; - icp.setMaxCorrespondenceDistance(search_radius) ; + pcl::IterativeClosestPoint icp; + icp.setInputSource(input_cloud); + icp.setInputTarget(target_cloud); + icp.setMaximumIterations(number_of_iterations_pair_wise_matching); + icp.setMaxCorrespondenceDistance(search_radius); - pcl::PointCloud Final; - icp.align(Final); + pcl::PointCloud Final; + icp.align(Final); - std::cout << "has converged:" << icp.hasConverged() << " score: " << - icp.getFitnessScore() << std::endl; - std::cout << "icp.getFinalTransformation()" << std::endl; - std::cout << icp.getFinalTransformation() << std::endl; + std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; + std::cout << "icp.getFinalTransformation()" << std::endl; + std::cout << icp.getFinalTransformation() << std::endl; - Eigen::Affine3d m_res; - m_res.matrix() = icp.getFinalTransformation().cast (); + Eigen::Affine3d m_res; + m_res.matrix() = icp.getFinalTransformation().cast(); - //--- + //--- #if 0 std::ofstream outfile; outfile.open("tmp.txt"); @@ -2158,326 +2313,351 @@ void PoseGraphSLAM::calculate_edges(std::vector& point_clouds){ exit(1); #endif - edges[i].m_relative_pose = m_res.inverse() * mt; - } - } + edges[i].m_relative_pose = m_res.inverse() * mt; + } + } #endif - } bool PoseGraphSLAM::optimize_with_GTSAM(PointClouds& point_clouds_container) { #if WITH_GTSAM - int number_of_unknowns = 6; - if (is_quaternion)number_of_unknowns = 7; - - for (auto& pc : point_clouds_container.point_clouds) { - pc.build_rgd(); - pc.cout_rgd(); - pc.compute_normal_vectors(0.5); - } - - //get edges based on overlap - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], 0.5); - - float overlap = float(nns.size()) / float(point_clouds_container.point_clouds[i].points_local.size()); - std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; - if (overlap > overlap_threshold) { - Edge edge; - edge.index_from = i; - edge.index_to = j; - edges.push_back(edge); - } - } - } - - calculate_edges(point_clouds_container.point_clouds); - - //graph slam - bool is_ok = true; - std::vector m_poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - m_poses.push_back(point_clouds_container.point_clouds[i].m_pose); - } - - using namespace std; - using namespace gtsam; - NonlinearFactorGraph graph; - - auto priorModel = noiseModel::Diagonal::Variances( - (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished()); - - graph.add(PriorFactor(0, Pose3(Pose3(m_poses[0].matrix())), priorModel)); - - for(size_t i = 0; i < edges.size(); i++){ - auto odometryNoise = noiseModel::Diagonal::Variances( - (Vector(6) << 1, 1, 1, 1, 1, 1).finished()); - - Eigen::Matrix4d update = edges[i].m_relative_pose.matrix(); - graph.emplace_shared >(edges[i].index_from, edges[i].index_to, Pose3(update), odometryNoise); - } - - Values initial; - for (int i = 0; i < m_poses.size(); i++){ - initial.insert(i, Pose3(m_poses[i].matrix())); - } - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - - for (int i =0; i < m_poses.size(); i++){ - auto v = result.at(i); - m_poses[i] = v.matrix(); - } - - if (is_ok) { - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - point_clouds_container.point_clouds[i].m_pose = m_poses[i]; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; - point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; - point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; - point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(point_clouds_container.point_clouds[i].pose.om); - point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); - point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); - } - } - -//clean - for (auto& pc : point_clouds_container.point_clouds) { - pc.clean(); - } - edges.clear(); - std::cout << "optimize_with_GTSAM DONE" << std::endl; + int number_of_unknowns = 6; + if (is_quaternion) + number_of_unknowns = 7; + + for (auto& pc : point_clouds_container.point_clouds) + { + pc.build_rgd(); + pc.cout_rgd(); + pc.compute_normal_vectors(0.5); + } + + // get edges based on overlap + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) + { + std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], 0.5); + + float overlap = float(nns.size()) / float(point_clouds_container.point_clouds[i].points_local.size()); + std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; + if (overlap > overlap_threshold) + { + Edge edge; + edge.index_from = i; + edge.index_to = j; + edges.push_back(edge); + } + } + } + + calculate_edges(point_clouds_container.point_clouds); + + // graph slam + bool is_ok = true; + std::vector m_poses; + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + m_poses.push_back(point_clouds_container.point_clouds[i].m_pose); + } + + using namespace std; + using namespace gtsam; + NonlinearFactorGraph graph; + + auto priorModel = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished()); + + graph.add(PriorFactor(0, Pose3(Pose3(m_poses[0].matrix())), priorModel)); + + for (size_t i = 0; i < edges.size(); i++) + { + auto odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1, 1, 1, 1, 1, 1).finished()); + + Eigen::Matrix4d update = edges[i].m_relative_pose.matrix(); + graph.emplace_shared>(edges[i].index_from, edges[i].index_to, Pose3(update), odometryNoise); + } + + Values initial; + for (int i = 0; i < m_poses.size(); i++) + { + initial.insert(i, Pose3(m_poses[i].matrix())); + } + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + for (int i = 0; i < m_poses.size(); i++) + { + auto v = result.at(i); + m_poses[i] = v.matrix(); + } + + if (is_ok) + { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + point_clouds_container.point_clouds[i].m_pose = m_poses[i]; + point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; + point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; + point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; + point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(point_clouds_container.point_clouds[i].pose.om); + point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); + point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); + } + } + + // clean + for (auto& pc : point_clouds_container.point_clouds) + { + pc.clean(); + } + edges.clear(); + std::cout << "optimize_with_GTSAM DONE" << std::endl; #endif - return true; + return true; } bool PoseGraphSLAM::optimize_with_manif(PointClouds& point_clouds_container) { #if WITH_MANIF - using manif::SE3d; - using manif::SE3Tangentd; - - int number_of_unknowns = 6; - if (is_quaternion)number_of_unknowns = 7; - - for (auto& pc : point_clouds_container.point_clouds) { - pc.build_rgd(); - pc.cout_rgd(); - pc.compute_normal_vectors(0.5); - } - - //get edges based on overlap - for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) { - for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) { - std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], 0.5); - - float overlap = float(nns.size()) / float(point_clouds_container.point_clouds[i].points_local.size()); - std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; - if (overlap > overlap_threshold) { - Edge edge; - edge.index_from = i; - edge.index_to = j; - edges.push_back(edge); - } - } - } - - calculate_edges(point_clouds_container.point_clouds); - - - //graph slam - bool is_ok = true; - std::vector m_poses; - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - m_poses.push_back(point_clouds_container.point_clouds[i].m_pose); - } - - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - std::vector X; - - for(size_t i = 0 ; i < m_poses.size(); i++){ - Eigen::Vector3d t(m_poses[i](0,3), m_poses[i](1,3), m_poses[i](2,3)); - Eigen::Quaterniond q(m_poses[i].rotation()); - X.push_back(SE3d(t,q)); - } - - for(size_t i = 0 ; i < edges.size(); i++){ - const int first = edges[i].index_from; - const int second = edges[i].index_to; - const Eigen::Affine3d& rel = edges[i].m_relative_pose; - Eigen::Vector3d t(rel(0,3), rel(1,3), rel(2,3)); - Eigen::Quaterniond q(rel.rotation()); - SE3d U = SE3d(t,q); - - SE3Tangentd d; - SE3Tangentd u; - Eigen::Matrix J_d_xi, J_d_xj; - - SE3d Xi, - Xj; - - Xi = X[first]; - Xj = X[second]; - - d = Xj.rminus(Xi, J_d_xj, J_d_xi); - u = U.log(); - - int ir = tripletListB.size(); - int ic_1 = first * 6; - int ic_2 = second * 6; - - for(size_t row = 0 ; row < 6; row ++){ - tripletListA.emplace_back(ir + row, ic_1 , -J_d_xi(row,0)); - tripletListA.emplace_back(ir + row, ic_1 + 1, -J_d_xi(row,1)); - tripletListA.emplace_back(ir + row, ic_1 + 2, -J_d_xi(row,2)); - tripletListA.emplace_back(ir + row, ic_1 + 3, -J_d_xi(row,3)); - tripletListA.emplace_back(ir + row, ic_1 + 4, -J_d_xi(row,4)); - tripletListA.emplace_back(ir + row, ic_1 + 5, -J_d_xi(row,5)); - - tripletListA.emplace_back(ir + row, ic_2 , -J_d_xj(row,0)); - tripletListA.emplace_back(ir + row, ic_2 + 1, -J_d_xj(row,1)); - tripletListA.emplace_back(ir + row, ic_2 + 2, -J_d_xj(row,2)); - tripletListA.emplace_back(ir + row, ic_2 + 3, -J_d_xj(row,3)); - tripletListA.emplace_back(ir + row, ic_2 + 4, -J_d_xj(row,4)); - tripletListA.emplace_back(ir + row, ic_2 + 5, -J_d_xj(row,5)); - } - - SE3Tangentd delta = d - u; - - tripletListB.emplace_back(ir, 0, delta.coeffs()(0)); - tripletListB.emplace_back(ir + 1, 0, delta.coeffs()(1)); - tripletListB.emplace_back(ir + 2, 0, delta.coeffs()(2)); - tripletListB.emplace_back(ir + 3, 0, delta.coeffs()(3)); - tripletListB.emplace_back(ir + 4, 0, delta.coeffs()(4)); - tripletListB.emplace_back(ir + 5, 0, delta.coeffs()(5)); - - tripletListP.emplace_back(ir , ir, 1); - tripletListP.emplace_back(ir + 1, ir + 1, 1); - tripletListP.emplace_back(ir + 2, ir + 2, 1); - tripletListP.emplace_back(ir + 3, ir + 3, 1); - tripletListP.emplace_back(ir + 4, ir + 4, 1); - tripletListP.emplace_back(ir + 5, ir + 5, 1); - } - - int ir = tripletListB.size(); - tripletListA.emplace_back(ir , 0, 1); - tripletListA.emplace_back(ir + 1 , 1, 1); - tripletListA.emplace_back(ir + 2 , 2, 1); - tripletListA.emplace_back(ir + 3 , 3, 1); - tripletListA.emplace_back(ir + 4 , 4, 1); - tripletListA.emplace_back(ir + 5 , 5, 1); - - tripletListP.emplace_back(ir , ir, 1000000); - tripletListP.emplace_back(ir + 1 , ir + 1, 1000000); - tripletListP.emplace_back(ir + 2 , ir + 2, 1000000); - tripletListP.emplace_back(ir + 3 , ir + 3, 1000000); - tripletListP.emplace_back(ir + 4 , ir + 4, 1000000); - tripletListP.emplace_back(ir + 5 , ir + 5, 1000000); - - tripletListB.emplace_back(ir , 0, 0); - tripletListB.emplace_back(ir + 1 , 0, 0); - tripletListB.emplace_back(ir + 2 , 0, 0); - tripletListB.emplace_back(ir + 3 , 0, 0); - tripletListB.emplace_back(ir + 4 , 0, 0); - tripletListB.emplace_back(ir + 5 , 0, 0); - - Eigen::SparseMatrix matA(tripletListB.size(), m_poses.size() * 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(m_poses.size() * 6 , m_poses.size() * 6); - Eigen::SparseMatrix AtPB(m_poses.size() * 6 , 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP) * matA; - AtPB = (AtP) * matB; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - - for (int k=0; k::InnerIterator it(x,k); it; ++it){ - h_x.push_back(it.value()); - } - } - - if(X.size() * 6 == h_x.size()){ - - int counter = 0; - for(size_t i = 0 ; i < X.size(); i++){ - SE3Tangentd dx; - dx.coeffs()(0) = h_x[counter++]; - dx.coeffs()(1) = h_x[counter++]; - dx.coeffs()(2) = h_x[counter++]; - dx.coeffs()(3) = h_x[counter++]; - dx.coeffs()(4) = h_x[counter++]; - dx.coeffs()(5) = h_x[counter++]; - - X[i] = X[i] + dx; - } - - for (int i = 0 ; i < m_poses.size(); i++){ - m_poses[i](0,0) = X[i].rotation()(0,0); - m_poses[i](1,0) = X[i].rotation()(1,0); - m_poses[i](2,0) = X[i].rotation()(2,0); - - m_poses[i](0,1) = X[i].rotation()(0,1); - m_poses[i](1,1) = X[i].rotation()(1,1); - m_poses[i](2,1) = X[i].rotation()(2,1); - - m_poses[i](0,2) = X[i].rotation()(0,2); - m_poses[i](1,2) = X[i].rotation()(1,2); - m_poses[i](2,2) = X[i].rotation()(2,2); - - m_poses[i](0,3) = X[i].translation()(0); - m_poses[i](1,3) = X[i].translation()(1); - m_poses[i](2,3) = X[i].translation()(2); - } - std::cout << "AtPA=AtPB SOLVED" << std::endl; - }else{ - std::cout << "h_x.size(): " << h_x.size() << std::endl; - std::cout << "AtPA=AtPB FAILED" << std::endl; - } - - if (is_ok) { - for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) { - point_clouds_container.point_clouds[i].m_pose = m_poses[i]; - point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); - point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; - point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; - point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; - point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(point_clouds_container.point_clouds[i].pose.om); - point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); - point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); - } - } - - //clean - for (auto& pc : point_clouds_container.point_clouds) { - pc.clean(); - } - edges.clear(); - std::cout << "optimize_with_manif DONE" << std::endl; + using manif::SE3d; + using manif::SE3Tangentd; + + int number_of_unknowns = 6; + if (is_quaternion) + number_of_unknowns = 7; + + for (auto& pc : point_clouds_container.point_clouds) + { + pc.build_rgd(); + pc.cout_rgd(); + pc.compute_normal_vectors(0.5); + } + + // get edges based on overlap + for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + for (int j = i + 1; j < point_clouds_container.point_clouds.size(); j++) + { + std::vector> nns = point_clouds_container.point_clouds[i].nns(point_clouds_container.point_clouds[j], 0.5); + + float overlap = float(nns.size()) / float(point_clouds_container.point_clouds[i].points_local.size()); + std::cout << "overlap: " << overlap << " between " << i << "," << j << std::endl; + if (overlap > overlap_threshold) + { + Edge edge; + edge.index_from = i; + edge.index_to = j; + edges.push_back(edge); + } + } + } + + calculate_edges(point_clouds_container.point_clouds); + + // graph slam + bool is_ok = true; + std::vector m_poses; + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + m_poses.push_back(point_clouds_container.point_clouds[i].m_pose); + } + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + std::vector X; + + for (size_t i = 0; i < m_poses.size(); i++) + { + Eigen::Vector3d t(m_poses[i](0, 3), m_poses[i](1, 3), m_poses[i](2, 3)); + Eigen::Quaterniond q(m_poses[i].rotation()); + X.push_back(SE3d(t, q)); + } + + for (size_t i = 0; i < edges.size(); i++) + { + const int first = edges[i].index_from; + const int second = edges[i].index_to; + const Eigen::Affine3d& rel = edges[i].m_relative_pose; + Eigen::Vector3d t(rel(0, 3), rel(1, 3), rel(2, 3)); + Eigen::Quaterniond q(rel.rotation()); + SE3d U = SE3d(t, q); + + SE3Tangentd d; + SE3Tangentd u; + Eigen::Matrix J_d_xi, J_d_xj; + + SE3d Xi, Xj; + + Xi = X[first]; + Xj = X[second]; + + d = Xj.rminus(Xi, J_d_xj, J_d_xi); + u = U.log(); + + int ir = tripletListB.size(); + int ic_1 = first * 6; + int ic_2 = second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -J_d_xi(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -J_d_xi(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -J_d_xi(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -J_d_xi(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -J_d_xi(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -J_d_xi(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -J_d_xj(row, 0)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -J_d_xj(row, 1)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -J_d_xj(row, 2)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -J_d_xj(row, 3)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -J_d_xj(row, 4)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -J_d_xj(row, 5)); + } + + SE3Tangentd delta = d - u; + + tripletListB.emplace_back(ir, 0, delta.coeffs()(0)); + tripletListB.emplace_back(ir + 1, 0, delta.coeffs()(1)); + tripletListB.emplace_back(ir + 2, 0, delta.coeffs()(2)); + tripletListB.emplace_back(ir + 3, 0, delta.coeffs()(3)); + tripletListB.emplace_back(ir + 4, 0, delta.coeffs()(4)); + tripletListB.emplace_back(ir + 5, 0, delta.coeffs()(5)); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + } + + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + + tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), m_poses.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(m_poses.size() * 6, m_poses.size() * 6); + Eigen::SparseMatrix AtPB(m_poses.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + + std::vector h_x; + + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + h_x.push_back(it.value()); + } + } + + if (X.size() * 6 == h_x.size()) + { + int counter = 0; + for (size_t i = 0; i < X.size(); i++) + { + SE3Tangentd dx; + dx.coeffs()(0) = h_x[counter++]; + dx.coeffs()(1) = h_x[counter++]; + dx.coeffs()(2) = h_x[counter++]; + dx.coeffs()(3) = h_x[counter++]; + dx.coeffs()(4) = h_x[counter++]; + dx.coeffs()(5) = h_x[counter++]; + + X[i] = X[i] + dx; + } + + for (int i = 0; i < m_poses.size(); i++) + { + m_poses[i](0, 0) = X[i].rotation()(0, 0); + m_poses[i](1, 0) = X[i].rotation()(1, 0); + m_poses[i](2, 0) = X[i].rotation()(2, 0); + + m_poses[i](0, 1) = X[i].rotation()(0, 1); + m_poses[i](1, 1) = X[i].rotation()(1, 1); + m_poses[i](2, 1) = X[i].rotation()(2, 1); + + m_poses[i](0, 2) = X[i].rotation()(0, 2); + m_poses[i](1, 2) = X[i].rotation()(1, 2); + m_poses[i](2, 2) = X[i].rotation()(2, 2); + + m_poses[i](0, 3) = X[i].translation()(0); + m_poses[i](1, 3) = X[i].translation()(1); + m_poses[i](2, 3) = X[i].translation()(2); + } + std::cout << "AtPA=AtPB SOLVED" << std::endl; + } + else + { + std::cout << "h_x.size(): " << h_x.size() << std::endl; + std::cout << "AtPA=AtPB FAILED" << std::endl; + } + + if (is_ok) + { + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + point_clouds_container.point_clouds[i].m_pose = m_poses[i]; + point_clouds_container.point_clouds[i].pose = pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; + point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; + point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; + point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(point_clouds_container.point_clouds[i].pose.om); + point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); + point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); + } + } + + // clean + for (auto& pc : point_clouds_container.point_clouds) + { + pc.clean(); + } + edges.clear(); + std::cout << "optimize_with_manif DONE" << std::endl; #endif - return true; + return true; } diff --git a/core/src/registration_plane_feature.cpp b/core/src/registration_plane_feature.cpp index 8f5c5863..37d9ecd6 100644 --- a/core/src/registration_plane_feature.cpp +++ b/core/src/registration_plane_feature.cpp @@ -1,30 +1,32 @@ +#include -#include #include -#include -#include -#include +#include -#include #include +#include + #include -#include -#include #include +#include +#include -std::vector RegistrationPlaneFeature::get_jobs(long long unsigned int size, int num_threads) { - +std::vector RegistrationPlaneFeature::get_jobs(long long unsigned int size, int num_threads) +{ int hc = size / num_threads; - if (hc < 1)hc = 1; + if (hc < 1) + hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < - size; i += hc) { + for (long long unsigned int i = 0; i < size; i += hc) + { long long unsigned int sequence_length = hc; - if (i + hc >= size) { + if (i + hc >= size) + { sequence_length = size - i; } - if (sequence_length == 0)break; + if (sequence_length == 0) + break; Job j; j.index_begin_inclusive = i; @@ -33,5 +35,3 @@ std::vector RegistrationPlaneFeature::get_jobs(lo } return jobs; } - - diff --git a/core/src/session.cpp b/core/src/session.cpp index 474b3c66..51a59574 100644 --- a/core/src/session.cpp +++ b/core/src/session.cpp @@ -1,11 +1,12 @@ -#include -#include -#include +#include + #include "../../shared/include/HDMapping/Version.hpp" +#include +#include namespace fs = std::filesystem; -bool Session::load(const std::string &file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset) +bool Session::load(const std::string& file_name, bool is_decimate, double bucket_x, double bucket_y, double bucket_z, bool calculate_offset) { this->session_file_name = file_name; std::cout << "Loading file: '" << file_name << "'" << std::endl; @@ -31,7 +32,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket std::string directory = fs::path(file_name).parent_path().string(); // Local pathUpdater lambda (keep directories unchanged, update files to be in session directory) - auto getNewPath = [&](const std::string &path) -> std::string + auto getNewPath = [&](const std::string& path) -> std::string { fs::path p(path); if (is_directory(p)) @@ -47,7 +48,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket nlohmann::json data = nlohmann::json::parse(fs); fs.close(); - const auto &project_settings_json = data["Session Settings"]; + const auto& project_settings_json = data["Session Settings"]; point_clouds_container.offset.x() = project_settings_json.value("offset_x", 0.0); point_clouds_container.offset.y() = project_settings_json.value("offset_y", 0.0); point_clouds_container.offset.z() = project_settings_json.value("offset_z", 0.0); @@ -63,7 +64,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket is_ground_truth = project_settings_json.value("ground_truth", false); - for (const auto &edge_json : data["loop_closure_edges"]) + for (const auto& edge_json : data["loop_closure_edges"]) { PoseGraphLoopClosure::Edge edge; edge.index_from = edge_json["index_from"]; @@ -89,7 +90,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket loop_closure_edges.push_back(edge); } - for (const auto &fn_json : data["laz_file_names"]) + for (const auto& fn_json : data["laz_file_names"]) { const std::string fn = getNewPath(fn_json["file_name"]); laz_file_names.push_back(fn); @@ -104,10 +105,8 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket } std::cout << "Loaded: " - << "offset_x: " << point_clouds_container.offset.x() - << ", offset_y: " << point_clouds_container.offset.y() - << ", offset_z: " << point_clouds_container.offset.z() - << " [m]\n"; + << "offset_x: " << point_clouds_container.offset.x() << ", offset_y: " << point_clouds_container.offset.y() + << ", offset_z: " << point_clouds_container.offset.z() << " [m]\n"; std::cout << "Folder_name: '" << folder_name << "'\n"; std::cout << "Out_folder_name: '" << out_folder_name << "'\n"; std::cout << "Initial_poses_file_name: '" << initial_poses_file_name << "'\n"; @@ -118,29 +117,22 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket { std::cout << "Loop closure edges:" << std::endl; - for (const auto &edge : loop_closure_edges) + for (const auto& edge : loop_closure_edges) { std::cout << "<<<<<<<<<<<<<<<<<<<" << std::endl; - std::cout << "index_from: " << edge.index_from - << ", index_to: " << edge.index_to << std::endl; - std::cout << "is_fixed fi: " << edge.is_fixed_fi - << ", ka: " << edge.is_fixed_ka - << ", om: " << edge.is_fixed_om << std::endl; - std::cout << "is_fixed px: " << edge.is_fixed_px - << ", py: " << edge.is_fixed_py - << ", pz: " << edge.is_fixed_pz << std::endl; - std::cout << "relative_pose_tb fi: " << edge.relative_pose_tb.fi - << ", ka: " << edge.relative_pose_tb.ka + std::cout << "index_from: " << edge.index_from << ", index_to: " << edge.index_to << std::endl; + std::cout << "is_fixed fi: " << edge.is_fixed_fi << ", ka: " << edge.is_fixed_ka << ", om: " << edge.is_fixed_om + << std::endl; + std::cout << "is_fixed px: " << edge.is_fixed_px << ", py: " << edge.is_fixed_py << ", pz: " << edge.is_fixed_pz + << std::endl; + std::cout << "relative_pose_tb fi: " << edge.relative_pose_tb.fi << ", ka: " << edge.relative_pose_tb.ka << ", om: " << edge.relative_pose_tb.om << std::endl; - std::cout << "relative_pose_tb px: " << edge.relative_pose_tb.px - << ", py: " << edge.relative_pose_tb.py + std::cout << "relative_pose_tb px: " << edge.relative_pose_tb.px << ", py: " << edge.relative_pose_tb.py << ", pz: " << edge.relative_pose_tb.pz << std::endl; std::cout << "relative_pose_tb_weights fi: " << edge.relative_pose_tb_weights.fi - << ", ka: " << edge.relative_pose_tb_weights.ka - << ", om: " << edge.relative_pose_tb_weights.om << std::endl; + << ", ka: " << edge.relative_pose_tb_weights.ka << ", om: " << edge.relative_pose_tb_weights.om << std::endl; std::cout << "relative_pose_tb_weights px: " << edge.relative_pose_tb_weights.px - << ", py: " << edge.relative_pose_tb_weights.py - << ", pz: " << edge.relative_pose_tb_weights.pz << std::endl; + << ", py: " << edge.relative_pose_tb_weights.py << ", pz: " << edge.relative_pose_tb_weights.pz << std::endl; } } @@ -150,7 +142,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket // std::cout << "'" << fn << "'" << std::endl; // } #if WITH_GUI == 1 - for (const auto &gcp_json : data["ground_control_points"]) + for (const auto& gcp_json : data["ground_control_points"]) { GroundControlPoint gcp; std::string name = gcp_json.value("name", ""); @@ -169,7 +161,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket std::cout << "adding gcp[" << name << "]" << std::endl; }; - for (const auto &cp_json : data["control_points"]) + for (const auto& cp_json : data["control_points"]) { ControlPoint cp; std::string name = cp_json.value("name", ""); @@ -194,7 +186,8 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket std::cout << "-----------------------------" << std::endl; // loading all data - point_clouds_container.load_whu_tls(laz_file_names, is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset, this->load_cache_mode); + point_clouds_container.load_whu_tls( + laz_file_names, is_decimate, bucket_x, bucket_y, bucket_z, calculate_offset, this->load_cache_mode); if (laz_file_names.size() > 0) { @@ -237,7 +230,7 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket render_color[2] = float(rand() % 255) / 255.0; int index = 0; - for (auto &pc : point_clouds_container.point_clouds) + for (auto& pc : point_clouds_container.point_clouds) { pc.render_color[0] = render_color[0]; pc.render_color[1] = render_color[1]; @@ -256,11 +249,12 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket std::cout << std::setprecision(10); for (int i = 0; i < point_clouds_container.point_clouds.size(); i++) - //auto &pc : point_clouds_container.point_clouds) + // auto &pc : point_clouds_container.point_clouds) { for (int j = 1; j < point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { - Eigen::Affine3d m = point_clouds_container.point_clouds[i].local_trajectory[j - 1].m_pose.inverse() * point_clouds_container.point_clouds[i].local_trajectory[j].m_pose; + Eigen::Affine3d m = point_clouds_container.point_clouds[i].local_trajectory[j - 1].m_pose.inverse() * + point_clouds_container.point_clouds[i].local_trajectory[j].m_pose; TaitBryanPose tb_pose = pose_tait_bryan_from_affine_matrix(m); @@ -288,37 +282,37 @@ bool Session::load(const std::string &file_name, bool is_decimate, double bucket TaitBryanPose tb_pose_mm = pose_tait_bryan_from_affine_matrix(mm); - Eigen::Vector3d diff (fabs(tb_pose_mm.om - tb_pose.om), fabs(tb_pose_mm.fi - tb_pose.fi), fabs(tb_pose_mm.ka - tb_pose.ka)); + Eigen::Vector3d diff(fabs(tb_pose_mm.om - tb_pose.om), fabs(tb_pose_mm.fi - tb_pose.fi), fabs(tb_pose_mm.ka - tb_pose.ka)); point_clouds_container.point_clouds[i].local_trajectory[j].imu_diff_angle_om_fi_ka_deg = diff; // TaitBryanPose tb_pose2; - //tb_pose2.om = point_clouds_container.point_clouds[i].local_trajectory[j-1].imu_om_fi_ka.x() + // tb_pose2.om = point_clouds_container.point_clouds[i].local_trajectory[j-1].imu_om_fi_ka.x() - //std::cout << tb_pose.om << " " << tb_pose.fi << " " << tb_pose.ka << " " + // std::cout << tb_pose.om << " " << tb_pose.fi << " " << tb_pose.ka << " " // << point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.x() << " " << - // point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.y() << " " + // point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.y() << " " // << point_clouds_container.point_clouds[i].local_trajectory[j].imu_om_fi_ka.z() << std::endl; } - //struct LocalTrajectoryNode{ + // struct LocalTrajectoryNode{ // std::pair timestamps; // Eigen::Affine3d m_pose; // Eigen::Vector3d imu_om_fi_ka; // Eigen::Vector3d imu_diff_angle_om_fi_ka_deg; - //}; - //pc.local_trajectory + //}; + // pc.local_trajectory } return true; - } - catch (const std::exception &e) + } catch (const std::exception& e) { std::cout << "can't load session: " << e.what() << std::endl; return false; } } -bool Session::save(const std::string &file_name, const std::string &poses_file_name, const std::string &initial_poses_file_name, bool is_subsession) +bool Session::save( + const std::string& file_name, const std::string& poses_file_name, const std::string& initial_poses_file_name, bool is_subsession) { std::cout << "saving file: '" << file_name << "'" << std::endl; @@ -333,7 +327,7 @@ bool Session::save(const std::string &file_name, const std::string &poses_file_n j["offset_to_apply_z"] = point_clouds_container.offset_to_apply.z(); j["folder_name"] = point_clouds_container.folder_name; j["out_folder_name"] = point_clouds_container.out_folder_name; - j["poses_file_name"] = poses_file_name; // point_clouds_container.poses_file_name; + j["poses_file_name"] = poses_file_name; // point_clouds_container.poses_file_name; j["initial_poses_file_name"] = initial_poses_file_name; // point_clouds_container.initial_poses_file_name; j["out_poses_file_name"] = point_clouds_container.out_poses_file_name; j["ground_truth"] = is_ground_truth; @@ -343,83 +337,71 @@ bool Session::save(const std::string &file_name, const std::string &poses_file_n nlohmann::json jloop_closure_edges; if (!is_subsession) { - for (const auto &edge : pose_graph_loop_closure.edges) + for (const auto& edge : pose_graph_loop_closure.edges) { - nlohmann::json jloop_closure_edge{ - {"px", edge.relative_pose_tb.px}, - {"py", edge.relative_pose_tb.py}, - {"pz", edge.relative_pose_tb.pz}, - {"om", edge.relative_pose_tb.om}, - {"fi", edge.relative_pose_tb.fi}, - {"ka", edge.relative_pose_tb.ka}, - {"w_px", edge.relative_pose_tb_weights.px}, - {"w_py", edge.relative_pose_tb_weights.py}, - {"w_pz", edge.relative_pose_tb_weights.pz}, - {"w_om", edge.relative_pose_tb_weights.om}, - {"w_fi", edge.relative_pose_tb_weights.fi}, - {"w_ka", edge.relative_pose_tb_weights.ka}, - {"index_from", edge.index_from}, - {"index_to", edge.index_to}, - {"is_fixed_px", edge.is_fixed_px}, - {"is_fixed_py", edge.is_fixed_py}, - {"is_fixed_pz", edge.is_fixed_pz}, - {"is_fixed_om", edge.is_fixed_om}, - {"is_fixed_fi", edge.is_fixed_fi}, - {"is_fixed_ka", edge.is_fixed_ka}}; + nlohmann::json jloop_closure_edge{ { "px", edge.relative_pose_tb.px }, + { "py", edge.relative_pose_tb.py }, + { "pz", edge.relative_pose_tb.pz }, + { "om", edge.relative_pose_tb.om }, + { "fi", edge.relative_pose_tb.fi }, + { "ka", edge.relative_pose_tb.ka }, + { "w_px", edge.relative_pose_tb_weights.px }, + { "w_py", edge.relative_pose_tb_weights.py }, + { "w_pz", edge.relative_pose_tb_weights.pz }, + { "w_om", edge.relative_pose_tb_weights.om }, + { "w_fi", edge.relative_pose_tb_weights.fi }, + { "w_ka", edge.relative_pose_tb_weights.ka }, + { "index_from", edge.index_from }, + { "index_to", edge.index_to }, + { "is_fixed_px", edge.is_fixed_px }, + { "is_fixed_py", edge.is_fixed_py }, + { "is_fixed_pz", edge.is_fixed_pz }, + { "is_fixed_om", edge.is_fixed_om }, + { "is_fixed_fi", edge.is_fixed_fi }, + { "is_fixed_ka", edge.is_fixed_ka } }; jloop_closure_edges.push_back(jloop_closure_edge); } } jj["loop_closure_edges"] = jloop_closure_edges; nlohmann::json jlaz_file_names; - for (const auto &pc : point_clouds_container.point_clouds) + for (const auto& pc : point_clouds_container.point_clouds) { if (is_subsession) { if (pc.visible) { - nlohmann::json jfn{ - {"file_name", pc.file_name}, - {"fuse_inclination_from_IMU", pc.fuse_inclination_from_IMU}, - {"fixed_x", pc.fixed_x}, - {"fixed_y", pc.fixed_y}, - {"fixed_z", pc.fixed_z}, - {"fixed_om", pc.fixed_om}, - {"fixed_fi", pc.fixed_fi}, - {"fixed_ka", pc.fixed_ka}}; + nlohmann::json jfn{ { "file_name", pc.file_name }, { "fuse_inclination_from_IMU", pc.fuse_inclination_from_IMU }, + { "fixed_x", pc.fixed_x }, { "fixed_y", pc.fixed_y }, + { "fixed_z", pc.fixed_z }, { "fixed_om", pc.fixed_om }, + { "fixed_fi", pc.fixed_fi }, { "fixed_ka", pc.fixed_ka } }; jlaz_file_names.push_back(jfn); } } else { - nlohmann::json jfn{ - {"file_name", pc.file_name}, - {"fuse_inclination_from_IMU", pc.fuse_inclination_from_IMU}, - {"fixed_x", pc.fixed_x}, - {"fixed_y", pc.fixed_y}, - {"fixed_z", pc.fixed_z}, - {"fixed_om", pc.fixed_om}, - {"fixed_fi", pc.fixed_fi}, - {"fixed_ka", pc.fixed_ka}}; + nlohmann::json jfn{ { "file_name", pc.file_name }, { "fuse_inclination_from_IMU", pc.fuse_inclination_from_IMU }, + { "fixed_x", pc.fixed_x }, { "fixed_y", pc.fixed_y }, + { "fixed_z", pc.fixed_z }, { "fixed_om", pc.fixed_om }, + { "fixed_fi", pc.fixed_fi }, { "fixed_ka", pc.fixed_ka } }; jlaz_file_names.push_back(jfn); } } jj["laz_file_names"] = jlaz_file_names; #if WITH_GUI == 1 nlohmann::json jgcps; - for (const auto &gcp : ground_control_points.gpcs) + for (const auto& gcp : ground_control_points.gpcs) { - nlohmann::json jgcp{ - {"name", gcp.name}, - {"x", gcp.x}, - {"y", gcp.y}, - {"z", gcp.z}, - {"sigma_x", gcp.sigma_x}, - {"sigma_y", gcp.sigma_y}, - {"sigma_z", gcp.sigma_z}, - {"lidar_height_above_ground", gcp.lidar_height_above_ground}, - {"index_to_node_inner", gcp.index_to_node_inner}, - {"index_to_node_outer", gcp.index_to_node_outer}}; + nlohmann::json jgcp{ { "name", gcp.name }, + { "x", gcp.x }, + { "y", gcp.y }, + { "z", gcp.z }, + { "sigma_x", gcp.sigma_x }, + { "sigma_y", gcp.sigma_y }, + { "sigma_z", gcp.sigma_z }, + { "lidar_height_above_ground", gcp.lidar_height_above_ground }, + { "index_to_node_inner", gcp.index_to_node_inner }, + { "index_to_node_outer", gcp.index_to_node_outer } }; jgcps.push_back(jgcp); } jj["ground_control_points"] = jgcps; @@ -427,21 +409,20 @@ bool Session::save(const std::string &file_name, const std::string &poses_file_n #if WITH_GUI == 1 nlohmann::json jcps; - for (const auto &cp : control_points.cps) + for (const auto& cp : control_points.cps) { - nlohmann::json jcp{ - {"name", cp.name}, - {"x_source_local", cp.x_source_local}, - {"y_source_local", cp.y_source_local}, - {"z_source_local", cp.z_source_local}, - {"x_target_global", cp.x_target_global}, - {"y_target_global", cp.y_target_global}, - {"z_target_global", cp.z_target_global}, - {"sigma_x", cp.sigma_x}, - {"sigma_y", cp.sigma_y}, - {"sigma_z", cp.sigma_z}, - {"index_to_pose", cp.index_to_pose}, - {"is_z_0", cp.is_z_0}}; + nlohmann::json jcp{ { "name", cp.name }, + { "x_source_local", cp.x_source_local }, + { "y_source_local", cp.y_source_local }, + { "z_source_local", cp.z_source_local }, + { "x_target_global", cp.x_target_global }, + { "y_target_global", cp.y_target_global }, + { "z_target_global", cp.z_target_global }, + { "sigma_x", cp.sigma_x }, + { "sigma_y", cp.sigma_y }, + { "sigma_z", cp.sigma_z }, + { "index_to_pose", cp.index_to_pose }, + { "is_z_0", cp.is_z_0 } }; jcps.push_back(jcp); } jj["control_points"] = jcps; @@ -456,8 +437,7 @@ bool Session::save(const std::string &file_name, const std::string &poses_file_n } void Session::fill_session_from_worker_data( - const std::vector &worker_data, bool save_selected, - bool filter_on_export, bool apply_pose, double threshould_output_filter) + const std::vector& worker_data, bool save_selected, bool filter_on_export, bool apply_pose, double threshould_output_filter) { this->point_clouds_container.point_clouds.clear(); // clear whatever was there for (int i = 0; i < worker_data.size(); i++) @@ -466,13 +446,14 @@ void Session::fill_session_from_worker_data( std::vector intermediate_points; if (!load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points)) { - std::cout << "problem with load_vector_data '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" << std::endl; + std::cout << "problem with load_vector_data '" << worker_data[i].intermediate_points_cache_file_name.string() << "'" + << std::endl; } if (!save_selected || worker_data[i].show) { PointCloud pc; - for (const auto &p : intermediate_points) + for (const auto& p : intermediate_points) { if (!filter_on_export || (p.point.norm() > threshould_output_filter)) { diff --git a/core/src/surface.cpp b/core/src/surface.cpp index e54871a2..f237d69f 100644 --- a/core/src/surface.cpp +++ b/core/src/surface.cpp @@ -1,13 +1,15 @@ +#include + #include -#include #include #include #include + #include <../../3rdparty/observation_equations/codes/common/include/cauchy.h> -#include <../../3rdparty/observation_equations/codes/python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h> #include <../../3rdparty/observation_equations/codes/python-scripts/constraints/relative_pose_tait_bryan_wc_jacobian.h> +#include <../../3rdparty/observation_equations/codes/python-scripts/point-to-point-metrics/point_to_point_source_to_target_tait_bryan_wc_jacobian.h> #include @@ -20,7 +22,7 @@ inline double random(double low, double high) return dist(gen); } -void Surface::generate_initial_surface(const std::vector &point_cloud) +void Surface::generate_initial_surface(const std::vector& point_cloud) { std::cout << "Surface::generate_initial_surface" << std::endl; @@ -41,7 +43,7 @@ void Surface::generate_initial_surface(const std::vector &point double min_z = 1000000.0; double max_z = -1000000.0; - for (const auto &p : point_cloud) + for (const auto& p : point_cloud) { if (p.x() < min_x) { @@ -153,7 +155,7 @@ void Surface::render() } } -void Surface::align_surface_to_ground_points(const std::vector &point_cloud) +void Surface::align_surface_to_ground_points(const std::vector& point_cloud) { std::cout << "Surface::align_surface_to_ground_points" << std::endl; std::vector> tripletListA; @@ -175,19 +177,20 @@ void Surface::align_surface_to_ground_points(const std::vector for (size_t i = 0; i < odo_edges.size(); i++) { Eigen::Matrix relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, - poses_odo[odo_edges[i].first].px, - poses_odo[odo_edges[i].first].py, - poses_odo[odo_edges[i].first].pz, - poses_odo[odo_edges[i].first].om, - poses_odo[odo_edges[i].first].fi, - poses_odo[odo_edges[i].first].ka, - poses_odo[odo_edges[i].second].px, - poses_odo[odo_edges[i].second].py, - poses_odo[odo_edges[i].second].pz, - poses_odo[odo_edges[i].second].om, - poses_odo[odo_edges[i].second].fi, - poses_odo[odo_edges[i].second].ka); + relative_pose_tait_bryan_wc_case1( + relative_pose_measurement_odo, + poses_odo[odo_edges[i].first].px, + poses_odo[odo_edges[i].first].py, + poses_odo[odo_edges[i].first].pz, + poses_odo[odo_edges[i].first].om, + poses_odo[odo_edges[i].first].fi, + poses_odo[odo_edges[i].first].ka, + poses_odo[odo_edges[i].second].px, + poses_odo[odo_edges[i].second].py, + poses_odo[odo_edges[i].second].pz, + poses_odo[odo_edges[i].second].om, + poses_odo[odo_edges[i].second].fi, + poses_odo[odo_edges[i].second].ka); Eigen::Matrix delta; relative_pose_obs_eq_tait_bryan_wc_case1( @@ -212,19 +215,20 @@ void Surface::align_surface_to_ground_points(const std::vector relative_pose_measurement_odo(5, 0)); Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); int ir = tripletListB.size(); @@ -279,10 +283,26 @@ void Surface::align_surface_to_ground_points(const std::vector double delta_x; double delta_y; double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); int ic = i * 6; tripletListA.emplace_back(ir, ic + 0, -jacobian(0, 0)); @@ -293,11 +313,14 @@ void Surface::align_surface_to_ground_points(const std::vector tripletListP.emplace_back(ir + 1, ir + 1, 1); tripletListP.emplace_back(ir + 2, ir + 2, 1); - if (robust_kernel){ - tripletListB.emplace_back(ir, 0, cauchy (delta_x, 1)); - tripletListB.emplace_back(ir + 1, 0, cauchy (delta_y, 1)); - tripletListB.emplace_back(ir + 2, 0, cauchy (delta_z, 1)); - }else{ + if (robust_kernel) + { + tripletListB.emplace_back(ir, 0, cauchy(delta_x, 1)); + tripletListB.emplace_back(ir + 1, 0, cauchy(delta_y, 1)); + tripletListB.emplace_back(ir + 2, 0, cauchy(delta_z, 1)); + } + else + { tripletListB.emplace_back(ir, 0, delta_x); tripletListB.emplace_back(ir + 1, 0, delta_y); tripletListB.emplace_back(ir + 2, 0, delta_z); @@ -380,7 +403,7 @@ void Surface::align_surface_to_ground_points(const std::vector } } -bool Surface::find_nearest_neighbour(Eigen::Vector3d &p_t, Eigen::Affine3d vertex, const std::vector &reference_points) +bool Surface::find_nearest_neighbour(Eigen::Vector3d& p_t, Eigen::Affine3d vertex, const std::vector& reference_points) { double min_dist_xy = 1000000.0; double search_radious = 1.0; @@ -388,7 +411,9 @@ bool Surface::find_nearest_neighbour(Eigen::Vector3d &p_t, Eigen::Affine3d verte for (size_t i = 0; i < reference_points.size(); i++) { - float dist = sqrt((vertex(0, 3) - reference_points[i].x()) * (vertex(0, 3) - reference_points[i].x()) + (vertex(1, 3) - reference_points[i].y()) * (vertex(1, 3) - reference_points[i].y())); + float dist = sqrt( + (vertex(0, 3) - reference_points[i].x()) * (vertex(0, 3) - reference_points[i].x()) + + (vertex(1, 3) - reference_points[i].y()) * (vertex(1, 3) - reference_points[i].y())); if (dist < search_radious) { @@ -405,7 +430,8 @@ bool Surface::find_nearest_neighbour(Eigen::Vector3d &p_t, Eigen::Affine3d verte return found; } -std::vector Surface::get_filtered_indexes(const std::vector &pc, const std::vector &lowest_points_indexes, Eigen::Vector2d bucket_dim_xy) +std::vector Surface::get_filtered_indexes( + const std::vector& pc, const std::vector& lowest_points_indexes, Eigen::Vector2d bucket_dim_xy) { bool multithread = true; std::vector out_indexes; @@ -424,9 +450,13 @@ std::vector Surface::get_filtered_indexes(const std::vector &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -444,7 +474,7 @@ std::vector Surface::get_filtered_indexes(const std::vector &index) + const auto hessian_fun = [&](const std::pair& index) { int index_element_source = index.second; Eigen::Vector3d source = lowest_points[index_element_source].first; @@ -482,8 +512,8 @@ std::vector Surface::get_filtered_indexes(const std::vector= 10) { @@ -517,7 +547,8 @@ std::vector Surface::get_filtered_indexes(const std::vector Surface::get_filtered_indexes(const std::vector Surface::get_points_without_surface(const std::vector &points, double distance_to_ground_threshold_bottom, - double distance_to_ground_threshold_up, Eigen::Vector2d bucket_dim_xy) +std::vector Surface::get_points_without_surface( + const std::vector& points, + double distance_to_ground_threshold_bottom, + double distance_to_ground_threshold_up, + Eigen::Vector2d bucket_dim_xy) { bool multithread = true; @@ -600,9 +634,13 @@ std::vector Surface::get_points_without_surface(const std::vect indexes.emplace_back(index, i); } - std::sort(indexes.begin(), indexes.end(), - [](const std::pair &a, const std::pair &b) - { return a.first < b.first; }); + std::sort( + indexes.begin(), + indexes.end(), + [](const std::pair& a, const std::pair& b) + { + return a.first < b.first; + }); std::unordered_map> buckets; @@ -634,8 +672,10 @@ std::vector Surface::get_points_without_surface(const std::vect points[i].z() > z_ground + distance_to_ground_threshold_up) { to_remove[i] = true; - // std::cout << vertices[index_element_target].translation().x() << " " << vertices[index_element_target].translation().y() << " " - // << vertices[index_element_target].translation().z() << " " << points[i].x() << " " << points[i].y() << " " << points[i].z() << std::endl; + // std::cout << vertices[index_element_target].translation().x() << " " << vertices[index_element_target].translation().y() + // << " " + // << vertices[index_element_target].translation().z() << " " << points[i].x() << " " << points[i].y() << " " << + // points[i].z() << std::endl; } } } diff --git a/core/src/utils.cpp b/core/src/utils.cpp index e6020718..46d28898 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -1,22 +1,22 @@ -#include -#include +#include + +// clang-format off #include #include +// clang-format on -#include "utils.hpp" +#include +#include +#include -#include "imgui_internal.h" +#include "utils.hpp" -#include -#include -#include #include -#include -#include #ifdef _WIN32 -#include #include +#include + #endif /////////////////////////////////////////////////////////////////////////////////// @@ -71,91 +71,90 @@ double scroll_hint_lastT = 0.0; bool show_about = false; // General shortcuts applicable to any app -static const std::vector shortcuts = { - {"Normal keys", "A", ""}, - {"", "Ctrl+A", ""}, - {"", "B", "camera Back"}, - {"", "Ctrl+B", ""}, - {"", "C", "Compass/ruler"}, - {"", "Ctrl+C", ""}, - {"", "D", ""}, - {"", "Ctrl+D", ""}, - {"", "E", ""}, - {"", "Ctrl+E", ""}, - {"", "F", "camera Front"}, - {"", "Ctrl+F", ""}, - {"", "G", ""}, - {"", "Ctrl+G", ""}, - {"", "H", ""}, - {"", "Ctrl+H", ""}, - {"", "I", "camera Isometric"}, - {"", "Ctrl+I", ""}, - {"", "J", ""}, - {"", "Ctrl+J", ""}, - {"", "K", ""}, - {"", "Ctrl+K", ""}, - {"", "L", "camera Left"}, - {"", "Ctrl+L", ""}, - {"", "M", ""}, - {"", "Ctrl+M", ""}, - {"", "N", ""}, - {"", "Ctrl+N", ""}, - {"", "O", "Ortographic view"}, - {"", "Ctrl+O", "Open/load session/data"}, - {"", "P", ""}, - {"", "Ctrl+P", ""}, - {"", "Q", ""}, - {"", "Ctrl+Q", ""}, - {"", "R", "camera Right"}, - {"", "Ctrl+R", ""}, - {"", "Shift+R", "Rotation center"}, - {"", "S", ""}, - {"", "Ctrl+S", ""}, - {"", "Ctrl+Shift+S", ""}, - {"", "T", "camera Top"}, - {"", "Ctrl+T", ""}, - {"", "U", "camera bottom (Under)"}, - {"", "Ctrl+U", ""}, - {"", "V", ""}, - {"", "Ctrl+V", ""}, - {"", "W", ""}, - {"", "Ctrl+W", ""}, - {"", "X", "show aXes"}, - {"", "Ctrl+X", ""}, - {"", "Y", ""}, - {"", "Ctrl+Y", ""}, - {"", "Z", "camera reset"}, - {"", "Ctrl+Z", ""}, - {"", "Shift+Z", "Lock Z"}, - {"", "1-9", "point size"}, - {"Special keys", "Up arrow", ""}, - {"", "Shift + up arrow", "camera translate Up"}, - {"", "Ctrl + up arrow", ""}, - {"", "Down arrow", ""}, - {"", "Shift + down arrow", "camera translate Down"}, - {"", "Ctrl + down arrow", ""}, - {"", "Left arrow", ""}, - {"", "Shift + left arrow", "camera translate Left"}, - {"", "Ctrl + left arrow", ""}, - {"", "Right arrow", ""}, - {"", "Shift + right arrow", "camera translate Right"}, - {"", "Ctrl + right arrow", ""}, - {"", "Pg down", ""}, - {"", "Pg up", ""}, - {"", "- key", ""}, - {"", "+ key", ""}, - {"Mouse related", "Left click + drag", "camera rotate"}, - {"", "Right click + drag", "camera pan"}, - {"", "Scroll", "camera zoom"}, - {"", "Shift + scroll", "camera 5x zoom"}, - {"", "Shift + drag", "Dock window to screen edges"}, - {"", "Ctrl + left click", ""}, - {"", "Ctrl + right click", "change center of rotation"}, - {"", "Ctrl + middle click", "change center of rotation (if no CP GUI active)"}}; +static const std::vector shortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "camera Back" }, + { "", "Ctrl+B", "" }, + { "", "C", "Compass/ruler" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "camera Front" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "camera Isometric" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+J", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "camera Left" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "Ortographic view" }, + { "", "Ctrl+O", "Open/load session/data" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "camera Right" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "Rotation center" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "camera Top" }, + { "", "Ctrl+T", "" }, + { "", "U", "camera bottom (Under)" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "show aXes" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "camera reset" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "Lock Z" }, + { "", "1-9", "point size" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "camera translate Up" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "camera translate Down" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "camera translate Left" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "camera translate Right" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "camera rotate" }, + { "", "Right click + drag", "camera pan" }, + { "", "Scroll", "camera zoom" }, + { "", "Shift + scroll", "camera 5x zoom" }, + { "", "Shift + drag", "Dock window to screen edges" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "change center of rotation" }, + { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; /////////////////////////////////////////////////////////////////////////////////// -std::string truncPath(const std::string &fullPath) +std::string truncPath(const std::string& fullPath) { namespace fs = std::filesystem; fs::path path(fullPath); @@ -166,11 +165,9 @@ std::string truncPath(const std::string &fullPath) return "..\\" + parent + "\\..\\" + filename; } - - void wheel(int button, int dir, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MouseWheel += dir; // or direction * 1.0f depending on your setup if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) @@ -248,12 +245,16 @@ void reshape(int w, int h) } else { - ImGuiIO &io = ImGui::GetIO(); + 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( + -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); @@ -262,7 +263,7 @@ void reshape(int w, int h) void motion(int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); io.MousePos = ImVec2((float)x, (float)y); if (!io.WantCaptureMouse) @@ -276,8 +277,10 @@ void motion(int x, int y) 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); + 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; @@ -338,7 +341,7 @@ ImGuiKey keyToImGuiKey(unsigned char key) void keyboardDown(unsigned char key, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); int mods = glutGetModifiers(); // Update modifier keys using the new API @@ -360,7 +363,7 @@ void keyboardDown(unsigned char key, int x, int y) void keyboardUp(unsigned char key, int x, int y) { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); int mods = glutGetModifiers(); // Update modifier keys using the new API @@ -383,9 +386,11 @@ static bool first_time = true; void ShowMainDockSpace() { - ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; + ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | + ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; - ImGuiViewport *viewport = ImGui::GetMainViewport(); + ImGuiViewport* viewport = ImGui::GetMainViewport(); ImGui::SetNextWindowPos(viewport->WorkPos); ImGui::SetNextWindowSize(viewport->WorkSize); ImGui::SetNextWindowViewport(viewport->ID); @@ -416,7 +421,7 @@ void ShowMainDockSpace() ImGui::End(); } -bool initGL(int *argc, char **argv, const std::string &winTitle, void (*display)(), void (*mouse)(int, int, int, int)) +bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int)) { glutInit(argc, argv); glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); @@ -451,10 +456,10 @@ bool initGL(int *argc, char **argv, const std::string &winTitle, void (*display) glutReshapeFunc(reshape); ImGui::CreateContext(); - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); (void)io; io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard // Enable Keyboard Controls - | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; + | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; io.ConfigDockingWithShift = true; io.MouseDrawCursor = false; // use OS cursor (for future ImGUI update to 1.93+) @@ -492,7 +497,7 @@ void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eig { for (double j = 0; j < 1.0; j += dj) // vertical { - double u = i * 2 * pi; // 0 to 2pi + double u = i * 2 * pi; // 0 to 2pi double v = (j - 0.5) * pi; //-pi/2 to pi/2 const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); @@ -785,7 +790,7 @@ void camMenu() void view_kbd_shortcuts() { - ImGuiIO &io = ImGui::GetIO(); + ImGuiIO& io = ImGui::GetIO(); if (io.WantCaptureKeyboard) return; @@ -941,7 +946,7 @@ void cor_window() } } -void ImGuiHyperlink(const char *url, ImVec4 color) +void ImGuiHyperlink(const char* url, ImVec4 color) { ImGui::PushStyleColor(ImGuiCol_Text, color); ImGui::TextUnformatted(url); @@ -958,11 +963,8 @@ void ImGuiHyperlink(const char *url, ImVec4 color) // Draw underline on hover if (ImGui::IsItemHovered()) { - ImDrawList *draw_list = ImGui::GetWindowDrawList(); - draw_list->AddLine( - ImVec2(pos.x, pos.y + size.y), - ImVec2(pos.x + size.x, pos.y + size.y), - ImColor(color)); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); } // Open URL on click @@ -982,8 +984,8 @@ void ImGuiHyperlink(const char *url, ImVec4 color) void ShowShortcutsTable(const std::vector appShortcuts) { - if (ImGui::BeginTable("ShortcutsTable", 2, - ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) + if (ImGui::BeginTable( + "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) { ImGui::TableSetupScrollFreeze(0, 1); ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); @@ -994,7 +996,7 @@ void ShowShortcutsTable(const std::vector appShortcuts) for (size_t i = 0; i < shortcuts.size(); ++i) { - const auto &s = shortcuts[i]; + const auto& s = shortcuts[i]; // Insert a "fake" type row when type changes if (!s.type.empty() && s.type != lastType) @@ -1030,15 +1032,18 @@ void ShowShortcutsTable(const std::vector appShortcuts) } } -void info_window(const std::vector &infoLines, const std::vector &appShortcuts) +void info_window(const std::vector& infoLines, const std::vector& appShortcuts) { if (!info_gui) return; - if (ImGui::Begin("Info", &info_gui, ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) + if (ImGui::Begin( + "Info", + &info_gui, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) { bool firstLine = true; - for (const auto &line : infoLines) + for (const auto& line : infoLines) { if (line.empty()) ImGui::NewLine(); @@ -1049,16 +1054,18 @@ void info_window(const std::vector &infoLines, const std::vector &sessions, int x, int y, const int first_session_index, const int second_session_index, const int number_visible_sessions, int &index_loop_closure_source, int &index_loop_closure_target, bool KeyShift) +void getClosestTrajectoriesPoint( + std::vector& sessions, + int x, + int y, + const int first_session_index, + const int second_session_index, + const int number_visible_sessions, + int& index_loop_closure_source, + int& index_loop_closure_target, + bool KeyShift) { const auto laser_beam = GetLaserBeam(x, y); double min_distance = std::numeric_limits::max(); @@ -1340,7 +1356,8 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c { for (size_t j = 0; j < sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { - const auto &p = sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + const auto& p = + sessions[first_session_index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); Eigen::Vector3d vp = sessions[first_session_index].point_clouds_container.point_clouds[i].m_pose * p; double dist = distance_point_to_line(vp, laser_beam); @@ -1358,7 +1375,7 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c new_rotation_center.z() = static_cast(vp.z()); index_loop_closure_source = i; } - else // io.KeyShift + else // io.KeyShift { index_loop_closure_target = i; } @@ -1370,11 +1387,11 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c else if (number_visible_sessions == 2) { int index = -1; - if (!KeyShift) // io.KeyCtrl + if (!KeyShift) // io.KeyCtrl { index = first_session_index; } - else // io.KeyShift + else // io.KeyShift { index = second_session_index; } @@ -1383,7 +1400,7 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c { for (size_t j = 0; j < sessions[index].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) { - const auto &p = sessions[index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + const auto& p = sessions[index].point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); Eigen::Vector3d vp = sessions[index].point_clouds_container.point_clouds[i].m_pose * p; double dist = distance_point_to_line(vp, laser_beam); @@ -1392,17 +1409,17 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c { min_distance = dist; - if (!KeyShift) // io.KeyCtrl + if (!KeyShift) // io.KeyCtrl { index_loop_closure_source = i; new_rotation_center.x() = static_cast(vp.x()); new_rotation_center.y() = static_cast(vp.y()); new_rotation_center.z() = static_cast(vp.z()); } - else // io.KeyShift + else // io.KeyShift { index_loop_closure_target = i; - } + } } } } @@ -1410,9 +1427,10 @@ void getClosestTrajectoriesPoint(std::vector &sessions, int x, int y, c else // (number_visible_sessions > 2) { // std::cout << "first_session_index " << first_session_index << std::endl; - for (size_t s = 0; s < sessions.size(); s++) { - if (sessions[s].visible) { - + for (size_t s = 0; s < sessions.size(); s++) + { + if (sessions[s].visible) + { for (size_t i = 0; i < sessions[s].point_clouds_container.point_clouds.size(); i++) { for (size_t j = 0; j < sessions[s].point_clouds_container.point_clouds[i].local_trajectory.size(); j++) @@ -1456,8 +1474,7 @@ void setNewRotationCenter(int x, int y) pl.d = 0; new_rotation_center = rayIntersection(laser_beam, pl).cast(); - std::cout << "Setting new rotation center to:\n" - << new_rotation_center << std::endl; + std::cout << "Setting new rotation center to:\n" << new_rotation_center << std::endl; new_rotate_x = rotate_x; new_rotate_y = rotate_y; diff --git a/core_hd_mapping/CMakeLists.txt b/core_hd_mapping/CMakeLists.txt index 123f52de..87fc4555 100644 --- a/core_hd_mapping/CMakeLists.txt +++ b/core_hd_mapping/CMakeLists.txt @@ -2,36 +2,37 @@ cmake_minimum_required(VERSION 4.0.0) project(core-hd-mapping) -set(THREADS_PREFER_PTHREAD_FLAG ON) -find_package(Threads REQUIRED) - -set(CORE_HD_MAPPING_SOURCE_FILES - src/laz_wrapper.cpp src/odo_with_gnss_fusion.cpp src/project_settings.cpp - src/roi_exporter.cpp src/single_trajectory_viewer.cpp) - -set(CORE_HD_MAPPING_INCLUDE_FILES - include/laz_wrapper.h include/odo_with_gnss_fusion.h - include/project_settings.h include/roi_exporter.h - include/single_trajectory_viewer.h) - -set(CORE_HD_MAPPING_FILES ${CORE_HD_MAPPING_SOURCE_FILES} - ${CORE_HD_MAPPING_INCLUDE_FILES}) - -add_library(core-hd-mapping STATIC ${CORE_HD_MAPPING_FILES}) - -target_link_libraries(core-hd-mapping PRIVATE core ${PLATFORM_MISCELLANEOUS_LIBS} Threads::Threads) - -target_include_directories( - core-hd-mapping - PRIVATE include - ${REPOSITORY_DIRECTORY}/core/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui - ${EXTERNAL_LIBRARIES_DIRECTORY}/imgui/backends - ${EXTERNAL_LIBRARIES_DIRECTORY}/ImGuizmo - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes - ${EIGEN3_INCLUDE_DIR} - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/portable-file-dialogs-master - ${EXTERNAL_LIBRARIES_DIRECTORY}/freeglut/include - ${LASZIP_INCLUDE_DIR}/LASzip/include - ) +add_library(core-hd-mapping STATIC + src/laz_wrapper.cpp + src/odo_with_gnss_fusion.cpp + src/project_settings.cpp + src/roi_exporter.cpp + src/single_trajectory_viewer.cpp +) + +target_precompile_headers(core-hd-mapping + PRIVATE include/pch/pch.h +) + +target_include_directories(core-hd-mapping + PRIVATE + include + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/freeglut/include + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${REPOSITORY_DIRECTORY}/core/include + ${EXTERNAL_LIBRARIES_DIRECTORY}/include +) + +target_link_libraries(core-hd-mapping + PRIVATE + core + ${PLATFORM_MISCELLANEOUS_LIBS} + Threads::Threads +) diff --git a/core_hd_mapping/include/laz_wrapper.h b/core_hd_mapping/include/laz_wrapper.h index acd841b9..0ec66154 100644 --- a/core_hd_mapping/include/laz_wrapper.h +++ b/core_hd_mapping/include/laz_wrapper.h @@ -1,29 +1,29 @@ -#ifndef _LAZ_WRAPPER_H_ -#define _LAZ_WRAPPER_H_ +#pragma once #include #include #include -#include #include -#include +#include + #include +#include -class LazWrapper { +class LazWrapper +{ public: - LazWrapper() { ; }; - ~LazWrapper() { ; }; - - void imgui(CommonData& common_data, const ProjectSettings& project_setings); - void render(const CommonData& common_data); - LAZSector load_sector(const std::string& filename, double shift_x, double shift_y); - // void reload_all_LAZ_files_job(int i, Job* job, /*LAZSector& s,*/ double shift_x, double shift_y); - void reload_all_LAZ_files(double shift_x, double shift_y); - - std::vector sectors; - int decimation = 100; - int num_threads = 16; -}; + LazWrapper() = default; + ~LazWrapper() = default; + + void imgui(const CommonData& common_data, const ProjectSettings& project_setings); + void render(const CommonData& common_data); + + LAZSector load_sector(const std::string& filename, const double shift_x, const double shift_y); -#endif \ No newline at end of file + void reload_all_LAZ_files(const double shift_x, const double shift_y); + + std::vector sectors = {}; + int32_t decimation = 100; + int32_t num_threads = 16; +}; diff --git a/core_hd_mapping/include/odo_with_gnss_fusion.h b/core_hd_mapping/include/odo_with_gnss_fusion.h index 4e27a59a..f01520e8 100644 --- a/core_hd_mapping/include/odo_with_gnss_fusion.h +++ b/core_hd_mapping/include/odo_with_gnss_fusion.h @@ -1,54 +1,50 @@ -#ifndef _ODO_WITH_GNSS_FUSION_H_ -#define _ODO_WITH_GNSS_FUSION_H_ +#pragma once #include #include #include -#include #include +#include + #include #include -class OdoWithGnssFusion { +class OdoWithGnssFusion +{ public: - OdoWithGnssFusion() { ; }; - ~OdoWithGnssFusion() { ; }; - - void imgui(CommonData& common_data); - void render(); - - std::vector load_trajectory(const std::string& file_name); - bool save_trajectory(const std::string& file_name); - void update_shift(float shift_x, float shift_y, CommonData& common_data); - std::vector> find_correspondences_from_lo_to_shifted_gnss(); - std::vector find_between_nodes(); - - bool rigid_registration(bool adaptive_robust_kernel); - bool semi_rigid_registration(); - - bool show_correspondences_rigid_registration = true; - bool show_correspondences_semi_rigid_registration = true; - - std::vector gnss_trajectory; - std::vector gnss_trajectory_shifted; - std::vector fused_trajectory; - std::vector fused_trajectory_motion_model; - std::vector lidar_odometry_trajectory_initial; - std::vector> correspondences_from_lo_to_shifted_gnss; - std::vector between_nodes; - //int offset_x = 0; - //int offset_y = 0; - std::string trajectory_file; - std::string motion_model_file_name; - float color_x = 0; - float color_y = 0; - float color_z = 0; - bool visible = true; - int line_width = 1; - bool set_initial_offset_from_trajectory = true; - + OdoWithGnssFusion() = default; + ~OdoWithGnssFusion() = default; + + void imgui(CommonData& common_data); + void render(); + + std::vector load_trajectory(const std::string& file_name); + bool save_trajectory(const std::string& file_name); + void update_shift(const float shift_x, const float shift_y, CommonData& common_data); + std::vector> find_correspondences_from_lo_to_shifted_gnss(); + std::vector find_between_nodes(); + + bool rigid_registration(bool adaptive_robust_kernel); + bool semi_rigid_registration(); + + bool show_correspondences_rigid_registration = true; + bool show_correspondences_semi_rigid_registration = true; + + std::vector gnss_trajectory = {}; + std::vector gnss_trajectory_shifted = {}; + std::vector fused_trajectory = {}; + std::vector fused_trajectory_motion_model = {}; + std::vector lidar_odometry_trajectory_initial = {}; + std::vector> correspondences_from_lo_to_shifted_gnss = {}; + std::vector between_nodes = {}; + std::string trajectory_file = {}; + std::string motion_model_file_name = {}; + float color_x = 0; + float color_y = 0; + float color_z = 0; + bool visible = true; + int line_width = 1; + bool set_initial_offset_from_trajectory = true; }; - -#endif \ No newline at end of file diff --git a/core_hd_mapping/include/pch/pch.h b/core_hd_mapping/include/pch/pch.h new file mode 100644 index 00000000..f89aca50 --- /dev/null +++ b/core_hd_mapping/include/pch/pch.h @@ -0,0 +1,11 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include \ No newline at end of file diff --git a/core_hd_mapping/include/project_settings.h b/core_hd_mapping/include/project_settings.h index 5e70a33a..54e100e6 100644 --- a/core_hd_mapping/include/project_settings.h +++ b/core_hd_mapping/include/project_settings.h @@ -1,43 +1,52 @@ -#ifndef _PROJECT_SETTINGS_H_ -#define _PROJECT_SETTINGS_H_ +#pragma once #include #include #include -#include #include -#include +#include #include #include - -class LazWrapper; - -class ProjectSettings { +class ProjectSettings +{ public: - ProjectSettings() { ; }; - ~ProjectSettings() { ; }; - - void imgui(OdoWithGnssFusion& odo_with_gnss_fusion, std::vector& sectors, std::vector& rois_with_constraints, CommonData& common_data); - bool save(const std::string& file_name, std::vector& sectors, - const std::vector& rois_with_constraints, const CommonData& common_data); - bool load(const std::string& file_name, std::vector& sectors, - std::vector& rois_with_constraints, CommonData& common_data); - bool add_trajectory(const std::string& file_name, const std::string& file_motion_model_name, float color_x, float color_y, float color_z); - void render(const std::vector& rois_with_constraints); - void pose_graph_slam(std::vector& rois_with_constraints); - std::vector find_between_nodes(std::vector& fused_trajectory); - void fuse_with_georeference(std::vector& bn, const std::vector& constraints, std::vector& fused_trajectory, const std::vector& motion_model); - - ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); - //float shift_x = 0.0; - //float shift_y = 0.0; - //std::string project_main_folder; - - std::vector trajectories; - double total_length = 0; + ProjectSettings() = default; + ~ProjectSettings() = default; + + void imgui( + OdoWithGnssFusion& odo_with_gnss_fusion, + std::vector& sectors, + std::vector& rois_with_constraints, + CommonData& common_data); + bool save( + const std::string& file_name, + std::vector& sectors, + const std::vector& rois_with_constraints, + const CommonData& common_data); + bool load( + const std::string& file_name, + std::vector& sectors, + std::vector& rois_with_constraints, + CommonData& common_data); + bool add_trajectory( + const std::string& file_name, const std::string& file_motion_model_name, float color_x, float color_y, float color_z); + void render(const std::vector& rois_with_constraints); + void pose_graph_slam(std::vector& rois_with_constraints); + std::vector find_between_nodes(std::vector& fused_trajectory); + void fuse_with_georeference( + std::vector& bn, + const std::vector& constraints, + std::vector& fused_trajectory, + const std::vector& motion_model); + + ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); + // float shift_x = 0.0; + // float shift_y = 0.0; + // std::string project_main_folder; + + std::vector trajectories; + double total_length = 0; }; - -#endif \ No newline at end of file diff --git a/core_hd_mapping/include/roi_exporter.h b/core_hd_mapping/include/roi_exporter.h index dc16d86d..51080f73 100644 --- a/core_hd_mapping/include/roi_exporter.h +++ b/core_hd_mapping/include/roi_exporter.h @@ -1,32 +1,31 @@ -#ifndef _ROI_EXPORTER_H_ -#define _ROI_EXPORTER_H_ +#pragma once #include #include #include -#include #include -#include +#include + #include +#include -class RoiExporter { +class RoiExporter +{ public: - RoiExporter() { ; }; - ~RoiExporter() { ; }; - - void imgui(CommonData& common_data, const ProjectSettings& project_setings, std::vector& sectors); - void render(const CommonData& common_data); - PointCloudWithPose get_pc_from_laz(CommonData& common_data, std::vector& sectors, double shift_x, double shift_y); - void export_to_RESSO_format(std::vector& roi_point_clouds); - void import_from_RESSO_format_as_constraints_to_georeference(); + RoiExporter() = default; + ~RoiExporter() = default; - std::vector roi_point_clouds; - int num_threads = 16; - int decimation = 100; + void imgui(CommonData& common_data, const ProjectSettings& project_setings, std::vector& sectors); + void render(const CommonData& common_data); + PointCloudWithPose get_pc_from_laz(CommonData& common_data, std::vector& sectors, double shift_x, double shift_y); + void export_to_RESSO_format(std::vector& roi_point_clouds); + void import_from_RESSO_format_as_constraints_to_georeference(); - std::vector rois_with_constraints; - Eigen::Affine3d m_fake; -}; + std::vector roi_point_clouds; + int num_threads = 16; + int decimation = 100; -#endif \ No newline at end of file + std::vector rois_with_constraints; + Eigen::Affine3d m_fake; +}; \ No newline at end of file diff --git a/core_hd_mapping/include/single_trajectory_viewer.h b/core_hd_mapping/include/single_trajectory_viewer.h index 12888d87..d216a46e 100644 --- a/core_hd_mapping/include/single_trajectory_viewer.h +++ b/core_hd_mapping/include/single_trajectory_viewer.h @@ -1,35 +1,35 @@ -#ifndef _SINGLE_TRAJECTORY_VIEWER_H_ -#define _SINGLE_TRAJECTORY_VIEWER_H_ +#pragma once #include #include #include -#include #include +#include + #include #include -class SingleTrajectoryViewer { +class SingleTrajectoryViewer +{ public: - SingleTrajectoryViewer() { ; }; - ~SingleTrajectoryViewer() { ; }; - void imgui(const CommonData& common_data); - void render(); - - OdoWithGnssFusion trajectory_container; - double current_time_stamp = 0.0f; - std::vector chunk_files; - std::string working_directory; - std::vector points; - std::vector point_clouds; - std::string trajectory_filename; - - std::vector get_point_cloud_for_roi(Eigen::Vector3d roi, float roi_size); - - bool load_fused_trajectory(const std::string& file_name); - bool load_fused_trajectory(); - std::vector load_points_and_transform_to_global(double ts, Eigen::Vector3d roi, float roi_size, int ext); -}; + SingleTrajectoryViewer() = default; + ~SingleTrajectoryViewer() = default; -#endif \ No newline at end of file + void imgui(const CommonData& common_data); + void render(); + + OdoWithGnssFusion trajectory_container; + double current_time_stamp = 0.0f; + std::vector chunk_files; + std::string working_directory; + std::vector points; + std::vector point_clouds; + std::string trajectory_filename; + + std::vector get_point_cloud_for_roi(Eigen::Vector3d roi, float roi_size); + + bool load_fused_trajectory(const std::string& file_name); + bool load_fused_trajectory(); + std::vector load_points_and_transform_to_global(double ts, Eigen::Vector3d roi, float roi_size, int ext); +}; diff --git a/core_hd_mapping/src/laz_wrapper.cpp b/core_hd_mapping/src/laz_wrapper.cpp index 7fe9bb41..312880a7 100644 --- a/core_hd_mapping/src/laz_wrapper.cpp +++ b/core_hd_mapping/src/laz_wrapper.cpp @@ -1,93 +1,117 @@ -#include - -#include -#include -#include +#include -#include +#include +#include #include +#include -#include -#include "pfd_wrapper.hpp" +#include +#include +#include -void LazWrapper::imgui(CommonData& common_data, const ProjectSettings& project_setings) { - ImGui::Begin("LazWrapper"); - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); +#include - if (ImGui::Button("Add point cloud from LAZ file")) { +void LazWrapper::imgui(const CommonData& common_data, const ProjectSettings& project_setings) +{ + ImGui::Begin("LazWrapper"); + ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - auto input_file_names = mandeye::fd::OpenFileDialog("Choose folder", {"LAZ files, *.laz"}, true); + if (ImGui::Button("Add point cloud from LAZ file")) + { + auto input_file_names = mandeye::fd::OpenFileDialog("Choose folder", { "LAZ files, *.laz" }, true); - for(int i = 0 ; i < input_file_names.size(); i++){ + for (int i = 0; i < input_file_names.size(); i++) + { std::cout << "loading progers: " << i + 1 << " of " << input_file_names.size() << std::endl; const auto& fn = input_file_names[i]; LAZSector sector = load_sector(fn, common_data.shift_x, common_data.shift_y); - if (sector.point_cloud.size() > 0) { + if (sector.point_cloud.size() > 0) + { sectors.push_back(sector); std::cout << "sector loaded from file '" << fn << "'" << std::endl; } } - } + } - if (ImGui::Button("LAZ hide all files")) { - for (size_t i = 0; i < sectors.size(); i++) { + if (ImGui::Button("LAZ hide all files")) + { + for (size_t i = 0; i < sectors.size(); i++) + { sectors[i].visible = false; } } bool can_show = false; - for (size_t i = 0; i < sectors.size(); i++) { - if (sectors[i].point_cloud.size() > 0) { + for (size_t i = 0; i < sectors.size(); i++) + { + if (sectors[i].point_cloud.size() > 0) + { can_show = true; } } - if (can_show) { + if (can_show) + { ImGui::SameLine(); - if (ImGui::Button("LAZ show all files")) { + if (ImGui::Button("LAZ show all files")) + { decimation = 100; - for (size_t i = 0; i < sectors.size(); i++) { - if (sectors[i].point_cloud.size() == 0) { + for (size_t i = 0; i < sectors.size(); i++) + { + if (sectors[i].point_cloud.size() == 0) + { LazWrapper lw; LAZSector sector = lw.load_sector(sectors[i].file_name, common_data.shift_x, common_data.shift_y); sectors[i].point_cloud = sector.point_cloud; - } sectors[i].visible = true; } } } - if (ImGui::Button("LAZ reload all files")) { + if (ImGui::Button("LAZ reload all files")) + { decimation = 100; reload_all_LAZ_files(common_data.shift_x, common_data.shift_y); - for (size_t i = 0; i < sectors.size(); i++) { + for (size_t i = 0; i < sectors.size(); i++) + { sectors[i].visible = true; } } ImGui::SliderInt("LAZ loader threads", &num_threads, 1, 128); ImGui::SliderInt("LAZ decimation", &decimation, 1, 1000); - - for (size_t i = 0; i < sectors.size(); i++) { + + for (size_t i = 0; i < sectors.size(); i++) + { bool previous_visibility = sectors[i].visible; - ImGui::Checkbox(("LAZ[" + std::to_string(i) + "]: " + sectors[i].file_name).c_str(), §ors[i].visible); + ImGui::Checkbox(("LAZ[" + std::to_string(i) + "]: " + sectors[i].file_name).c_str(), §ors[i].visible); ImGui::SameLine(); ImGui::PushButtonRepeat(true); float spacing = ImGui::GetStyle().ItemInnerSpacing.x; - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##sector_left").c_str(), ImGuiDir_Left)) { (sectors[i].point_size)--; } + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##sector_left").c_str(), ImGuiDir_Left)) + { + (sectors[i].point_size)--; + } ImGui::SameLine(0.0f, spacing); - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##sector_right").c_str(), ImGuiDir_Right)) { (sectors[i].point_size)++; } + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##sector_right").c_str(), ImGuiDir_Right)) + { + (sectors[i].point_size)++; + } ImGui::PopButtonRepeat(); ImGui::SameLine(); ImGui::Text("point size %d", sectors[i].point_size); - if (sectors[i].point_size < 1) sectors[i].point_size = 1; + if (sectors[i].point_size < 1) + sectors[i].point_size = 1; - if (sectors[i].visible != previous_visibility) { - if (sectors[i].visible) { - if (sectors[i].point_cloud.size() == 0) { + if (sectors[i].visible != previous_visibility) + { + if (sectors[i].visible) + { + if (sectors[i].point_cloud.size() == 0) + { LAZSector sector = load_sector(sectors[i].file_name, common_data.shift_x, common_data.shift_y); sectors[i].point_cloud = sector.point_cloud; } @@ -95,19 +119,22 @@ void LazWrapper::imgui(CommonData& common_data, const ProjectSettings& project_s } } - ImGui::End(); + ImGui::End(); } -void LazWrapper::render(const CommonData& common_data) { - +void LazWrapper::render(const CommonData& common_data) +{ glColor3f(0.5, 0.5, 0.5); - for (const auto& s : sectors) { - if (s.visible) { + for (const auto& s : sectors) + { + if (s.visible) + { glPointSize(s.point_size); glBegin(GL_POINTS); - //for (const auto& p : s.point_cloud) { - for(int i = 0 ; i < s.point_cloud.size(); i+= decimation){ - const auto &p = s.point_cloud[i]; + // for (const auto& p : s.point_cloud) { + for (int i = 0; i < s.point_cloud.size(); i += decimation) + { + const auto& p = s.point_cloud[i]; glColor3f(p.r, p.g, p.b); glVertex3f(p.x, p.y, p.z); } @@ -117,10 +144,10 @@ void LazWrapper::render(const CommonData& common_data) { } } -LAZSector LazWrapper::load_sector(const std::string& filename, double shift_x, double shift_y) +LAZSector LazWrapper::load_sector(const std::string& filename, const double shift_x, const double shift_y) { - LAZSector sector; - sector.file_name = filename; + LAZSector sector; + sector.file_name = filename; laszip_POINTER laszip_reader; if (laszip_create(&laszip_reader)) @@ -151,7 +178,7 @@ LAZSector LazWrapper::load_sector(const std::string& filename, double shift_x, d std::abort(); } // int64_t point_count; - + sector.point_cloud.reserve(header->number_of_point_records); sector.min_x = 1000000000000; @@ -178,41 +205,47 @@ LAZSector LazWrapper::load_sector(const std::string& filename, double shift_x, d sector.point_cloud.push_back(p); - if (p.x < sector.min_x) { + if (p.x < sector.min_x) + { sector.min_x = p.x; } - if (p.x > sector.max_x) { + if (p.x > sector.max_x) + { sector.max_x = p.x; } - if (p.y < sector.min_y) { + if (p.y < sector.min_y) + { sector.min_y = p.y; } - if (p.y > sector.max_y) { + if (p.y > sector.max_y) + { sector.max_y = p.y; } } - - return sector; + + return sector; } -void reload_all_LAZ_files_job(int i, Job* job, LAZSector* s, double shift_x, double shift_y) { +void reload_all_LAZ_files_job(int i, Job* job, LAZSector* s, double shift_x, double shift_y) +{ LazWrapper lw; LAZSector sector = lw.load_sector((*s).file_name, shift_x, shift_y); (*s).point_cloud = sector.point_cloud; } - -void LazWrapper::reload_all_LAZ_files(double shift_x, double shift_y) +void LazWrapper::reload_all_LAZ_files(const double shift_x, const double shift_y) { std::vector jobs = get_jobs(sectors.size(), num_threads); std::vector threads; - for (size_t i = 0; i < jobs.size(); i++) { + for (size_t i = 0; i < jobs.size(); i++) + { threads.push_back(std::thread(reload_all_LAZ_files_job, i, &jobs[i], &(sectors[i]), shift_x, shift_y)); } - for (size_t j = 0; j < threads.size(); j++) { + for (size_t j = 0; j < threads.size(); j++) + { threads[j].join(); } threads.clear(); -} \ No newline at end of file +} diff --git a/core_hd_mapping/src/odo_with_gnss_fusion.cpp b/core_hd_mapping/src/odo_with_gnss_fusion.cpp index 8745c87f..a49eba46 100644 --- a/core_hd_mapping/src/odo_with_gnss_fusion.cpp +++ b/core_hd_mapping/src/odo_with_gnss_fusion.cpp @@ -1,863 +1,1002 @@ -#include -#include +#include -#include -#include -#include -#include -#include -#include -#include +#include #include +#include -#include -#include #include +#include +#include #include -#include "pfd_wrapper.hpp" +#include -void OdoWithGnssFusion::imgui(CommonData& common_data) { - ImGui::Begin("odometry with gnss fusion"); - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); +void OdoWithGnssFusion::imgui(CommonData& common_data) +{ + ImGui::Begin("odometry with gnss fusion"); + ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - ImGui::Checkbox("set_initial_offset_from_trajectory", &set_initial_offset_from_trajectory); + ImGui::Checkbox("set_initial_offset_from_trajectory", &set_initial_offset_from_trajectory); - if (ImGui::Button("load GNSS trajectory")) { + if (ImGui::Button("load GNSS trajectory")) + { std::string input_file_name = ""; input_file_name = mandeye::fd::OpenFileDialogOneFile("Load GNSS trajectory", {}); - if (input_file_name.size() > 0) { + if (input_file_name.size() > 0) + { gnss_trajectory = load_trajectory(input_file_name); - for (size_t i = 0; i < gnss_trajectory.size(); i++) { - double tmp = gnss_trajectory[i].m_pose(1, 3); - gnss_trajectory[i].m_pose(1, 3) = gnss_trajectory[i].m_pose(0, 3); - gnss_trajectory[i].m_pose(0, 3) = tmp; - } - gnss_trajectory_shifted = gnss_trajectory; - - if (set_initial_offset_from_trajectory) { - float shift_x = -gnss_trajectory[gnss_trajectory.size() / 2].m_pose(0, 3); - float shift_y = -gnss_trajectory[gnss_trajectory.size() / 2].m_pose(1, 3); - update_shift(shift_x, shift_y, common_data); - set_initial_offset_from_trajectory = false; - } - else { - update_shift(common_data.shift_x, common_data.shift_y, common_data); - } - - + for (size_t i = 0; i < gnss_trajectory.size(); i++) + { + double tmp = gnss_trajectory[i].m_pose(1, 3); + gnss_trajectory[i].m_pose(1, 3) = gnss_trajectory[i].m_pose(0, 3); + gnss_trajectory[i].m_pose(0, 3) = tmp; + } + gnss_trajectory_shifted = gnss_trajectory; + + if (set_initial_offset_from_trajectory) + { + float shift_x = -gnss_trajectory[gnss_trajectory.size() / 2].m_pose(0, 3); + float shift_y = -gnss_trajectory[gnss_trajectory.size() / 2].m_pose(1, 3); + update_shift(shift_x, shift_y, common_data); + set_initial_offset_from_trajectory = false; + } + else + { + update_shift(common_data.shift_x, common_data.shift_y, common_data); + } } - } - ImGui::SameLine(); - if (ImGui::Button("load LiDAR odometry trajectory")) { + } + ImGui::SameLine(); + if (ImGui::Button("load LiDAR odometry trajectory")) + { std::string input_file_name = ""; input_file_name = mandeye::fd::OpenFileDialogOneFile("Load LiDAR odometry trajectory", {}); std::cout << "trajectory file: '" << input_file_name << "'" << std::endl; - if (input_file_name.size() > 0) { - fused_trajectory = load_trajectory(input_file_name); - lidar_odometry_trajectory_initial = fused_trajectory; + if (input_file_name.size() > 0) + { + fused_trajectory = load_trajectory(input_file_name); + lidar_odometry_trajectory_initial = fused_trajectory; + } + } + ImGui::SameLine(); + if (ImGui::Button("save fused trajectory")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save fused trajectory", {}); + if (output_file_name.size() > 0) + { + if (save_trajectory(output_file_name)) + { + std::cout << "trajectory saved to file: " << output_file_name << std::endl; + } + else + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } + + auto new_path = std::filesystem::path(output_file_name).parent_path(); + auto file_name = std::filesystem::path(output_file_name).stem(); + + new_path /= file_name; + new_path += "_motion_model.csv"; + + if (save_trajectory(new_path.string())) + { + std::cout << "motion model saved to file: " << new_path.string() << std::endl; + } + else + { + std::cout << "problem with saving file: " << new_path.string() << std::endl; + } + } + } + + if (ImGui::Button("find correspondences")) + { + correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); + } + + ImGui::SameLine(); + if (ImGui::Button("rigid registration")) + { + correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); + rigid_registration(false); + } + ImGui::SameLine(); + if (ImGui::Button("rigid registration x6")) + { + for (int i = 0; i < 6; i++) + { + correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); + rigid_registration(false); + } + } + ImGui::SameLine(); + ImGui::Checkbox("show_correspondences_rigid_registration", &show_correspondences_rigid_registration); + + if (ImGui::Button("find between nodes")) + { + between_nodes = find_between_nodes(); + } + + ImGui::SameLine(); + if (ImGui::Button("semi rigid registration")) + { + between_nodes = find_between_nodes(); + semi_rigid_registration(); + } + ImGui::SameLine(); + if (ImGui::Button("semi rigid registration x30")) + { + for (int i = 0; i < 30; i++) + { + between_nodes = find_between_nodes(); + semi_rigid_registration(); } - } - ImGui::SameLine(); - if (ImGui::Button("save fused trajectory")) { - std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("Save fused trajectory", {}); - if (output_file_name.size() > 0) { - if (save_trajectory(output_file_name)) { - std::cout << "trajectory saved to file: " << output_file_name << std::endl; - } - else { - std::cout << "problem with saving file: " << output_file_name << std::endl; - } - - auto new_path = std::filesystem::path(output_file_name).parent_path(); - auto file_name = std::filesystem::path(output_file_name).stem(); - - new_path /= file_name; - new_path += "_motion_model.csv"; - - if (save_trajectory(new_path.string())) { - std::cout << "motion model saved to file: " << new_path.string() << std::endl; - } - else { - std::cout << "problem with saving file: " << new_path.string() << std::endl; - } - } - } - - if (ImGui::Button("find correspondences")) { - correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); - } - - ImGui::SameLine(); - if (ImGui::Button("rigid registration")) { - correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); - rigid_registration(false); - } - ImGui::SameLine(); - if (ImGui::Button("rigid registration x6")) { - for (int i = 0; i < 6; i++) { - correspondences_from_lo_to_shifted_gnss = find_correspondences_from_lo_to_shifted_gnss(); - rigid_registration(false); - } - } - ImGui::SameLine(); - ImGui::Checkbox("show_correspondences_rigid_registration", &show_correspondences_rigid_registration); - - if (ImGui::Button("find between nodes")) { - between_nodes = find_between_nodes(); - } - - ImGui::SameLine(); - if (ImGui::Button("semi rigid registration")) { - between_nodes = find_between_nodes(); - semi_rigid_registration(); - } - ImGui::SameLine(); - if (ImGui::Button("semi rigid registration x30")) { - for (int i = 0; i < 30; i++) { - between_nodes = find_between_nodes(); - semi_rigid_registration(); - } - } - ImGui::SameLine(); - ImGui::Checkbox("show_correspondences_semi_rigid_registration", &show_correspondences_semi_rigid_registration); - - ImGui::End(); + } + ImGui::SameLine(); + ImGui::Checkbox("show_correspondences_semi_rigid_registration", &show_correspondences_semi_rigid_registration); + + ImGui::End(); } void OdoWithGnssFusion::render() { - glColor3d(1, 0, 0); - glBegin(GL_POINTS); - for (size_t i = 0; i < gnss_trajectory_shifted.size(); i++) { - glVertex3f(gnss_trajectory_shifted[i].m_pose(0,3), gnss_trajectory_shifted[i].m_pose(1, 3), gnss_trajectory_shifted[i].m_pose(2, 3)); - } - glEnd(); - - glColor3d(0, 1, 0); - glBegin(GL_POINTS); - for (size_t i = 0; i < fused_trajectory.size(); i++) { - glVertex3f(fused_trajectory[i].m_pose(0, 3), fused_trajectory[i].m_pose(1, 3), fused_trajectory[i].m_pose(2, 3)); - } - glEnd(); - - if (show_correspondences_rigid_registration) { - glColor3f(0.0f, 0.0f, 1.0f); - glBegin(GL_LINES); - for (auto& c : correspondences_from_lo_to_shifted_gnss) { - glVertex3f(fused_trajectory[c.first].m_pose.translation().x(), - fused_trajectory[c.first].m_pose.translation().y(), - fused_trajectory[c.first].m_pose.translation().z()); - glVertex3f(gnss_trajectory_shifted[c.second].m_pose.translation().x(), - gnss_trajectory_shifted[c.second].m_pose.translation().y(), - gnss_trajectory_shifted[c.second].m_pose.translation().z()); - } - glEnd(); - } - - if (show_correspondences_semi_rigid_registration) { - for (const auto& n : between_nodes) { - glColor3f(n.color_x, n.color_y, n.color_z); - glPointSize(10); - glBegin(GL_POINTS); - glVertex3f(n.node_outer.m_pose.translation().x(), n.node_outer.m_pose.translation().y(), n.node_outer.m_pose.translation().z()); - glEnd(); - glPointSize(1); - - for (const auto& ni : n.nodes_between) { - - Eigen::Vector3d v = n.node_outer.m_pose * ni.m_pose.translation(); - glPointSize(2); - glBegin(GL_POINTS); - glVertex3f(v.x(), v.y(), v.z()); - glEnd(); - glPointSize(1); - - if (ni.index_to_gnss != -1) { - glBegin(GL_LINES); - glVertex3f(v.x(), v.y(), v.z()); - glVertex3f(gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().x(), - gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().y(), - gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().z()); - glEnd(); - } - } - } - } + glColor3d(1, 0, 0); + glBegin(GL_POINTS); + for (size_t i = 0; i < gnss_trajectory_shifted.size(); i++) + { + glVertex3f( + gnss_trajectory_shifted[i].m_pose(0, 3), gnss_trajectory_shifted[i].m_pose(1, 3), gnss_trajectory_shifted[i].m_pose(2, 3)); + } + glEnd(); + + glColor3d(0, 1, 0); + glBegin(GL_POINTS); + for (size_t i = 0; i < fused_trajectory.size(); i++) + { + glVertex3f(fused_trajectory[i].m_pose(0, 3), fused_trajectory[i].m_pose(1, 3), fused_trajectory[i].m_pose(2, 3)); + } + glEnd(); + + if (show_correspondences_rigid_registration) + { + glColor3f(0.0f, 0.0f, 1.0f); + glBegin(GL_LINES); + for (auto& c : correspondences_from_lo_to_shifted_gnss) + { + glVertex3f( + fused_trajectory[c.first].m_pose.translation().x(), + fused_trajectory[c.first].m_pose.translation().y(), + fused_trajectory[c.first].m_pose.translation().z()); + glVertex3f( + gnss_trajectory_shifted[c.second].m_pose.translation().x(), + gnss_trajectory_shifted[c.second].m_pose.translation().y(), + gnss_trajectory_shifted[c.second].m_pose.translation().z()); + } + glEnd(); + } + + if (show_correspondences_semi_rigid_registration) + { + for (const auto& n : between_nodes) + { + glColor3f(n.color_x, n.color_y, n.color_z); + glPointSize(10); + glBegin(GL_POINTS); + glVertex3f(n.node_outer.m_pose.translation().x(), n.node_outer.m_pose.translation().y(), n.node_outer.m_pose.translation().z()); + glEnd(); + glPointSize(1); + + for (const auto& ni : n.nodes_between) + { + Eigen::Vector3d v = n.node_outer.m_pose * ni.m_pose.translation(); + glPointSize(2); + glBegin(GL_POINTS); + glVertex3f(v.x(), v.y(), v.z()); + glEnd(); + glPointSize(1); + + if (ni.index_to_gnss != -1) + { + glBegin(GL_LINES); + glVertex3f(v.x(), v.y(), v.z()); + glVertex3f( + gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().x(), + gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().y(), + gnss_trajectory_shifted[ni.index_to_gnss].m_pose.translation().z()); + glEnd(); + } + } + } + } } -class WordDelimitedBySemicolons : public std::string {}; +class WordDelimitedBySemicolons : public std::string +{ +}; -inline std::istream& operator >> (std::istream& is, WordDelimitedBySemicolons& output) +inline std::istream& operator>>(std::istream& is, WordDelimitedBySemicolons& output) { - std::getline(is, output, ';'); - return is; + std::getline(is, output, ';'); + return is; } std::vector OdoWithGnssFusion::load_trajectory(const std::string& file_name) { - std::vector trajectory; - - std::ifstream f; - f.open(file_name.c_str()); - if (f.good()) { - std::cout << std::setprecision(20); - std::cout << "Loading trj from file: " << file_name << std::endl; - - std::string s; - getline(f, s); //read header - std::cout << s << "\n"; - - while (f.good()) - { - getline(f, s); - std::istringstream iss(s); - std::vector results((std::istream_iterator(iss)), std::istream_iterator()); - if (results.size() != 13) - { - std::cout << "results.size() != 13 it is " << results.size() << std::endl; - break; - } - Node n; - // std::cout<<"size: "< trajectory; + + std::ifstream f; + f.open(file_name.c_str()); + if (f.good()) + { + std::cout << std::setprecision(20); + std::cout << "Loading trj from file: " << file_name << std::endl; + + std::string s; + getline(f, s); // read header + std::cout << s << "\n"; + + while (f.good()) + { + getline(f, s); + std::istringstream iss(s); + std::vector results( + (std::istream_iterator(iss)), std::istream_iterator()); + if (results.size() != 13) + { + std::cout << "results.size() != 13 it is " << results.size() << std::endl; + break; + } + Node n; + // std::cout<<"size: "<fused_trajectory.size(); i++) { - - outfile << this->fused_trajectory[i].timestamp << ";" - << this->fused_trajectory[i].m_pose(0, 3) << ";" - << this->fused_trajectory[i].m_pose(1, 3) << ";" - << this->fused_trajectory[i].m_pose(2, 3) << ";" - << this->fused_trajectory[i].m_pose(0, 0) << ";" - << this->fused_trajectory[i].m_pose(0, 1) << ";" - << this->fused_trajectory[i].m_pose(0, 2) << ";" - << this->fused_trajectory[i].m_pose(1, 0) << ";" - << this->fused_trajectory[i].m_pose(1, 1) << ";" - << this->fused_trajectory[i].m_pose(1, 2) << ";" - << this->fused_trajectory[i].m_pose(2, 0) << ";" - << this->fused_trajectory[i].m_pose(2, 1) << ";" - << this->fused_trajectory[i].m_pose(2, 2) << std::endl; - } - - outfile.close(); - return true; - return true; + std::ofstream outfile; + outfile << std::setprecision(20); + outfile.open(file_name); + outfile << "timestamp;t0;t1;t2;r00;r01;r02;r10;r11;r12;r20;r21;r22" << std::endl; + + for (size_t i = 0; i < this->fused_trajectory.size(); i++) + { + outfile << this->fused_trajectory[i].timestamp << ";" << this->fused_trajectory[i].m_pose(0, 3) << ";" + << this->fused_trajectory[i].m_pose(1, 3) << ";" << this->fused_trajectory[i].m_pose(2, 3) << ";" + << this->fused_trajectory[i].m_pose(0, 0) << ";" << this->fused_trajectory[i].m_pose(0, 1) << ";" + << this->fused_trajectory[i].m_pose(0, 2) << ";" << this->fused_trajectory[i].m_pose(1, 0) << ";" + << this->fused_trajectory[i].m_pose(1, 1) << ";" << this->fused_trajectory[i].m_pose(1, 2) << ";" + << this->fused_trajectory[i].m_pose(2, 0) << ";" << this->fused_trajectory[i].m_pose(2, 1) << ";" + << this->fused_trajectory[i].m_pose(2, 2) << std::endl; + } + + outfile.close(); + return true; + return true; } - -void OdoWithGnssFusion::update_shift(float shift_x, float shift_y, CommonData& common_data) +void OdoWithGnssFusion::update_shift(const float shift_x, float const shift_y, CommonData& common_data) { - common_data.shift_x = shift_x; - common_data.shift_y = shift_y; - - for (size_t i = 0; i < gnss_trajectory.size(); i++) { - gnss_trajectory_shifted[i].m_pose(0, 3) = gnss_trajectory[i].m_pose(0, 3) + shift_x; - gnss_trajectory_shifted[i].m_pose(1, 3) = gnss_trajectory[i].m_pose(1, 3) + shift_y; - } + common_data.shift_x = shift_x; + common_data.shift_y = shift_y; + + for (size_t i = 0; i < gnss_trajectory.size(); i++) + { + gnss_trajectory_shifted[i].m_pose(0, 3) = gnss_trajectory[i].m_pose(0, 3) + shift_x; + gnss_trajectory_shifted[i].m_pose(1, 3) = gnss_trajectory[i].m_pose(1, 3) + shift_y; + } } -std::vector> OdoWithGnssFusion::find_correspondences_from_lo_to_shifted_gnss() { - std::vector> correspondences; - - double dist_along = 0.0f; - - for (size_t i = 1; i < fused_trajectory.size(); i++) { - dist_along += (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); - if (dist_along > 5) { - auto it = std::lower_bound(gnss_trajectory_shifted.begin(), gnss_trajectory_shifted.end(), fused_trajectory[i], - [](Node lhs, Node rhs) -> bool { return lhs.timestamp < rhs.timestamp; }); - - if (fabs(it->timestamp - fused_trajectory[i].timestamp) < 0.01) { - std::pair correspondence; - correspondence.first = i; - correspondence.second = it - gnss_trajectory_shifted.begin(); - correspondences.emplace_back(correspondence); - dist_along = 0.0; - } - } - } - return correspondences; +std::vector> OdoWithGnssFusion::find_correspondences_from_lo_to_shifted_gnss() +{ + std::vector> correspondences; + + double dist_along = 0.0f; + + for (size_t i = 1; i < fused_trajectory.size(); i++) + { + dist_along += (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); + if (dist_along > 5) + { + auto it = std::lower_bound( + gnss_trajectory_shifted.begin(), + gnss_trajectory_shifted.end(), + fused_trajectory[i], + [](Node lhs, Node rhs) -> bool + { + return lhs.timestamp < rhs.timestamp; + }); + + if (fabs(it->timestamp - fused_trajectory[i].timestamp) < 0.01) + { + std::pair correspondence; + correspondence.first = i; + correspondence.second = it - gnss_trajectory_shifted.begin(); + correspondences.emplace_back(correspondence); + dist_along = 0.0; + } + } + } + return correspondences; } bool OdoWithGnssFusion::rigid_registration(bool adaptive_robust_kernel) { - //std::cout << "rigid_registration()" << std::endl; - - //barron - double min_sum_x = 1000000000000;// std::numeric_limits::max(); - double min_sum_y = 1000000000000;// std::numeric_limits::max(); - double min_sum_z = 1000000000000;// std::numeric_limits::max(); - - double barron_alpha_x = 2.; - double barron_alpha_y = 2.; - double barron_alpha_z = 2.; - - Eigen::Affine3d m_pose_source = Eigen::Affine3d::Identity(); - TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(m_pose_source); - - float scale_factor_x = 1; - float scale_factor_y = 1; - float scale_factor_z = 1; - - double barron_c = 1.0; - if (adaptive_robust_kernel) { - for (double alpha = -10; alpha <= 2; alpha += 0.1) { - double Z_tilde = get_approximate_partition_function(-10, 10, alpha, barron_c, 100); - - double sum_x = 0; - double sum_y = 0; - double sum_z = 0; - - for (auto& c : correspondences_from_lo_to_shifted_gnss) { - Eigen::Vector3d p_s(fused_trajectory[c.first].m_pose.translation().x(), - fused_trajectory[c.first].m_pose.translation().y(), - fused_trajectory[c.first].m_pose.translation().z()); - Eigen::Vector3d p_t(gnss_trajectory_shifted[c.second].m_pose.translation().x(), - gnss_trajectory_shifted[c.second].m_pose.translation().y(), - gnss_trajectory_shifted[c.second].m_pose.translation().z()); - double delta_x = p_t.x() - p_s.x(); - double delta_y = p_t.y() - p_s.y(); - double delta_z = p_t.z() - p_s.z(); - - if (!(delta_x == delta_x)) { - continue; - } - if (!(delta_y == delta_y)) { - continue; - } - if (!(delta_z == delta_z)) { - continue; - } - - delta_x *= scale_factor_x; - delta_y *= scale_factor_y; - delta_z *= scale_factor_z; - - sum_x += get_truncated_robust_kernel(delta_x, alpha, barron_c, Z_tilde); - sum_y += get_truncated_robust_kernel(delta_y, alpha, barron_c, Z_tilde); - sum_z += get_truncated_robust_kernel(delta_z, alpha, barron_c, Z_tilde); - - //std::cout << "sum_x: " << sum_x << " sum_y: " << sum_y << " sum_z: " << sum_z << std::endl; - - if (sum_x < min_sum_x) { - min_sum_x = sum_x; - barron_alpha_x = alpha; - } - if (sum_y < min_sum_y) { - min_sum_y = sum_y; - barron_alpha_y = alpha; - } - if (sum_z < min_sum_z) { - min_sum_z = sum_z; - barron_alpha_z = alpha; - } - } - } - - std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y << " barron_alpha_z: " << barron_alpha_z << std::endl; - } - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - for (auto& c : correspondences_from_lo_to_shifted_gnss) { - Eigen::Vector3d p_s(fused_trajectory[c.first].m_pose.translation().x(), - fused_trajectory[c.first].m_pose.translation().y(), - fused_trajectory[c.first].m_pose.translation().z()); - Eigen::Vector3d p_t(gnss_trajectory_shifted[c.second].m_pose.translation().x(), - gnss_trajectory_shifted[c.second].m_pose.translation().y(), - gnss_trajectory_shifted[c.second].m_pose.translation().z()); - - double delta_x; - double delta_y; - double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); - - Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, p_s.x(), p_s.y(), p_s.z()); - - int ir = tripletListB.size(); - int ic = 0; - for (int row = 0; row < 3; row++) { - for (int col = 0; col < 6; col++) { - if (jacobian(row, col) != 0.0) { - tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); - } - } - } - if (adaptive_robust_kernel) { - tripletListP.emplace_back(ir, ir, get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c)); - tripletListP.emplace_back(ir + 1, ir + 1, get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c)); - tripletListP.emplace_back(ir + 2, ir + 2, get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c)); - } - else { - tripletListP.emplace_back(ir, ir, 1); - tripletListP.emplace_back(ir + 1, ir + 1, 1); - tripletListP.emplace_back(ir + 2, ir + 2, 1); - } - - tripletListB.emplace_back(ir, 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - } - - Eigen::SparseMatrix matA(tripletListB.size(), 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(6, 6); - Eigen::SparseMatrix AtPB(6, 1); - - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = AtP * matA; - AtPB = AtP * matB; - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - Eigen::SimplicialCholesky> solver(AtPA); - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - h_x.push_back(it.value()); - } - } - - if (h_x.size() == 6) { - std::cout << "ICP solution" << std::endl; - std::cout << "x,y,z,om,fi,ka" << std::endl; - std::cout << h_x[0] << "," << h_x[1] << "," << h_x[2] << "," << h_x[3] << "," << h_x[4] << "," << h_x[5] << std::endl; - - TaitBryanPose pose_result; - pose_result.px = h_x[0]; - pose_result.py = h_x[1]; - pose_result.pz = h_x[2]; - pose_result.om = h_x[3]; - pose_result.fi = h_x[4]; - pose_result.ka = h_x[5]; - - Eigen::Affine3d m_res = affine_matrix_from_pose_tait_bryan(pose_result); - - for (auto& c : fused_trajectory) { - c.m_pose = m_res * c.m_pose; - } - } - else { - std::cout << "AtPA=AtPB FAILED" << std::endl; - return false; - } - return true; + // std::cout << "rigid_registration()" << std::endl; + + // barron + double min_sum_x = 1000000000000; // std::numeric_limits::max(); + double min_sum_y = 1000000000000; // std::numeric_limits::max(); + double min_sum_z = 1000000000000; // std::numeric_limits::max(); + + double barron_alpha_x = 2.; + double barron_alpha_y = 2.; + double barron_alpha_z = 2.; + + Eigen::Affine3d m_pose_source = Eigen::Affine3d::Identity(); + TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(m_pose_source); + + float scale_factor_x = 1; + float scale_factor_y = 1; + float scale_factor_z = 1; + + double barron_c = 1.0; + if (adaptive_robust_kernel) + { + for (double alpha = -10; alpha <= 2; alpha += 0.1) + { + double Z_tilde = get_approximate_partition_function(-10, 10, alpha, barron_c, 100); + + double sum_x = 0; + double sum_y = 0; + double sum_z = 0; + + for (auto& c : correspondences_from_lo_to_shifted_gnss) + { + Eigen::Vector3d p_s( + fused_trajectory[c.first].m_pose.translation().x(), + fused_trajectory[c.first].m_pose.translation().y(), + fused_trajectory[c.first].m_pose.translation().z()); + Eigen::Vector3d p_t( + gnss_trajectory_shifted[c.second].m_pose.translation().x(), + gnss_trajectory_shifted[c.second].m_pose.translation().y(), + gnss_trajectory_shifted[c.second].m_pose.translation().z()); + double delta_x = p_t.x() - p_s.x(); + double delta_y = p_t.y() - p_s.y(); + double delta_z = p_t.z() - p_s.z(); + + if (!(delta_x == delta_x)) + { + continue; + } + if (!(delta_y == delta_y)) + { + continue; + } + if (!(delta_z == delta_z)) + { + continue; + } + + delta_x *= scale_factor_x; + delta_y *= scale_factor_y; + delta_z *= scale_factor_z; + + sum_x += get_truncated_robust_kernel(delta_x, alpha, barron_c, Z_tilde); + sum_y += get_truncated_robust_kernel(delta_y, alpha, barron_c, Z_tilde); + sum_z += get_truncated_robust_kernel(delta_z, alpha, barron_c, Z_tilde); + + // std::cout << "sum_x: " << sum_x << " sum_y: " << sum_y << " sum_z: " << sum_z << std::endl; + + if (sum_x < min_sum_x) + { + min_sum_x = sum_x; + barron_alpha_x = alpha; + } + if (sum_y < min_sum_y) + { + min_sum_y = sum_y; + barron_alpha_y = alpha; + } + if (sum_z < min_sum_z) + { + min_sum_z = sum_z; + barron_alpha_z = alpha; + } + } + } + + std::cout << "barron_alpha_x: " << barron_alpha_x << " barron_alpha_y: " << barron_alpha_y << " barron_alpha_z: " << barron_alpha_z + << std::endl; + } + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + for (auto& c : correspondences_from_lo_to_shifted_gnss) + { + Eigen::Vector3d p_s( + fused_trajectory[c.first].m_pose.translation().x(), + fused_trajectory[c.first].m_pose.translation().y(), + fused_trajectory[c.first].m_pose.translation().z()); + Eigen::Vector3d p_t( + gnss_trajectory_shifted[c.second].m_pose.translation().x(), + gnss_trajectory_shifted[c.second].m_pose.translation().y(), + gnss_trajectory_shifted[c.second].m_pose.translation().z()); + + double delta_x; + double delta_y; + double delta_z; + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + Eigen::Matrix jacobian; + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z()); + + int ir = tripletListB.size(); + int ic = 0; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); + } + } + } + if (adaptive_robust_kernel) + { + tripletListP.emplace_back(ir, ir, get_barron_w(delta_x * scale_factor_x, barron_alpha_x, barron_c)); + tripletListP.emplace_back(ir + 1, ir + 1, get_barron_w(delta_y * scale_factor_y, barron_alpha_y, barron_c)); + tripletListP.emplace_back(ir + 2, ir + 2, get_barron_w(delta_z * scale_factor_z, barron_alpha_z, barron_c)); + } + else + { + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + } + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + } + + Eigen::SparseMatrix matA(tripletListB.size(), 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(6, 6); + Eigen::SparseMatrix AtPB(6, 1); + + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = AtP * matA; + AtPB = AtP * matB; + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + Eigen::SimplicialCholesky> solver(AtPA); + Eigen::SparseMatrix x = solver.solve(AtPB); + + std::vector h_x; + + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + h_x.push_back(it.value()); + } + } + + if (h_x.size() == 6) + { + std::cout << "ICP solution" << std::endl; + std::cout << "x,y,z,om,fi,ka" << std::endl; + std::cout << h_x[0] << "," << h_x[1] << "," << h_x[2] << "," << h_x[3] << "," << h_x[4] << "," << h_x[5] << std::endl; + + TaitBryanPose pose_result; + pose_result.px = h_x[0]; + pose_result.py = h_x[1]; + pose_result.pz = h_x[2]; + pose_result.om = h_x[3]; + pose_result.fi = h_x[4]; + pose_result.ka = h_x[5]; + + Eigen::Affine3d m_res = affine_matrix_from_pose_tait_bryan(pose_result); + + for (auto& c : fused_trajectory) + { + c.m_pose = m_res * c.m_pose; + } + } + else + { + std::cout << "AtPA=AtPB FAILED" << std::endl; + return false; + } + return true; } -std::vector OdoWithGnssFusion::find_between_nodes() { - std::vector between_nodes; - double dist_along = 0.0f; - double dist_along_gnss = 0.0f; - - BetweenNode node_outer; - node_outer.node_outer.index_to_lidar_odometry_odo = 0; - node_outer.node_outer.m_pose = fused_trajectory[0].m_pose; - node_outer.node_outer.timestamp = fused_trajectory[0].timestamp; - - Node node_inner; - //node_inner.index_to_lidar_odometry_odo = 0; - //node_inner.m_pose = lidar_odometry_trajectory[0].m_pose; - //node_inner.timestamp = lidar_odometry_trajectory[0].timestamp; - //node_outer.nodes_between.push_back(node_inner); - - - for (size_t i = 1; i < fused_trajectory.size(); i++) { - node_inner.index_to_lidar_odometry_odo = i; - node_inner.index_to_gnss = -1; - node_inner.m_pose = fused_trajectory[i].m_pose; - node_inner.timestamp = fused_trajectory[i].timestamp; - - double dist_increment = (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); - dist_along += dist_increment; - dist_along_gnss += dist_increment; - - //std::cout << "dist_increment " << dist_increment << std::endl; - - if (dist_along_gnss > 10) { - auto it = std::lower_bound(gnss_trajectory_shifted.begin(), gnss_trajectory_shifted.end(), fused_trajectory[i], - [](Node lhs, Node rhs) -> bool { return lhs.timestamp < rhs.timestamp; }); - if (fabs(it->timestamp - fused_trajectory[i].timestamp) < 0.01) { - int index_to_gnss = it - gnss_trajectory_shifted.begin(); - int res_index = index_to_gnss; - double dist_min = 1000000.0; - for (int index = index_to_gnss - 100; index < index_to_gnss + 100; index++) { - if (index >= 0 && index < gnss_trajectory_shifted.size()) { - double distance = sqrt((fused_trajectory[i].m_pose(0,3) - gnss_trajectory_shifted[index].m_pose(0,3))* - (fused_trajectory[i].m_pose(0, 3) - gnss_trajectory_shifted[index].m_pose(0, 3)) + - (fused_trajectory[i].m_pose(1, 3) - gnss_trajectory_shifted[index].m_pose(1, 3)) * - (fused_trajectory[i].m_pose(1, 3) - gnss_trajectory_shifted[index].m_pose(1, 3))); - - if (distance < dist_min) { - dist_min = distance; - res_index = index; - } - } - } - node_inner.index_to_gnss = res_index; - dist_along_gnss = 0.0; - //node_inner.index_to_gnss = it - gnss_trajectory_shifted.begin(); - //dist_along_gnss = 0.0; - } - } - - node_outer.nodes_between.push_back(node_inner); - - if ((dist_along > 100 || i == fused_trajectory.size() - 1)) { - for (auto& n : node_outer.nodes_between) { - n.m_pose = node_outer.node_outer.m_pose.inverse() * n.m_pose; - } - node_outer.color_x = float(rand() % 255) / 256.0; - node_outer.color_y = float(rand() % 255) / 256.0; - node_outer.color_z = float(rand() % 255) / 256.0; - - between_nodes.push_back(node_outer); - - node_outer.nodes_between.clear(); - node_outer.node_outer.index_to_lidar_odometry_odo = i; - node_outer.node_outer.m_pose = fused_trajectory[i].m_pose; - node_outer.node_outer.timestamp = fused_trajectory[i].timestamp; - - dist_along = 0.0; - } - } - - /*for (size_t i = 0; i < between_nodes.size(); i++) { - std::cout << "-------------------------" << std::endl; - std::cout << between_nodes[i].node_outer.index_to_lidar_odometry_odo << std::endl; - - for (size_t j = 0; j < between_nodes[i].nodes_between.size(); j++) { - std::cout << between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo << " "; - } - std::cout << std::endl; - }*/ - - return between_nodes; +std::vector OdoWithGnssFusion::find_between_nodes() +{ + std::vector between_nodes; + double dist_along = 0.0f; + double dist_along_gnss = 0.0f; + + BetweenNode node_outer; + node_outer.node_outer.index_to_lidar_odometry_odo = 0; + node_outer.node_outer.m_pose = fused_trajectory[0].m_pose; + node_outer.node_outer.timestamp = fused_trajectory[0].timestamp; + + Node node_inner; + // node_inner.index_to_lidar_odometry_odo = 0; + // node_inner.m_pose = lidar_odometry_trajectory[0].m_pose; + // node_inner.timestamp = lidar_odometry_trajectory[0].timestamp; + // node_outer.nodes_between.push_back(node_inner); + + for (size_t i = 1; i < fused_trajectory.size(); i++) + { + node_inner.index_to_lidar_odometry_odo = i; + node_inner.index_to_gnss = -1; + node_inner.m_pose = fused_trajectory[i].m_pose; + node_inner.timestamp = fused_trajectory[i].timestamp; + + double dist_increment = (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); + dist_along += dist_increment; + dist_along_gnss += dist_increment; + + // std::cout << "dist_increment " << dist_increment << std::endl; + + if (dist_along_gnss > 10) + { + auto it = std::lower_bound( + gnss_trajectory_shifted.begin(), + gnss_trajectory_shifted.end(), + fused_trajectory[i], + [](Node lhs, Node rhs) -> bool + { + return lhs.timestamp < rhs.timestamp; + }); + if (fabs(it->timestamp - fused_trajectory[i].timestamp) < 0.01) + { + int index_to_gnss = it - gnss_trajectory_shifted.begin(); + int res_index = index_to_gnss; + double dist_min = 1000000.0; + for (int index = index_to_gnss - 100; index < index_to_gnss + 100; index++) + { + if (index >= 0 && index < gnss_trajectory_shifted.size()) + { + double distance = sqrt( + (fused_trajectory[i].m_pose(0, 3) - gnss_trajectory_shifted[index].m_pose(0, 3)) * + (fused_trajectory[i].m_pose(0, 3) - gnss_trajectory_shifted[index].m_pose(0, 3)) + + (fused_trajectory[i].m_pose(1, 3) - gnss_trajectory_shifted[index].m_pose(1, 3)) * + (fused_trajectory[i].m_pose(1, 3) - gnss_trajectory_shifted[index].m_pose(1, 3))); + + if (distance < dist_min) + { + dist_min = distance; + res_index = index; + } + } + } + node_inner.index_to_gnss = res_index; + dist_along_gnss = 0.0; + // node_inner.index_to_gnss = it - gnss_trajectory_shifted.begin(); + // dist_along_gnss = 0.0; + } + } + + node_outer.nodes_between.push_back(node_inner); + + if ((dist_along > 100 || i == fused_trajectory.size() - 1)) + { + for (auto& n : node_outer.nodes_between) + { + n.m_pose = node_outer.node_outer.m_pose.inverse() * n.m_pose; + } + node_outer.color_x = float(rand() % 255) / 256.0; + node_outer.color_y = float(rand() % 255) / 256.0; + node_outer.color_z = float(rand() % 255) / 256.0; + + between_nodes.push_back(node_outer); + + node_outer.nodes_between.clear(); + node_outer.node_outer.index_to_lidar_odometry_odo = i; + node_outer.node_outer.m_pose = fused_trajectory[i].m_pose; + node_outer.node_outer.timestamp = fused_trajectory[i].timestamp; + + dist_along = 0.0; + } + } + + /*for (size_t i = 0; i < between_nodes.size(); i++) { + std::cout << "-------------------------" << std::endl; + std::cout << between_nodes[i].node_outer.index_to_lidar_odometry_odo << std::endl; + + for (size_t j = 0; j < between_nodes[i].nodes_between.size(); j++) { + std::cout << between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo << " "; + } + std::cout << std::endl; + }*/ + + return between_nodes; } bool OdoWithGnssFusion::semi_rigid_registration() { - std::vector> odo_edges; - for (size_t i = 1; i < between_nodes.size(); i++) { - odo_edges.emplace_back(i - 1, i); - } - - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - std::vector poses; - std::vector poses_desired; - - for (size_t i = 0; i < between_nodes.size(); i++) { - poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); - } - //poses_desired = poses; - - for (size_t i = 0; i < between_nodes.size(); i++) { - //poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); - poses_desired.push_back(pose_tait_bryan_from_affine_matrix(lidar_odometry_trajectory_initial[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose) ); - } - - - - //poses_desired = poses; - - /*for (auto& p : poses) { - p.px += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; - p.py += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; - p.pz += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; - - p.om += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; - p.fi += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; - p.ka += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; - }*/ - - for (size_t i = 0; i < odo_edges.size(); i++) { - Eigen::Matrix relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, - poses_desired[odo_edges[i].first].px, - poses_desired[odo_edges[i].first].py, - poses_desired[odo_edges[i].first].pz, - poses_desired[odo_edges[i].first].om, - poses_desired[odo_edges[i].first].fi, - poses_desired[odo_edges[i].first].ka, - poses_desired[odo_edges[i].second].px, - poses_desired[odo_edges[i].second].py, - poses_desired[odo_edges[i].second].pz, - poses_desired[odo_edges[i].second].om, - poses_desired[odo_edges[i].second].fi, - poses_desired[odo_edges[i].second].ka); - - Eigen::Matrix delta; - relative_pose_obs_eq_tait_bryan_wc_case1( - delta, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - relative_pose_measurement_odo(0, 0), - relative_pose_measurement_odo(1, 0), - relative_pose_measurement_odo(2, 0), - normalize_angle(relative_pose_measurement_odo(3, 0)), - normalize_angle(relative_pose_measurement_odo(4, 0)), - normalize_angle(relative_pose_measurement_odo(5, 0))); - - Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); - - int ir = tripletListB.size(); - - int ic_1 = odo_edges[i].first * 6; - int ic_2 = odo_edges[i].second * 6; - - for (size_t row = 0; row < 6; row++) { - tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); - tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); - tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); - tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); - tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); - tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); - - tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); - tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); - tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); - tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); - tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); - tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); - } - - tripletListB.emplace_back(ir, 0, delta(0, 0)); - tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); - tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); - tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0))); - tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); - tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); - - tripletListP.emplace_back(ir, ir , 10000); - tripletListP.emplace_back(ir + 1, ir + 1, 10000); - tripletListP.emplace_back(ir + 2, ir + 2, 10000); - tripletListP.emplace_back(ir + 3, ir + 3, 1000000); - tripletListP.emplace_back(ir + 4, ir + 4, 1000000); - tripletListP.emplace_back(ir + 5, ir + 5, 1000000); - } - - //gnss correspondences - for (size_t i = 0; i < between_nodes.size(); i++) { - TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); - - for (size_t j = 0; j < between_nodes[i].nodes_between.size(); j++) { - if (between_nodes[i].nodes_between[j].index_to_gnss != -1) { - Eigen::Vector3d p_s( - between_nodes[i].nodes_between[j].m_pose.translation().x(), - between_nodes[i].nodes_between[j].m_pose.translation().y(), - between_nodes[i].nodes_between[j].m_pose.translation().z()); - Eigen::Vector3d p_t( - gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().x(), - gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().y(), - gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().z()); - - double delta_x; - double delta_y; - double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); - - if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) > 100.0)continue; - - Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, p_s.x(), p_s.y(), p_s.z()); - - int ir = tripletListB.size(); - int ic = i * 6; - for (int row = 0; row < 3; row++) { - for (int col = 0; col < 6; col++) { - if (jacobian(row, col) != 0.0) { - tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); - } - } - } - - tripletListP.emplace_back(ir , ir, get_cauchy_w(delta_x, 10)); - tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 10)); - tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 10)); - - //tripletListP.emplace_back(ir , ir, 1); - //tripletListP.emplace_back(ir + 1, ir + 1, 1); - //tripletListP.emplace_back(ir + 2, ir + 2, 1); - - tripletListB.emplace_back(ir , 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - } - } - } - - int ir = tripletListB.size(); - tripletListA.emplace_back(ir, 0, 1); - tripletListA.emplace_back(ir + 1, 1, 1); - tripletListA.emplace_back(ir + 2, 2, 1); - tripletListA.emplace_back(ir + 3, 3, 1); - tripletListA.emplace_back(ir + 4, 4, 1); - tripletListA.emplace_back(ir + 5, 5, 1); - - tripletListP.emplace_back(ir, ir, 1); - tripletListP.emplace_back(ir + 1, ir + 1, 1); - tripletListP.emplace_back(ir + 2, ir + 2, 1); - tripletListP.emplace_back(ir + 3, ir + 3, 1); - tripletListP.emplace_back(ir + 4, ir + 4, 1); - tripletListP.emplace_back(ir + 5, ir + 5, 1); - - tripletListB.emplace_back(ir, 0, 0); - tripletListB.emplace_back(ir + 1, 0, 0); - tripletListB.emplace_back(ir + 2, 0, 0); - tripletListB.emplace_back(ir + 3, 0, 0); - tripletListB.emplace_back(ir + 4, 0, 0); - tripletListB.emplace_back(ir + 5, 0, 0); - - Eigen::SparseMatrix matA(tripletListB.size(), between_nodes.size() * 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(between_nodes.size() * 6, between_nodes.size() * 6); - Eigen::SparseMatrix AtPB(between_nodes.size() * 6, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP)*matA; - AtPB = (AtP)*matB; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - std::cout << "AtPA.size: " << AtPA.size() << std::endl; - std::cout << "AtPB.size: " << AtPB.size() << std::endl; - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - std::cout << "result: row, col, value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - //std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; - h_x.push_back(it.value()); - } - } - - if (h_x.size() == 6 * between_nodes.size()) { - int counter = 0; - - for (size_t i = 0; i < between_nodes.size(); i++) { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - - between_nodes[i].node_outer.m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - std::cout << "optimizing with tait bryan finished" << std::endl; - - for (size_t i = 0; i < between_nodes.size(); i++) { - fused_trajectory[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose = between_nodes[i].node_outer.m_pose; - } - for (size_t i = 0; i < between_nodes.size(); i++) { - for (size_t j = 0; j < between_nodes[i].nodes_between.size() - 1; j++) { - fused_trajectory[between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo].m_pose = - between_nodes[i].node_outer.m_pose * between_nodes[i].nodes_between[j].m_pose; - } - } - - fused_trajectory[fused_trajectory.size() - 1].m_pose = - between_nodes[between_nodes.size() - 1].node_outer.m_pose * - between_nodes[between_nodes.size() - 1].nodes_between[between_nodes[between_nodes.size() - 1].nodes_between.size() - 1].m_pose; - } - else { - std::cout << "optimizing with tait bryan FAILED" << std::endl; - } - - return true; + std::vector> odo_edges; + for (size_t i = 1; i < between_nodes.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); + } + // poses_desired = poses; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + // poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); + poses_desired.push_back(pose_tait_bryan_from_affine_matrix( + lidar_odometry_trajectory_initial[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose)); + } + + // poses_desired = poses; + + /*for (auto& p : poses) { + p.px += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; + p.py += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; + p.pz += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.001; + + p.om += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; + p.fi += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; + p.ka += (((rand() % 1000000) / 1000000.0f) - 0.5) * 2.0 * 0.000001; + }*/ + + for (size_t i = 0; i < odo_edges.size(); i++) + { + Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1( + relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + normalize_angle(relative_pose_measurement_odo(3, 0)), + normalize_angle(relative_pose_measurement_odo(4, 0)), + normalize_angle(relative_pose_measurement_odo(5, 0))); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0))); + tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); + tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); + + tripletListP.emplace_back(ir, ir, 10000); + tripletListP.emplace_back(ir + 1, ir + 1, 10000); + tripletListP.emplace_back(ir + 2, ir + 2, 10000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000); + } + + // gnss correspondences + for (size_t i = 0; i < between_nodes.size(); i++) + { + TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); + + for (size_t j = 0; j < between_nodes[i].nodes_between.size(); j++) + { + if (between_nodes[i].nodes_between[j].index_to_gnss != -1) + { + Eigen::Vector3d p_s( + between_nodes[i].nodes_between[j].m_pose.translation().x(), + between_nodes[i].nodes_between[j].m_pose.translation().y(), + between_nodes[i].nodes_between[j].m_pose.translation().z()); + Eigen::Vector3d p_t( + gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().x(), + gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().y(), + gnss_trajectory_shifted[between_nodes[i].nodes_between[j].index_to_gnss].m_pose.translation().z()); + + double delta_x; + double delta_y; + double delta_z; + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) > 100.0) + continue; + + Eigen::Matrix jacobian; + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z()); + + int ir = tripletListB.size(); + int ic = i * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); + } + } + } + + tripletListP.emplace_back(ir, ir, get_cauchy_w(delta_x, 10)); + tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 10)); + tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 10)); + + // tripletListP.emplace_back(ir , ir, 1); + // tripletListP.emplace_back(ir + 1, ir + 1, 1); + // tripletListP.emplace_back(ir + 2, ir + 2, 1); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + } + } + } + + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, 0, 1); + tripletListA.emplace_back(ir + 1, 1, 1); + tripletListA.emplace_back(ir + 2, 2, 1); + tripletListA.emplace_back(ir + 3, 3, 1); + tripletListA.emplace_back(ir + 4, 4, 1); + tripletListA.emplace_back(ir + 5, 5, 1); + + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), between_nodes.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(between_nodes.size() * 6, between_nodes.size() * 6); + Eigen::SparseMatrix AtPB(between_nodes.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + std::cout << "AtPA.size: " << AtPA.size() << std::endl; + std::cout << "AtPB.size: " << AtPB.size() << std::endl; + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + + std::vector h_x; + std::cout << "result: row, col, value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + // std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; + h_x.push_back(it.value()); + } + } + + if (h_x.size() == 6 * between_nodes.size()) + { + int counter = 0; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + between_nodes[i].node_outer.m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + std::cout << "optimizing with tait bryan finished" << std::endl; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + fused_trajectory[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose = between_nodes[i].node_outer.m_pose; + } + for (size_t i = 0; i < between_nodes.size(); i++) + { + for (size_t j = 0; j < between_nodes[i].nodes_between.size() - 1; j++) + { + fused_trajectory[between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo].m_pose = + between_nodes[i].node_outer.m_pose * between_nodes[i].nodes_between[j].m_pose; + } + } + + fused_trajectory[fused_trajectory.size() - 1].m_pose = between_nodes[between_nodes.size() - 1].node_outer.m_pose * + between_nodes[between_nodes.size() - 1].nodes_between[between_nodes[between_nodes.size() - 1].nodes_between.size() - 1].m_pose; + } + else + { + std::cout << "optimizing with tait bryan FAILED" << std::endl; + } + + return true; } diff --git a/core_hd_mapping/src/project_settings.cpp b/core_hd_mapping/src/project_settings.cpp index ae945c3d..3a8ecf08 100644 --- a/core_hd_mapping/src/project_settings.cpp +++ b/core_hd_mapping/src/project_settings.cpp @@ -1,439 +1,516 @@ +#include + #include +#include #include - #include -#include -#include -#include - -#include - -#include -#include #include +#include +#include + #include -#include "pfd_wrapper.hpp" +#include -void ProjectSettings::imgui(OdoWithGnssFusion& odo_with_gnss_fusion, std::vector& sectors, std::vector& rois_with_constraints, - CommonData &common_data ) +void ProjectSettings::imgui( + OdoWithGnssFusion& odo_with_gnss_fusion, + std::vector& sectors, + std::vector& rois_with_constraints, + CommonData& common_data) { - ImGui::Begin("project settings"); - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - - ImGui::Checkbox("regions of interes", &common_data.roi_exorter); - if (common_data.roi_exorter) { - common_data.odo_with_gnss_fusion = false; - common_data.single_trajectory_viewer = false; - } - - ImGui::Checkbox("laz manager", &common_data.laz_wrapper); - if (common_data.laz_wrapper) { - common_data.odo_with_gnss_fusion = false; - common_data.single_trajectory_viewer = false; - } - - ImGui::Checkbox("single trajectory viewer", &common_data.single_trajectory_viewer); - if (common_data.single_trajectory_viewer) { - common_data.odo_with_gnss_fusion = false; - } - - ImGui::Checkbox("odo with gnss fusion", &common_data.odo_with_gnss_fusion); - - - if (common_data.odo_with_gnss_fusion) { - common_data.roi_exorter = false; - common_data.laz_wrapper = false; - common_data.single_trajectory_viewer = false; - } - if (common_data.single_trajectory_viewer) { - common_data.roi_exorter = false; - common_data.laz_wrapper = false; - common_data.odo_with_gnss_fusion = false; - } - - - if (ImGui::Button("load project")) { - std::string input_file_name = ""; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Load Project", {}); - - std::cout << "project settings file: '" << input_file_name << "'" << std::endl; - if (input_file_name.size() > 0) { - load(std::filesystem::path(input_file_name).string(), sectors, rois_with_constraints, common_data); - odo_with_gnss_fusion.update_shift(common_data.shift_x, common_data.shift_y, common_data); - } - } - ImGui::SameLine(); - if (ImGui::Button("save project")) { - std::string output_file_name = ""; - output_file_name = mandeye::fd::SaveFileDialog("save project", {}); - std::cout << "project settings file to save: '" << output_file_name << "'" << std::endl; - - if (output_file_name.size() > 0) { - save(std::filesystem::path(output_file_name).string(), sectors, rois_with_constraints, common_data); - } - } - - ImGui::ColorEdit3("clear color", (float*)&clear_color); - - float old_shift_x = common_data.shift_x; - float old_shift_y = common_data.shift_y; - - ImGui::InputFloat("shift_x", &common_data.shift_x, 1.0f, 100.0f, "%.3f", 0); - ImGui::InputFloat("shift_y", &common_data.shift_y, 1.0f, 100.0f, "%.3f", 0); - - if (old_shift_x != common_data.shift_x || old_shift_y != common_data.shift_y) { - odo_with_gnss_fusion.update_shift(common_data.shift_x, common_data.shift_y, common_data); - } - - /*if (ImGui::Button("set project main folder")) { - static std::shared_ptr folder; - std::string folder_name = ""; - ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)folder); - const auto t = [&]() { - auto sel = pfd::select_folder("Choose folder", "C:\\").result(); - folder_name = sel; - std::cout << "project_main_folder: '" << folder_name << "'" << std::endl; - }; - std::thread t1(t); - t1.join(); - - if (folder_name.size() > 0) { - project_main_folder = folder_name; - } - } - if (project_main_folder.size() > 0) { - ImGui::SameLine(); - ImGui::Text(std::string("project_main_folder: " + project_main_folder).c_str() ); - }*/ - - if (ImGui::Button("add trajectory")) { - std::string input_file_name; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Choose trajectory", {}); - std::string input_file_name_mm = ""; - input_file_name_mm = mandeye::fd::OpenFileDialogOneFile("Choose motion model trajectory", {}); - - std::cout << "trajectory file: '" << input_file_name << "'" << std::endl; - std::cout << "motion model file: '" << input_file_name_mm << "'" << std::endl; - - if (input_file_name.size() > 0) { - //create motion model trajectory - - add_trajectory(input_file_name, input_file_name_mm, float(rand()%255)/256.0, float(rand() % 255) / 256.0, float(rand() % 255) / 256.0); - } - } - - for (size_t i = 0; i < trajectories.size(); i++) { - ImGui::Text(std::string(trajectories[i].trajectory_file).c_str()); - ImGui::SameLine(); - ImGui::Checkbox(("[" + std::to_string(i) + "] visible").c_str(), &trajectories[i].visible); - ImGui::SameLine(); - - ImGui::PushButtonRepeat(true); - float spacing = ImGui::GetStyle().ItemInnerSpacing.x; - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##left").c_str(), ImGuiDir_Left)) { (trajectories[i].line_width)--; } - ImGui::SameLine(0.0f, spacing); - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##right").c_str(), ImGuiDir_Right)) { (trajectories[i].line_width)++; } - ImGui::PopButtonRepeat(); - ImGui::SameLine(); - ImGui::Text("line width %d", trajectories[i].line_width); - if (trajectories[i].line_width < 1) trajectories[i].line_width = 1; - - ImVec4 color; - color.x = trajectories[i].color_x; - color.y = trajectories[i].color_y; - color.z = trajectories[i].color_z; - color.w = 1; - ImGui::ColorEdit3(("[" + std::to_string(i) + "]:trajectory color").c_str(), (float*)&color); - trajectories[i].color_x = color.x; - trajectories[i].color_y = color.y; - trajectories[i].color_z = color.z; - - //ImGui::SameLine(); - - - - } - ImGui::Text(("total_length: " + std::to_string(int(total_length)) + "[m]").c_str()); - - if (ImGui::Button("pose graph slam")) { - pose_graph_slam(rois_with_constraints); - } - - ImGui::End(); + ImGui::Begin("project settings"); + ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); + + ImGui::Checkbox("regions of interes", &common_data.roi_exorter); + if (common_data.roi_exorter) + { + common_data.odo_with_gnss_fusion = false; + common_data.single_trajectory_viewer = false; + } + + ImGui::Checkbox("laz manager", &common_data.laz_wrapper); + if (common_data.laz_wrapper) + { + common_data.odo_with_gnss_fusion = false; + common_data.single_trajectory_viewer = false; + } + + ImGui::Checkbox("single trajectory viewer", &common_data.single_trajectory_viewer); + if (common_data.single_trajectory_viewer) + { + common_data.odo_with_gnss_fusion = false; + } + + ImGui::Checkbox("odo with gnss fusion", &common_data.odo_with_gnss_fusion); + + if (common_data.odo_with_gnss_fusion) + { + common_data.roi_exorter = false; + common_data.laz_wrapper = false; + common_data.single_trajectory_viewer = false; + } + if (common_data.single_trajectory_viewer) + { + common_data.roi_exorter = false; + common_data.laz_wrapper = false; + common_data.odo_with_gnss_fusion = false; + } + + if (ImGui::Button("load project")) + { + std::string input_file_name = ""; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load Project", {}); + + std::cout << "project settings file: '" << input_file_name << "'" << std::endl; + if (input_file_name.size() > 0) + { + load(std::filesystem::path(input_file_name).string(), sectors, rois_with_constraints, common_data); + odo_with_gnss_fusion.update_shift(common_data.shift_x, common_data.shift_y, common_data); + } + } + ImGui::SameLine(); + if (ImGui::Button("save project")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("save project", {}); + std::cout << "project settings file to save: '" << output_file_name << "'" << std::endl; + + if (output_file_name.size() > 0) + { + save(std::filesystem::path(output_file_name).string(), sectors, rois_with_constraints, common_data); + } + } + + ImGui::ColorEdit3("clear color", (float*)&clear_color); + + float old_shift_x = common_data.shift_x; + float old_shift_y = common_data.shift_y; + + ImGui::InputFloat("shift_x", &common_data.shift_x, 1.0f, 100.0f, "%.3f", 0); + ImGui::InputFloat("shift_y", &common_data.shift_y, 1.0f, 100.0f, "%.3f", 0); + + if (old_shift_x != common_data.shift_x || old_shift_y != common_data.shift_y) + { + odo_with_gnss_fusion.update_shift(common_data.shift_x, common_data.shift_y, common_data); + } + + /*if (ImGui::Button("set project main folder")) { + static std::shared_ptr folder; + std::string folder_name = ""; + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, (bool)folder); + const auto t = [&]() { + auto sel = pfd::select_folder("Choose folder", "C:\\").result(); + folder_name = sel; + std::cout << "project_main_folder: '" << folder_name << "'" << std::endl; + }; + std::thread t1(t); + t1.join(); + + if (folder_name.size() > 0) { + project_main_folder = folder_name; + } + } + if (project_main_folder.size() > 0) { + ImGui::SameLine(); + ImGui::Text(std::string("project_main_folder: " + project_main_folder).c_str() ); + }*/ + + if (ImGui::Button("add trajectory")) + { + std::string input_file_name; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Choose trajectory", {}); + std::string input_file_name_mm = ""; + input_file_name_mm = mandeye::fd::OpenFileDialogOneFile("Choose motion model trajectory", {}); + + std::cout << "trajectory file: '" << input_file_name << "'" << std::endl; + std::cout << "motion model file: '" << input_file_name_mm << "'" << std::endl; + + if (input_file_name.size() > 0) + { + // create motion model trajectory + + add_trajectory( + input_file_name, input_file_name_mm, float(rand() % 255) / 256.0, float(rand() % 255) / 256.0, float(rand() % 255) / 256.0); + } + } + + for (size_t i = 0; i < trajectories.size(); i++) + { + ImGui::Text(std::string(trajectories[i].trajectory_file).c_str()); + ImGui::SameLine(); + ImGui::Checkbox(("[" + std::to_string(i) + "] visible").c_str(), &trajectories[i].visible); + ImGui::SameLine(); + + ImGui::PushButtonRepeat(true); + float spacing = ImGui::GetStyle().ItemInnerSpacing.x; + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##left").c_str(), ImGuiDir_Left)) + { + (trajectories[i].line_width)--; + } + ImGui::SameLine(0.0f, spacing); + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##right").c_str(), ImGuiDir_Right)) + { + (trajectories[i].line_width)++; + } + ImGui::PopButtonRepeat(); + ImGui::SameLine(); + ImGui::Text("line width %d", trajectories[i].line_width); + if (trajectories[i].line_width < 1) + trajectories[i].line_width = 1; + + ImVec4 color; + color.x = trajectories[i].color_x; + color.y = trajectories[i].color_y; + color.z = trajectories[i].color_z; + color.w = 1; + ImGui::ColorEdit3(("[" + std::to_string(i) + "]:trajectory color").c_str(), (float*)&color); + trajectories[i].color_x = color.x; + trajectories[i].color_y = color.y; + trajectories[i].color_z = color.z; + + // ImGui::SameLine(); + } + ImGui::Text(("total_length: " + std::to_string(int(total_length)) + "[m]").c_str()); + + if (ImGui::Button("pose graph slam")) + { + pose_graph_slam(rois_with_constraints); + } + + ImGui::End(); } -bool ProjectSettings::save(const std::string& file_name, std::vector& sectors, - const std::vector& rois_with_constraints, const CommonData& common_data) +bool ProjectSettings::save( + const std::string& file_name, + std::vector& sectors, + const std::vector& rois_with_constraints, + const CommonData& common_data) { - nlohmann::json jj; - nlohmann::json j; - - j["shift_x"] = common_data.shift_x; - j["shift_y"] = common_data.shift_y; - - j["clear_color_x"] = clear_color.x; - j["clear_color_y"] = clear_color.y; - j["clear_color_z"] = clear_color.z; - j["clear_color_w"] = clear_color.w; - - //j["project_main_folder"] = project_main_folder; - - nlohmann::json json_trajectories; - for (const auto& t : trajectories) { - - //auto new_path = std::filesystem::path(t.trajectory_file).parent_path(); - //auto file_name = std::filesystem::path(t.trajectory_file).stem(); - - //new_path /= file_name; - //new_path += "_motion_model.csv"; - - nlohmann::json trajectory{ - {"trajectory_file", t.trajectory_file}, {"trajectory_motion_model_file", t.motion_model_file_name}, {"colors",{t.color_x, t.color_y, t.color_z} } }; - json_trajectories.push_back(trajectory); - } - - j["Trajectories"] = json_trajectories; - - - nlohmann::json json_laz_sectors; - for (const auto& s : sectors) { - nlohmann::json json_sector{ {"laz_file", s.file_name}, {"local bounding box", {s.min_x, s.max_x, s.min_y, s.max_y} } }; - json_laz_sectors.push_back(json_sector); - } - j["LAZ sectors"] = json_laz_sectors; - - nlohmann::json json_rois_with_constraints; - for (const auto& rwc : rois_with_constraints) { - nlohmann::json json_rwc; - for (const auto& c : rwc.constraints) { - nlohmann::json json_roi{ - {"trajectory_name", c.trajectory_file_name}, - {"time_stamp", c.time_stamp}, - {"m_pose", {c.m_pose(0,0), c.m_pose(0,1), c.m_pose(0,2), c.m_pose(0,3), - c.m_pose(1,0), c.m_pose(1,1), c.m_pose(1,2), c.m_pose(1,3), - c.m_pose(2,0), c.m_pose(2,1), c.m_pose(2,2), c.m_pose(2,3)} } - }; - json_rwc.push_back(json_roi); - } - json_rois_with_constraints.push_back(json_rwc); - } - j["ROIs with constraints"] = json_rois_with_constraints; - - jj["Project Settings"] = j; - - std::ofstream fs(file_name); - if (!fs.good())return false; - fs << jj.dump(2); - fs.close(); - - return true; + nlohmann::json jj; + nlohmann::json j; + + j["shift_x"] = common_data.shift_x; + j["shift_y"] = common_data.shift_y; + + j["clear_color_x"] = clear_color.x; + j["clear_color_y"] = clear_color.y; + j["clear_color_z"] = clear_color.z; + j["clear_color_w"] = clear_color.w; + + // j["project_main_folder"] = project_main_folder; + + nlohmann::json json_trajectories; + for (const auto& t : trajectories) + { + // auto new_path = std::filesystem::path(t.trajectory_file).parent_path(); + // auto file_name = std::filesystem::path(t.trajectory_file).stem(); + + // new_path /= file_name; + // new_path += "_motion_model.csv"; + + nlohmann::json trajectory{ { "trajectory_file", t.trajectory_file }, + { "trajectory_motion_model_file", t.motion_model_file_name }, + { "colors", { t.color_x, t.color_y, t.color_z } } }; + json_trajectories.push_back(trajectory); + } + + j["Trajectories"] = json_trajectories; + + nlohmann::json json_laz_sectors; + for (const auto& s : sectors) + { + nlohmann::json json_sector{ { "laz_file", s.file_name }, { "local bounding box", { s.min_x, s.max_x, s.min_y, s.max_y } } }; + json_laz_sectors.push_back(json_sector); + } + j["LAZ sectors"] = json_laz_sectors; + + nlohmann::json json_rois_with_constraints; + for (const auto& rwc : rois_with_constraints) + { + nlohmann::json json_rwc; + for (const auto& c : rwc.constraints) + { + nlohmann::json json_roi{ { "trajectory_name", c.trajectory_file_name }, + { "time_stamp", c.time_stamp }, + { "m_pose", + { c.m_pose(0, 0), + c.m_pose(0, 1), + c.m_pose(0, 2), + c.m_pose(0, 3), + c.m_pose(1, 0), + c.m_pose(1, 1), + c.m_pose(1, 2), + c.m_pose(1, 3), + c.m_pose(2, 0), + c.m_pose(2, 1), + c.m_pose(2, 2), + c.m_pose(2, 3) } } }; + json_rwc.push_back(json_roi); + } + json_rois_with_constraints.push_back(json_rwc); + } + j["ROIs with constraints"] = json_rois_with_constraints; + + jj["Project Settings"] = j; + + std::ofstream fs(file_name); + if (!fs.good()) + return false; + fs << jj.dump(2); + fs.close(); + + return true; } -bool ProjectSettings::load(const std::string& file_name, std::vector& sectors, - std::vector& rois_with_constraints, CommonData& common_data) { - sectors.clear(); - rois_with_constraints.clear(); - trajectories.clear(); - - std::ifstream fs(file_name); - if (!fs.good())return false; - nlohmann::json data = nlohmann::json::parse(fs); - fs.close(); - - auto project_settings_json = data["Project Settings"]; - - clear_color.x = project_settings_json["clear_color_x"]; - clear_color.y = project_settings_json["clear_color_y"]; - clear_color.z = project_settings_json["clear_color_z"]; - clear_color.w = project_settings_json["clear_color_w"]; - common_data.shift_x = project_settings_json["shift_x"]; - common_data.shift_y = project_settings_json["shift_y"]; - //project_main_folder = project_settings_json["project_main_folder"]; - - for (const auto& t_json : project_settings_json["Trajectories"]) - { - add_trajectory(t_json["trajectory_file"], t_json["trajectory_motion_model_file"], t_json["colors"][0], t_json["colors"][1], t_json["colors"][2]); - } - - for (const auto& s_json : project_settings_json["LAZ sectors"]) - { - LAZSector s; - s.file_name = s_json["laz_file"]; - s.min_x = s_json["local bounding box"][0]; - s.max_x = s_json["local bounding box"][1]; - s.min_y = s_json["local bounding box"][2]; - s.max_y = s_json["local bounding box"][3]; - s.visible = false; - sectors.push_back(s); - } - - for (const auto& s_json : project_settings_json["ROIs with constraints"]) { - ROIwithConstraints roic; - for (const auto& r_json : s_json) { - ConstraintToGeoreference c; - c.trajectory_file_name = r_json["trajectory_name"]; - c.time_stamp = r_json["time_stamp"]; - c.m_pose = Eigen::Affine3d::Identity(); - c.m_pose(0, 0) = r_json["m_pose"][0]; - c.m_pose(0, 1) = r_json["m_pose"][1]; - c.m_pose(0, 2) = r_json["m_pose"][2]; - c.m_pose(0, 3) = r_json["m_pose"][3]; - - c.m_pose(1, 0) = r_json["m_pose"][4]; - c.m_pose(1, 1) = r_json["m_pose"][5]; - c.m_pose(1, 2) = r_json["m_pose"][6]; - c.m_pose(1, 3) = r_json["m_pose"][7]; - - c.m_pose(2, 0) = r_json["m_pose"][8]; - c.m_pose(2, 1) = r_json["m_pose"][9]; - c.m_pose(2, 2) = r_json["m_pose"][10]; - c.m_pose(2, 3) = r_json["m_pose"][11]; - roic.constraints.push_back(c); - } - rois_with_constraints.push_back(roic); - } - - //total_length - total_length = 0.0; - for (size_t i = 0; i < trajectories.size(); i++) { - for (size_t j = 1; j < trajectories[i].fused_trajectory.size(); j++) { - total_length += (trajectories[i].fused_trajectory[j].m_pose.translation() - trajectories[i].fused_trajectory[j - 1].m_pose.translation()).norm(); - } - } - - return true; +bool ProjectSettings::load( + const std::string& file_name, + std::vector& sectors, + std::vector& rois_with_constraints, + CommonData& common_data) +{ + sectors.clear(); + rois_with_constraints.clear(); + trajectories.clear(); + + std::ifstream fs(file_name); + if (!fs.good()) + return false; + nlohmann::json data = nlohmann::json::parse(fs); + fs.close(); + + auto project_settings_json = data["Project Settings"]; + + clear_color.x = project_settings_json["clear_color_x"]; + clear_color.y = project_settings_json["clear_color_y"]; + clear_color.z = project_settings_json["clear_color_z"]; + clear_color.w = project_settings_json["clear_color_w"]; + common_data.shift_x = project_settings_json["shift_x"]; + common_data.shift_y = project_settings_json["shift_y"]; + // project_main_folder = project_settings_json["project_main_folder"]; + + for (const auto& t_json : project_settings_json["Trajectories"]) + { + add_trajectory( + t_json["trajectory_file"], + t_json["trajectory_motion_model_file"], + t_json["colors"][0], + t_json["colors"][1], + t_json["colors"][2]); + } + + for (const auto& s_json : project_settings_json["LAZ sectors"]) + { + LAZSector s; + s.file_name = s_json["laz_file"]; + s.min_x = s_json["local bounding box"][0]; + s.max_x = s_json["local bounding box"][1]; + s.min_y = s_json["local bounding box"][2]; + s.max_y = s_json["local bounding box"][3]; + s.visible = false; + sectors.push_back(s); + } + + for (const auto& s_json : project_settings_json["ROIs with constraints"]) + { + ROIwithConstraints roic; + for (const auto& r_json : s_json) + { + ConstraintToGeoreference c; + c.trajectory_file_name = r_json["trajectory_name"]; + c.time_stamp = r_json["time_stamp"]; + c.m_pose = Eigen::Affine3d::Identity(); + c.m_pose(0, 0) = r_json["m_pose"][0]; + c.m_pose(0, 1) = r_json["m_pose"][1]; + c.m_pose(0, 2) = r_json["m_pose"][2]; + c.m_pose(0, 3) = r_json["m_pose"][3]; + + c.m_pose(1, 0) = r_json["m_pose"][4]; + c.m_pose(1, 1) = r_json["m_pose"][5]; + c.m_pose(1, 2) = r_json["m_pose"][6]; + c.m_pose(1, 3) = r_json["m_pose"][7]; + + c.m_pose(2, 0) = r_json["m_pose"][8]; + c.m_pose(2, 1) = r_json["m_pose"][9]; + c.m_pose(2, 2) = r_json["m_pose"][10]; + c.m_pose(2, 3) = r_json["m_pose"][11]; + roic.constraints.push_back(c); + } + rois_with_constraints.push_back(roic); + } + + // total_length + total_length = 0.0; + for (size_t i = 0; i < trajectories.size(); i++) + { + for (size_t j = 1; j < trajectories[i].fused_trajectory.size(); j++) + { + total_length += + (trajectories[i].fused_trajectory[j].m_pose.translation() - trajectories[i].fused_trajectory[j - 1].m_pose.translation()) + .norm(); + } + } + + return true; } -bool ProjectSettings::add_trajectory(const std::string& file_name, const std::string& file_motion_model_name, float color_x, float color_y, float color_z) +bool ProjectSettings::add_trajectory( + const std::string& file_name, const std::string& file_motion_model_name, float color_x, float color_y, float color_z) { - OdoWithGnssFusion trajectory; - trajectory.color_x = color_x; - trajectory.color_y = color_y; - trajectory.color_z = color_z; - trajectory.trajectory_file = file_name; - trajectory.motion_model_file_name = file_motion_model_name; + OdoWithGnssFusion trajectory; + trajectory.color_x = color_x; + trajectory.color_y = color_y; + trajectory.color_z = color_z; + trajectory.trajectory_file = file_name; + trajectory.motion_model_file_name = file_motion_model_name; - trajectory.fused_trajectory = trajectory.load_trajectory(trajectory.trajectory_file); - trajectory.fused_trajectory_motion_model = trajectory.load_trajectory(trajectory.motion_model_file_name); + trajectory.fused_trajectory = trajectory.load_trajectory(trajectory.trajectory_file); + trajectory.fused_trajectory_motion_model = trajectory.load_trajectory(trajectory.motion_model_file_name); - trajectories.push_back(trajectory); + trajectories.push_back(trajectory); - return true; + return true; } void ProjectSettings::render(const std::vector& rois_with_constraints) { - for (const auto& trj : trajectories) { - if (trj.visible) { - glColor3f(trj.color_x, trj.color_y, trj.color_z); - glLineWidth(trj.line_width); - glBegin(GL_LINE_STRIP); - for (const auto& node : trj.fused_trajectory) { - glVertex3f(node.m_pose(0, 3), node.m_pose(1, 3), node.m_pose(2, 3)); - } - glEnd(); - } - glLineWidth(1); - } - - for (const auto &rwcs: rois_with_constraints) { - for (const auto& c : rwcs.constraints) { - for (const auto& trj : trajectories) { - if (c.trajectory_file_name == trj.trajectory_file) { - auto it = std::lower_bound(trj.fused_trajectory.begin(), trj.fused_trajectory.end(), - c.time_stamp, [](Node lhs, double time) -> bool { return lhs.timestamp < time; }); - - int index = it - trj.fused_trajectory.begin(); - - if (fabs(trj.fused_trajectory[index].timestamp - c.time_stamp) < 0.1) { - glColor3f(0.1, 0.1, 0.1); - glBegin(GL_LINES); - glVertex3f(trj.fused_trajectory[index].m_pose.translation().x(), - trj.fused_trajectory[index].m_pose.translation().y(), - trj.fused_trajectory[index].m_pose.translation().z()); - - glVertex3f(c.m_pose.translation().x(), c.m_pose.translation().y(), c.m_pose.translation().z()); - glEnd(); - } - } - } - } - } + for (const auto& trj : trajectories) + { + if (trj.visible) + { + glColor3f(trj.color_x, trj.color_y, trj.color_z); + glLineWidth(trj.line_width); + glBegin(GL_LINE_STRIP); + for (const auto& node : trj.fused_trajectory) + { + glVertex3f(node.m_pose(0, 3), node.m_pose(1, 3), node.m_pose(2, 3)); + } + glEnd(); + } + glLineWidth(1); + } + + for (const auto& rwcs : rois_with_constraints) + { + for (const auto& c : rwcs.constraints) + { + for (const auto& trj : trajectories) + { + if (c.trajectory_file_name == trj.trajectory_file) + { + auto it = std::lower_bound( + trj.fused_trajectory.begin(), + trj.fused_trajectory.end(), + c.time_stamp, + [](Node lhs, double time) -> bool + { + return lhs.timestamp < time; + }); + + int index = it - trj.fused_trajectory.begin(); + + if (fabs(trj.fused_trajectory[index].timestamp - c.time_stamp) < 0.1) + { + glColor3f(0.1, 0.1, 0.1); + glBegin(GL_LINES); + glVertex3f( + trj.fused_trajectory[index].m_pose.translation().x(), + trj.fused_trajectory[index].m_pose.translation().y(), + trj.fused_trajectory[index].m_pose.translation().z()); + + glVertex3f(c.m_pose.translation().x(), c.m_pose.translation().y(), c.m_pose.translation().z()); + glEnd(); + } + } + } + } + } } void ProjectSettings::pose_graph_slam(std::vector& rois_with_constraints) { - /*for (auto& trj : trajectories) { - for (auto& node : trj.fused_trajectory) { - node.m_pose(2, 3) += 10; - std::cout << node.index_to_gnss << " "; - } - }*/ - - std::vector constraints; - - for (const auto& rwcs : rois_with_constraints) { - for (const auto& c : rwcs.constraints) { - for (auto& trj : trajectories) { - if (c.trajectory_file_name == trj.trajectory_file) { - auto it = std::lower_bound(trj.fused_trajectory.begin(), trj.fused_trajectory.end(), - c.time_stamp, [](Node lhs, double time) -> bool { return lhs.timestamp < time; }); - - int index = it - trj.fused_trajectory.begin(); - - if (fabs(trj.fused_trajectory[index].timestamp - c.time_stamp) < 0.1) { - Eigen::Affine3d constraint = c.m_pose; - trj.fused_trajectory[index].index_to_gnss = constraints.size(); - constraints.push_back(constraint); - } - } - } - } - } - - for (auto& trj : trajectories) { - std::vector bn = find_between_nodes(trj.fused_trajectory); - fuse_with_georeference(bn, constraints, trj.fused_trajectory, trj.fused_trajectory_motion_model); - //for (auto& b : bn) { - // std::cout << b.node_outer.index_to_gnss << std::endl; - //} - } - /*for (auto& trj : trajectories) { - for (auto& node : trj.fused_trajectory) { - node.m_pose(2, 3) += 10; - std::cout << node.index_to_gnss << " "; - if(node.index_to_gnss != -1) - return; - } - }*/ + /*for (auto& trj : trajectories) { + for (auto& node : trj.fused_trajectory) { + node.m_pose(2, 3) += 10; + std::cout << node.index_to_gnss << " "; + } + }*/ + + std::vector constraints; + + for (const auto& rwcs : rois_with_constraints) + { + for (const auto& c : rwcs.constraints) + { + for (auto& trj : trajectories) + { + if (c.trajectory_file_name == trj.trajectory_file) + { + auto it = std::lower_bound( + trj.fused_trajectory.begin(), + trj.fused_trajectory.end(), + c.time_stamp, + [](Node lhs, double time) -> bool + { + return lhs.timestamp < time; + }); + + int index = it - trj.fused_trajectory.begin(); + + if (fabs(trj.fused_trajectory[index].timestamp - c.time_stamp) < 0.1) + { + Eigen::Affine3d constraint = c.m_pose; + trj.fused_trajectory[index].index_to_gnss = constraints.size(); + constraints.push_back(constraint); + } + } + } + } + } + + for (auto& trj : trajectories) + { + std::vector bn = find_between_nodes(trj.fused_trajectory); + fuse_with_georeference(bn, constraints, trj.fused_trajectory, trj.fused_trajectory_motion_model); + // for (auto& b : bn) { + // std::cout << b.node_outer.index_to_gnss << std::endl; + //} + } + /*for (auto& trj : trajectories) { + for (auto& node : trj.fused_trajectory) { + node.m_pose(2, 3) += 10; + std::cout << node.index_to_gnss << " "; + if(node.index_to_gnss != -1) + return; + } + }*/ } +std::vector ProjectSettings::find_between_nodes(std::vector& fused_trajectory) +{ + std::vector between_nodes; + double dist_along = 0.0f; + // double dist_along_gnss = 0.0f; -std::vector ProjectSettings::find_between_nodes(std::vector &fused_trajectory) { - std::vector between_nodes; - double dist_along = 0.0f; - //double dist_along_gnss = 0.0f; + BetweenNode node_outer; + node_outer.node_outer.index_to_lidar_odometry_odo = 0; + node_outer.node_outer.m_pose = fused_trajectory[0].m_pose; + node_outer.node_outer.timestamp = fused_trajectory[0].timestamp; - BetweenNode node_outer; - node_outer.node_outer.index_to_lidar_odometry_odo = 0; - node_outer.node_outer.m_pose = fused_trajectory[0].m_pose; - node_outer.node_outer.timestamp = fused_trajectory[0].timestamp; + Node node_inner; - Node node_inner; - - for (size_t i = 1; i < fused_trajectory.size(); i++) { - node_inner.index_to_lidar_odometry_odo = i; - node_inner.index_to_gnss = -1; - node_inner.m_pose = fused_trajectory[i].m_pose; - node_inner.timestamp = fused_trajectory[i].timestamp; + for (size_t i = 1; i < fused_trajectory.size(); i++) + { + node_inner.index_to_lidar_odometry_odo = i; + node_inner.index_to_gnss = -1; + node_inner.m_pose = fused_trajectory[i].m_pose; + node_inner.timestamp = fused_trajectory[i].timestamp; - double dist_increment = (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); - dist_along += dist_increment; - //dist_along_gnss += dist_increment; + double dist_increment = (fused_trajectory[i].m_pose.translation() - fused_trajectory[i - 1].m_pose.translation()).norm(); + dist_along += dist_increment; + // dist_along_gnss += dist_increment; - //std::cout << "dist_increment " << dist_increment << std::endl; + // std::cout << "dist_increment " << dist_increment << std::endl; #if 0 if (dist_along_gnss > 10) { auto it = std::lower_bound(gnss_trajectory_shifted.begin(), gnss_trajectory_shifted.end(), fused_trajectory[i], @@ -462,272 +539,313 @@ std::vector ProjectSettings::find_between_nodes(std::vector & } } #endif - node_outer.nodes_between.push_back(node_inner); - - if ((dist_along > 100 || i == fused_trajectory.size() - 1) || fused_trajectory[i].index_to_gnss != -1) { - for (auto& n : node_outer.nodes_between) { - n.m_pose = node_outer.node_outer.m_pose.inverse() * n.m_pose; - } - node_outer.color_x = float(rand() % 255) / 256.0; - node_outer.color_y = float(rand() % 255) / 256.0; - node_outer.color_z = float(rand() % 255) / 256.0; - - between_nodes.push_back(node_outer); - - node_outer.nodes_between.clear(); - node_outer.node_outer.index_to_lidar_odometry_odo = i; - node_outer.node_outer.index_to_gnss = fused_trajectory[i].index_to_gnss; - node_outer.node_outer.m_pose = fused_trajectory[i].m_pose; - node_outer.node_outer.timestamp = fused_trajectory[i].timestamp; - - dist_along = 0.0; - } - } - - return between_nodes; + node_outer.nodes_between.push_back(node_inner); + + if ((dist_along > 100 || i == fused_trajectory.size() - 1) || fused_trajectory[i].index_to_gnss != -1) + { + for (auto& n : node_outer.nodes_between) + { + n.m_pose = node_outer.node_outer.m_pose.inverse() * n.m_pose; + } + node_outer.color_x = float(rand() % 255) / 256.0; + node_outer.color_y = float(rand() % 255) / 256.0; + node_outer.color_z = float(rand() % 255) / 256.0; + + between_nodes.push_back(node_outer); + + node_outer.nodes_between.clear(); + node_outer.node_outer.index_to_lidar_odometry_odo = i; + node_outer.node_outer.index_to_gnss = fused_trajectory[i].index_to_gnss; + node_outer.node_outer.m_pose = fused_trajectory[i].m_pose; + node_outer.node_outer.timestamp = fused_trajectory[i].timestamp; + + dist_along = 0.0; + } + } + + return between_nodes; } -void ProjectSettings::fuse_with_georeference(std::vector& between_nodes, const std::vector& constraints, std::vector& fused_trajectory, const std::vector& motion_model) { - std::vector> odo_edges; - for (size_t i = 1; i < between_nodes.size(); i++) { - odo_edges.emplace_back(i - 1, i); - } - - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - std::vector poses; - std::vector poses_desired; - - for (size_t i = 0; i < between_nodes.size(); i++) { - poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); - - poses_desired.push_back(pose_tait_bryan_from_affine_matrix(motion_model[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose)); - - } - - //motiuon_model - - - poses_desired = poses; - - for (size_t i = 0; i < odo_edges.size(); i++) { - Eigen::Matrix relative_pose_measurement_odo; - relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, - poses_desired[odo_edges[i].first].px, - poses_desired[odo_edges[i].first].py, - poses_desired[odo_edges[i].first].pz, - poses_desired[odo_edges[i].first].om, - poses_desired[odo_edges[i].first].fi, - poses_desired[odo_edges[i].first].ka, - poses_desired[odo_edges[i].second].px, - poses_desired[odo_edges[i].second].py, - poses_desired[odo_edges[i].second].pz, - poses_desired[odo_edges[i].second].om, - poses_desired[odo_edges[i].second].fi, - poses_desired[odo_edges[i].second].ka); - - Eigen::Matrix delta; - relative_pose_obs_eq_tait_bryan_wc_case1( - delta, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka, - relative_pose_measurement_odo(0, 0), - relative_pose_measurement_odo(1, 0), - relative_pose_measurement_odo(2, 0), - normalize_angle(relative_pose_measurement_odo(3, 0)), - normalize_angle(relative_pose_measurement_odo(4, 0)), - normalize_angle(relative_pose_measurement_odo(5, 0))); - - Eigen::Matrix jacobian; - relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, - poses[odo_edges[i].first].px, - poses[odo_edges[i].first].py, - poses[odo_edges[i].first].pz, - poses[odo_edges[i].first].om, - poses[odo_edges[i].first].fi, - poses[odo_edges[i].first].ka, - poses[odo_edges[i].second].px, - poses[odo_edges[i].second].py, - poses[odo_edges[i].second].pz, - poses[odo_edges[i].second].om, - poses[odo_edges[i].second].fi, - poses[odo_edges[i].second].ka); - - int ir = tripletListB.size(); - - int ic_1 = odo_edges[i].first * 6; - int ic_2 = odo_edges[i].second * 6; - - for (size_t row = 0; row < 6; row++) { - tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); - tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); - tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); - tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); - tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); - tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); - - tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); - tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); - tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); - tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); - tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); - tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); - } - - tripletListB.emplace_back(ir, 0, delta(0, 0)); - tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); - tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); - tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0))); - tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); - tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); - - tripletListP.emplace_back(ir, ir, 10000); - tripletListP.emplace_back(ir + 1, ir + 1, 10000); - tripletListP.emplace_back(ir + 2, ir + 2, 10000); - tripletListP.emplace_back(ir + 3, ir + 3, 10000); - tripletListP.emplace_back(ir + 4, ir + 4, 10000); - tripletListP.emplace_back(ir + 5, ir + 5, 10000); - } - - //gnss correspondences - - for (int i = 0; i < between_nodes.size(); i++) { - if (between_nodes[i].node_outer.index_to_gnss != -1) { - TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); - - const Eigen::Affine3d& m_geo = constraints[between_nodes[i].node_outer.index_to_gnss]; - - Eigen::Vector3d p_s( - //between_nodes[i].nodes_between[j].m_pose.translation().x(), - //between_nodes[i].nodes_between[j].m_pose.translation().y(), - //between_nodes[i].nodes_between[j].m_pose.translation().z()); - 0, - 0, - 0); - Eigen::Vector3d p_t( - m_geo.translation().x(), - m_geo.translation().y(), - m_geo.translation().z()); - - double delta_x; - double delta_y; - double delta_z; - point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, - pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, - p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z()); - - Eigen::Matrix jacobian; - point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, - pose_source.px, pose_source.py, pose_source.pz, pose_source.om, pose_source.fi, pose_source.ka, p_s.x(), p_s.y(), p_s.z()); - - int ir = tripletListB.size(); - int ic = i * 6; - for (int row = 0; row < 3; row++) { - for (int col = 0; col < 6; col++) { - if (jacobian(row, col) != 0.0) { - tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); - } - } - } - - tripletListP.emplace_back(ir, ir, get_cauchy_w(delta_x, 1)); - tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 1)); - tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 1)); - - //tripletListP.emplace_back(ir , ir, 1); - //tripletListP.emplace_back(ir + 1, ir + 1, 1); - //tripletListP.emplace_back(ir + 2, ir + 2, 1); - - tripletListB.emplace_back(ir , 0, delta_x); - tripletListB.emplace_back(ir + 1, 0, delta_y); - tripletListB.emplace_back(ir + 2, 0, delta_z); - - - } - } - - Eigen::SparseMatrix matA(tripletListB.size(), between_nodes.size() * 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(between_nodes.size() * 6, between_nodes.size() * 6); - Eigen::SparseMatrix AtPB(between_nodes.size() * 6, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP)*matA; - AtPB = (AtP)*matB; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - std::cout << "AtPA.size: " << AtPA.size() << std::endl; - std::cout << "AtPB.size: " << AtPB.size() << std::endl; - - std::cout << "start solving AtPA=AtPB" << std::endl; - Eigen::SimplicialCholesky> solver(AtPA); - - std::cout << "x = solver.solve(AtPB)" << std::endl; - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - std::cout << "result: row, col, value" << std::endl; - for (int k = 0; k < x.outerSize(); ++k) { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - //std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; - h_x.push_back(it.value()); - } - } - - if (h_x.size() == 6 * between_nodes.size()) { - int counter = 0; - - for (size_t i = 0; i < between_nodes.size(); i++) { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); - pose.px += h_x[counter++]; - pose.py += h_x[counter++]; - pose.pz += h_x[counter++]; - pose.om += h_x[counter++]; - pose.fi += h_x[counter++]; - pose.ka += h_x[counter++]; - - between_nodes[i].node_outer.m_pose = affine_matrix_from_pose_tait_bryan(pose); - } - std::cout << "optimizing with tait bryan finished" << std::endl; - - for (size_t i = 0; i < between_nodes.size(); i++) { - fused_trajectory[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose = between_nodes[i].node_outer.m_pose; - } - for (size_t i = 0; i < between_nodes.size(); i++) { - for (size_t j = 0; j < between_nodes[i].nodes_between.size() - 1; j++) { - fused_trajectory[between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo].m_pose = - between_nodes[i].node_outer.m_pose * between_nodes[i].nodes_between[j].m_pose; - } - } - - fused_trajectory[fused_trajectory.size() - 1].m_pose = - between_nodes[between_nodes.size() - 1].node_outer.m_pose * - between_nodes[between_nodes.size() - 1].nodes_between[between_nodes[between_nodes.size() - 1].nodes_between.size() - 1].m_pose; - } - else { - std::cout << "optimizing with tait bryan FAILED" << std::endl; - } +void ProjectSettings::fuse_with_georeference( + std::vector& between_nodes, + const std::vector& constraints, + std::vector& fused_trajectory, + const std::vector& motion_model) +{ + std::vector> odo_edges; + for (size_t i = 1; i < between_nodes.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose)); + + poses_desired.push_back( + pose_tait_bryan_from_affine_matrix(motion_model[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose)); + } + + // motiuon_model + + poses_desired = poses; + + for (size_t i = 0; i < odo_edges.size(); i++) + { + Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1( + relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + normalize_angle(relative_pose_measurement_odo(3, 0)), + normalize_angle(relative_pose_measurement_odo(4, 0)), + normalize_angle(relative_pose_measurement_odo(5, 0))); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian( + jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, normalize_angle(delta(3, 0))); + tripletListB.emplace_back(ir + 4, 0, normalize_angle(delta(4, 0))); + tripletListB.emplace_back(ir + 5, 0, normalize_angle(delta(5, 0))); + + tripletListP.emplace_back(ir, ir, 10000); + tripletListP.emplace_back(ir + 1, ir + 1, 10000); + tripletListP.emplace_back(ir + 2, ir + 2, 10000); + tripletListP.emplace_back(ir + 3, ir + 3, 10000); + tripletListP.emplace_back(ir + 4, ir + 4, 10000); + tripletListP.emplace_back(ir + 5, ir + 5, 10000); + } + + // gnss correspondences + + for (int i = 0; i < between_nodes.size(); i++) + { + if (between_nodes[i].node_outer.index_to_gnss != -1) + { + TaitBryanPose pose_source = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); + + const Eigen::Affine3d& m_geo = constraints[between_nodes[i].node_outer.index_to_gnss]; + + Eigen::Vector3d p_s( + // between_nodes[i].nodes_between[j].m_pose.translation().x(), + // between_nodes[i].nodes_between[j].m_pose.translation().y(), + // between_nodes[i].nodes_between[j].m_pose.translation().z()); + 0, + 0, + 0); + Eigen::Vector3d p_t(m_geo.translation().x(), m_geo.translation().y(), m_geo.translation().z()); + + double delta_x; + double delta_y; + double delta_z; + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + Eigen::Matrix jacobian; + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, + pose_source.px, + pose_source.py, + pose_source.pz, + pose_source.om, + pose_source.fi, + pose_source.ka, + p_s.x(), + p_s.y(), + p_s.z()); + + int ir = tripletListB.size(); + int ic = i * 6; + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, ic + col, -jacobian(row, col)); + } + } + } + + tripletListP.emplace_back(ir, ir, get_cauchy_w(delta_x, 1)); + tripletListP.emplace_back(ir + 1, ir + 1, get_cauchy_w(delta_y, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, get_cauchy_w(delta_z, 1)); + + // tripletListP.emplace_back(ir , ir, 1); + // tripletListP.emplace_back(ir + 1, ir + 1, 1); + // tripletListP.emplace_back(ir + 2, ir + 2, 1); + + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + } + } + + Eigen::SparseMatrix matA(tripletListB.size(), between_nodes.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(between_nodes.size() * 6, between_nodes.size() * 6); + Eigen::SparseMatrix AtPB(between_nodes.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + std::cout << "AtPA.size: " << AtPA.size() << std::endl; + std::cout << "AtPB.size: " << AtPB.size() << std::endl; + + std::cout << "start solving AtPA=AtPB" << std::endl; + Eigen::SimplicialCholesky> solver(AtPA); + + std::cout << "x = solver.solve(AtPB)" << std::endl; + Eigen::SparseMatrix x = solver.solve(AtPB); + + std::vector h_x; + std::cout << "result: row, col, value" << std::endl; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + // std::cout << it.row() << " " << it.col() << " " << it.value() << std::endl; + h_x.push_back(it.value()); + } + } + + if (h_x.size() == 6 * between_nodes.size()) + { + int counter = 0; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(between_nodes[i].node_outer.m_pose); + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + between_nodes[i].node_outer.m_pose = affine_matrix_from_pose_tait_bryan(pose); + } + std::cout << "optimizing with tait bryan finished" << std::endl; + + for (size_t i = 0; i < between_nodes.size(); i++) + { + fused_trajectory[between_nodes[i].node_outer.index_to_lidar_odometry_odo].m_pose = between_nodes[i].node_outer.m_pose; + } + for (size_t i = 0; i < between_nodes.size(); i++) + { + for (size_t j = 0; j < between_nodes[i].nodes_between.size() - 1; j++) + { + fused_trajectory[between_nodes[i].nodes_between[j].index_to_lidar_odometry_odo].m_pose = + between_nodes[i].node_outer.m_pose * between_nodes[i].nodes_between[j].m_pose; + } + } + + fused_trajectory[fused_trajectory.size() - 1].m_pose = between_nodes[between_nodes.size() - 1].node_outer.m_pose * + between_nodes[between_nodes.size() - 1].nodes_between[between_nodes[between_nodes.size() - 1].nodes_between.size() - 1].m_pose; + } + else + { + std::cout << "optimizing with tait bryan FAILED" << std::endl; + } } \ No newline at end of file diff --git a/core_hd_mapping/src/roi_exporter.cpp b/core_hd_mapping/src/roi_exporter.cpp index 0179164e..0f4d7d13 100644 --- a/core_hd_mapping/src/roi_exporter.cpp +++ b/core_hd_mapping/src/roi_exporter.cpp @@ -1,87 +1,106 @@ +#include + #include -#include -#include -#include -#include #include +#include +#include -#include -#include -#include -#include -#include "pfd_wrapper.hpp" +#include +#include +#include -std::vector decimate(std::vector pc) { +std::vector decimate(const std::vector& pc) +{ std::vector pc_out; double resolution_X = 0.2; double resolution_Y = 0.2; double resolution_Z = 0.2; - double min_x = 1000000000;// std::numeric_limits::max(); - double max_x = -1000000000;//std::numeric_limits::min(); + double min_x = 1000000000; // std::numeric_limits::max(); + double max_x = -1000000000; // std::numeric_limits::min(); - double min_y = 1000000000;//std::numeric_limits::max(); - double max_y = -1000000000;//std::numeric_limits::min(); + double min_y = 1000000000; // std::numeric_limits::max(); + double max_y = -1000000000; // std::numeric_limits::min(); - double min_z = 1000000000;//std::numeric_limits::max(); - double max_z = -1000000000;//std::numeric_limits::min(); + double min_z = 1000000000; // std::numeric_limits::max(); + double max_z = -1000000000; // std::numeric_limits::min(); - for (size_t i = 0; i < pc.size(); i++) { - if (pc[i].x < min_x) min_x = pc[i].x; - if (pc[i].x > max_x) max_x = pc[i].x; + for (size_t i = 0; i < pc.size(); i++) + { + if (pc[i].x < min_x) + min_x = pc[i].x; + if (pc[i].x > max_x) + max_x = pc[i].x; - if (pc[i].y < min_y) min_y = pc[i].y; - if (pc[i].y > max_y) max_y = pc[i].y; + if (pc[i].y < min_y) + min_y = pc[i].y; + if (pc[i].y > max_y) + max_y = pc[i].y; - if (pc[i].z < min_z) min_z = pc[i].z; - if (pc[i].z > max_z) max_z = pc[i].z; + if (pc[i].z < min_z) + min_z = pc[i].z; + if (pc[i].z > max_z) + max_z = pc[i].z; } - long long unsigned int number_of_buckets_X = ((max_x - min_x) / resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / resolution_Z) + 1; - - long long unsigned int number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + uint64_t number_of_buckets_X = ((max_x - min_x) / resolution_X) + 1; + uint64_t number_of_buckets_Y = ((max_y - min_y) / resolution_Y) + 1; + uint64_t number_of_buckets_Z = ((max_z - min_z) / resolution_Z) + 1; - std::vector> indexes; + uint64_t number_of_buckets = static_cast(number_of_buckets_X) * static_cast(number_of_buckets_Y) * + static_cast(number_of_buckets_Z); - for (long long unsigned int i = 0; i < pc.size(); i++) { - long long unsigned int ix = (pc[i].x - min_x) / resolution_X; - long long unsigned int iy = (pc[i].y - min_y) / resolution_Y; - long long unsigned int iz = (pc[i].z - min_z) / resolution_Z; + std::vector> indexes; - long long unsigned int index_of_bucket = ix * static_cast(number_of_buckets_Y) * - static_cast(number_of_buckets_Z) + iy * static_cast(number_of_buckets_Z) + iz; + for (uint64_t i = 0; i < pc.size(); i++) + { + uint64_t ix = (pc[i].x - min_x) / resolution_X; + uint64_t iy = (pc[i].y - min_y) / resolution_Y; + uint64_t iz = (pc[i].z - min_z) / resolution_Z; + + uint64_t index_of_bucket = ix * static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z) + + iy * static_cast(number_of_buckets_Z) + iz; indexes.emplace_back(i, index_of_bucket); } - std::sort(indexes.begin(), indexes.end(), [](std::pair& a, - const std::pair& b) { return (a.second < b.second) ; }); - - for (int i = 1; i < indexes.size(); i++) { - if (indexes[i - 1].second != indexes[i].second) { + std::sort( + indexes.begin(), + indexes.end(), + [](std::pair& a, const std::pair& b) + { + return (a.second < b.second); + }); + + for (int i = 1; i < indexes.size(); i++) + { + if (indexes[i - 1].second != indexes[i].second) + { pc_out.emplace_back(pc[indexes[i].first]); } } return pc_out; } -void get_point_cloud_for_roi_job(int i, Job* job, std::vector* pc, const ProjectSettings& project_setings, Eigen::Vector3d roi, float roi_size) { - for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { +void get_point_cloud_for_roi_job( + int i, Job* job, std::vector* pc, const ProjectSettings& project_setings, Eigen::Vector3d roi, float roi_size) +{ + for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + { SingleTrajectoryViewer tv; tv.load_fused_trajectory(project_setings.trajectories[ii].trajectory_file); - //update trajectories from project - //for(size_t i = 0 ; i < tv.trajectory_container.fused_trajectory) + // update trajectories from project + // for(size_t i = 0 ; i < tv.trajectory_container.fused_trajectory) tv.trajectory_container.fused_trajectory = project_setings.trajectories[ii].fused_trajectory; - + auto point_clouds = tv.get_point_cloud_for_roi(roi, roi_size); - - if (point_clouds.size() > 0) { - for (auto& pc : point_clouds) { + + if (point_clouds.size() > 0) + { + for (auto& pc : point_clouds) + { pc.points_global = decimate(pc.points_global); } (*pc) = point_clouds; @@ -89,53 +108,70 @@ void get_point_cloud_for_roi_job(int i, Job* job, std::vector& sectors) { - ImGui::Begin("ROI exporter"); - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); +void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_setings, std::vector& sectors) +{ + ImGui::Begin("ROI exporter"); + ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - //if (ImGui::Button("reset view (angles)")) { + // if (ImGui::Button("reset view (angles)")) { // common_data.rotate_x = 0.0; // common_data.rotate_y = 0.0; //} - //ImGui::SameLine(); + // ImGui::SameLine(); ImGui::Checkbox("ROI::is_ortho", &common_data.is_ortho); - if (common_data.is_ortho) { + if (common_data.is_ortho) + { ImGui::PushButtonRepeat(true); float spacing = ImGui::GetStyle().ItemInnerSpacing.x; - if (ImGui::ArrowButton("##roi_size_left_left", ImGuiDir_Left)) { common_data.roi_size -= 1.0; } + if (ImGui::ArrowButton("##roi_size_left_left", ImGuiDir_Left)) + { + common_data.roi_size -= 1.0; + } ImGui::SameLine(0.0f, spacing); - if (ImGui::ArrowButton("##roi_size_right_left", ImGuiDir_Right)) { common_data.roi_size += 1.0; } + if (ImGui::ArrowButton("##roi_size_right_left", ImGuiDir_Right)) + { + common_data.roi_size += 1.0; + } ImGui::PopButtonRepeat(); ImGui::SameLine(); ImGui::Text("roi size %0.1f", common_data.roi_size); - if (common_data.roi_size < 10) common_data.roi_size = 10; - if (common_data.roi_size > 100) common_data.roi_size = 100; + if (common_data.roi_size < 10) + common_data.roi_size = 10; + if (common_data.roi_size > 100) + common_data.roi_size = 100; ImGui::SameLine(); - if (ImGui::Button("remove observations inside ROI")) { - + if (ImGui::Button("remove observations inside ROI")) + { std::vector filtred_rois_with_constraints; - for (size_t i = 0; i < rois_with_constraints.size(); i++) { + for (size_t i = 0; i < rois_with_constraints.size(); i++) + { std::vector constraints; - for (size_t j = 0; j < rois_with_constraints[i].constraints.size(); j++) { + for (size_t j = 0; j < rois_with_constraints[i].constraints.size(); j++) + { Eigen::Affine3d& m = rois_with_constraints[i].constraints[j].m_pose; bool inside = false; - if ((common_data.roi.x() - common_data.roi_size < m(0,3)) && (common_data.roi.x() + common_data.roi_size > m(0, 3))) { - if ((common_data.roi.y() - common_data.roi_size < m(1, 3)) && (common_data.roi.y() + common_data.roi_size > m(1, 3))) { + if ((common_data.roi.x() - common_data.roi_size < m(0, 3)) && (common_data.roi.x() + common_data.roi_size > m(0, 3))) + { + if ((common_data.roi.y() - common_data.roi_size < m(1, 3)) && + (common_data.roi.y() + common_data.roi_size > m(1, 3))) + { inside = true; } } - if (!inside) { + if (!inside) + { constraints.push_back(rois_with_constraints[i].constraints[j]); } } std::cout << "constraints.size() " << constraints.size() << std::endl; - if (constraints.size() > 0) { + if (constraints.size() > 0) + { ROIwithConstraints roi; roi.constraints = constraints; filtred_rois_with_constraints.push_back(roi); @@ -145,35 +181,46 @@ void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_ } } - - - if (ImGui::Button("get point clouds for ROI")) { + if (ImGui::Button("get point clouds for ROI")) + { roi_point_clouds.clear(); - //georeference from LAZ + // georeference from LAZ PointCloudWithPose pc_laz = get_pc_from_laz(common_data, sectors, common_data.shift_x, common_data.shift_y); pc_laz.m_pose = Eigen::Affine3d::Identity(); - if (pc_laz.points_global.size() > 0) { + if (pc_laz.points_global.size() > 0) + { roi_point_clouds.push_back(pc_laz); } std::vector jobs = get_jobs(project_setings.trajectories.size(), num_threads); std::vector threads; - std::vector < std::vector> roi_point_clouds_per_job; + std::vector> roi_point_clouds_per_job; roi_point_clouds_per_job.resize(project_setings.trajectories.size()); - for (size_t i = 0; i < jobs.size(); i++) { - threads.push_back(std::thread(get_point_cloud_for_roi_job, i, &jobs[i], &(roi_point_clouds_per_job[i]), project_setings, common_data.roi, common_data.roi_size)); + for (size_t i = 0; i < jobs.size(); i++) + { + threads.push_back(std::thread( + get_point_cloud_for_roi_job, + i, + &jobs[i], + &(roi_point_clouds_per_job[i]), + project_setings, + common_data.roi, + common_data.roi_size)); } - for (size_t j = 0; j < threads.size(); j++) { + for (size_t j = 0; j < threads.size(); j++) + { threads[j].join(); } threads.clear(); - for (size_t i = 0; i < roi_point_clouds_per_job.size(); i++) { - if (roi_point_clouds_per_job[i].size() > 0) { + for (size_t i = 0; i < roi_point_clouds_per_job.size(); i++) + { + if (roi_point_clouds_per_job[i].size() > 0) + { roi_point_clouds.insert(roi_point_clouds.end(), roi_point_clouds_per_job[i].begin(), roi_point_clouds_per_job[i].end()); } } @@ -191,61 +238,77 @@ void RoiExporter::imgui(CommonData& common_data, const ProjectSettings& project_ ImGui::SliderInt("roi threads", &num_threads, 1, 128); ImGui::SliderInt("roi decimation", &decimation, 1, 1000); - - - if (ImGui::Button("export ROI in RESSO format")) { + if (ImGui::Button("export ROI in RESSO format")) + { export_to_RESSO_format(roi_point_clouds); } ImGui::SameLine(); - if (ImGui::Button("import ROI in RESSO format as constraints to georeference")) { + if (ImGui::Button("import ROI in RESSO format as constraints to georeference")) + { import_from_RESSO_format_as_constraints_to_georeference(); } - if (ImGui::Button("all ROI point clouds visible")) { - for (int i = 0; i < roi_point_clouds.size(); i++) { + if (ImGui::Button("all ROI point clouds visible")) + { + for (int i = 0; i < roi_point_clouds.size(); i++) + { roi_point_clouds[i].visible = true; } } ImGui::SameLine(); - if (ImGui::Button("all ROI point clouds not visible")) { - for (int i = 0; i < roi_point_clouds.size(); i++) { + if (ImGui::Button("all ROI point clouds not visible")) + { + for (int i = 0; i < roi_point_clouds.size(); i++) + { roi_point_clouds[i].visible = false; } } - for (int i = 0; i < roi_point_clouds.size(); i++) { - ImGui::Checkbox(std::string("pc[" + std::to_string(i) + "] " + roi_point_clouds[i].trajectory_file_name).c_str(), &roi_point_clouds[i].visible); + for (int i = 0; i < roi_point_clouds.size(); i++) + { + ImGui::Checkbox( + std::string("pc[" + std::to_string(i) + "] " + roi_point_clouds[i].trajectory_file_name).c_str(), &roi_point_clouds[i].visible); ImGui::SameLine(); ImGui::PushButtonRepeat(true); float spacing = ImGui::GetStyle().ItemInnerSpacing.x; - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##roi_left").c_str(), ImGuiDir_Left)) { (roi_point_clouds[i].point_size)--; } + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##roi_left").c_str(), ImGuiDir_Left)) + { + (roi_point_clouds[i].point_size)--; + } ImGui::SameLine(0.0f, spacing); - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##roi_right").c_str(), ImGuiDir_Right)) { (roi_point_clouds[i].point_size)++; } + if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##roi_right").c_str(), ImGuiDir_Right)) + { + (roi_point_clouds[i].point_size)++; + } ImGui::PopButtonRepeat(); ImGui::SameLine(); ImGui::Text("point size %d", roi_point_clouds[i].point_size); - if (roi_point_clouds[i].point_size < 1) roi_point_clouds[i].point_size = 1; - + if (roi_point_clouds[i].point_size < 1) + roi_point_clouds[i].point_size = 1; } - - ImGui::End(); + + ImGui::End(); } -void RoiExporter::render(const CommonData &common_data) { +void RoiExporter::render(const CommonData& common_data) +{ glColor3f(0, 0, 0); glBegin(GL_LINE_STRIP); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); - glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); + glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); + glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); + glVertex3f(common_data.roi.x() + common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); + glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() + common_data.roi_size, common_data.roi.z()); + glVertex3f(common_data.roi.x() - common_data.roi_size, common_data.roi.y() - common_data.roi_size, common_data.roi.z()); glEnd(); - - for (const auto& pp : roi_point_clouds) { - if (pp.visible) { + + for (const auto& pp : roi_point_clouds) + { + if (pp.visible) + { glPointSize(pp.point_size); glBegin(GL_POINTS); - for (size_t i = 0; i < pp.points_global.size(); i += decimation) { + for (size_t i = 0; i < pp.points_global.size(); i += decimation) + { const auto& p = pp.points_global[i]; glColor3f((float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0); glVertex3f(p.x, p.y, p.z); @@ -255,9 +318,11 @@ void RoiExporter::render(const CommonData &common_data) { } } - for (size_t i = 0; i < rois_with_constraints.size(); i++) { - for (size_t j = 0; j < rois_with_constraints[i].constraints.size(); j++) { - // glBegin(GL_LINES); + for (size_t i = 0; i < rois_with_constraints.size(); i++) + { + for (size_t j = 0; j < rois_with_constraints[i].constraints.size(); j++) + { + // glBegin(GL_LINES); Eigen::Affine3d& m = rois_with_constraints[i].constraints[j].m_pose; /*float scale = 10; glColor3f(1, 0, 0); @@ -295,24 +360,41 @@ PointCloudWithPose RoiExporter::get_pc_from_laz(CommonData& common_data, std::ve std::vector indexes; - for (size_t i = 0; i < sectors.size(); i++) { - if ((common_data.roi.x() - common_data.roi_size > sectors[i].min_x) && (common_data.roi.x() - common_data.roi_size < sectors[i].max_x)) { - if ((common_data.roi.y() - common_data.roi_size > sectors[i].min_y) && (common_data.roi.y() - common_data.roi_size < sectors[i].max_y)) { + for (size_t i = 0; i < sectors.size(); i++) + { + if ((common_data.roi.x() - common_data.roi_size > sectors[i].min_x) && + (common_data.roi.x() - common_data.roi_size < sectors[i].max_x)) + { + if ((common_data.roi.y() - common_data.roi_size > sectors[i].min_y) && + (common_data.roi.y() - common_data.roi_size < sectors[i].max_y)) + { indexes.push_back(i); } } - if ((common_data.roi.x() - common_data.roi_size > sectors[i].min_x) && (common_data.roi.x() - common_data.roi_size < sectors[i].max_x)) { - if ((common_data.roi.y() + common_data.roi_size > sectors[i].min_y) && (common_data.roi.y() + common_data.roi_size < sectors[i].max_y)) { + if ((common_data.roi.x() - common_data.roi_size > sectors[i].min_x) && + (common_data.roi.x() - common_data.roi_size < sectors[i].max_x)) + { + if ((common_data.roi.y() + common_data.roi_size > sectors[i].min_y) && + (common_data.roi.y() + common_data.roi_size < sectors[i].max_y)) + { indexes.push_back(i); } } - if ((common_data.roi.x() + common_data.roi_size > sectors[i].min_x) && (common_data.roi.x() + common_data.roi_size < sectors[i].max_x)) { - if ((common_data.roi.y() + common_data.roi_size > sectors[i].min_y) && (common_data.roi.y() + common_data.roi_size < sectors[i].max_y)) { + if ((common_data.roi.x() + common_data.roi_size > sectors[i].min_x) && + (common_data.roi.x() + common_data.roi_size < sectors[i].max_x)) + { + if ((common_data.roi.y() + common_data.roi_size > sectors[i].min_y) && + (common_data.roi.y() + common_data.roi_size < sectors[i].max_y)) + { indexes.push_back(i); } } - if ((common_data.roi.x() + common_data.roi_size > sectors[i].min_x) && (common_data.roi.x() + common_data.roi_size < sectors[i].max_x)) { - if ((common_data.roi.y() - common_data.roi_size > sectors[i].min_y) && (common_data.roi.y() - common_data.roi_size < sectors[i].max_y)) { + if ((common_data.roi.x() + common_data.roi_size > sectors[i].min_x) && + (common_data.roi.x() + common_data.roi_size < sectors[i].max_x)) + { + if ((common_data.roi.y() - common_data.roi_size > sectors[i].min_y) && + (common_data.roi.y() - common_data.roi_size < sectors[i].max_y)) + { indexes.push_back(i); } } @@ -322,35 +404,40 @@ PointCloudWithPose RoiExporter::get_pc_from_laz(CommonData& common_data, std::ve auto last = std::unique(indexes.begin(), indexes.end()); indexes.erase(last, indexes.end()); - for (const auto &i : indexes) { - if (sectors[i].point_cloud.size() == 0) { + for (const auto& i : indexes) + { + if (sectors[i].point_cloud.size() == 0) + { LazWrapper lw; LAZSector sector = lw.load_sector(sectors[i].file_name, shift_x, shift_y); sectors[i].point_cloud = sector.point_cloud; sectors[i].visible = false; } - for (const auto& p : sectors[i].point_cloud) { + for (const auto& p : sectors[i].point_cloud) + { float roi_size_extended = common_data.roi_size * 1.5; - if ((p.x > common_data.roi.x() - roi_size_extended) && (p.x < common_data.roi.x() + roi_size_extended)) { - if ((p.y > common_data.roi.y() - roi_size_extended) && (p.y < common_data.roi.y() + roi_size_extended)) { + if ((p.x > common_data.roi.x() - roi_size_extended) && (p.x < common_data.roi.x() + roi_size_extended)) + { + if ((p.y > common_data.roi.y() - roi_size_extended) && (p.y < common_data.roi.y() + roi_size_extended)) + { Point new_point; new_point.x = p.x; new_point.y = p.y; new_point.z = p.z; new_point.time = 0.0; - new_point.intensity = ((float(p.r) + float(p.g) + float(p.b) ) / 3.0f) *256; + new_point.intensity = ((float(p.r) + float(p.g) + float(p.b)) / 3.0f) * 256; pc.points_global.push_back(new_point); } } } } - + std::cout << pc.points_global.size() << std::endl; return pc; } -typedef std::vector > Cloud; +typedef std::vector> Cloud; void RoiExporter::export_to_RESSO_format(std::vector& roi_point_clouds) { @@ -358,10 +445,12 @@ void RoiExporter::export_to_RESSO_format(std::vector& roi_po output_file_name = mandeye::fd::SaveFileDialog("Choose file", {}); std::cout << "file to save: '" << output_file_name << "'" << std::endl; - if (output_file_name.size() > 0) { + if (output_file_name.size() > 0) + { std::ofstream outfile; outfile.open(output_file_name); - if (!outfile.good()) { + if (!outfile.good()) + { std::cout << "can not save file: " << output_file_name << std::endl; return; } @@ -369,11 +458,15 @@ void RoiExporter::export_to_RESSO_format(std::vector& roi_po m_fake = roi_point_clouds[1].m_pose.inverse(); outfile << roi_point_clouds.size() << std::endl; - for (size_t i = 0; i < roi_point_clouds.size(); i++) { - outfile << std::to_string(i)+".ply" << std::endl; - outfile << (m_fake * roi_point_clouds[i].m_pose)(0, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(0, 1) << " " << (m_fake * roi_point_clouds[i].m_pose)(0, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(0, 3) << std::endl; - outfile << (m_fake * roi_point_clouds[i].m_pose)(1, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(1, 1) << " " << (m_fake * roi_point_clouds[i].m_pose)(1, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(1, 3) << std::endl; - outfile << (m_fake * roi_point_clouds[i].m_pose)(2, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(2, 1) << " " << (m_fake * roi_point_clouds[i].m_pose)(2, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(2, 3) << std::endl; + for (size_t i = 0; i < roi_point_clouds.size(); i++) + { + outfile << std::to_string(i) + ".ply" << std::endl; + outfile << (m_fake * roi_point_clouds[i].m_pose)(0, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(0, 1) << " " + << (m_fake * roi_point_clouds[i].m_pose)(0, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(0, 3) << std::endl; + outfile << (m_fake * roi_point_clouds[i].m_pose)(1, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(1, 1) << " " + << (m_fake * roi_point_clouds[i].m_pose)(1, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(1, 3) << std::endl; + outfile << (m_fake * roi_point_clouds[i].m_pose)(2, 0) << " " << (m_fake * roi_point_clouds[i].m_pose)(2, 1) << " " + << (m_fake * roi_point_clouds[i].m_pose)(2, 2) << " " << (m_fake * roi_point_clouds[i].m_pose)(2, 3) << std::endl; outfile << "0 0 0 1" << std::endl; } outfile.close(); @@ -381,7 +474,8 @@ void RoiExporter::export_to_RESSO_format(std::vector& roi_po std::filesystem::path my_path(output_file_name); std::cout << my_path.parent_path() << std::endl; - for (size_t i = 0; i < roi_point_clouds.size(); i++) { + for (size_t i = 0; i < roi_point_clouds.size(); i++) + { std::filesystem::path pp = my_path.parent_path(); pp /= std::to_string(i) + ".ply"; std::string out_file_name = pp.string(); @@ -390,7 +484,8 @@ void RoiExporter::export_to_RESSO_format(std::vector& roi_po Cloud points; Eigen::Affine3d m_inv = ((roi_point_clouds[i].m_pose)).inverse(); - for (auto& p : roi_point_clouds[i].points_global) { + for (auto& p : roi_point_clouds[i].points_global) + { Eigen::Vector3d p_src(p.x, p.y, p.z); Eigen::Vector3d p_trg = m_inv * p_src; @@ -400,7 +495,7 @@ void RoiExporter::export_to_RESSO_format(std::vector& roi_po pp.push_back(p_trg.z()); points.push_back(pp); } - + plycpp::fromPointCloud(points, newPlyData); std::cout << "trying save to: '" << out_file_name << "'" << std::endl; @@ -414,12 +509,13 @@ void RoiExporter::import_from_RESSO_format_as_constraints_to_georeference() std::string input_file_name = ""; input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO file", {}); - if (input_file_name.size() > 0) { + if (input_file_name.size() > 0) + { std::vector m_poses; - std::ifstream infile(input_file_name); - if (!infile.good()) { + if (!infile.good()) + { std::cout << "problem with file: '" << input_file_name << "'" << std::endl; return; } @@ -432,7 +528,8 @@ void RoiExporter::import_from_RESSO_format_as_constraints_to_georeference() std::cout << "number of constraints: " << num_scans << std::endl; - for (size_t i = 0; i < num_scans; i++) { + for (size_t i = 0; i < num_scans; i++) + { std::getline(infile, line); std::istringstream iss(line); std::string point_cloud_file_name; @@ -476,13 +573,15 @@ void RoiExporter::import_from_RESSO_format_as_constraints_to_georeference() } infile.close(); - for (size_t i = 0; i < m_poses.size(); i++) { + for (size_t i = 0; i < m_poses.size(); i++) + { m_poses[i] = m_fake.inverse() * m_poses[i]; } ROIwithConstraints roi_with_constraints; - for (size_t i = 1; i < m_poses.size(); i++) { + for (size_t i = 1; i < m_poses.size(); i++) + { ConstraintToGeoreference roi_c; roi_c.m_pose = m_poses[i]; roi_c.time_stamp = roi_point_clouds[i].timestamp; diff --git a/core_hd_mapping/src/single_trajectory_viewer.cpp b/core_hd_mapping/src/single_trajectory_viewer.cpp index 90675db5..132a2296 100644 --- a/core_hd_mapping/src/single_trajectory_viewer.cpp +++ b/core_hd_mapping/src/single_trajectory_viewer.cpp @@ -1,19 +1,22 @@ #include -#include "pfd_wrapper.hpp" + +#include + #include +#include #include -#include - namespace fs = std::filesystem; -bool SingleTrajectoryViewer::load_fused_trajectory(const std::string &file_name) +bool SingleTrajectoryViewer::load_fused_trajectory(const std::string& file_name) { trajectory_filename = file_name; trajectory_container.fused_trajectory = trajectory_container.load_trajectory(trajectory_filename); - if (trajectory_container.fused_trajectory.size() > 0) { + if (trajectory_container.fused_trajectory.size() > 0) + { current_time_stamp = (trajectory_container.fused_trajectory[0].timestamp + - trajectory_container.fused_trajectory[trajectory_container.fused_trajectory.size() - 1].timestamp) * 0.5; + trajectory_container.fused_trajectory[trajectory_container.fused_trajectory.size() - 1].timestamp) * + 0.5; } working_directory = fs::path(trajectory_filename).parent_path().string(); @@ -23,7 +26,8 @@ bool SingleTrajectoryViewer::load_fused_trajectory(const std::string &file_name) bool SingleTrajectoryViewer::load_fused_trajectory() { auto selectedFile = mandeye::fd::OpenFileDialogOneFile("Choose folder", {}); - if (!selectedFile.empty()) { + if (!selectedFile.empty()) + { return load_fused_trajectory(selectedFile); } return false; @@ -34,32 +38,48 @@ std::vector SingleTrajectoryViewer::load_points_and_transform_to_global(d std::vector pp; int index_pc = -1; - for (size_t i = 0; i < chunk_files.size(); i++) { - if ((chunk_files[i].time_begin_inclusive <= ts && chunk_files[i].time_end_inclusive >= ts)) { + for (size_t i = 0; i < chunk_files.size(); i++) + { + if ((chunk_files[i].time_begin_inclusive <= ts && chunk_files[i].time_end_inclusive >= ts)) + { index_pc = i; } } - if (index_pc != -1) { + if (index_pc != -1) + { points.clear(); - for (int ind = index_pc - ext; ind <= index_pc + ext; ind++) { - if (ind >= 0 && ind < chunk_files.size()) { + for (int ind = index_pc - ext; ind <= index_pc + ext; ind++) + { + if (ind >= 0 && ind < chunk_files.size()) + { std::vector points_tmp; - load_vector_data(working_directory.c_str() + std::string("/") + std::to_string(chunk_files[ind].filename) + ".bin", points_tmp); - - for (size_t ii = 0; ii < points_tmp.size(); ii++) { - auto it = std::lower_bound(trajectory_container.fused_trajectory.begin(), trajectory_container.fused_trajectory.end(), - points_tmp[ii].time, [](Node lhs, double time) -> bool { return lhs.timestamp < time; }); + load_vector_data( + working_directory.c_str() + std::string("/") + std::to_string(chunk_files[ind].filename) + ".bin", points_tmp); + + for (size_t ii = 0; ii < points_tmp.size(); ii++) + { + auto it = std::lower_bound( + trajectory_container.fused_trajectory.begin(), + trajectory_container.fused_trajectory.end(), + points_tmp[ii].time, + [](Node lhs, double time) -> bool + { + return lhs.timestamp < time; + }); int index1 = it - trajectory_container.fused_trajectory.begin() - 1; - if (index1 < 0) { + if (index1 < 0) + { index1 = 0; } int index2 = index1 + 1; - if (index2 >= trajectory_container.fused_trajectory.size()) index2 = index1; + if (index2 >= trajectory_container.fused_trajectory.size()) + index2 = index1; - Eigen::Affine3d pi = pose_interpolation(points_tmp[ii].time, + Eigen::Affine3d pi = pose_interpolation( + points_tmp[ii].time, trajectory_container.fused_trajectory[index1].timestamp, trajectory_container.fused_trajectory[index2].timestamp, trajectory_container.fused_trajectory[index1].m_pose, @@ -68,23 +88,28 @@ std::vector SingleTrajectoryViewer::load_points_and_transform_to_global(d Eigen::Vector3d p(points_tmp[ii].x, points_tmp[ii].y, points_tmp[ii].z); Eigen::Vector3d pt = pi * p; - if (p.norm() > 10 && p.norm() < 50) { + if (p.norm() > 10 && p.norm() < 50) + { points_tmp[ii].x = pt.x(); points_tmp[ii].y = pt.y(); points_tmp[ii].z = pt.z(); - if (pt.x() >= (roi.x() - roi_size) && pt.x() <= (roi.x() + roi_size)) { - if (pt.y() >= (roi.y() - roi_size) && pt.y() <= (roi.y() + roi_size)) { + if (pt.x() >= (roi.x() - roi_size) && pt.x() <= (roi.x() + roi_size)) + { + if (pt.y() >= (roi.y() - roi_size) && pt.y() <= (roi.y() + roi_size)) + { pp.emplace_back(points_tmp[ii]); } } } - if ((trajectory_container.fused_trajectory[index1].m_pose.translation() - trajectory_container.fused_trajectory[index2].m_pose.translation()).norm() < 0.1) { + if ((trajectory_container.fused_trajectory[index1].m_pose.translation() - + trajectory_container.fused_trajectory[index2].m_pose.translation()) + .norm() < 0.1) + { ii += 10000; - //std::cout << "pp " << ii << " "; + // std::cout << "pp " << ii << " "; } - } } } @@ -92,96 +117,114 @@ std::vector SingleTrajectoryViewer::load_points_and_transform_to_global(d return pp; } -void SingleTrajectoryViewer::imgui(const CommonData& common_data){ - ImGui::Begin("single trajectory viewer"); - ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); +void SingleTrajectoryViewer::imgui(const CommonData& common_data) +{ + ImGui::Begin("single trajectory viewer"); + ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); - if (ImGui::Button("load fused trajectory")) { + if (ImGui::Button("load fused trajectory")) + { load_fused_trajectory(); } - if (trajectory_container.fused_trajectory.size() > 0) { + if (trajectory_container.fused_trajectory.size() > 0) + { double cts = current_time_stamp; - + ImGui::InputDouble("input current_time_stamp", ¤t_time_stamp, 1, 10, "%.3f"); - if (current_time_stamp < trajectory_container.fused_trajectory[0].timestamp) { + if (current_time_stamp < trajectory_container.fused_trajectory[0].timestamp) + { current_time_stamp = trajectory_container.fused_trajectory[0].timestamp; } - if (current_time_stamp >= trajectory_container.fused_trajectory[trajectory_container.fused_trajectory.size() - 1].timestamp) { + if (current_time_stamp >= trajectory_container.fused_trajectory[trajectory_container.fused_trajectory.size() - 1].timestamp) + { current_time_stamp = trajectory_container.fused_trajectory[trajectory_container.fused_trajectory.size() - 1].timestamp; } - if (cts != current_time_stamp) { + if (cts != current_time_stamp) + { points = load_points_and_transform_to_global(current_time_stamp, common_data.roi, common_data.roi_size * 100000, 5); } - if (ImGui::Button("get point clouds for roi")) { + if (ImGui::Button("get point clouds for roi")) + { point_clouds = get_point_cloud_for_roi(common_data.roi, common_data.roi_size); } } - ImGui::End(); + ImGui::End(); } -void SingleTrajectoryViewer::render() { +void SingleTrajectoryViewer::render() +{ glColor3d(0.4, 0, 1); glBegin(GL_POINTS); - for (size_t i = 0; i < trajectory_container.fused_trajectory.size(); i++) { - glVertex3f(trajectory_container.fused_trajectory[i].m_pose(0, 3), trajectory_container.fused_trajectory[i].m_pose(1, 3), trajectory_container.fused_trajectory[i].m_pose(2, 3)); + for (size_t i = 0; i < trajectory_container.fused_trajectory.size(); i++) + { + glVertex3f( + trajectory_container.fused_trajectory[i].m_pose(0, 3), + trajectory_container.fused_trajectory[i].m_pose(1, 3), + trajectory_container.fused_trajectory[i].m_pose(2, 3)); } glEnd(); glBegin(GL_POINTS); - for (const auto& p : points) { + for (const auto& p : points) + { glColor3f((float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0); glVertex3f(p.x, p.y, p.z); } glEnd(); - for (const auto& pc : point_clouds) { + for (const auto& pc : point_clouds) + { glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(pc.m_pose(0,3), pc.m_pose(1, 3), pc.m_pose(2, 3)); - glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 0), pc.m_pose(1, 3) + pc.m_pose(1, 0), pc.m_pose(2, 3) + pc.m_pose(2, 0)); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 0), pc.m_pose(1, 3) + pc.m_pose(1, 0), pc.m_pose(2, 3) + pc.m_pose(2, 0)); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); - glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 1), pc.m_pose(1, 3) + pc.m_pose(1, 1), pc.m_pose(2, 3) + pc.m_pose(2, 1)); + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 1), pc.m_pose(1, 3) + pc.m_pose(1, 1), pc.m_pose(2, 3) + pc.m_pose(2, 1)); - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); - glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 2), pc.m_pose(1, 3) + pc.m_pose(1, 2), pc.m_pose(2, 3) + pc.m_pose(2, 2)); + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(pc.m_pose(0, 3), pc.m_pose(1, 3), pc.m_pose(2, 3)); + glVertex3f(pc.m_pose(0, 3) + pc.m_pose(0, 2), pc.m_pose(1, 3) + pc.m_pose(1, 2), pc.m_pose(2, 3) + pc.m_pose(2, 2)); glEnd(); glBegin(GL_POINTS); - for (const auto& p : pc.points_global) { + for (const auto& p : pc.points_global) + { glColor3f((float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0, (float(p.intensity) + 100) / 256.0); glVertex3f(p.x, p.y, p.z); } glEnd(); - } } - -bool inside_roi(const Eigen::Affine3d& m_pose, const Eigen::Vector3d& roi, const float& roi_size) { - if (m_pose.translation().x() >= (roi.x() - roi_size) && m_pose.translation().x() <= (roi.x() + roi_size)) { - if (m_pose.translation().y() >= (roi.y() - roi_size) && m_pose.translation().y() <= (roi.y() + roi_size)) { +bool inside_roi(const Eigen::Affine3d& m_pose, const Eigen::Vector3d& roi, const float& roi_size) +{ + if (m_pose.translation().x() >= (roi.x() - roi_size) && m_pose.translation().x() <= (roi.x() + roi_size)) + { + if (m_pose.translation().y() >= (roi.y() - roi_size) && m_pose.translation().y() <= (roi.y() + roi_size)) + { return true; } } return false; } - std::vector SingleTrajectoryViewer::get_point_cloud_for_roi(Eigen::Vector3d roi, float roi_size) { std::vector pcs; - for (size_t i = 1; i < trajectory_container.fused_trajectory.size(); i++) { - if (!inside_roi(trajectory_container.fused_trajectory[i - 1].m_pose, roi, roi_size) && inside_roi(trajectory_container.fused_trajectory[i].m_pose, roi, roi_size)) { + for (size_t i = 1; i < trajectory_container.fused_trajectory.size(); i++) + { + if (!inside_roi(trajectory_container.fused_trajectory[i - 1].m_pose, roi, roi_size) && + inside_roi(trajectory_container.fused_trajectory[i].m_pose, roi, roi_size)) + { PointCloudWithPose pc; pc.m_pose = trajectory_container.fused_trajectory[i].m_pose; pc.timestamp = trajectory_container.fused_trajectory[i].timestamp; @@ -190,7 +233,8 @@ std::vector SingleTrajectoryViewer::get_point_cloud_for_roi( } } - for (auto& pc : pcs) { + for (auto& pc : pcs) + { pc.points_global = load_points_and_transform_to_global(pc.timestamp, roi, roi_size, 5); } diff --git a/external/README.md b/external/README.md new file mode 100644 index 00000000..0bb6f18e --- /dev/null +++ b/external/README.md @@ -0,0 +1 @@ +# This directory contains 3rdparty libraries that are not distributed using GitHub (or are modified) and are copied \ No newline at end of file diff --git a/core/include/WGS84toCartesian.hpp b/external/include/WGS84toCartesian.hpp similarity index 100% rename from core/include/WGS84toCartesian.hpp rename to external/include/WGS84toCartesian.hpp diff --git a/core/include/plycpp.h b/external/include/plycpp.h similarity index 99% rename from core/include/plycpp.h rename to external/include/plycpp.h index d7a1ebbd..40e1f88f 100644 --- a/core/include/plycpp.h +++ b/external/include/plycpp.h @@ -341,4 +341,4 @@ namespace plycpp plyData.push_back("vertex", vertex); } -} +} \ No newline at end of file diff --git a/core/include/wgs84_do_puwg92.h b/external/include/wgs84_do_puwg92.h similarity index 100% rename from core/include/wgs84_do_puwg92.h rename to external/include/wgs84_do_puwg92.h diff --git a/core/src/plycpp.cpp b/external/src/plycpp.cpp similarity index 99% rename from core/src/plycpp.cpp rename to external/src/plycpp.cpp index 07ef9c6f..98e44e79 100644 --- a/core/src/plycpp.cpp +++ b/external/src/plycpp.cpp @@ -641,4 +641,4 @@ namespace plycpp //throw Exception("Problem while writing binary data"); } } -} +} \ No newline at end of file diff --git a/core/src/wgs84_do_puwg92.cc b/external/src/wgs84_do_puwg92.cc similarity index 100% rename from core/src/wgs84_do_puwg92.cc rename to external/src/wgs84_do_puwg92.cc diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index df980fd9..14ad656c 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -74,11 +74,11 @@ target_include_directories(core_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ) target_include_directories(lidar_odometry_py @@ -88,11 +88,12 @@ target_include_directories(lidar_odometry_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ) target_include_directories(multi_view_tls_registration_py @@ -102,11 +103,11 @@ target_include_directories(multi_view_tls_registration_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${EXTERNAL_LIBRARIES_DIRECTORY}/Fusion/Fusion - ${EXTERNAL_LIBRARIES_DIRECTORY}/json/include - ${EXTERNAL_LIBRARIES_DIRECTORY} - ${EXTERNAL_LIBRARIES_DIRECTORY}/glm - ${EXTERNAL_LIBRARIES_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${THIRDPARTY_DIRECTORY}/observation_equations/codes ) target_link_libraries(core_py diff --git a/pybind/core_binding.cpp b/pybind/core_binding.cpp index f76e7866..72a4acaa 100644 --- a/pybind/core_binding.cpp +++ b/pybind/core_binding.cpp @@ -1,14 +1,15 @@ +#include +#include #include -#include #include -#include -#include +#include #include -#include +#include namespace py = pybind11; -PYBIND11_MODULE(core_py, m) { +PYBIND11_MODULE(core_py, m) +{ m.doc() = "Python bindings for HDMapping core"; // Bind Point3Di @@ -19,8 +20,7 @@ PYBIND11_MODULE(core_py, m) { .def_readwrite("intensity", &Point3Di::intensity) .def_readwrite("index_pose", &Point3Di::index_pose) .def_readwrite("lidarid", &Point3Di::lidarid) - .def_readwrite("index_point", &Point3Di::index_point) - ; + .def_readwrite("index_point", &Point3Di::index_point); // Bind WorkerData py::class_(m, "WorkerData") @@ -32,22 +32,22 @@ PYBIND11_MODULE(core_py, m) { .def_readwrite("intermediate_trajectory_motion_model", &WorkerData::intermediate_trajectory_motion_model) .def_readwrite("intermediate_trajectory_timestamps", &WorkerData::intermediate_trajectory_timestamps) .def_readwrite("imu_om_fi_ka", &WorkerData::imu_om_fi_ka) - .def_readwrite("show", &WorkerData::show) - ; + .def_readwrite("show", &WorkerData::show); // Bind Session py::class_(m, "Session") .def(py::init<>()) .def("load", &Session::load) .def("save", &Session::save) - .def("fill_session_from_worker_data", &Session::fill_session_from_worker_data) - ; + .def("fill_session_from_worker_data", &Session::fill_session_from_worker_data); // Bind save_all_to_las - m.def("save_all_to_las", &save_all_to_las, - py::arg("session"), - py::arg("output_las_name"), - py::arg("as_local") = false, - py::arg("skip_ts_0") = false, - "Saves point cloud data from session into a .las/.laz file."); + m.def( + "save_all_to_las", + &save_all_to_las, + py::arg("session"), + py::arg("output_las_name"), + py::arg("as_local") = false, + py::arg("skip_ts_0") = false, + "Saves point cloud data from session into a .las/.laz file."); } \ No newline at end of file diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp index 49786fd9..a58965f7 100644 --- a/pybind/lidar_odometry_binding.cpp +++ b/pybind/lidar_odometry_binding.cpp @@ -1,16 +1,17 @@ -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include #include -#include +#include namespace py = pybind11; -PYBIND11_MODULE(lidar_odometry_py, m) { +PYBIND11_MODULE(lidar_odometry_py, m) +{ m.doc() = "Python bindings for lidar odometry"; // Bind GridParameters @@ -80,7 +81,9 @@ PYBIND11_MODULE(lidar_odometry_py, m) { .def_readwrite("distance_bucket_rigid_sf", &LidarOdometryParams::distance_bucket_rigid_sf) .def_readwrite("polar_angle_deg_rigid_sf", &LidarOdometryParams::polar_angle_deg_rigid_sf) .def_readwrite("azimutal_angle_deg_rigid_sf", &LidarOdometryParams::azimutal_angle_deg_rigid_sf) - .def_readwrite("robust_and_accurate_lidar_odometry_rigid_sf_iterations", &LidarOdometryParams::robust_and_accurate_lidar_odometry_rigid_sf_iterations) + .def_readwrite( + "robust_and_accurate_lidar_odometry_rigid_sf_iterations", + &LidarOdometryParams::robust_and_accurate_lidar_odometry_rigid_sf_iterations) .def_readwrite("max_distance_lidar_rigid_sf", &LidarOdometryParams::max_distance_lidar_rigid_sf) .def_readwrite("rgd_sf_sigma_x_m", &LidarOdometryParams::rgd_sf_sigma_x_m) .def_readwrite("rgd_sf_sigma_y_m", &LidarOdometryParams::rgd_sf_sigma_y_m) @@ -106,28 +109,32 @@ PYBIND11_MODULE(lidar_odometry_py, m) { .def_readwrite("lidar_odometry_motion_model_om_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_om_1_sigma_deg) .def_readwrite("lidar_odometry_motion_model_fi_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fi_1_sigma_deg) .def_readwrite("lidar_odometry_motion_model_ka_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_ka_1_sigma_deg) - .def_readwrite("lidar_odometry_motion_model_fix_origin_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_x_1_sigma_m) - .def_readwrite("lidar_odometry_motion_model_fix_origin_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_y_1_sigma_m) - .def_readwrite("lidar_odometry_motion_model_fix_origin_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_z_1_sigma_m) - .def_readwrite("lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_om_1_sigma_deg) - .def_readwrite("lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg) - .def_readwrite("lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg) - ; + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_x_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_x_1_sigma_m) + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_y_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_y_1_sigma_m) + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_z_1_sigma_m", &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_z_1_sigma_m) + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_om_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_om_1_sigma_deg) + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg) + .def_readwrite( + "lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg", + &LidarOdometryParams::lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg); // Bind run_lidar_odometry function - m.def("run_lidar_odometry", &run_lidar_odometry, - py::arg("input_dir"), - py::arg("params"), - "Run lidar odometry with given parameters"); + m.def("run_lidar_odometry", &run_lidar_odometry, py::arg("input_dir"), py::arg("params"), "Run lidar odometry with given parameters"); // Bind run_consistency function - m.def("run_consistency", &run_consistency, - py::arg("worker_data"), - py::arg("params"), - "Run trajectory consistency."); - + m.def("run_consistency", &run_consistency, py::arg("worker_data"), py::arg("params"), "Run trajectory consistency."); + // Bind save_results_automatic function - m.def("save_results_automatic", &save_results_automatic, + m.def( + "save_results_automatic", + &save_results_automatic, py::arg("params"), py::arg("worker_data"), py::arg("working_directory"), @@ -135,20 +142,21 @@ PYBIND11_MODULE(lidar_odometry_py, m) { "Save output results from lidar odometry pipeline."); // Bind functions from utils - m.def("loadLaz", &loadLaz, + m.def( + "loadLaz", + &loadLaz, py::arg("filename"), py::arg("points_out"), py::arg("index_poses_wd"), py::arg("intermediate_trajectory"), py::arg("inverse_pose"), "Load .laz file to reconstruct WorkerData points"); - - m.def("load_poses", &load_poses, - py::arg("poses_file"), - py::arg("out_poses"), - "Load poses from file"); - m.def("load_trajectory_csv", &load_trajectory_csv, + m.def("load_poses", &load_poses, py::arg("poses_file"), py::arg("out_poses"), "Load poses from file"); + + m.def( + "load_trajectory_csv", + &load_trajectory_csv, py::arg("filename"), py::arg("m_pose"), py::arg("intermediate_trajectory_timestamps"), @@ -156,20 +164,17 @@ PYBIND11_MODULE(lidar_odometry_py, m) { py::arg("imu_om_fi_ka"), "Load trajectory CSV file"); - m.def("load_point_sizes", &load_point_sizes, - py::arg("path"), - py::arg("vector"), - "Load original point sizes from JSON file"); + m.def("load_point_sizes", &load_point_sizes, py::arg("path"), py::arg("vector"), "Load original point sizes from JSON file"); - m.def("load_index_poses", &load_index_poses, - py::arg("path"), - py::arg("vector"), - "Load index poses from JSON file"); + m.def("load_index_poses", &load_index_poses, py::arg("path"), py::arg("vector"), "Load index poses from JSON file"); - m.def("load_worker_data_from_results", - [](const fs::path& session_file) { + m.def( + "load_worker_data_from_results", + [](const fs::path& session_file) + { std::vector worker_data_out; - if (!load_worker_data_from_results(session_file, worker_data_out)) { + if (!load_worker_data_from_results(session_file, worker_data_out)) + { throw std::runtime_error("Failed to load worker data from session file: " + session_file.string()); } return worker_data_out; diff --git a/pybind/tls_registration_binding.cpp b/pybind/tls_registration_binding.cpp index 4878a52e..531a8cc1 100644 --- a/pybind/tls_registration_binding.cpp +++ b/pybind/tls_registration_binding.cpp @@ -1,17 +1,18 @@ +#include #include #include -#include -#include -#include -#include #include -#include +#include +#include +#include #include +#include namespace py = pybind11; -PYBIND11_MODULE(multi_view_tls_registration_py, m) { +PYBIND11_MODULE(multi_view_tls_registration_py, m) +{ py::class_(m, "TLSRegistration") .def(py::init<>()) .def_readwrite("use_ndt", &TLSRegistration::use_ndt) @@ -60,31 +61,26 @@ PYBIND11_MODULE(multi_view_tls_registration_py, m) { .def_readwrite("write_lidar_timestamp", &TLSRegistration::write_lidar_timestamp) .def_readwrite("write_unix_timestamp", &TLSRegistration::write_unix_timestamp) .def_readwrite("use_quaternions", &TLSRegistration::use_quaternions) - .def("set_zoller_frohlich_tls_imager_5006i_errors", + .def( + "set_zoller_frohlich_tls_imager_5006i_errors", &TLSRegistration::set_zoller_frohlich_tls_imager_5006i_errors, "Set the Zoller-Frohlich TLS Imager 5006i errors") - .def("set_zoller_frohlich_tls_imager_5010c_errors", + .def( + "set_zoller_frohlich_tls_imager_5010c_errors", &TLSRegistration::set_zoller_frohlich_tls_imager_5010c_errors, "Set the Zoller-Frohlich TLS Imager 5010c errors") - .def("set_zoller_frohlich_tls_imager_5016_errors", + .def( + "set_zoller_frohlich_tls_imager_5016_errors", &TLSRegistration::set_zoller_frohlich_tls_imager_5016_errors, "Set the Zoller-Frohlich TLS Imager 5016 errors") - .def("set_faro_focus3d_errors", - &TLSRegistration::set_faro_focus3d_errors, - "Set the FARO Focus3D errors") - .def("set_leica_scanstation_c5_c10_errors", + .def("set_faro_focus3d_errors", &TLSRegistration::set_faro_focus3d_errors, "Set the FARO Focus3D errors") + .def( + "set_leica_scanstation_c5_c10_errors", &TLSRegistration::set_leica_scanstation_c5_c10_errors, "Set the Leica ScanStation C5/C10 errors") - .def("set_riegl_vz400_errors", - &TLSRegistration::set_riegl_vz400_errors, - "Set the Riegl VZ400 errors") - .def("set_leica_hds6100_errors", - &TLSRegistration::set_leica_hds6100_errors, - "Set the Leica HDS6100 errors") - .def("set_leica_p40_errors", - &TLSRegistration::set_leica_p40_errors, - "Set the Leica P40 errors") - ; + .def("set_riegl_vz400_errors", &TLSRegistration::set_riegl_vz400_errors, "Set the Riegl VZ400 errors") + .def("set_leica_hds6100_errors", &TLSRegistration::set_leica_hds6100_errors, "Set the Leica HDS6100 errors") + .def("set_leica_p40_errors", &TLSRegistration::set_leica_p40_errors, "Set the Leica P40 errors"); py::class_(m, "ICP") .def(py::init<>()) @@ -141,32 +137,29 @@ PYBIND11_MODULE(multi_view_tls_registration_py, m) { .def_readwrite("is_fixed_pz", &PoseGraphLoopClosure::Edge::is_fixed_pz) .def_readwrite("is_fixed_om", &PoseGraphLoopClosure::Edge::is_fixed_om) .def_readwrite("is_fixed_fi", &PoseGraphLoopClosure::Edge::is_fixed_fi) - .def_readwrite("is_fixed_ka", &PoseGraphLoopClosure::Edge::is_fixed_ka); + .def_readwrite("is_fixed_ka", &PoseGraphLoopClosure::Edge::is_fixed_ka); py::class_(m, "PoseGraphLoopClosure") .def(py::init<>()) .def_readwrite("edges", &PoseGraphLoopClosure::edges) .def_readwrite("poses_motion_model", &PoseGraphLoopClosure::poses_motion_model) - .def("set_initial_poses_as_motion_model", + .def( + "set_initial_poses_as_motion_model", &PoseGraphLoopClosure::set_initial_poses_as_motion_model, "Set initial poses as motion model") - .def("set_current_poses_as_motion_model", + .def( + "set_current_poses_as_motion_model", &PoseGraphLoopClosure::set_current_poses_as_motion_model, "Set current poses as motion model") - .def("graph_slam", - &PoseGraphLoopClosure::graph_slam, - "Run graph SLAM for given point clouds, GNSS and control points") - .def("run_icp", - &PoseGraphLoopClosure::run_icp, - "Run ICP for the selected active edge") - .def("add_edge", - &PoseGraphLoopClosure::add_edge, - "Add new edge to the pose graph") - ; + .def("graph_slam", &PoseGraphLoopClosure::graph_slam, "Run graph SLAM for given point clouds, GNSS and control points") + .def("run_icp", &PoseGraphLoopClosure::run_icp, "Run ICP for the selected active edge") + .def("add_edge", &PoseGraphLoopClosure::add_edge, "Add new edge to the pose graph"); - m.def("run_multi_view_tls_registration", &run_multi_view_tls_registration, - py::arg("input_file_name"), - py::arg("tls_registration"), - py::arg("output_dir"), - "Run multi-view TLS registration with the provided input file, output file, and TLS registration configuration."); + m.def( + "run_multi_view_tls_registration", + &run_multi_view_tls_registration, + py::arg("input_file_name"), + py::arg("tls_registration"), + py::arg("output_dir"), + "Run multi-view TLS registration with the provided input file, output file, and TLS registration configuration."); } \ No newline at end of file