From 0b0536f00b601aebfd5d45349fda8e29cf025afe Mon Sep 17 00:00:00 2001 From: Danil An Date: Mon, 26 Jan 2026 22:39:11 +0400 Subject: [PATCH 1/8] Add spdlog submodule pinned to v1.17.0 --- .gitmodules | 3 +++ 3rdparty/spdlog | 1 + 2 files changed, 4 insertions(+) create mode 160000 3rdparty/spdlog diff --git a/.gitmodules b/.gitmodules index 47269dfd..180090b3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -47,3 +47,6 @@ [submodule "3rdparty/unordered_dense"] path = 3rdparty/unordered_dense url = https://github.com/martinus/unordered_dense.git +[submodule "3rdparty/spdlog"] + path = 3rdparty/spdlog + url = https://github.com/gabime/spdlog.git diff --git a/3rdparty/spdlog b/3rdparty/spdlog new file mode 160000 index 00000000..472945ba --- /dev/null +++ b/3rdparty/spdlog @@ -0,0 +1 @@ +Subproject commit 472945ba489e3f5684761affc431ae532ab5ed8c From 47c8667986fe4606182eb8955bed649833316819 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 11:38:55 +0400 Subject: [PATCH 2/8] spdlog v1.17.0 --- 3rdparty/spdlog | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/3rdparty/spdlog b/3rdparty/spdlog index 472945ba..79524ddd 160000 --- a/3rdparty/spdlog +++ b/3rdparty/spdlog @@ -1 +1 @@ -Subproject commit 472945ba489e3f5684761affc431ae532ab5ed8c +Subproject commit 79524ddd08a4ec981b7fea76afd08ee05f83755d From ac3dfe4f7ba7b3833424be85cc70932871aa7cc8 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 13:13:35 +0400 Subject: [PATCH 3/8] replace std::cout/cerr with spdlog in lidar_odometry_utils_optimizers.cpp --- CMakeLists.txt | 4 + apps/compare_trajectories/CMakeLists.txt | 1 + apps/concatenate_multi_livox/CMakeLists.txt | 1 + apps/lidar_odometry_step_1/CMakeLists.txt | 1 + .../lidar_odometry_gui.cpp | 4 + .../lidar_odometry_utils_optimizers.cpp | 126 +++++++----------- .../CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + apps/mandeye_raw_data_viewer/CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + .../CMakeLists.txt | 1 + apps/quick_start_demo/CMakeLists.txt | 1 + pybind/CMakeLists.txt | 1 + 13 files changed, 68 insertions(+), 76 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 848a90c3..37091138 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -366,6 +366,10 @@ endif() add_subdirectory(${THIRDPARTY_DIRECTORY}/unordered_dense) MESSAGE(STATUS "Using bundled unordered_dense from: ${THIRDPARTY_DIRECTORY}/unordered_dense") +# Add spdlog logging library +add_subdirectory(${THIRDPARTY_DIRECTORY}/spdlog) +MESSAGE(STATUS "Using bundled spdlog from: ${THIRDPARTY_DIRECTORY}/spdlog") + option(PYBIND "Enable pybind11 bindings" OFF) if(PYBIND) message(STATUS "PYBIND is enabled: fetching pybind.") diff --git a/apps/compare_trajectories/CMakeLists.txt b/apps/compare_trajectories/CMakeLists.txt index a16a137a..afc1b698 100644 --- a/apps/compare_trajectories/CMakeLists.txt +++ b/apps/compare_trajectories/CMakeLists.txt @@ -31,6 +31,7 @@ target_link_libraries( mandeye_compare_trajectories PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/concatenate_multi_livox/CMakeLists.txt b/apps/concatenate_multi_livox/CMakeLists.txt index b59c6567..ce887016 100644 --- a/apps/concatenate_multi_livox/CMakeLists.txt +++ b/apps/concatenate_multi_livox/CMakeLists.txt @@ -25,6 +25,7 @@ target_include_directories( target_link_libraries(concatenate_multi_livox PRIVATE core unordered_dense::unordered_dense + spdlog::spdlog ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS}) diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 3642bc6a..12008a7d 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -46,6 +46,7 @@ target_link_libraries( lidar_odometry_step_1 PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 035e7ec9..4cf2f4a4 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #ifdef _WIN32 #include "resource.h" @@ -2309,6 +2311,8 @@ void mouse(int glut_button, int state, int x, int y) int main(int argc, char* argv[]) { + spdlog::cfg::load_env_levels(); + spdlog::flush_on(spdlog::level::warn); set_lidar_odometry_default_params(params); try 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 72492eb4..89d6d21d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,4 +1,6 @@ #include "lidar_odometry_utils.h" +#include +#include #include #include #include @@ -774,7 +776,7 @@ void optimize_sf( intermediate_trajectory_motion_model = int_tr; } else - std::cout << "optimization failed" << std::endl; + spdlog::warn("optimization failed"); } void optimize_sf2( @@ -2093,7 +2095,7 @@ void optimize_lidar_odometry( if ((p1 - p2).norm() < 1.0) intermediate_trajectory[i] = affine_matrix_from_pose_tait_bryan(pose); else - std::cout << "big jump on trajectory: " << (p1 - p2).norm() << std::endl; + spdlog::warn("big jump on trajectory: {}", (p1 - p2).norm()); } delta = 0.0; for (int i = 0; i < h_x.size(); i++) @@ -2216,7 +2218,7 @@ void align_to_reference( m_g = affine_matrix_from_pose_tait_bryan(pose); } else - std::cout << "align_to_reference FAILED" << std::endl; + spdlog::warn("align_to_reference FAILED"); } bool compute_step_2( @@ -2235,18 +2237,17 @@ bool compute_step_2( { if (is_integer_bucket_ratio(params.in_out_params_indoor, params.in_out_params_outdoor)) { - std::cout << "hierarchical_rgd: integer bucket ratio detected, bucket linking enabled" << std::endl; + spdlog::info("hierarchical_rgd: integer bucket ratio detected, bucket linking enabled"); } else { - std::cout << "hierarchical_rgd: non-integer bucket ratio, bucket linking disabled" << std::endl; + spdlog::info("hierarchical_rgd: non-integer bucket ratio, bucket linking disabled"); } } if (worker_data.size() != 0) { - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); + spdlog::stopwatch stopwatch_total; double acc_distance = 0.0; int64_t total_iterations = 0; double total_optimization_time_seconds = 0.0; @@ -2289,7 +2290,7 @@ bool compute_step_2( if (!fs::exists(params.working_directory_preview)) { - std::cout << "Creating folder: " << params.working_directory_preview << "\n"; + spdlog::info("Creating folder: {}", params.working_directory_preview); fs::create_directory(params.working_directory_preview); } @@ -2303,7 +2304,7 @@ bool compute_step_2( { while (pause) { - std::cout << "pause" << std::endl; + spdlog::info("pause"); std::this_thread::sleep_for(std::chrono::seconds(1)); } } @@ -2325,7 +2326,7 @@ bool compute_step_2( if (mean_shift.norm() > 1.0) { - std::cout << "!!!mean_shift.norm() > 1.0!!!" << std::endl; + spdlog::warn("mean_shift.norm() > 1.0"); mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); } @@ -2352,7 +2353,7 @@ bool compute_step_2( bool add_pitch_roll_constraint = false; - const auto start1 = std::chrono::high_resolution_clock::now(); + spdlog::stopwatch stopwatch_worker; auto tmp_worker_data = worker_data[i].intermediate_trajectory; @@ -2401,7 +2402,7 @@ bool compute_step_2( } } /// - std::cout << "optimize_sf2" << std::endl; + spdlog::info("optimize_sf2"); std::vector pointcloud; std::vector intensity; @@ -2448,7 +2449,7 @@ bool compute_step_2( std::string output_file_name = "optimize_sf2_" + std::to_string(index_fn++) + ".laz"; if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) - std::cout << "problem with saving file: " << output_file_name << std::endl; + spdlog::warn("problem with saving file: {}", output_file_name); } for (auto& t : tr) @@ -2484,8 +2485,7 @@ bool compute_step_2( double delta = 100000.0; double lm_factor = 1.0; - std::chrono::time_point start, end; - start = std::chrono::system_clock::now(); + spdlog::stopwatch stopwatch_realtime; int iter_end = 0; @@ -2532,34 +2532,26 @@ bool compute_step_2( if (iter % 10 == 0 && iter > 0) { if (debugMsg) - std::cout << "\nlm_factor " << lm_factor << ", delta " << std::setprecision(10) << delta << "\n"; + spdlog::info("lm_factor {}, delta {:.10f}", lm_factor, delta); lm_factor *= 10.0; } - end = std::chrono::system_clock::now(); - std::chrono::duration elapsed_seconds = end - start; - - if (elapsed_seconds.count() > params.real_time_threshold_seconds) + if (stopwatch_realtime.elapsed().count() > params.real_time_threshold_seconds) break; } - const auto end1 = std::chrono::high_resolution_clock::now(); - const double elapsed_seconds1 = std::chrono::duration(end1 - start1).count(); + const double elapsed_seconds1 = stopwatch_worker.elapsed().count(); total_iterations += iter_end + 1; total_optimization_time_seconds += elapsed_seconds1; - 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(3) << acc_distance << "[m] in " << fixed << elapsed_seconds1 << "[s], delta "; if (delta > params.convergence_delta_threshold) - std::cout << std::scientific << delta << "!!!" << std::fixed; + spdlog::info("finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta {:e}!!!", + iter_end + 1, params.nr_iter, i + 1, worker_data.size(), acc_distance, elapsed_seconds1, delta); else - std::cout << "< " << std::scientific << std::setprecision(1) << params.convergence_delta_threshold << " (converged)" - << std::fixed << std::setprecision(3); - std::cout << std::endl; + spdlog::info("finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta < {:.1e} (converged)", + iter_end + 1, params.nr_iter, i + 1, worker_data.size(), acc_distance, elapsed_seconds1, params.convergence_delta_threshold); loProgress.store((float)(i + 1) / worker_data.size()); @@ -2591,12 +2583,11 @@ bool compute_step_2( if (!(acc_distance == acc_distance)) // NaN check { worker_data[i].intermediate_trajectory = tmp_worker_data; - std::cout << "CHALLENGING DATA OCCURED!!!" << std::endl; + spdlog::warn("CHALLENGING DATA OCCURED!!!"); acc_distance = acc_distance_tmp; - std::cout << "please split data set into subsets" << std::endl; + spdlog::warn("please split data set into subsets"); ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - std::cout << "calculations canceled for TIMESTAMP: " << (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first - << std::endl; + spdlog::warn("calculations canceled for TIMESTAMP: {}", (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first); return false; } @@ -2625,8 +2616,7 @@ bool compute_step_2( // if(reference_points.size() == 0){ if (acc_distance > params.sliding_window_trajectory_length_threshold) { - std::chrono::time_point startu, endu; - startu = std::chrono::system_clock::now(); + spdlog::stopwatch stopwatch_update; if (params.reference_points.size() == 0) { @@ -2668,14 +2658,8 @@ bool compute_step_2( worker_data[i].intermediate_trajectory[0].translation(), &lookup_stats.indoor_lookups); } - // - endu = std::chrono::system_clock::now(); - - std::chrono::duration elapsed_secondsu = endu - startu; - std::time_t end_timeu = std::chrono::system_clock::to_time_t(endu); - - // std::cout << "finished computation at " << std::ctime(&end_timeu) - // << "elapsed time update: " << std::setprecision(0) << elapsed_secondsu.count() << "s\n"; + // elapsed_secondsu previously commented out, now using stopwatch + // spdlog::info("elapsed time update: {:.0f}s", stopwatch_update); } else { @@ -2720,14 +2704,7 @@ bool compute_step_2( for (int i = 0; i < worker_data.size(); i++) worker_data[i].intermediate_trajectory_motion_model = worker_data[i].intermediate_trajectory; - 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::tm local_tm = *std::localtime(&end_time); - std::cout << "finished computation at " << std::put_time(&local_tm, "%Y-%m-%d %H:%M:%S") - << ", elapsed time: " << std::setprecision(2) << elapsed_seconds.count() << "s\n"; + spdlog::info("finished computation, elapsed time: {:.2f}s", stopwatch_total); params.total_length_of_calculated_trajectory = 0; for (int i = 1; i < worker_data.size(); i++) @@ -2735,14 +2712,13 @@ bool compute_step_2( (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; - std::cout << "total_iterations: " << total_iterations << std::endl; - std::cout << "total_optimization_time: " << std::setprecision(2) << total_optimization_time_seconds << "s" << std::endl; + spdlog::info("total_length_of_calculated_trajectory: {} [m]", params.total_length_of_calculated_trajectory); + spdlog::info("total_iterations: {}", total_iterations); + spdlog::info("total_optimization_time: {:.2f}s", total_optimization_time_seconds); const double avg_iteration_ms = (total_iterations > 0) ? (total_optimization_time_seconds * 1000.0 / total_iterations) : 0.0; - std::cout << "avg_iteration_time: " << std::setprecision(3) << avg_iteration_ms << "ms" << std::endl; - std::cout << "lookup_stats: indoor=" << lookup_stats.indoor_lookups << " outdoor_lookups=" << lookup_stats.outdoor_lookups - << " outdoor_pointer_hits=" << lookup_stats.outdoor_pointer_hits << " link_time=" << lookup_stats.link_time_seconds << "s" - << std::endl; + spdlog::info("avg_iteration_time: {:.3f}ms", avg_iteration_ms); + spdlog::debug("lookup_stats: indoor={} outdoor_lookups={} outdoor_pointer_hits={} link_time={:.3f}s", + lookup_stats.indoor_lookups, lookup_stats.outdoor_lookups, lookup_stats.outdoor_pointer_hits, lookup_stats.link_time_seconds); } return true; @@ -2762,7 +2738,7 @@ void Consistency(std::vector& worker_data, const LidarOdometryParams std::vector> indexes; bool multithread = false; - std::cout << "preparing data START" << std::endl; + spdlog::info("preparing data START"); for (int i = 0; i < worker_data.size(); i++) { for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) @@ -2773,14 +2749,13 @@ void Consistency(std::vector& worker_data, const LidarOdometryParams indexes.emplace_back(i, j); } } - std::cout << "preparing data FINISHED" << std::endl; + spdlog::info("preparing data FINISHED"); for (int i = 0; i < worker_data.size(); i++) { 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; + spdlog::warn("problem with load_vector_data file '{}'", worker_data[i].intermediate_points_cache_file_name.string()); for (int j = 0; j < intermediate_points.size(); j++) all_points_local.push_back(intermediate_points[j]); @@ -2944,7 +2919,7 @@ void Consistency(std::vector& worker_data, const LidarOdometryParams // ndt - std::cout << "NDT observations START" << std::endl; + spdlog::info("NDT observations START"); for (size_t index_point_begin = 0; index_point_begin < all_points_local.size(); index_point_begin += 1000000) { NDTBucketMapType buckets; @@ -3089,7 +3064,7 @@ void Consistency(std::vector& worker_data, const LidarOdometryParams AtPB += AtPBndt; } } - std::cout << "NDT observations FINISHED" << std::endl; + spdlog::info("NDT observations FINISHED"); Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); @@ -3131,7 +3106,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam std::vector> indexes; bool multithread = true; - std::cout << "preparing data START" << std::endl; + spdlog::info("preparing data START"); for (int i = 0; i < worker_data.size(); i++) { for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) @@ -3142,14 +3117,13 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam indexes.emplace_back(i, j); } } - std::cout << "preparing data FINISHED" << std::endl; + spdlog::info("preparing data FINISHED"); for (int i = 0; i < worker_data.size(); i++) { 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; + spdlog::warn("problem with load_vector_data for file '{}'", worker_data[i].intermediate_points_cache_file_name.string()); for (int j = 0; j < intermediate_points.size(); j++) all_points_local.push_back(intermediate_points[j]); @@ -3310,7 +3284,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam tripletListP.clear(); tripletListB.clear(); - std::cout << "reindex data START" << std::endl; + spdlog::info("reindex data START"); std::vector points_local; std::vector points_global; @@ -3357,9 +3331,9 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam counter++; } - std::cout << "reindex data FINISHED" << std::endl; + spdlog::info("reindex data FINISHED"); - std::cout << "build regular grid decomposition START" << std::endl; + spdlog::info("build regular grid decomposition START"); // update_rgd2(params.in_out_params, buckets, points_global, trajectory[points_global[0].index_pose].translation()); @@ -3464,16 +3438,16 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam bb.second.valid = false; } } - std::cout << "build regular grid decomposition FINISHED" << std::endl; + spdlog::info("build regular grid decomposition FINISHED"); - std::cout << "ndt observations start START" << std::endl; + spdlog::info("ndt observations start START"); counter = 0; for (int i = 0; i < points_global.size(); i++) { if (i % 10000 == 0) - std::cout << "computing " << i << " of " << points_global.size() << std::endl; + spdlog::info("computing {} of {}", i, points_global.size()); if (points_local[i].point.norm() < 1.0) continue; @@ -3621,7 +3595,7 @@ void Consistency2(std::vector& worker_data, const LidarOdometryParam AtPB += AtPBndt; } - std::cout << "ndt observations start FINISHED" << std::endl; + spdlog::info("ndt observations start FINISHED"); Eigen::SimplicialCholesky> solver(AtPA); Eigen::SparseMatrix x = solver.solve(AtPB); diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index 4de91df0..b006f7d6 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -32,6 +32,7 @@ target_link_libraries( livox_mid_360_intrinsic_calibration PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index b4f92ea6..be54497d 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -42,6 +42,7 @@ target_link_libraries( mandeye_mission_recorder_calibration PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index f6f9074e..7d805e4e 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -45,6 +45,7 @@ target_link_libraries( mandeye_raw_data_viewer PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/mandeye_single_session_viewer/CMakeLists.txt b/apps/mandeye_single_session_viewer/CMakeLists.txt index d2ddc227..57396796 100644 --- a/apps/mandeye_single_session_viewer/CMakeLists.txt +++ b/apps/mandeye_single_session_viewer/CMakeLists.txt @@ -41,6 +41,7 @@ target_link_libraries( mandeye_single_session_viewer PRIVATE Fusion unordered_dense::unordered_dense + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 6e2fd8db..e54ec9a6 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -45,6 +45,7 @@ target_include_directories( # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib PRIVATE ${FREEGLUT_LIBRARY} unordered_dense::unordered_dense + spdlog::spdlog ${OPENGL_gl_LIBRARY} OpenGL::GLU ${PLATFORM_LASZIP_LIB} diff --git a/apps/quick_start_demo/CMakeLists.txt b/apps/quick_start_demo/CMakeLists.txt index 0402513d..1fd7c847 100644 --- a/apps/quick_start_demo/CMakeLists.txt +++ b/apps/quick_start_demo/CMakeLists.txt @@ -31,6 +31,7 @@ target_include_directories( target_link_libraries( quick_start_demo PRIVATE Fusion + spdlog::spdlog ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index d2effa62..0e3441b1 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -123,6 +123,7 @@ target_link_libraries(lidar_odometry_py PRIVATE pybind11::module core_no_gui unordered_dense::unordered_dense + spdlog::spdlog ${PLATFORM_LASZIP_LIB} Fusion ${PLATFORM_MISCELLANEOUS_LIBS} From ca9782bb4a6800e373f799ecdaf07426cddfeb4a Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 13:16:41 +0400 Subject: [PATCH 4/8] UTL library added to 3rdparty (v9.0.0) --- .gitmodules | 3 +++ 3rdparty/UTL | 1 + 2 files changed, 4 insertions(+) create mode 160000 3rdparty/UTL diff --git a/.gitmodules b/.gitmodules index 180090b3..da3afb9d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -50,3 +50,6 @@ [submodule "3rdparty/spdlog"] path = 3rdparty/spdlog url = https://github.com/gabime/spdlog.git +[submodule "3rdparty/UTL"] + path = 3rdparty/UTL + url = https://github.com/DmitriBogdanov/UTL.git diff --git a/3rdparty/UTL b/3rdparty/UTL new file mode 160000 index 00000000..f98fa2ad --- /dev/null +++ b/3rdparty/UTL @@ -0,0 +1 @@ +Subproject commit f98fa2ad8a9579ac5b37453ab7edd1fbeb84ba7d From a8f4b26c46921bbb4581d17ef481e29dce0ed63b Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 16:38:24 +0400 Subject: [PATCH 5/8] feat: add UTL profiler instrumentation to lidar odometry hot path --- CMakeLists.txt | 4 ++ apps/lidar_odometry_step_1/CMakeLists.txt | 7 ++- .../lidar_odometry_gui.cpp | 2 +- .../lidar_odometry_utils_optimizers.cpp | 63 ++++++++++++++++--- .../CMakeLists.txt | 3 + .../CMakeLists.txt | 3 + apps/mandeye_raw_data_viewer/CMakeLists.txt | 3 + apps/quick_start_demo/CMakeLists.txt | 3 + pybind/CMakeLists.txt | 5 +- 9 files changed, 82 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 37091138..b5bead87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -370,6 +370,10 @@ MESSAGE(STATUS "Using bundled unordered_dense from: ${THIRDPARTY_DIRECTORY}/unor add_subdirectory(${THIRDPARTY_DIRECTORY}/spdlog) MESSAGE(STATUS "Using bundled spdlog from: ${THIRDPARTY_DIRECTORY}/spdlog") +# Add UTL header-only utility library +add_subdirectory(${THIRDPARTY_DIRECTORY}/UTL) +MESSAGE(STATUS "Using bundled UTL from: ${THIRDPARTY_DIRECTORY}/UTL") + option(PYBIND "Enable pybind11 bindings" OFF) if(PYBIND) message(STATUS "PYBIND is enabled: fetching pybind.") diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 12008a7d..588a4c45 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -20,7 +20,11 @@ add_executable( lidar_odometry_step_1 ${SOURCES} ) -target_compile_definitions(lidar_odometry_step_1 PRIVATE -DWITH_GUI=1) +option(UTL_PROFILER_DISABLE "Disable UTL profiler (zero overhead when ON)" ON) + +target_compile_definitions(lidar_odometry_step_1 PRIVATE + WITH_GUI=1 + $<$:UTL_PROFILER_DISABLE>) target_include_directories( lidar_odometry_step_1 @@ -47,6 +51,7 @@ target_link_libraries( PRIVATE Fusion unordered_dense::unordered_dense spdlog::spdlog + UTL::include # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 4cf2f4a4..8a5f7230 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -25,8 +25,8 @@ #include #include #include -#include #include +#include #ifdef _WIN32 #include "resource.h" 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 89d6d21d..dbb6e4f5 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,7 +1,8 @@ #include "lidar_odometry_utils.h" +#include +#include #include #include -#include #include #include #include @@ -1774,6 +1775,8 @@ void optimize_lidar_odometry( bool ablation_study_use_view_point_and_normal_vectors, LookupStats& lookup_stats) { + UTL_PROFILER_SCOPE("optimize_lidar_odometry"); + UTL_PROFILER_BEGIN(pre_hessian, "pre_hessian"); 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; @@ -1805,6 +1808,9 @@ void optimize_lidar_odometry( for (const auto& pose : intermediate_trajectory) tait_bryan_poses.emplace_back(pose_tait_bryan_from_affine_matrix(pose)); + UTL_PROFILER_END(pre_hessian); + + UTL_PROFILER_BEGIN(hessian_compute, "hessian_compute"); if (multithread) { using MatrixPair = std::pair; @@ -1880,7 +1886,9 @@ void optimize_lidar_odometry( lookup_stats); } } + UTL_PROFILER_END(hessian_compute); + UTL_PROFILER_BEGIN(post_hessian, "post_hessian"); std::vector> odo_edges; for (size_t i = 1; i < intermediate_trajectory.size(); i++) odo_edges.emplace_back(i - 1, i); @@ -2101,6 +2109,7 @@ void optimize_lidar_odometry( for (int i = 0; i < h_x.size(); i++) delta += sqrt(h_x[i] * h_x[i]); } + UTL_PROFILER_END(post_hessian); return; } @@ -2233,6 +2242,8 @@ bool compute_step_2( bool debug = false; bool debug2 = true; + UTL_PROFILER_SCOPE("compute_step_2"); + if (params.ablation_study_use_hierarchical_rgd) { if (is_integer_bucket_ratio(params.in_out_params_indoor, params.in_out_params_outdoor)) @@ -2296,6 +2307,7 @@ bool compute_step_2( for (int i = 0; i < worker_data.size(); i++) { + UTL_PROFILER_BEGIN(before_iter, "before_iterations"); std::vector intermediate_points; // = worker_data[i].load_points(worker_data[i].intermediate_points_cache_file_name); load_vector_data(worker_data[i].intermediate_points_cache_file_name.string(), intermediate_points); @@ -2489,6 +2501,9 @@ bool compute_step_2( int iter_end = 0; + UTL_PROFILER_END(before_iter); + UTL_PROFILER_BEGIN(iter_loop, "iteration_loop"); + for (int iter = 0; iter < params.nr_iter; iter++) { iter_end = iter; @@ -2540,22 +2555,40 @@ bool compute_step_2( if (stopwatch_realtime.elapsed().count() > params.real_time_threshold_seconds) break; } + UTL_PROFILER_END(iter_loop); + UTL_PROFILER_BEGIN(after_iter, "after_iterations"); const double elapsed_seconds1 = stopwatch_worker.elapsed().count(); total_iterations += iter_end + 1; total_optimization_time_seconds += elapsed_seconds1; 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, i + 1, worker_data.size(), acc_distance, elapsed_seconds1, delta); + spdlog::info( + "finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta {:e}!!!", + iter_end + 1, + params.nr_iter, + i + 1, + worker_data.size(), + acc_distance, + elapsed_seconds1, + delta); else - spdlog::info("finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta < {:.1e} (converged)", - iter_end + 1, params.nr_iter, i + 1, worker_data.size(), acc_distance, elapsed_seconds1, params.convergence_delta_threshold); + spdlog::info( + "finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta < {:.1e} " + "(converged)", + iter_end + 1, + params.nr_iter, + i + 1, + worker_data.size(), + acc_distance, + elapsed_seconds1, + params.convergence_delta_threshold); loProgress.store((float)(i + 1) / worker_data.size()); // temp save + UTL_PROFILER_BEGIN(temp_save, "temp_save_laz"); if (i % 100 == 0) { std::vector global_pointcloud; @@ -2574,6 +2607,8 @@ bool compute_step_2( std::string fn = params.working_directory_preview + "/temp_point_cloud_" + std::to_string(i) + ".laz"; exportLaz(fn.c_str(), global_pointcloud, intensity, timestamps); } + UTL_PROFILER_END(temp_save); + auto acc_distance_tmp = acc_distance; acc_distance += ((worker_data[i].intermediate_trajectory[0].inverse()) * worker_data[i].intermediate_trajectory[worker_data[i].intermediate_trajectory.size() - 1]) @@ -2587,10 +2622,12 @@ bool compute_step_2( acc_distance = acc_distance_tmp; spdlog::warn("please split data set into subsets"); ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - spdlog::warn("calculations canceled for TIMESTAMP: {}", (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first); + spdlog::warn( + "calculations canceled for TIMESTAMP: {}", (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first); return false; } + UTL_PROFILER_BEGIN(propagate_traj, "propagate_trajectory"); for (int j = i + 1; j < worker_data.size(); j++) { Eigen::Affine3d m_last = worker_data[j - 1].intermediate_trajectory[worker_data[j - 1].intermediate_trajectory.size() - 1]; @@ -2605,15 +2642,19 @@ bool compute_step_2( worker_data[j].intermediate_trajectory[k] = m_last; } } + UTL_PROFILER_END(propagate_traj); + UTL_PROFILER_BEGIN(transform_pts, "transform_points_global"); for (int j = 0; j < intermediate_points.size(); j++) { Point3Di pp = intermediate_points[j]; pp.point = worker_data[i].intermediate_trajectory[intermediate_points[j].index_pose] * pp.point; points_global.push_back(pp); } + UTL_PROFILER_END(transform_pts); // if(reference_points.size() == 0){ + UTL_PROFILER_BEGIN(update_rgd_after, "update_rgd"); if (acc_distance > params.sliding_window_trajectory_length_threshold) { spdlog::stopwatch stopwatch_update; @@ -2691,6 +2732,7 @@ bool compute_step_2( &lookup_stats.indoor_lookups); } } + UTL_PROFILER_END(update_rgd_after); if (i > 1) { @@ -2699,6 +2741,7 @@ bool compute_step_2( .norm(); params.consecutive_distance += translation; } + UTL_PROFILER_END(after_iter); } for (int i = 0; i < worker_data.size(); i++) @@ -2717,8 +2760,12 @@ bool compute_step_2( spdlog::info("total_optimization_time: {:.2f}s", total_optimization_time_seconds); const double avg_iteration_ms = (total_iterations > 0) ? (total_optimization_time_seconds * 1000.0 / total_iterations) : 0.0; spdlog::info("avg_iteration_time: {:.3f}ms", avg_iteration_ms); - spdlog::debug("lookup_stats: indoor={} outdoor_lookups={} outdoor_pointer_hits={} link_time={:.3f}s", - lookup_stats.indoor_lookups, lookup_stats.outdoor_lookups, lookup_stats.outdoor_pointer_hits, lookup_stats.link_time_seconds); + spdlog::debug( + "lookup_stats: indoor={} outdoor_lookups={} outdoor_pointer_hits={} link_time={:.3f}s", + lookup_stats.indoor_lookups, + lookup_stats.outdoor_lookups, + lookup_stats.outdoor_pointer_hits, + lookup_stats.link_time_seconds); } return true; diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index b006f7d6..ae2a6317 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -28,11 +28,14 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) +target_compile_definitions(livox_mid_360_intrinsic_calibration PRIVATE UTL_PROFILER_DISABLE) + target_link_libraries( livox_mid_360_intrinsic_calibration PRIVATE Fusion unordered_dense::unordered_dense spdlog::spdlog + UTL::include ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index be54497d..360fd81b 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -38,11 +38,14 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) +target_compile_definitions(mandeye_mission_recorder_calibration PRIVATE UTL_PROFILER_DISABLE) + target_link_libraries( mandeye_mission_recorder_calibration PRIVATE Fusion unordered_dense::unordered_dense spdlog::spdlog + UTL::include ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index 7d805e4e..971360d3 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -41,11 +41,14 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) +target_compile_definitions(mandeye_raw_data_viewer PRIVATE UTL_PROFILER_DISABLE) + target_link_libraries( mandeye_raw_data_viewer PRIVATE Fusion unordered_dense::unordered_dense spdlog::spdlog + UTL::include ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/apps/quick_start_demo/CMakeLists.txt b/apps/quick_start_demo/CMakeLists.txt index 1fd7c847..429563f8 100644 --- a/apps/quick_start_demo/CMakeLists.txt +++ b/apps/quick_start_demo/CMakeLists.txt @@ -28,10 +28,13 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) +target_compile_definitions(quick_start_demo PRIVATE UTL_PROFILER_DISABLE) + target_link_libraries( quick_start_demo PRIVATE Fusion spdlog::spdlog + UTL::include ${FREEGLUT_LIBRARY} ${OPENGL_gl_LIBRARY} OpenGL::GLU diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index 0e3441b1..4f93b3e2 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -34,7 +34,9 @@ if (MSVC) endif() add_library(lidar_odometry_py MODULE lidar_odometry_binding.cpp) -target_compile_definitions(lidar_odometry_py PRIVATE -DWITH_GUI=0) +target_compile_definitions(lidar_odometry_py PRIVATE + WITH_GUI=0 + $<$:UTL_PROFILER_DISABLE>) if (MSVC) target_compile_options(lidar_odometry_py PRIVATE /bigobj) endif() @@ -124,6 +126,7 @@ target_link_libraries(lidar_odometry_py core_no_gui unordered_dense::unordered_dense spdlog::spdlog + UTL::include ${PLATFORM_LASZIP_LIB} Fusion ${PLATFORM_MISCELLANEOUS_LIBS} From 6420608d531ba1f3463a98c211647242ed3717e4 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 17:47:51 +0400 Subject: [PATCH 6/8] fix: end UTL profiler segment before early return --- apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp | 1 + 1 file changed, 1 insertion(+) 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 dbb6e4f5..23289a5d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2624,6 +2624,7 @@ bool compute_step_2( ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; spdlog::warn( "calculations canceled for TIMESTAMP: {}", (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first); + UTL_PROFILER_END(after_iter); return false; } From 26b8aa99da76d0b74f01dda2922b15d5e335bcc9 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 18:03:56 +0400 Subject: [PATCH 7/8] refactor: move UTL_PROFILER_DISABLE option to root CMakeLists --- CMakeLists.txt | 2 ++ apps/lidar_odometry_step_1/CMakeLists.txt | 2 -- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b5bead87..f63a086f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,8 @@ if (MSVC) add_definitions(-DWIN32_LEAN_AND_MEAN) endif() +option(UTL_PROFILER_DISABLE "Disable UTL profiler (zero overhead when ON)" ON) + get_filename_component(REPOSITORY_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} ABSOLUTE) set(THIRDPARTY_DIRECTORY ${REPOSITORY_DIRECTORY}/3rdparty) set(THIRDPARTY_DIRECTORY_BINARY ${REPOSITORY_DIRECTORY}/3rdpartyBinary) diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 588a4c45..d09526a9 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -20,8 +20,6 @@ add_executable( lidar_odometry_step_1 ${SOURCES} ) -option(UTL_PROFILER_DISABLE "Disable UTL profiler (zero overhead when ON)" ON) - target_compile_definitions(lidar_odometry_step_1 PRIVATE WITH_GUI=1 $<$:UTL_PROFILER_DISABLE>) From 69eca5296b80e52a7bef90ed5e68f767920ddd7f Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 27 Jan 2026 19:21:13 +0400 Subject: [PATCH 8/8] pybind linux build fix --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f63a086f..bd24a000 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -369,6 +369,7 @@ add_subdirectory(${THIRDPARTY_DIRECTORY}/unordered_dense) MESSAGE(STATUS "Using bundled unordered_dense from: ${THIRDPARTY_DIRECTORY}/unordered_dense") # Add spdlog logging library +set(SPDLOG_BUILD_PIC ON CACHE BOOL "Build spdlog with position-independent code") add_subdirectory(${THIRDPARTY_DIRECTORY}/spdlog) MESSAGE(STATUS "Using bundled spdlog from: ${THIRDPARTY_DIRECTORY}/spdlog")