diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d53ebea..92a54b32 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,6 @@ 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)") set_property(CACHE HD_CPU_OPTIMIZATION PROPERTY STRINGS AUTO INTEL AMD ARM GENERIC) @@ -464,6 +463,32 @@ add_subdirectory(apps/livox_mid_360_intrinsic_calibration) add_subdirectory(apps/single_session_manual_coloring) add_subdirectory(apps/concatenate_multi_livox) +include(cmake/disable_target_warnings.cmake) + +if(NOT WIN32) + disable_target_warnings(laszip) + disable_target_warnings(laszip_api) +else() + disable_target_warnings(laszip3) + disable_target_warnings(laszip_api3) +endif() + + +if(NOT WIN32) + disable_target_warnings(tbb) + disable_target_warnings(tbbbind_2_5) +else() + disable_target_warnings(tbb) +endif() + +disable_target_warnings(freeglut) +disable_target_warnings(freeglut_static) +disable_target_warnings(libglew_static) +disable_target_warnings(libglew_shared) +disable_target_warnings(spdlog) +disable_target_warnings(vqf) +disable_target_warnings(Fusion) + # CPack configuration set(CPACK_PACKAGE_NAME "hd_mapping") set(CPACK_PACKAGE_VERSION "${HDMAPPING_VERSION_MAJOR}.${HDMAPPING_VERSION_MINOR}.${HDMAPPING_VERSION_PATCH}") diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index 0db11a6e..945e1024 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -18,7 +18,7 @@ target_include_directories( ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include) -target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB} spdlog::spdlog) +target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB} core spdlog::spdlog) add_executable(multiply_timestamps_session_trajectory_csv multiply_timestamps_session_trajectory_csv.cpp) diff --git a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index e574d07f..4de85861 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -21,9 +21,12 @@ #include "toml_io.h" #include + #include #include #include +#include + #include #include @@ -272,7 +275,7 @@ std::string formatCompletionTime(double remainingSeconds) return std::string(timeStr); } -int get_index_3d(const set& s, int k) +int get_index_3d(const std::set& s, int k) { int index = 0; for (auto u : s) @@ -1451,7 +1454,7 @@ void progress_window() { ImGui::Begin("Progress", nullptr, ImGuiWindowFlags_AlwaysAutoResize); - ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); + ImGui::Text("Working directory: %s", working_directory.c_str()); // Calculate elapsed time and ETA auto currentTime = std::chrono::system_clock::now(); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 7ffb6250..2f6cc8d3 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -1036,7 +1036,7 @@ void filter_reference_buckets(LidarOdometryParams& params) void save_trajectory_to_ascii(std::vector& worker_data, std::string output_file_name) { - ofstream file; + std::ofstream file; file.open(output_file_name); for (const auto& wd : worker_data) { @@ -1241,7 +1241,7 @@ void save_processing_results_json(const LidarOdometryParams& params, const fs::p file << results.dump(4); // Pretty print with 4-space indentation file.close(); std::cout << "Processing results saved to JSON file: " << json_path << std::endl; - std::cout << "Processing time: " << fixed << std::setprecision(2) << elapsed_seconds << " [s]" << std::endl; + std::cout << "Processing time: " << std::fixed << std::setprecision(2) << elapsed_seconds << " [s]" << std::endl; } else { diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 4035ab6f..91c03491 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -6,7 +6,6 @@ #include "lidar_odometry_utils.h" #include "toml_io.h" #include -#include #include #include diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 24c2a5a3..26d2fd96 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -21,9 +21,12 @@ #include "toml_io.h" #include + #include #include #include +#include + #include #include @@ -303,7 +306,8 @@ std::vector> get_batches_of_points(std::string laz_file, i } #endif -int get_index_3d(const set& s, int k) +// TODO(m.wlasiuk) : unify (multiple declarations ...) +int get_index_3d(const std::set& s, int k) { int index = 0; for (auto u : s) @@ -1684,7 +1688,7 @@ void progress_window() { ImGui::Begin("Progress", nullptr, ImGuiWindowFlags_AlwaysAutoResize); - ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); + ImGui::Text("Working directory: %s", working_directory.c_str()); // Calculate elapsed time and ETA auto currentTime = std::chrono::system_clock::now(); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 09123ae5..95500e8d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -8,6 +8,8 @@ #include +#include + #include namespace fs = std::filesystem; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index fe0beebf..4e0c2567 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -12,7 +12,6 @@ #include #include -#include #include #include 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 c4bb4b09..cb8578b0 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 @@ -28,6 +28,8 @@ #include #include +#include + #include #define SAMPLE_PERIOD (1.0 / 200.0) 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 e9e05e6f..d2e37322 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -12,6 +12,8 @@ #include #include +#include + #include #include 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 294b384a..508affb8 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1265,7 +1265,7 @@ void imu_data_gui() output_file_name = mandeye::fd::SaveFileDialog("Save IMU data", {}, ""); spdlog::info("file to save: '{}'", output_file_name); - ofstream file; + std::ofstream file; file.open(output_file_name); file << std::setprecision(20); for (size_t i = 0; i < imu_data_plot.timestampLidar.size(); i++) @@ -1800,7 +1800,7 @@ void display() { ImGui::BeginTooltip(); std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; - ImGui::Text(fn.c_str()); + ImGui::Text("%s", fn.c_str()); ImGui::EndTooltip(); } ImGui::SameLine(); @@ -1809,7 +1809,7 @@ void display() { ImGui::BeginTooltip(); std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; - ImGui::Text(fn.c_str()); + ImGui::Text("%s", fn.c_str()); ImGui::EndTooltip(); } ImGui::PopItemWidth(); 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 fa339884..401d7652 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -756,15 +756,15 @@ void openSession() void session_gui() { ImGui::Text("File name:"); - ImGui::Text(session.session_file_name.c_str()); + ImGui::Text("%s", session.session_file_name.c_str()); ImGui::Text("Working directory:"); - ImGui::Text(session.working_directory.c_str()); + ImGui::Text("%s", session.working_directory.c_str()); ImGui::Separator(); ImGui::Text("Is ground truth: %s", session.is_ground_truth ? "yes" : "no"); ImGui::Text("Number of clouds: %zu", session.point_clouds_container.point_clouds.size()); - ImGui::Text("Number of points: %zu", session_total_number_of_points); + ImGui::Text("Number of points: %d", session_total_number_of_points); ImGui::Separator(); @@ -848,9 +848,9 @@ void session_gui() void index_gui() { ImGui::Text("File name:"); - ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); + ImGui::Text("%s", session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - ImGui::Text("Current index: %zu / %zu", index_rendered_points_local, session.point_clouds_container.point_clouds.size()); + ImGui::Text("Current index: %d / %zu", index_rendered_points_local, session.point_clouds_container.point_clouds.size()); ImGui::Separator(); @@ -1319,7 +1319,7 @@ void display() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); + ImGui::Text("%s", 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; @@ -1333,7 +1333,7 @@ void display() if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); + ImGui::Text("%s", 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; diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 2c187642..20da7f99 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -637,8 +637,8 @@ void TimeStampCount() } ImGui::Text("Packed uint32_t: %u", packedValue); - ImGui::Text("Decoded timestamp: %llu", decodedValue); - ImGui::Text("Timestamp: %llu", fullTimestamp); + ImGui::Text("Decoded timestamp: %zu", decodedValue); + ImGui::Text("Timestamp: %zu", fullTimestamp); double timestampSeconds = static_cast(fullTimestamp) / 1'000'000'000.0; ImGui::Text("Timestamp (formatted): %.6f", timestampSeconds); @@ -1264,7 +1264,7 @@ void display() { auto index = std::distance(SystemData::pointPickedImage.begin(), it); const auto& p = *it; - ImGui::Text("%d : %.1f,%.1f", index, p.x, p.y); + ImGui::Text("%d : %.1f,%.1f", (int)index, p.x, p.y); ImGui::SameLine(); const auto label = std::string("-##2s") + std::to_string(index); if (ImGui::Button(label.c_str())) diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index ace798f7..99dc0276 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -400,7 +400,7 @@ void loop_closure_gui() } std::string number_active_edges = "number_edges: " + std::to_string(edges.size()); - ImGui::Text(number_active_edges.c_str()); + ImGui::Text("%s", number_active_edges.c_str()); if (edges.size() > 0) { ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge); @@ -437,13 +437,13 @@ void loop_closure_gui() } std::string txt = "index_session_from: " + std::to_string(edges[index_active_edge].index_session_from); - ImGui::Text(txt.c_str()); + ImGui::Text("%s", txt.c_str()); txt = "index_session_to: " + std::to_string(edges[index_active_edge].index_session_to); - ImGui::Text(txt.c_str()); + ImGui::Text("%s", txt.c_str()); txt = "index_from: " + std::to_string(edges[index_active_edge].index_from); - ImGui::Text(txt.c_str()); + ImGui::Text("%s", txt.c_str()); txt = "index_to: " + std::to_string(edges[index_active_edge].index_to); - ImGui::Text(txt.c_str()); + ImGui::Text("%s", txt.c_str()); if (remove_edge_index != -1) { @@ -2309,9 +2309,9 @@ void settings_gui() for (size_t i = 0; i < project_settings.session_file_names.size(); i++) { - ImGui::Text(truncPath(project_settings.session_file_names[i]).c_str()); + ImGui::Text("%s", truncPath(project_settings.session_file_names[i]).c_str()); if (ImGui::IsItemHovered()) - ImGui::SetTooltip(project_settings.session_file_names[i].c_str()); + ImGui::SetTooltip("%s", project_settings.session_file_names[i].c_str()); if (project_settings.session_file_names.size() == sessions.size()) { 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 158a254c..5146b61a 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 @@ -795,13 +795,7 @@ void pose_graph_slam_gui() 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]; - // double rms_initial = 0.0; - // double rms_final = 0.0; - // 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); - // spdlog::info("mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << - // std::endl; } #if WITH_GTSAM @@ -816,8 +810,6 @@ 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); - // spdlog::info("mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << - // std::endl; } #endif @@ -893,7 +885,7 @@ void observation_picking_gui() } ImGui::EndDisabled(); - ImGui::Text((std::string("Number of observations: ") + std::to_string(observation_picking.observations.size())).c_str()); + ImGui::Text("Number of observations: %zu", observation_picking.observations.size()); if (ImGui::Button("Load observations")) { @@ -934,7 +926,7 @@ void observation_picking_gui() observation_picking.export_observation(output_file_name); } - ImGui::Text((std::string("Loaded observations from file: '") + observations_file_name + std::string("'")).c_str()); + ImGui::Text("Loaded observations from file: %s", observations_file_name.c_str()); if (ImGui::Button("Compute RMS (xy)")) { @@ -1829,7 +1821,7 @@ void settings_gui() ImGui::Begin("Settings", &is_settings_gui); { std::string wd = "Working directory: '" + session.working_directory + "'"; - ImGui::Text(wd.c_str()); + ImGui::Text("%s", wd.c_str()); ImGui::NewLine(); @@ -1999,7 +1991,7 @@ void settings_gui() } } ImGui::SameLine(); - ImGui::Text(session.point_clouds_container.initial_poses_file_name.c_str()); + ImGui::Text("%s", session.point_clouds_container.initial_poses_file_name.c_str()); if (ImGui::Button("Update poses from RESSO file")) { @@ -2047,7 +2039,7 @@ void settings_gui() } } ImGui::SameLine(); - ImGui::Text(session.point_clouds_container.poses_file_name.c_str()); + ImGui::Text("%s", session.point_clouds_container.poses_file_name.c_str()); ImGui::Separator(); @@ -3258,9 +3250,9 @@ void display() { ImGui::BeginTooltip(); ImGui::Text("Loaded session:"); - ImGui::Text(std::string(session.session_file_name).c_str()); + ImGui::Text("%s", session.session_file_name.c_str()); ImGui::Separator(); - ImGui::Text("Total number of points: %zu", session_total_number_of_points); + ImGui::Text("Total number of points: %d", session_total_number_of_points); if (ImGui::BeginTable("Dimensions", 4)) { diff --git a/cmake/disable_target_warnings.cmake b/cmake/disable_target_warnings.cmake new file mode 100644 index 00000000..21dfe95b --- /dev/null +++ b/cmake/disable_target_warnings.cmake @@ -0,0 +1,29 @@ +include_guard() + +function(disable_target_warnings target_name) + if(NOT TARGET ${target_name}) + message(FATAL_ERROR "Target ${target_name} does not exist") + endif() + + get_target_property(target_type ${target_name} TYPE) + + if(target_type STREQUAL "INTERFACE_LIBRARY") + if(MSVC) + target_compile_options(${target_name} INTERFACE /W0) + set(_warn_flag "/W0") + else() + target_compile_options(${target_name} INTERFACE -w) + set(_warn_flag "-w") + endif() + else() + if(MSVC) + target_compile_options(${target_name} PRIVATE /W0) + set(_warn_flag "/W0") + else() + target_compile_options(${target_name} PRIVATE -w) + set(_warn_flag "-w") + endif() + endif() + + message(STATUS "Disabled warnings for target '${target_name}' using flag '${_warn_flag}'") +endfunction() \ No newline at end of file diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 46660c97..f64a5c11 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -5,6 +5,7 @@ project(core) set(CORE_BASE_SOURCES src/color_las_loader.cpp src/control_points.cpp + src/export_laz.cpp src/gnss.cpp src/ground_control_points.cpp src/hash_utils.cpp diff --git a/core/include/Core/color_las_loader.h b/core/include/Core/color_las_loader.h index 11fa7915..ab734703 100644 --- a/core/include/Core/color_las_loader.h +++ b/core/include/Core/color_las_loader.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include namespace mandeye diff --git a/core/include/Core/export_laz.h b/core/include/Core/export_laz.h index 9ace206f..ed138203 100644 --- a/core/include/Core/export_laz.h +++ b/core/include/Core/export_laz.h @@ -9,349 +9,25 @@ #include #include -#include -#include - namespace fs = std::filesystem; -class LazWriter -{ -public: - LazWriter() = default; - ~LazWriter() - { - if (writer_) - close(); - } - - LazWriter(const LazWriter&) = delete; - LazWriter& operator=(const LazWriter&) = delete; - - bool open(const std::string& filename, double offset_x, double offset_y, double offset_z) - { - constexpr double scale = 0.0001; - - if (laszip_create(&writer_)) - { - fprintf(stderr, "DLL ERROR: creating laszip writer\n"); - return false; - } - - laszip_header* header; - if (laszip_get_header_pointer(writer_, &header)) - { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n"); - return false; - } - - header->file_source_ID = 4711; - header->global_encoding = (1 << 0); - header->version_major = 1; - header->version_minor = 2; - header->point_data_format = 1; - header->point_data_record_length = 28; - header->number_of_point_records = 0; - header->number_of_points_by_return[0] = 0; - header->number_of_points_by_return[1] = 0; - header->x_scale_factor = scale; - header->y_scale_factor = scale; - header->z_scale_factor = scale; - header->x_offset = offset_x; - header->y_offset = offset_y; - header->z_offset = offset_z; - - laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); - if (laszip_open_writer(writer_, filename.c_str(), compress)) - { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str()); - return false; - } - - fprintf(stderr, "writing %scompressed file '%s'\n", (compress ? "" : "un"), filename.c_str()); - - if (laszip_get_point_pointer(writer_, &point_)) - { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n"); - return false; - } - - return true; - } - - bool writePoint( - const Eigen::Vector3d& position, unsigned short intensity, double timestamp, double offset_x, double offset_y, double offset_z) - { - point_->intensity = intensity; - point_->return_number = 1; - point_->number_of_returns = 1; - point_->gps_time = timestamp * 1e9; - - laszip_F64 coordinates[3]; - coordinates[0] = position.x() + offset_x; - coordinates[1] = position.y() + offset_y; - coordinates[2] = position.z() + offset_z; - - if (laszip_set_coordinates(writer_, coordinates)) - { - fprintf(stderr, "DLL ERROR: setting coordinates\n"); - return false; - } - - if (laszip_write_point(writer_)) - { - fprintf(stderr, "DLL ERROR: writing point\n"); - return false; - } - - if (laszip_update_inventory(writer_)) - { - fprintf(stderr, "DLL ERROR: updating inventory\n"); - return false; - } - - return true; - } - - bool close() - { - if (!writer_) - return true; - - laszip_I64 p_count = 0; - if (laszip_get_point_count(writer_, &p_count)) - { - fprintf(stderr, "DLL ERROR: getting point count\n"); - return false; - } - - fprintf(stderr, "successfully written %lld points\n", p_count); - - if (laszip_close_writer(writer_)) - { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); - return false; - } - - if (laszip_destroy(writer_)) - { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); - return false; - } - - writer_ = nullptr; - point_ = nullptr; - return true; - } - -private: - laszip_POINTER writer_ = nullptr; - laszip_point* point_ = nullptr; -}; - -inline bool exportLaz( +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) -{ - LazWriter writer; - if (!writer.open(filename, offset_x, offset_y, offset_alt)) - return false; - - for (size_t i = 0; i < pointcloud.size(); i++) - { - if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt)) - return false; - } + double offset_alt = 0.0); - return writer.close(); -} - -inline void save_processed_pc( +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; - - laszip_POINTER laszip_reader; - if (laszip_create(&laszip_reader)) - { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); - std::abort(); - } - - laszip_POINTER laszip_writer; - if (laszip_create(&laszip_writer)) - { - fprintf(stderr, "DLL ERROR: creating laszip reader\n"); - std::abort(); - } - - const std::string file_name_in = file_path_in.string(); - const std::string file_name_out = file_path_put.string(); - - laszip_BOOL is_compressed = 0; - if (laszip_open_reader(laszip_reader, file_name_in.c_str(), &is_compressed)) - { - fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", file_name_in.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(); - } - - std::cout << "header min before:" << std::endl; - std::cout << header->min_x << " " << header->min_y << " " << header->min_z << std::endl; + bool override_compressed = false); - std::cout << "header max before:" << std::endl; - std::cout << header->max_x << " " << header->max_y << " " << header->max_z << std::endl; - - std::cout << "header before:" << std::endl; - std::cout << header->x_offset << " " << header->y_offset << " " << header->z_offset << std::endl; - - header->x_offset = offset.x(); - header->y_offset = offset.y(); - header->z_offset = offset.z(); - - if (laszip_set_header(laszip_writer, header)) - { - fprintf(stderr, "DLL ERROR: setting header pointer from laszip reader\n"); - std::abort(); - } - - fprintf(stderr, "file '%s' contains %u points\n", file_name_in.c_str(), header->number_of_point_records); - - if (override_compressed) - { - is_compressed = 0; - std::cout << "compressed : " << is_compressed << std::endl; - } - - if (laszip_open_writer(laszip_writer, file_name_out.c_str(), is_compressed)) - { - fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", file_name_out.c_str()); - return; - } - - 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; - if (laszip_get_point_pointer(laszip_writer, &output_point)) - { - fprintf(stderr, "DLL ERROR: getting point pointer from laszip reader\n"); - std::abort(); - } - - for (uint32_t i = 0; i < header->number_of_point_records; i++) - { - if (laszip_read_point(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: reading point %u\n", i); - std::abort(); - } - - laszip_F64 input_coordinates[3]; - if (laszip_get_coordinates(laszip_reader, input_coordinates)) - { - fprintf(stderr, "DLL ERROR: laszip_set_coordinates %u\n", i); - std::abort(); - } - - Eigen::Vector3d pg = Eigen::Vector3d(input_coordinates[0], input_coordinates[1], input_coordinates[2]); - pg = m_pose * pg; - // adjustPoint(input_coordinates, m_pose); - laszip_F64 output_coordinates[3]; - output_coordinates[0] = pg.x(); - output_coordinates[1] = pg.y(); - output_coordinates[2] = pg.z(); - - if (laszip_set_coordinates(laszip_writer, output_coordinates)) - // if (laszip_set_coordinates(laszip_writer, input_coordinates)) - { - fprintf(stderr, "DLL ERROR: laszip_set_coordinates %u\n", i); - std::abort(); - } - output_point->intensity = input_point->intensity; - output_point->classification = input_point->classification; - output_point->num_extra_bytes = input_point->num_extra_bytes; - output_point->gps_time = input_point->gps_time; - memcpy(output_point->extra_bytes, input_point->extra_bytes, output_point->num_extra_bytes); - - if (laszip_write_point(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: writing point %u\n", i); - return; - } - - if (laszip_update_inventory(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: updating inventory for point %u\n", i); - return; - } - } - - // close the reader - if (laszip_close_reader(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: closing laszip reader\n"); - return; - } - - // destroy the reader - - if (laszip_destroy(laszip_reader)) - { - fprintf(stderr, "DLL ERROR: destroying laszip reader\n"); - return; - } - - laszip_I64 p_count{ 0 }; - if (laszip_get_point_count(laszip_writer, &p_count)) - { - fprintf(stderr, "DLL ERROR: getting point count\n"); - return; - } - - fprintf(stderr, "successfully written %lld points\n", p_count); - - // close the writer - - if (laszip_close_writer(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: closing laszip writer\n"); - return; - } - - // destroy the writer - - // ToDo --> solve it - if (laszip_destroy(laszip_writer)) - { - fprintf(stderr, "DLL ERROR: destroying laszip writer\n"); - return; - } - - std::cout << "saving to " << file_path_put << std::endl; -} - -inline void points_to_vector( +void points_to_vector( const std::vector points, std::vector& trajectory, double threshold_output_filter, @@ -360,79 +36,6 @@ inline void points_to_vector( 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) - { - Point3Di p = org_p; - if (p.point.norm() > threshold_output_filter) - { - if (use_first_pose) - { - p.point = m_pose * (trajectory[org_p.index_pose] * org_p.point); - } - else - { - p.point = trajectory[org_p.index_pose] * org_p.point; - } - pointcloud.push_back(p.point); - intensity.push_back(p.intensity); - timestamps.push_back(p.timestamp); - if (save_index_pose) - { - if (index_poses) - { - index_poses->push_back(org_p.index_pose); - } - } - } - } -} - -inline void save_all_to_las(const Session& session, std::string output_las_name, bool as_local, bool skip_ts_0) -{ - Eigen::Affine3d first_pose = Eigen::Affine3d::Identity(); - bool found_first_pose = false; - - const auto& offset = session.point_clouds_container.offset; - LazWriter writer; - if (!writer.open(output_las_name, offset.x(), offset.y(), offset.z())) - { - std::cout << "problem with saving file: " << output_las_name << std::endl; - return; - } - - for (const auto& p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - if (!found_first_pose) - { - found_first_pose = true; - first_pose = p.m_pose; - } - - for (size_t i = 0; i < p.points_local.size(); ++i) - { - if (skip_ts_0) - { - if (i >= p.timestamps.size() || p.timestamps[i] == 0.0) - continue; - } - - Eigen::Vector3d vp = p.m_pose * p.points_local[i]; - if (as_local) - vp = first_pose * vp; - - unsigned short inten = (i < p.intensities.size()) ? p.intensities[i] : 0; - double ts = (i < p.timestamps.size()) ? p.timestamps[i] : 0.0; - - if (!writer.writePoint(vp, inten, ts, offset.x(), offset.y(), offset.z())) - return; - } - } - } + bool save_index_pose); - writer.close(); -} +void save_all_to_las(const Session& session, std::string output_las_name, bool as_local, bool skip_ts_0); \ No newline at end of file diff --git a/core/src/color_las_loader.cpp b/core/src/color_las_loader.cpp index d74fafae..53018bf6 100644 --- a/core/src/color_las_loader.cpp +++ b/core/src/color_las_loader.cpp @@ -2,6 +2,8 @@ #include "Core/color_las_loader.h" +#include + #include std::vector mandeye::load(const std::string& lazFile) diff --git a/core/src/export_laz.cpp b/core/src/export_laz.cpp new file mode 100644 index 00000000..c567f8db --- /dev/null +++ b/core/src/export_laz.cpp @@ -0,0 +1,437 @@ +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include + +namespace fs = std::filesystem; + +class LazWriter +{ +public: + LazWriter() = default; + ~LazWriter() + { + if (writer_) + close(); + } + + LazWriter(const LazWriter&) = delete; + LazWriter& operator=(const LazWriter&) = delete; + + bool open(const std::string& filename, double offset_x, double offset_y, double offset_z) + { + constexpr double scale = 0.0001; + + if (laszip_create(&writer_)) + { + spdlog::error("DLL ERROR: creating laszip writer"); + return false; + } + + laszip_header* header; + if (laszip_get_header_pointer(writer_, &header)) + { + spdlog::error("DLL ERROR: getting header pointer from laszip writer"); + return false; + } + + header->file_source_ID = 4711; + header->global_encoding = (1 << 0); + header->version_major = 1; + header->version_minor = 2; + header->point_data_format = 1; + header->point_data_record_length = 28; + header->number_of_point_records = 0; + header->number_of_points_by_return[0] = 0; + header->number_of_points_by_return[1] = 0; + header->x_scale_factor = scale; + header->y_scale_factor = scale; + header->z_scale_factor = scale; + header->x_offset = offset_x; + header->y_offset = offset_y; + header->z_offset = offset_z; + + laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); + if (laszip_open_writer(writer_, filename.c_str(), compress)) + { + spdlog::error("DLL ERROR: opening laszip writer for '{}'", filename); + return false; + } + + spdlog::info("writing {} compressed file '{}'\n", (compress ? "" : "un"), filename); + + if (laszip_get_point_pointer(writer_, &point_)) + { + spdlog::error("DLL ERROR: getting point pointer from laszip writer"); + return false; + } + + return true; + } + + bool writePoint( + const Eigen::Vector3d& position, unsigned short intensity, double timestamp, double offset_x, double offset_y, double offset_z) + { + point_->intensity = intensity; + point_->return_number = 1; + point_->number_of_returns = 1; + point_->gps_time = timestamp * 1e9; + + laszip_F64 coordinates[3]; + coordinates[0] = position.x() + offset_x; + coordinates[1] = position.y() + offset_y; + coordinates[2] = position.z() + offset_z; + + if (laszip_set_coordinates(writer_, coordinates)) + { + spdlog::error("DLL ERROR: setting coordinates"); + return false; + } + + if (laszip_write_point(writer_)) + { + spdlog::error("DLL ERROR: writing point"); + return false; + } + + if (laszip_update_inventory(writer_)) + { + spdlog::error("DLL ERROR: updating inventory"); + return false; + } + + return true; + } + + bool close() + { + if (!writer_) + return true; + + laszip_I64 p_count = 0; + if (laszip_get_point_count(writer_, &p_count)) + { + spdlog::error("DLL ERROR: getting point count"); + return false; + } + + spdlog::error("successfully written {} points", p_count); + + if (laszip_close_writer(writer_)) + { + spdlog::error("DLL ERROR: closing laszip writer"); + return false; + } + + if (laszip_destroy(writer_)) + { + spdlog::error("DLL ERROR: destroying laszip writer"); + return false; + } + + writer_ = nullptr; + point_ = nullptr; + return true; + } + +private: + laszip_POINTER writer_ = nullptr; + laszip_point* point_ = nullptr; +}; + +bool exportLaz( + const std::string& filename, + const std::vector& pointcloud, + const std::vector& intensity, + const std::vector& timestamps, + double offset_x, + double offset_y, + double offset_alt) +{ + LazWriter writer; + if (!writer.open(filename, offset_x, offset_y, offset_alt)) + return false; + + for (size_t i = 0; i < pointcloud.size(); i++) + { + if (!writer.writePoint(pointcloud[i], intensity[i], timestamps[i], offset_x, offset_y, offset_alt)) + return false; + } + + return writer.close(); +} + +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) +{ + std::cout << "processing: " << file_path_in << std::endl; + + laszip_POINTER laszip_reader; + if (laszip_create(&laszip_reader)) + { + spdlog::error("DLL ERROR: creating laszip reader"); + std::abort(); + } + + laszip_POINTER laszip_writer; + if (laszip_create(&laszip_writer)) + { + spdlog::error("DLL ERROR: creating laszip reader"); + std::abort(); + } + + const std::string file_name_in = file_path_in.string(); + const std::string file_name_out = file_path_put.string(); + + laszip_BOOL is_compressed = 0; + if (laszip_open_reader(laszip_reader, file_name_in.c_str(), &is_compressed)) + { + spdlog::error("DLL ERROR: opening laszip reader for '{}'", file_name_in); + std::abort(); + } + std::cout << "compressed : " << is_compressed << std::endl; + + laszip_header* header; + + if (laszip_get_header_pointer(laszip_reader, &header)) + { + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); + std::abort(); + } + + std::cout << "header min before:" << std::endl; + std::cout << header->min_x << " " << header->min_y << " " << header->min_z << std::endl; + + std::cout << "header max before:" << std::endl; + std::cout << header->max_x << " " << header->max_y << " " << header->max_z << std::endl; + + std::cout << "header before:" << std::endl; + std::cout << header->x_offset << " " << header->y_offset << " " << header->z_offset << std::endl; + + header->x_offset = offset.x(); + header->y_offset = offset.y(); + header->z_offset = offset.z(); + + if (laszip_set_header(laszip_writer, header)) + { + spdlog::error("DLL ERROR: setting header pointer from laszip reader"); + std::abort(); + } + + spdlog::info("file '{}' contains {} points", file_name_in, header->number_of_point_records); + + if (override_compressed) + { + is_compressed = 0; + std::cout << "compressed : " << is_compressed << std::endl; + } + + if (laszip_open_writer(laszip_writer, file_name_out.c_str(), is_compressed)) + { + spdlog::error("DLL ERROR: opening laszip writer for '{}'", file_name_out); + return; + } + + laszip_point* input_point; + if (laszip_get_point_pointer(laszip_reader, &input_point)) + { + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); + std::abort(); + } + + laszip_point* output_point; + if (laszip_get_point_pointer(laszip_writer, &output_point)) + { + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); + std::abort(); + } + + for (uint32_t i = 0; i < header->number_of_point_records; i++) + { + if (laszip_read_point(laszip_reader)) + { + spdlog::error("DLL ERROR: reading point {}", i); + std::abort(); + } + + laszip_F64 input_coordinates[3]; + if (laszip_get_coordinates(laszip_reader, input_coordinates)) + { + spdlog::error("DLL ERROR: laszip_set_coordinates {}", i); + std::abort(); + } + + Eigen::Vector3d pg = Eigen::Vector3d(input_coordinates[0], input_coordinates[1], input_coordinates[2]); + pg = m_pose * pg; + // adjustPoint(input_coordinates, m_pose); + laszip_F64 output_coordinates[3]; + output_coordinates[0] = pg.x(); + output_coordinates[1] = pg.y(); + output_coordinates[2] = pg.z(); + + if (laszip_set_coordinates(laszip_writer, output_coordinates)) + { + spdlog::error("DLL ERROR: laszip_set_coordinates {}", i); + std::abort(); + } + output_point->intensity = input_point->intensity; + output_point->classification = input_point->classification; + output_point->num_extra_bytes = input_point->num_extra_bytes; + output_point->gps_time = input_point->gps_time; + memcpy(output_point->extra_bytes, input_point->extra_bytes, output_point->num_extra_bytes); + + if (laszip_write_point(laszip_writer)) + { + spdlog::error("DLL ERROR: writing point {}", i); + return; + } + + if (laszip_update_inventory(laszip_writer)) + { + spdlog::error("DLL ERROR: updating inventory for point {}", i); + return; + } + } + + // close the reader + if (laszip_close_reader(laszip_reader)) + { + spdlog::error("DLL ERROR: closing laszip reader"); + return; + } + + // destroy the reader + + if (laszip_destroy(laszip_reader)) + { + spdlog::error("DLL ERROR: destroying laszip reader"); + return; + } + + laszip_I64 p_count{ 0 }; + if (laszip_get_point_count(laszip_writer, &p_count)) + { + spdlog::error("DLL ERROR: getting point count"); + return; + } + + spdlog::info("successfully written {} points", p_count); + + // close the writer + + if (laszip_close_writer(laszip_writer)) + { + spdlog::error("DLL ERROR: closing laszip writer"); + return; + } + + // destroy the writer + + // ToDo --> solve it + if (laszip_destroy(laszip_writer)) + { + spdlog::error("DLL ERROR: destroying laszip writer"); + return; + } + + std::cout << "saving to " << file_path_put << std::endl; +} + +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& timestamps, + bool use_first_pose, + bool save_index_pose) +{ + Eigen::Affine3d m_pose = trajectory[0].inverse(); + for (const auto& org_p : points) + { + Point3Di p = org_p; + if (p.point.norm() > threshold_output_filter) + { + if (use_first_pose) + { + p.point = m_pose * (trajectory[org_p.index_pose] * org_p.point); + } + else + { + p.point = trajectory[org_p.index_pose] * org_p.point; + } + pointcloud.push_back(p.point); + intensity.push_back(p.intensity); + timestamps.push_back(p.timestamp); + if (save_index_pose) + { + if (index_poses) + { + index_poses->push_back(org_p.index_pose); + } + } + } + } +} + +void save_all_to_las(const Session& session, std::string output_las_name, bool as_local, bool skip_ts_0) +{ + Eigen::Affine3d first_pose = Eigen::Affine3d::Identity(); + bool found_first_pose = false; + + const auto& offset = session.point_clouds_container.offset; + LazWriter writer; + if (!writer.open(output_las_name, offset.x(), offset.y(), offset.z())) + { + std::cout << "problem with saving file: " << output_las_name << std::endl; + return; + } + + for (const auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + if (!found_first_pose) + { + found_first_pose = true; + first_pose = p.m_pose; + } + + for (size_t i = 0; i < p.points_local.size(); ++i) + { + if (skip_ts_0) + { + if (i >= p.timestamps.size() || p.timestamps[i] == 0.0) + continue; + } + + Eigen::Vector3d vp = p.m_pose * p.points_local[i]; + if (as_local) + vp = first_pose * vp; + + unsigned short inten = (i < p.intensities.size()) ? p.intensities[i] : 0; + double ts = (i < p.timestamps.size()) ? p.timestamps[i] : 0.0; + + if (!writer.writePoint(vp, inten, ts, offset.x(), offset.y(), offset.z())) + return; + } + } + } + + writer.close(); +} diff --git a/core/src/ground_control_points.cpp b/core/src/ground_control_points.cpp index 2e530c4a..37b33376 100644 --- a/core/src/ground_control_points.cpp +++ b/core/src/ground_control_points.cpp @@ -61,7 +61,7 @@ void GroundControlPoints::imgui(PointClouds& point_clouds_container) int remove_gcp_index = -1; for (int i = 0; i < gpcs.size(); i++) { - ImGui::Text(("GCP_" + std::to_string(i) + " (" + gpcs[i].name + ")").c_str()); + ImGui::Text("GCP_%d (%s)", i, gpcs[i].name); ImGui::SameLine(); if (ImGui::Button(("Remove##" + std::to_string(i)).c_str())) { diff --git a/core/src/pose_graph_slam.cpp b/core/src/pose_graph_slam.cpp index baa533de..c745af9e 100644 --- a/core/src/pose_graph_slam.cpp +++ b/core/src/pose_graph_slam.cpp @@ -34,10 +34,6 @@ #endif -#if WITH_MANIF -#include -#endif - std::random_device rd; std::mt19937 gen(rd()); diff --git a/core/src/utils.cpp b/core/src/utils.cpp index de2c1986..32aa275f 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -1096,7 +1096,7 @@ void info_window(const std::vector& infoLines, const std::vector -#include -#include -#include - -#include - -#include - -// TODO(mwlasiuk) : cross includes ??? -#include - -class LazWrapper -{ -public: - 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); - - 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/src/laz_wrapper.cpp b/core_hd_mapping/src/laz_wrapper.cpp deleted file mode 100644 index 684b4568..00000000 --- a/core_hd_mapping/src/laz_wrapper.cpp +++ /dev/null @@ -1,255 +0,0 @@ -#include - -#include - -#include -#include -#include - -#include -#include -#include - -#include - -#include - -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); - - 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++) - { - 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) - { - 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++) - { - sectors[i].visible = false; - } - } - - bool can_show = false; - for (size_t i = 0; i < sectors.size(); i++) - { - if (sectors[i].point_cloud.size() > 0) - { - can_show = true; - } - } - - if (can_show) - { - ImGui::SameLine(); - 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) - { - 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")) - { - decimation = 100; - reload_all_LAZ_files(common_data.shift_x, common_data.shift_y); - - 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++) - { - bool previous_visibility = sectors[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)--; - } - ImGui::SameLine(0.0f, spacing); - 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].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; - } - } - } - } - - ImGui::End(); -} - -void LazWrapper::render(const CommonData& common_data) -{ - glColor3f(0.5, 0.5, 0.5); - 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]; - glColor3f(p.r, p.g, p.b); - glVertex3f(p.x, p.y, p.z); - } - glEnd(); - glPointSize(1); - } - } -} - -LAZSector LazWrapper::load_sector(const std::string& filename, const double shift_x, const double shift_y) -{ - LAZSector sector; - sector.file_name = filename; - - laszip_POINTER laszip_reader; - if (laszip_create(&laszip_reader)) - { - spdlog::error("DLL ERROR: creating laszip reader"); - std::abort(); - } - - laszip_BOOL is_compressed = 0; - if (laszip_open_reader(laszip_reader, filename.c_str(), &is_compressed)) - { - spdlog::error("DLL ERROR: opening laszip reader for '{}'", filename.c_str()); - std::abort(); - } - std::cout << "compressed : " << is_compressed << std::endl; - laszip_header* header; - - if (laszip_get_header_pointer(laszip_reader, &header)) - { - spdlog::error("DLL ERROR: getting header pointer from laszip reader"); - std::abort(); - } - - spdlog::info("file '{}' contains {} points", filename, header->number_of_point_records); - - laszip_point* point; - if (laszip_get_point_pointer(laszip_reader, &point)) - { - spdlog::error("DLL ERROR: getting point pointer from laszip reader"); - std::abort(); - } - - sector.point_cloud.reserve(header->number_of_point_records); - - sector.min_x = 1000000000000; - sector.max_x = -1000000000000; - sector.min_y = 1000000000000; - sector.max_y = -1000000000000; - - for (int i = 0; i < header->number_of_point_records; i++) - { - if (laszip_read_point(laszip_reader)) - { - spdlog::error("DLL ERROR: reading point {}", i); - std::abort(); - } - - LAZPoint p; - p.x = header->x_offset + header->x_scale_factor * static_cast(point->X) + shift_x; - p.y = header->y_offset + header->y_scale_factor * static_cast(point->Y) + shift_y; - p.z = header->z_offset + header->z_scale_factor * static_cast(point->Z); - - p.r = float(point->rgb[0]) / 65536.0f; - p.g = float(point->rgb[1]) / 65536.0f; - p.b = float(point->rgb[2]) / 65536.0f; - p.classsification = point->classification; - - sector.point_cloud.push_back(p); - - if (p.x < sector.min_x) - { - sector.min_x = p.x; - } - if (p.x > sector.max_x) - { - sector.max_x = p.x; - } - if (p.y < sector.min_y) - { - sector.min_y = p.y; - } - if (p.y > sector.max_y) - { - sector.max_y = p.y; - } - } - - return sector; -} - -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(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++) - { - 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++) - { - threads[j].join(); - } - threads.clear(); -} diff --git a/core_hd_mapping/src/project_settings.cpp b/core_hd_mapping/src/project_settings.cpp index b4b3d94d..366e1d6f 100644 --- a/core_hd_mapping/src/project_settings.cpp +++ b/core_hd_mapping/src/project_settings.cpp @@ -137,7 +137,7 @@ void ProjectSettings::imgui( for (size_t i = 0; i < trajectories.size(); i++) { - ImGui::Text(std::string(trajectories[i].trajectory_file).c_str()); + ImGui::Text("%s", trajectories[i].trajectory_file.c_str()); ImGui::SameLine(); ImGui::Checkbox(("[" + std::to_string(i) + "] visible").c_str(), &trajectories[i].visible); ImGui::SameLine(); @@ -171,7 +171,8 @@ void ProjectSettings::imgui( // ImGui::SameLine(); } - ImGui::Text(("total_length: " + std::to_string(int(total_length)) + "[m]").c_str()); + + ImGui::Text("total_length: %d [m]", (int)total_length); if (ImGui::Button("pose graph slam")) { diff --git a/core_hd_mapping/src/roi_exporter.cpp b/core_hd_mapping/src/roi_exporter.cpp index fd89a1da..27772f9f 100644 --- a/core_hd_mapping/src/roi_exporter.cpp +++ b/core_hd_mapping/src/roi_exporter.cpp @@ -2,7 +2,6 @@ #include -#include #include #include @@ -349,95 +348,6 @@ void RoiExporter::render(const CommonData& common_data) } } -PointCloudWithPose RoiExporter::get_pc_from_laz(CommonData& common_data, std::vector& sectors, double shift_x, double shift_y) -{ - std::cout << "Number of LAZSectors: " << sectors.size() << std::endl; - - PointCloudWithPose pc; - pc.trajectory_file_name = "georeference"; - pc.m_pose = Eigen::Affine3d::Identity(); - pc.timestamp = 0.0; - pc.visible = true; - - 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)) - { - 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)) - { - 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)) - { - 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)) - { - indexes.push_back(i); - } - } - } - - std::sort(indexes.begin(), indexes.end()); - 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) - { - 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) - { - 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)) - { - 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; - pc.points_global.push_back(new_point); - } - } - } - } - - std::cout << pc.points_global.size() << std::endl; - return pc; -} - typedef std::vector> Cloud; void RoiExporter::export_to_RESSO_format(std::vector& roi_point_clouds) diff --git a/external/include/plycpp.h b/external/include/plycpp.h index 40e1f88f..7658d8b9 100644 --- a/external/include/plycpp.h +++ b/external/include/plycpp.h @@ -84,9 +84,13 @@ namespace plycpp { auto it = std::find_if(begin(), end(), [&key](const MyKeyData& a){ return a.key == key; }); if (it != end()) + { return it->data; - else {} - //throw Exception("Invalid key."); + } + + return nullptr; + + //throw Exception("Invalid key."); } const std::shared_ptr operator[] (const Key& key) const diff --git a/external/src/plycpp.cpp b/external/src/plycpp.cpp index 98e44e79..d97f7713 100644 --- a/external/src/plycpp.cpp +++ b/external/src/plycpp.cpp @@ -108,8 +108,12 @@ namespace plycpp { return it->second; } - else{} - //throw Exception(std::string("Unkown data type:" + name)); + else + { + return typeid(void); + } + + //throw Exception(std::string("Unkown data type:" + name)); } std::string dataTypeToString(const std::type_index& type)