diff --git a/CMakeLists.txt b/CMakeLists.txt index 284094c9..fccf8971 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) project(hd-mapping) set (HDMAPPING_VERSION_MAJOR 0) -set (HDMAPPING_VERSION_MINOR 96) +set (HDMAPPING_VERSION_MINOR 97) set (HDMAPPING_VERSION_PATCH 0) add_definitions(-DHDMAPPING_VERSION_MAJOR=${HDMAPPING_VERSION_MAJOR}) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 97d13f98..4bb624c7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -31,7 +31,6 @@ #ifdef _WIN32 #include "resource.h" #include - #endif /////////////////////////////////////////////////////////////////////////////////// @@ -985,7 +984,7 @@ void settings_gui() { ImGui::BeginTooltip(); ImGui::Text("This process makes trajectory smooth, point cloud will be more consistent"); - ImGui::SetTooltip("Press optionally before pressing 'Save result'"); + ImGui::Text("Press optionally before pressing 'Save result'"); ImGui::EndTooltip(); } @@ -996,11 +995,13 @@ void settings_gui() if (ImGui::Button("Save result")) 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"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Press this button for saving resulting trajectory and point clouds"); + ImGui::Text("as single session for 'multi_view_tls_registration_step_2' program"); + ImGui::EndTooltip(); + } } if (step_1_done && step_2_done) { @@ -1515,6 +1516,18 @@ void progress_window() ImGui::ProgressBar(progress, ImVec2(-1.0f, 0.0f), progressText); ImGui::Text("%s", timeInfo); +#ifdef _WIN32 + // Update Windows taskbar progress + if (progress > 0.01f && progress < 1.0f) + { + SetTaskbarProgress(progress); + } + else if (progress >= 1.0f) + { + ClearTaskbarProgress(); + } +#endif + ImGui::NewLine(); if (!loPause) @@ -1873,7 +1886,7 @@ void display() { if (ImGui::BeginMenu("Presets")) { - if (ImGui::MenuItem("1 Velocity < 8km/h, tiny spaces", nullptr, (lastPar == 1))) + if (ImGui::MenuItem("1 Velocity < 8km/h, tiny spaces (default)", nullptr, (lastPar == 1))) { lastPar = 1; @@ -2336,7 +2349,32 @@ int main(int argc, char* argv[]) return 0; } - if (argc == 4) // runnning from command line + if (argc == 2) // running from command line + { + auto path = fs::path(argv[1]); + if (is_directory(path)) + { + std::string working_directory; + std::vector worker_data; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + std::atomic loPause{ false }; + step1(path.string(), 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) + << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n"; + + save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, argv[3]); + } + } + else if (argc == 4) // runnning from command line with custom params { // Load parameters from file using original TomlIO class TomlIO toml_io; @@ -2369,6 +2407,10 @@ int main(int argc, char* argv[]) initGL(&argc, argv, winTitle, display, mouse); glutCloseFunc(on_exit); +#ifdef _WIN32 + InitTaskbarProgress(); +#endif + glutMainLoop(); ImGui_ImplOpenGL2_Shutdown(); 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 5444958f..fece74a1 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -117,7 +117,7 @@ std::vector> nns(const std::vector& points_global, } } - // std::cout << "------------" << std::endl; + // spdlog::info("------------"); for (size_t y = 0; y < min_distances.size(); y++) if (indexes_target[y] != -1) nn.emplace_back(index_element_source, indexes_target[y]); @@ -596,12 +596,12 @@ void optimize_sf( if (points_local.size() > 100) { - // std::cout << "start adding lidar observations" << std::endl; + // spdlog::info("start adding lidar observations"); if (multithread) std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun); else std::for_each(std::begin(points_local), std::end(points_local), hessian_fun); - // std::cout << "adding lidar observations finished" << std::endl; + // spdlog::info("adding lidar observations finished"); } std::vector> odo_edges; @@ -1220,12 +1220,12 @@ void optimize_rigid_sf( if ((infm.array() > threshold).any()) { - // std::cout << "infm.array() " << infm.array() << std::endl; + // spdlog::info("infm.array() {}", infm.array()); return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; } if ((infm.array() < -threshold).any()) { - // std::cout << "infm.array() " << infm.array() << std::endl; + // spdlog::info("infm.array() {}", infm.array()); return { Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), c }; } @@ -1314,7 +1314,7 @@ void optimize_rigid_sf( SumBlocks(blocks, AtPAndt, AtPBndt); - // std::cout << "number_of_observations " << number_of_observations << std::endl; + // spdlog::info("number_of_observations {}", number_of_observations); std::vector> tripletListA; std::vector> tripletListP; @@ -1387,7 +1387,7 @@ void optimize_rigid_sf( { for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) { - // std::cout << "it.value() " << it.value() << std::endl; + // spdlog::info("it.value() {}", it.value()); h_x.push_back(it.value()); } } @@ -2083,7 +2083,7 @@ void optimize_lidar_odometry( Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz); Eigen::Vector3d p2(pose.px, pose.py, pose.pz); - // std::cout << "(p1 - p2).norm() " << (p1 - p2).norm() << std::endl; + // spdlog::info("(p1 - p2).norm() {}", (p1 - p2).norm()); if ((p1 - p2).norm() < 1.0) intermediate_trajectory[i] = affine_matrix_from_pose_tait_bryan(pose); @@ -2513,7 +2513,7 @@ bool compute_step_2( lookup_stats); if (delta < params.convergence_delta_threshold) { - // std::cout << "finished at iteration " << iter + 1 << "/" << params.nr_iter; + // spdlog::info("finished at iteration {}/{}", iter + 1, params.nr_iter); break; } @@ -2538,24 +2538,24 @@ bool compute_step_2( if (delta > params.convergence_delta_threshold) spdlog::info( - "finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta {:e}!!!", - iter_end + 1, - params.nr_iter, + "finished optimizing worker_data {}/{} at iteration {}/{} in {:.3f}[s] with acc_distance {:.3f}[m], delta {:e}!!!", i + 1, worker_data.size(), - acc_distance, + iter_end + 1, + params.nr_iter, elapsed_seconds1, + acc_distance, delta); else spdlog::info( - "finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta < {:.1e} " + "finished optimizing worker_data {}/{} at iteration {}/{} in {:.3f}[s] with acc_distance {:.3f}[m], delta<{:.1e} " "(converged)", - iter_end + 1, - params.nr_iter, i + 1, worker_data.size(), - acc_distance, + iter_end + 1, + params.nr_iter, elapsed_seconds1, + acc_distance, params.convergence_delta_threshold); loProgress.store((float)(i + 1) / worker_data.size()); @@ -3456,7 +3456,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam } spdlog::info("build regular grid decomposition FINISHED"); - spdlog::info("ndt observations start START"); + spdlog::info("NDT observations start START"); counter = 0; @@ -3494,7 +3494,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam Eigen::Vector3d viewport = trajectory[points_global[i].index_pose].translation(); if (nv.dot(viewport - this_bucket.mean) < 0) { - // std::cout << "nv!"; + // spdlog::warning("nv!"); continue; } @@ -3611,7 +3611,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam AtPB += AtPBndt; } - spdlog::info("ndt observations start FINISHED"); + spdlog::info("NDT observations start FINISHED"); Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); diff --git a/apps/lidar_odometry_step_1/resource.rc b/apps/lidar_odometry_step_1/resource.rc index b18b318f..accd06be 100644 --- a/apps/lidar_odometry_step_1/resource.rc +++ b/apps/lidar_odometry_step_1/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 6 -PRODUCTVERSION 0, 0, 9, 6 +FILEVERSION 0, 0, 9, 7 +PRODUCTVERSION 0, 0, 9, 7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 1\0" -VALUE "FileVersion", "0.9.6\0" +VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Odometry\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "lidar_odometry_step_1.exe\0" -VALUE "ProductVersion", "0.9.6\0" +VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 418a99de..f809d404 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,7 @@ #include #include +#include #include #include @@ -114,32 +115,32 @@ void load_pc( fprintf(stderr, "DLL ERROR: opening laszip reader for '%s'\n", lazFile.c_str()); std::abort(); } - std::cout << "Compressed : " << is_compressed << std::endl; + spdlog::info("Compressed : {}", is_compressed); laszip_header* header; if (laszip_get_header_pointer(laszip_reader, &header)) { - fprintf(stderr, "DLL ERROR: getting header pointer from laszip reader\n"); + spdlog::error("DLL ERROR: getting header pointer from laszip reader"); 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"); + spdlog::error("DLL ERROR: getting point pointer from laszip reader"); std::abort(); } int counter_ts0 = 0; int counter_filtered_points = 0; - std::cout << "Header -> number_of_point_records: " << header->number_of_point_records << std::endl; + spdlog::info("Header number_of_point_records: {}", header->number_of_point_records); 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); + spdlog::error("DLL ERROR: reading point {}", j); laszip_close_reader(laszip_reader); } @@ -213,9 +214,9 @@ void load_pc( } } } - std::cout << "Number points with timestamp == 0: " << counter_ts0 << std::endl; - std::cout << "Counter filtered points: " << counter_filtered_points << std::endl; - std::cout << "Total number of points: " << points.size() << std::endl; + spdlog::info("Number points with timestamp == 0: {}", counter_ts0); + spdlog::info("Counter filtered points: {}", counter_filtered_points); + spdlog::info("Total number of points: {}", points.size()); laszip_close_reader(laszip_reader); } @@ -258,7 +259,7 @@ void settings_gui() std::string l1 = idToSn.at(0); std::string l2 = idToSn.at(1); - std::cout << "output_file_name: " << input_file_name << std::endl; + spdlog::info("output_file_name: {}", input_file_name); nlohmann::json j; j["calibration"][l1.c_str()]["identity"] = "true"; @@ -297,7 +298,7 @@ void settings_gui() calibration_file_name = mandeye::fd::OpenFileDialogOneFile("Calibration files", mandeye::fd::Calibration_filter); if (calibration_file_name.size() > 0) { - std::cout << "loading file: " << calibration_file_name << std::endl; + spdlog::info("loading file: {}", calibration_file_name); calibration = MLvxCalib::GetCalibrationFromFile(calibration_file_name); imuSnToUse = MLvxCalib::GetImuSnToUse(calibration_file_name); @@ -307,12 +308,12 @@ void settings_gui() if (!calibration.empty()) { - std::cout << "Loaded calibration for: \n"; + spdlog::info("Loaded calibration for:"); for (const auto& [sn, _] : calibration) { - std::cout << " -> " << sn << std::endl; + spdlog::info(" -> {}", sn); } - std::cout << "imuSnToUse: " << imuSnToUse << std::endl; + spdlog::info("imuSnToUse: {}", imuSnToUse); } } } @@ -423,17 +424,15 @@ void settings_gui() lidar1.emplace_back(p); } - std::cout << "Decimation: " << params.decimation << std::endl; - std::cout << "Point cloud size before" << std::endl; - std::cout << "LiDAR0 size: " << lidar0.size() << std::endl; - std::cout << "LidDAR1 size: " << lidar1.size() << std::endl; + spdlog::info("Downsampling: {}", params.decimation); + spdlog::info("Point cloud size before"); + spdlog::info("LiDAR0: {}; LiDAR1: {}", lidar0.size(), lidar1.size()); lidar0 = decimate(lidar0, params.decimation, params.decimation, params.decimation); lidar1 = decimate(lidar1, params.decimation, params.decimation, params.decimation); - std::cout << "Point cloud size after" << std::endl; - std::cout << "LiDAR0 size: " << lidar0.size() << std::endl; - std::cout << "LiDAR1 size: " << lidar1.size() << std::endl; + spdlog::info("Point cloud size after"); + spdlog::info("LiDAR0: {}, LiDAR1: {}", lidar0.size(), lidar1.size()); std::vector pc0; std::vector pc1; @@ -557,7 +556,7 @@ void settings_gui() if (new_calibration_file_name.size() > 0) { - std::cout << "Output file name: " << new_calibration_file_name << std::endl; + spdlog::info("Output file name: {}", new_calibration_file_name); nlohmann::json j; j["calibration"][idToSn.at(0)]["order"] = "ROW"; @@ -915,14 +914,14 @@ int main(int argc, char* argv[]) ImGui::DestroyContext(); } catch (const std::bad_alloc& e) { - std::cerr << "System is out of memory : " << e.what() << std::endl; + spdlog::error("System is out of memory : {}", e.what()); mandeye::fd::OutOfMemMessage(); } catch (const std::exception& e) { - std::cout << e.what(); + spdlog::error(e.what()); } catch (...) { - std::cerr << "Unknown fatal error occurred." << std::endl; + spdlog::error("Unknown fatal error occurred!"); } return 0; diff --git a/apps/mandeye_mission_recorder_calibration/resource.rc b/apps/mandeye_mission_recorder_calibration/resource.rc index 0ed87c81..f4a78bff 100644 --- a/apps/mandeye_mission_recorder_calibration/resource.rc +++ b/apps/mandeye_mission_recorder_calibration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,6 - PRODUCTVERSION 0,0,9,6 + FILEVERSION 0,0,9,7 + PRODUCTVERSION 0,0,9,7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping MR Calibration\0" - VALUE "FileVersion", "0.9.6\0" + VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Mission Recorder Calibration\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_mission_recorder_calibration.exe\0" - VALUE "ProductVersion", "0.9.6\0" + VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END 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 f6e436c7..2f7cc6a1 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -176,7 +177,7 @@ void render_nearest_photo(double ts) { return; } - std::cout << "render_nearest_photo: " << photo_file << std::endl; + spdlog::info("render_nearest_photo: {}", photo_file); cv::Mat img = cv::imread(photo_file); if (img.empty()) { @@ -251,7 +252,7 @@ void optimize() } } - //std::cout << "points_local.size(): " << points_local.size() << std::endl; + //spdlog::info("points_local.size(): {}", points_local.size()); // points_local // points_local_sf @@ -313,7 +314,7 @@ void optimize() // std::cout << infm << std::endl; const Eigen::Affine3d& m_pose = tr[points_local[i].index_pose]; - //std::cout << "points_local[i].index_pose " << points_local[i].index_pose << std::endl; + //spdlog::info("points_local[i].index_pose {}", points_local[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; @@ -328,9 +329,9 @@ void optimize() 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()); - // std::cout << "delta_x: " << delta_x << std::endl; - // std::cout << "delta_y: " << delta_y << std::endl; - // std::cout << "delta_z: " << delta_z << std::endl; + // spdlog::info("delta_x: {}", delta_x); + // spdlog::info("delta_y: {}", delta_y); + // spdlog::info("delta_z: {}", delta_z); if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) < 0.001) { @@ -636,7 +637,7 @@ void optimize() } } - std::cout << "optimize_sf2" << std::endl; + spdlog::info("optimize_sf2"); for (int iter = 0; iter < robust_and_accurate_lidar_odometry_iterations; iter++) { optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, useMultithread, wx, wy, wz, wom, wfi, wka); @@ -948,9 +949,9 @@ void loadFiles(std::vector input_file_names) std::string cam_id = filename.substr(0, filename.find("_")); std::string timestamp = filename.substr(filename.find("_") + 1, filename.size()); - std::cout << "cam_id: " << cam_id << std::endl; - std::cout << "timestamp: " << timestamp << std::endl; - std::cout << "filename: " << filename << std::endl; + spdlog::info("cam_id: {}", cam_id); + spdlog::info("timestamp: {}", timestamp); + spdlog::info("filename: {}", filename); if (cam_id == "cam0" && !timestamp.empty()) { @@ -960,7 +961,7 @@ void loadFiles(std::vector input_file_names) photo_files_ts[ts] = fileName; } catch (const std::exception& e) { - std::cerr << "Error parsing timestamp from filename: " << filename << " - " << e.what() << std::endl; + spdlog::error("Error parsing timestamp from filename: {} - {}", filename, e.what()); } } } @@ -1014,12 +1015,12 @@ void loadFiles(std::vector input_file_names) /*if (ts_diff < 0) { - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; - std::cout << "WARNING!!!!" << std::endl; + spdlog::warning("WARNING!!!!"); + spdlog::warning("WARNING!!!!"); + spdlog::warning("WARNING!!!!"); + spdlog::warning("WARNING!!!!"); + spdlog::warning("WARNING!!!!"); + spdlog::warning("WARNING!!!!"); } if (ts_diff < 0.01) @@ -1046,8 +1047,13 @@ void loadFiles(std::vector input_file_names) counter++; if (counter % 100 == 0) { - std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " [" - << counter++ << " of " << imu_data.size() << "]" << std::endl; + spdlog::info( + "[{} of {}]: Roll {}, Pitch {}, Yaw {}", + counter++, + imu_data.size(), + euler.angle.roll, + euler.angle.pitch, + euler.angle.yaw); } // log it for implot @@ -1077,9 +1083,9 @@ void loadFiles(std::vector input_file_names) for (const auto& pp : pointsPerFile) number_of_points += pp.size(); - std::cout << "Number of points: " << number_of_points << std::endl; + spdlog::info("Number of points: {}", number_of_points); - std::cout << "Start indexing points" << std::endl; + spdlog::info("Start indexing points"); // std::vector points_global; std::vector points_local; @@ -1100,7 +1106,8 @@ void loadFiles(std::vector input_file_names) for (size_t i = 0; i < pointsPerFile.size(); i++) { - std::cout << "Indexed: " << i + 1 << " of " << pointsPerFile.size() << " files\r"; + std::cout << "Indexed file " << i + 1 << "/" << pointsPerFile.size() << "\r"; + for (const auto& pp : pointsPerFile[i]) { auto lower = std::lower_bound( @@ -1175,9 +1182,9 @@ void loadFiles(std::vector input_file_names) 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; + // spdlog::info("ts_begin {}", ts_begin); + // spdlog::info("ts_step {}", ts_step); + // spdlog::info("ts_end {}", data.timestamps[data.timestamps.size() - 1].first); for (size_t pp = 0; pp < data.points_local.size(); pp++) data.points_local[pp].timestamp = ts_begin + pp * ts_step; @@ -1192,7 +1199,7 @@ void loadFiles(std::vector input_file_names) } } - std::cout << "\nIndexing points finished\n\n"; + spdlog::info("Indexing points finished\n"); if (all_data.size() > 0) { @@ -1208,7 +1215,7 @@ void openFolder() std::vector input_file_names; input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); - std::cout << "Selected folder: '" << input_folder_name << std::endl; + spdlog::info("Selected folder: '{}'", input_folder_name); if (fs::exists(input_folder_name)) { @@ -1305,7 +1312,7 @@ void imu_data_gui() { std::string output_file_name = ""; output_file_name = mandeye::fd::SaveFileDialog("Save IMU data", {}, ""); - std::cout << "file to save: '" << output_file_name << "'" << std::endl; + spdlog::info("file to save: '{}'", output_file_name); ofstream file; file.open(output_file_name); @@ -1374,7 +1381,7 @@ void settings_gui() if (output_file_name.size() > 0) { - std::cout << "las or laz file to save: '" << output_file_name << "'" << std::endl; + spdlog::info("las or laz file to save: '{}'", output_file_name); std::vector pointcloud; std::vector intensity; @@ -1392,7 +1399,7 @@ void settings_gui() }*/ if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) - std::cout << "problem with saving file: " << output_file_name << std::endl; + spdlog::error("problem with saving file: '{}'", output_file_name); } } ImGui::EndDisabled(); @@ -1447,7 +1454,6 @@ void settings_gui() if (ImGui::Button("Print to console debug text")) { double max_diff = 0; - std::cout << std::setprecision(20); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) @@ -1463,29 +1469,35 @@ void settings_gui() int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + spdlog::info( + "poses {}/{}: point TS [s] - pose TS [s] = ... [s]", + index_pose, + all_data[index_rendered_points_local].poses.size()); + 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; + spdlog::info( + "{:.20g} - {:.20g} = {:.20g}", + all_data[index_rendered_points_local].points_local[i].timestamp, + all_data[index_rendered_points_local].timestamps[index_pose].first, + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); 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; + // spdlog::info("{:.20g}", all_data[index_rendered_points_local].points_local[i].timestamp); 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 << "max_diff " << max_diff << std::endl; - std::cout << "----------------" << std::endl; + spdlog::info("max_diff: {} [s]", max_diff); + spdlog::info("----------------"); for (size_t k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) - std::cout << all_data[index_rendered_points_local].timestamps[k].first << std::endl; + spdlog::info("{:.20g}", all_data[index_rendered_points_local].timestamps[k].first); } } } @@ -2010,14 +2022,14 @@ int main(int argc, char* argv[]) ImPlot::DestroyContext(); } catch (const std::bad_alloc& e) { - std::cerr << "System is out of memory : " << e.what() << std::endl; + spdlog::error("System is out of memory : {}", e.what()); mandeye::fd::OutOfMemMessage(); } catch (const std::exception& e) { - std::cout << e.what(); + spdlog::error(e.what()); } catch (...) { - std::cerr << "Unknown fatal error occurred." << std::endl; + spdlog::error("Unknown fatal error occurred!"); } return 0; diff --git a/apps/mandeye_raw_data_viewer/resource.rc b/apps/mandeye_raw_data_viewer/resource.rc index 8ecd2dbf..9e27a586 100644 --- a/apps/mandeye_raw_data_viewer/resource.rc +++ b/apps/mandeye_raw_data_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,6 - PRODUCTVERSION 0,0,9,6 + FILEVERSION 0,0,9,7 + PRODUCTVERSION 0,0,9,7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Raw Data Viewer\0" - VALUE "FileVersion", "0.9.6\0" + VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Raw data viewer\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_raw_data_viewer.exe\0" - VALUE "ProductVersion", "0.9.6\0" + VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp index 027df9bc..86da4ec6 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -13,6 +13,7 @@ #include #include +#include #include #include @@ -1510,14 +1511,14 @@ int main(int argc, char* argv[]) ImGui::DestroyContext(); } catch (const std::bad_alloc& e) { - std::cerr << "System is out of memory : " << e.what() << std::endl; + spdlog::error("System is out of memory : {}", e.what()); mandeye::fd::OutOfMemMessage(); } catch (const std::exception& e) { - std::cout << e.what(); + spdlog::error(e.what()); } catch (...) { - std::cerr << "Unknown fatal error occurred." << std::endl; + spdlog::error("Unknown fatal error occurred!"); } return 0; diff --git a/apps/mandeye_single_session_viewer/resource.rc b/apps/mandeye_single_session_viewer/resource.rc index b477d6ac..51d5aa89 100644 --- a/apps/mandeye_single_session_viewer/resource.rc +++ b/apps/mandeye_single_session_viewer/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO - FILEVERSION 0,0,9,6 - PRODUCTVERSION 0,0,9,6 + FILEVERSION 0,0,9,7 + PRODUCTVERSION 0,0,9,7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BEGIN BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Session Viewer\0" - VALUE "FileVersion", "0.9.6\0" + VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Single session viewer\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "mandeye_single_session_viewer.exe\0" - VALUE "ProductVersion", "0.9.6\0" + VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/multi_session_registration/resource.rc b/apps/multi_session_registration/resource.rc index 543c017d..c7f59bd8 100644 --- a/apps/multi_session_registration/resource.rc +++ b/apps/multi_session_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 6 -PRODUCTVERSION 0, 0, 9, 6 +FILEVERSION 0, 0, 9, 7 +PRODUCTVERSION 0, 0, 9, 7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 3\0" -VALUE "FileVersion", "0.9.6\0" +VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Multi session registration\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_session_registration_step_3.exe\0" -VALUE "ProductVersion", "0.9.6\0" +VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/apps/multi_view_tls_registration/resource.rc b/apps/multi_view_tls_registration/resource.rc index d12b43d0..da63eab9 100644 --- a/apps/multi_view_tls_registration/resource.rc +++ b/apps/multi_view_tls_registration/resource.rc @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico" // VS_VERSION_INFO VERSIONINFO -FILEVERSION 0, 0, 9, 6 -PRODUCTVERSION 0, 0, 9, 6 +FILEVERSION 0, 0, 9, 7 +PRODUCTVERSION 0, 0, 9, 7 FILEFLAGSMASK 0x3fL FILEOS 0x40004 FILETYPE 0x1 @@ -32,11 +32,11 @@ BLOCK "040904B0" BEGIN VALUE "CompanyName", "Mandeye\0" VALUE "FileDescription", "HDMapping Step 2\0" -VALUE "FileVersion", "0.9.6\0" +VALUE "FileVersion", "0.9.7\0" VALUE "InternalName", "Multi view TLS registration\0" VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" VALUE "OriginalFilename", "multi_view_tls_registration_step_2.exe\0" -VALUE "ProductVersion", "0.9.6\0" +VALUE "ProductVersion", "0.9.7\0" VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" VALUE "ProductName", "HDMapping\0" END diff --git a/core/include/utils.hpp b/core/include/utils.hpp index 5819be33..57c866d5 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -113,6 +113,15 @@ std::string truncPath(const std::string& fullPath); void wheel(int button, int dir, int x, int y); void reshape(int w, int h); void ShowMainDockSpace(); + +#ifdef _WIN32 +void InitTaskbarProgress(); +void SetTaskbarProgress(double progress01); +void ClearTaskbarProgress(); +// g_taskbar->SetProgressState(hwnd, TBPF_ERROR); // red +// g_taskbar->SetProgressState(hwnd, TBPF_PAUSED); // yellow +#endif + bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int)); void draw_ellipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd); diff --git a/core/src/pfd_wrapper.cpp b/core/src/pfd_wrapper.cpp index b07e4b5a..16231d83 100644 --- a/core/src/pfd_wrapper.cpp +++ b/core/src/pfd_wrapper.cpp @@ -73,7 +73,7 @@ namespace mandeye::fd std::string output_folder_name = ""; output_folder_name = pfd::select_folder(title, internal::lastLocationHint).result(); - std::cout << "folder: '" << output_folder_name << "'" << std::endl; + // std::cout << "folder: '" << output_folder_name << "'" << std::endl; return output_folder_name; } diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index d2c5f438..66612aab 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -1236,8 +1236,8 @@ bool PointClouds::load_whu_tls( 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::cout << "Loading " << i + 1 << "/" << input_file_names.size() << ": data (" + << (std::filesystem::path(input_file_names[i]).filename().string()) << "), "; auto& pc = point_clouds_nodata[i]; // reference directly to vector slot @@ -1268,12 +1268,10 @@ bool PointClouds::load_whu_tls( trj_path /= trj_fn; - std::cout << "From trajectory file (" << (std::filesystem::path(trj_path).filename().string()) << ")"; + std::cout << "trajectory (" << (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()); @@ -1359,13 +1357,13 @@ bool PointClouds::load_whu_tls( } } - std::cout << local_trajectory.size() << " local nodes" << std::endl; + 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; + std::cerr << "trajectory path: '" << trj_path.string() << "' does not exist" << std::endl; } //// load actual pointclouds @@ -1384,13 +1382,11 @@ bool PointClouds::load_whu_tls( { 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 << " / " + std::cout << "downsampling finished, sum_points before/after: " << sum_points_before_decimation << " / " << sum_points_after_decimation << std::endl; } } diff --git a/core/src/utils.cpp b/core/src/utils.cpp index a866f116..b46725f9 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -21,6 +21,7 @@ #ifdef _WIN32 #include +#include // ITaskbarList3 #include #endif @@ -168,10 +169,11 @@ std::string truncPath(const std::string& fullPath) namespace fs = std::filesystem; fs::path path(fullPath); - auto parent = path.parent_path().parent_path().filename().string(); // second to last folder + auto parent1 = path.parent_path().filename().string(); + auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder auto filename = path.filename().string(); - return "..\\" + parent + "\\..\\" + filename; + return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; } void wheel(int button, int dir, int x, int y) @@ -426,6 +428,40 @@ void ShowMainDockSpace() ImGui::End(); } +#ifdef _WIN32 +HWND hwnd; +ITaskbarList3* g_taskbar = nullptr; + +void InitTaskbarProgress() +{ + CoInitialize(nullptr); + + HRESULT hr = CoCreateInstance(CLSID_TaskbarList, nullptr, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&g_taskbar)); + + if (SUCCEEDED(hr)) + g_taskbar->HrInit(); +} + +void SetTaskbarProgress(double progress01) +{ + if (!g_taskbar) + return; + + const ULONGLONG total = 1000; + ULONGLONG value = (ULONGLONG)(progress01 * total); + + g_taskbar->SetProgressState(hwnd, TBPF_NORMAL); + g_taskbar->SetProgressValue(hwnd, value, total); +} + +void ClearTaskbarProgress() +{ + if (!g_taskbar) + return; + g_taskbar->SetProgressState(hwnd, TBPF_NOPROGRESS); +} +#endif + bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int)) { glutInit(argc, argv); @@ -435,7 +471,7 @@ bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display) return false; // window creation failed #ifdef _WIN32 - HWND hwnd = FindWindow(NULL, winTitle.c_str()); // The window title must match exactly + hwnd = FindWindow(NULL, winTitle.c_str()); // The window title must match exactly if (!hwnd) return false; // couldn't find window handle HINSTANCE hInstance = GetModuleHandle(NULL);