From 7b8f553877e0377574f78575cd7ee2fe461366ca Mon Sep 17 00:00:00 2001 From: Danil An Date: Mon, 19 Jan 2026 14:50:55 +0400 Subject: [PATCH 1/6] convergence_delta_threshold parameter added to .toml config (lidar_odometry_control section) log print fixed --- apps/lidar_odometry_step_1/lidar_odometry_utils.h | 1 + .../lidar_odometry_utils_optimizers.cpp | 12 ++++++------ apps/lidar_odometry_step_1/toml_io.h | 4 +++- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 0729ae87..c57a74ce 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -76,6 +76,7 @@ struct LidarOdometryParams double max_distance_lidar = 70.0; // I am not processing data above dist distance in lidar odometry int threshold_initial_points = 10000; int threshold_nr_poses = 20; + double convergence_delta_threshold = 1e-12; // convergence threshold for optimization // lidar odometry debug info bool save_calibration_validation = false; 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 53d808e4..c69f151a 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2349,7 +2349,7 @@ bool compute_step_2( params.ablation_study_use_norm, params.ablation_study_use_hierarchical_rgd, params.ablation_study_use_view_point_and_normal_vectors); - if (delta < 1e-12) + if (delta < params.convergence_delta_threshold) { // std::cout << "finished at iteration " << iter + 1 << "/" << params.nr_iter; break; @@ -2379,12 +2379,12 @@ bool compute_step_2( std::cout << " optimizing worker_data " << i + 1 << "/" << worker_data.size() << " with acc_distance " << fixed << std::setprecision(2) << acc_distance << "[m] in " << fixed << std::setprecision(2) << elapsed_seconds1.count() << "[s], delta "; - if (delta > 1e-12) - std::cout << std::setprecision(10) << delta << "!!!"; + if (delta > params.convergence_delta_threshold) + std::cout << std::scientific << std::setprecision(3) << delta << "!!!" << std::fixed << std::setprecision(3); else - std::cout << "< 1e-12"; - - std::cout << "\n"; + std::cout << "< " << std::scientific << std::setprecision(1) << params.convergence_delta_threshold << " (converged)" + << std::fixed << std::setprecision(3); + std::cout << std::endl; loProgress.store((float)(i + 1) / worker_data.size()); diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index 75648e64..68740e6b 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -44,6 +44,7 @@ class TomlIO { "max_distance_lidar", &LidarOdometryParams::max_distance_lidar }, { "threshold_initial_points", &LidarOdometryParams::threshold_initial_points }, { "threshold_nr_poses", &LidarOdometryParams::threshold_nr_poses }, + { "convergence_delta_threshold", &LidarOdometryParams::convergence_delta_threshold }, { "save_calibration_validation", &LidarOdometryParams::save_calibration_validation }, { "calibration_validation_points", &LidarOdometryParams::calibration_validation_points }, { "num_constistency_iter", &LidarOdometryParams::num_constistency_iter }, @@ -107,7 +108,8 @@ class TomlIO "sliding_window_trajectory_length_threshold", "max_distance_lidar", "threshold_initial_points", - "threshold_nr_poses" } }, + "threshold_nr_poses", + "convergence_delta_threshold" } }, { "lidar_odometry_debug_info", { "save_calibration_validation", "calibration_validation_points" } }, { "consistency", { "num_constistency_iter", "use_mutliple_gaussian" } }, { "motion_model_uncertainty", From 361cd2b6e65246fd529aa0a16ae2d674f1453b4a Mon Sep 17 00:00:00 2001 From: Danil An Date: Mon, 19 Jan 2026 20:11:55 +0400 Subject: [PATCH 2/6] compute_hessian_indoor and compute_hessian_outdoor moved to separate functions --- .../lidar_odometry_utils_optimizers.cpp | 522 ++++++++++-------- 1 file changed, 287 insertions(+), 235 deletions(-) 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 c69f151a..2e74bd13 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1421,6 +1421,268 @@ void optimize_rigid_sf( intermediate_trajectory_motion_model = _intermediate_trajectory; } +static void compute_hessian_indoor( + const Point3Di& intermediate_points_i, + const std::vector& intermediate_trajectory, + const NDTBucketMapType& buckets_indoor, + const Eigen::Vector3d& b_indoor, + double max_distance, + bool ablation_study_use_view_point_and_normal_vectors, + bool ablation_study_use_planarity, + bool ablation_study_use_norm, + std::vector& mutexes, + Eigen::MatrixXd& AtPAndt, + Eigen::MatrixXd& AtPBndt) +{ + if (intermediate_points_i.point.norm() < 0.1 || intermediate_points_i.point.norm() > max_distance) // ToDo + return; + + Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; + auto index_of_bucket = get_rgd_index(point_global, b_indoor); + + auto bucket_it = buckets_indoor.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets_indoor.end()) + return; + + auto& this_bucket = bucket_it->second; + + // if(buckets[index_of_bucket].number_of_points >= 5){ + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); + const double threshold = 100000.0; + + if ((infm.array() > threshold).any()) + return; + + if ((infm.array() < -threshold).any()) + return; + + if (ablation_study_use_view_point_and_normal_vectors) + { + // check nv + const Eigen::Vector3d& nv = this_bucket.normal_vector; + Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); + if (nv.dot(viewport - this_bucket.mean) < 0) + return; + } + + const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points_i.point; + const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + // + + Eigen::Matrix AtPA; + point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( + AtPA, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); + + Eigen::Matrix AtPB; + point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( + AtPB, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); + + int c = intermediate_points_i.index_pose * 6; + + double planarity = 1.0; + + if (ablation_study_use_planarity) + { + // planarity + Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); + auto eigen_values = eigen_solver.eigenvalues(); + auto eigen_vectors = eigen_solver.eigenvectors(); + double ev1 = eigen_values.x(); + double ev2 = eigen_values.y(); + double ev3 = eigen_values.z(); + double sum_ev = ev1 + ev2 + ev3; + planarity = 1 - ((3 * ev1 / sum_ev) * (3 * ev2 / sum_ev) * (3 * ev3 / sum_ev)); + } + + double norm = 1.0; + if (ablation_study_use_norm) + norm = p_s.norm(); + + double w = planarity * norm; + if (w > 10.0) + w = 10.0; + + if (ablation_study_use_planarity || ablation_study_use_norm) + { + AtPA *= w; + AtPB *= w; + } + + std::mutex& m = mutexes[intermediate_points_i.index_pose]; + std::unique_lock lck(m); + AtPAndt.block<6, 6>(c, c) += AtPA; + AtPBndt.block<6, 1>(c, 0) -= AtPB; +} + +static void compute_hessian_outdoor( + const Point3Di& intermediate_points_i, + const std::vector& intermediate_trajectory, + const NDTBucketMapType& buckets_outdoor, + const Eigen::Vector3d& b_outdoor, + double max_distance, + bool ablation_study_use_view_point_and_normal_vectors, + bool ablation_study_use_planarity, + std::vector& mutexes, + Eigen::MatrixXd& AtPAndt, + Eigen::MatrixXd& AtPBndt) +{ + if (intermediate_points_i.point.norm() < 5.0 || intermediate_points_i.point.norm() > max_distance) // ToDo + return; + + Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; + auto index_of_bucket = get_rgd_index(point_global, b_outdoor); + + auto bucket_it = buckets_outdoor.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets_outdoor.end()) + return; + + auto& this_bucket = bucket_it->second; + + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); + const double threshold = 100000.0; + + if ((infm.array() > threshold).any()) + return; + + if ((infm.array() < -threshold).any()) + return; + + if (ablation_study_use_view_point_and_normal_vectors) + { + // check nv + const Eigen::Vector3d& nv = this_bucket.normal_vector; + Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); + if (nv.dot(viewport - this_bucket.mean) < 0) + return; + } + + const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; + const Eigen::Vector3d& p_s = intermediate_points_i.point; + const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + // + + Eigen::Matrix AtPA; + point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( + AtPA, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2)); + + Eigen::Matrix AtPB; + point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( + AtPB, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + infm(0, 0), + infm(0, 1), + infm(0, 2), + infm(1, 0), + infm(1, 1), + infm(1, 2), + infm(2, 0), + infm(2, 1), + infm(2, 2), + this_bucket.mean.x(), + this_bucket.mean.y(), + this_bucket.mean.z()); + + int c = intermediate_points_i.index_pose * 6; + + double planarity = 1.0; + + if (ablation_study_use_planarity) + { + // planarity + Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); + auto eigen_values = eigen_solver.eigenvalues(); + auto eigen_vectors = eigen_solver.eigenvectors(); + double ev1 = eigen_values.x(); + double ev2 = eigen_values.y(); + double ev3 = eigen_values.z(); + double sum_ev = ev1 + ev2 + ev3; + planarity = 1 - ((3 * ev1 / sum_ev) * (3 * ev2 / sum_ev) * (3 * ev3 / sum_ev)); + + // double norm = p_s.norm(); + + // if (norm < 5.0) + //{ + // return; + // } + + AtPA *= planarity; + AtPB *= planarity; + } + + std::mutex& m = mutexes[intermediate_points_i.index_pose]; + std::unique_lock lck(m); + AtPAndt.block<6, 6>(c, c) += AtPA; + AtPBndt.block<6, 1>(c, 0) -= AtPB; +} + void optimize_lidar_odometry( std::vector& intermediate_points, std::vector& intermediate_trajectory, @@ -1475,127 +1737,20 @@ void optimize_lidar_odometry( std::vector mutexes(intermediate_trajectory.size()); - const auto hessian_fun_indoor = [&](const Point3Di& intermediate_points_i) + auto hessian_fun_indoor = [&](const Point3Di& pt) { - if (intermediate_points_i.point.norm() < 0.1 || intermediate_points_i.point.norm() > max_distance) // ToDo - return; - - Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; - auto index_of_bucket = get_rgd_index(point_global, b_indoor); - - auto bucket_it = buckets_indoor.find(index_of_bucket); - // no bucket found - if (bucket_it == buckets_indoor.end()) - return; - - auto& this_bucket = bucket_it->second; - - // if(buckets[index_of_bucket].number_of_points >= 5){ - const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); - const double threshold = 100000.0; - - if ((infm.array() > threshold).any()) - return; - - if ((infm.array() < -threshold).any()) - return; - - if (ablation_study_use_view_point_and_normal_vectors) - { - // check nv - Eigen::Vector3d& nv = this_bucket.normal_vector; - Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); - if (nv.dot(viewport - this_bucket.mean) < 0) - return; - } - - const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - // - - Eigen::Matrix AtPA; - point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( - AtPA, - pose_s.px, - pose_s.py, - pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, - p_s.x(), - p_s.y(), - p_s.z(), - infm(0, 0), - infm(0, 1), - infm(0, 2), - infm(1, 0), - infm(1, 1), - infm(1, 2), - infm(2, 0), - infm(2, 1), - infm(2, 2)); - - Eigen::Matrix AtPB; - point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( - AtPB, - pose_s.px, - pose_s.py, - pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, - p_s.x(), - p_s.y(), - p_s.z(), - infm(0, 0), - infm(0, 1), - infm(0, 2), - infm(1, 0), - infm(1, 1), - infm(1, 2), - infm(2, 0), - infm(2, 1), - infm(2, 2), - this_bucket.mean.x(), - this_bucket.mean.y(), - this_bucket.mean.z()); - - int c = intermediate_points_i.index_pose * 6; - - double planarity = 1.0; - - if (ablation_study_use_planarity) - { - // planarity - Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); - auto eigen_values = eigen_solver.eigenvalues(); - auto eigen_vectors = eigen_solver.eigenvectors(); - double ev1 = eigen_values.x(); - double ev2 = eigen_values.y(); - double ev3 = eigen_values.z(); - double sum_ev = ev1 + ev2 + ev3; - planarity = 1 - ((3 * ev1 / sum_ev) * (3 * ev2 / sum_ev) * (3 * ev3 / sum_ev)); - } - - double norm = 1.0; - if (ablation_study_use_norm) - norm = p_s.norm(); - - double w = planarity * norm; - if (w > 10.0) - w = 10.0; - - if (ablation_study_use_planarity || ablation_study_use_norm) - { - AtPA *= w; - AtPB *= w; - } - - std::mutex& m = mutexes[intermediate_points_i.index_pose]; - std::unique_lock lck(m); - AtPAndt.block<6, 6>(c, c) += AtPA; - AtPBndt.block<6, 1>(c, 0) -= AtPB; + compute_hessian_indoor( + pt, + intermediate_trajectory, + buckets_indoor, + b_indoor, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity, + ablation_study_use_norm, + mutexes, + AtPAndt, + AtPBndt); }; if (multithread) @@ -1607,122 +1762,19 @@ void optimize_lidar_odometry( { Eigen::Vector3d b_outdoor(rgd_params_outdoor.resolution_X, rgd_params_outdoor.resolution_Y, rgd_params_outdoor.resolution_Z); - const auto hessian_fun_outdoor = [&](const Point3Di& intermediate_points_i) + auto hessian_fun_outdoor = [&](const Point3Di& pt) { - if (intermediate_points_i.point.norm() < 5.0 || intermediate_points_i.point.norm() > max_distance) // ToDo - return; - - Eigen::Vector3d point_global = intermediate_trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point; - auto index_of_bucket = get_rgd_index(point_global, b_outdoor); - - auto bucket_it = buckets_outdoor.find(index_of_bucket); - // no bucket found - if (bucket_it == buckets_outdoor.end()) - return; - - auto& this_bucket = bucket_it->second; - - const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); - const double threshold = 100000.0; - - if ((infm.array() > threshold).any()) - return; - - if ((infm.array() < -threshold).any()) - return; - - if (ablation_study_use_view_point_and_normal_vectors) - { - // check nv - Eigen::Vector3d& nv = this_bucket.normal_vector; - Eigen::Vector3d viewport = intermediate_trajectory[intermediate_points_i.index_pose].translation(); - if (nv.dot(viewport - this_bucket.mean) < 0) - return; - } - - const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; - const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - // - - Eigen::Matrix AtPA; - point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( - AtPA, - pose_s.px, - pose_s.py, - pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, - p_s.x(), - p_s.y(), - p_s.z(), - infm(0, 0), - infm(0, 1), - infm(0, 2), - infm(1, 0), - infm(1, 1), - infm(1, 2), - infm(2, 0), - infm(2, 1), - infm(2, 2)); - - Eigen::Matrix AtPB; - point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( - AtPB, - pose_s.px, - pose_s.py, - pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, - p_s.x(), - p_s.y(), - p_s.z(), - infm(0, 0), - infm(0, 1), - infm(0, 2), - infm(1, 0), - infm(1, 1), - infm(1, 2), - infm(2, 0), - infm(2, 1), - infm(2, 2), - this_bucket.mean.x(), - this_bucket.mean.y(), - this_bucket.mean.z()); - - int c = intermediate_points_i.index_pose * 6; - - double planarity = 1.0; - - if (ablation_study_use_planarity) - { - // planarity - Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); - auto eigen_values = eigen_solver.eigenvalues(); - auto eigen_vectors = eigen_solver.eigenvectors(); - double ev1 = eigen_values.x(); - double ev2 = eigen_values.y(); - double ev3 = eigen_values.z(); - double sum_ev = ev1 + ev2 + ev3; - planarity = 1 - ((3 * ev1 / sum_ev) * (3 * ev2 / sum_ev) * (3 * ev3 / sum_ev)); - - // double norm = p_s.norm(); - - // if (norm < 5.0) - //{ - // return; - // } - - AtPA *= planarity; - AtPB *= planarity; - } - - std::mutex& m = mutexes[intermediate_points_i.index_pose]; - std::unique_lock lck(m); - AtPAndt.block<6, 6>(c, c) += AtPA; - AtPBndt.block<6, 1>(c, 0) -= AtPB; + compute_hessian_outdoor( + pt, + intermediate_trajectory, + buckets_outdoor, + b_outdoor, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity, + mutexes, + AtPAndt, + AtPBndt); }; if (multithread) From 6646cc337d52f0ba34d2ab00da5b879b9a2a14f9 Mon Sep 17 00:00:00 2001 From: Danil An Date: Mon, 19 Jan 2026 21:19:57 +0400 Subject: [PATCH 3/6] tait-bryan poses precalculated for intermediated trajectory (lidar odometry) --- .../lidar_odometry_utils_optimizers.cpp | 41 +++++++++++++++---- core/include/transformations.h | 2 +- 2 files changed, 34 insertions(+), 9 deletions(-) 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 2e74bd13..8b446345 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1424,6 +1424,7 @@ void optimize_rigid_sf( static void compute_hessian_indoor( const Point3Di& intermediate_points_i, const std::vector& intermediate_trajectory, + const std::vector& tait_bryan_poses, const NDTBucketMapType& buckets_indoor, const Eigen::Vector3d& b_indoor, double max_distance, @@ -1466,10 +1467,8 @@ static void compute_hessian_indoor( return; } - const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - // + const TaitBryanPose& pose_s = tait_bryan_poses[intermediate_points_i.index_pose]; Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( @@ -1558,6 +1557,7 @@ static void compute_hessian_indoor( static void compute_hessian_outdoor( const Point3Di& intermediate_points_i, const std::vector& intermediate_trajectory, + const std::vector& tait_bryan_poses, const NDTBucketMapType& buckets_outdoor, const Eigen::Vector3d& b_outdoor, double max_distance, @@ -1598,10 +1598,8 @@ static void compute_hessian_outdoor( return; } - const Eigen::Affine3d& m_pose = intermediate_trajectory[intermediate_points_i.index_pose]; const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); - // + const TaitBryanPose& pose_s = tait_bryan_poses[intermediate_points_i.index_pose]; Eigen::Matrix AtPA; point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( @@ -1735,13 +1733,30 @@ void optimize_lidar_odometry( AtPBndt.setZero(); Eigen::Vector3d b_indoor(rgd_params_indoor.resolution_X, rgd_params_indoor.resolution_Y, rgd_params_indoor.resolution_Z); + // Pre-compute TaitBryanPose conversions for all trajectory poses + std::vector tait_bryan_poses; + tait_bryan_poses.reserve(intermediate_trajectory.size()); + for (const auto& pose : intermediate_trajectory) + tait_bryan_poses.emplace_back(pose_tait_bryan_from_affine_matrix(pose)); + std::vector mutexes(intermediate_trajectory.size()); - auto hessian_fun_indoor = [&](const Point3Di& pt) + auto hessian_fun_indoor = [&intermediate_trajectory, + &tait_bryan_poses, + &buckets_indoor, + &b_indoor, + &mutexes, + &AtPAndt, + &AtPBndt, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity, + ablation_study_use_norm](const Point3Di& pt) { compute_hessian_indoor( pt, intermediate_trajectory, + tait_bryan_poses, buckets_indoor, b_indoor, max_distance, @@ -1762,11 +1777,21 @@ void optimize_lidar_odometry( { Eigen::Vector3d b_outdoor(rgd_params_outdoor.resolution_X, rgd_params_outdoor.resolution_Y, rgd_params_outdoor.resolution_Z); - auto hessian_fun_outdoor = [&](const Point3Di& pt) + auto hessian_fun_outdoor = [&intermediate_trajectory, + &tait_bryan_poses, + &buckets_outdoor, + &b_outdoor, + &mutexes, + &AtPAndt, + &AtPBndt, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity](const Point3Di& pt) { compute_hessian_outdoor( pt, intermediate_trajectory, + tait_bryan_poses, buckets_outdoor, b_outdoor, max_distance, diff --git a/core/include/transformations.h b/core/include/transformations.h index f48e8d03..58370f68 100644 --- a/core/include/transformations.h +++ b/core/include/transformations.h @@ -22,7 +22,7 @@ inline double rad2deg(double rad) return (rad * 180.0) / M_PI; } -inline TaitBryanPose pose_tait_bryan_from_affine_matrix(Eigen::Affine3d m) +inline TaitBryanPose pose_tait_bryan_from_affine_matrix(const Eigen::Affine3d& m) { TaitBryanPose pose; From c6fd957ce19c6e6a25dd727645a68d49d5105d91 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 20 Jan 2026 18:24:56 +0400 Subject: [PATCH 4/6] perf: use precomputed sin/cos in lidar odometry AtPA/AtPB kernels --- 3rdparty/observation_equations | 2 +- .../lidar_odometry_utils_optimizers.cpp | 78 +++++++++++++------ 2 files changed, 57 insertions(+), 23 deletions(-) diff --git a/3rdparty/observation_equations b/3rdparty/observation_equations index 34dd3f0c..6850f190 160000 --- a/3rdparty/observation_equations +++ b/3rdparty/observation_equations @@ -1 +1 @@ -Subproject commit 34dd3f0c72e8e820135f0040c7226d53faafe76a +Subproject commit 6850f190e380b763c0f6a4f93520e61e7c853aae 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 8b446345..420db7c6 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1421,10 +1421,30 @@ void optimize_rigid_sf( intermediate_trajectory_motion_model = _intermediate_trajectory; } +// Struct to hold pre-computed TaitBryanPose with trigonometric values +struct TaitBryanPoseWithTrig +{ + TaitBryanPose pose; + double sin_om, cos_om; + double sin_fi, cos_fi; + double sin_ka, cos_ka; + + TaitBryanPoseWithTrig(const TaitBryanPose& p) + : pose(p), + sin_om(sin(p.om)), + cos_om(cos(p.om)), + sin_fi(sin(p.fi)), + cos_fi(cos(p.fi)), + sin_ka(sin(p.ka)), + cos_ka(cos(p.ka)) + { + } +}; + static void compute_hessian_indoor( const Point3Di& intermediate_points_i, const std::vector& intermediate_trajectory, - const std::vector& tait_bryan_poses, + const std::vector& tait_bryan_poses, const NDTBucketMapType& buckets_indoor, const Eigen::Vector3d& b_indoor, double max_distance, @@ -1468,17 +1488,21 @@ static void compute_hessian_indoor( } const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose& pose_s = tait_bryan_poses[intermediate_points_i.index_pose]; + const TaitBryanPoseWithTrig& pose_trig = tait_bryan_poses[intermediate_points_i.index_pose]; + const TaitBryanPose& pose_s = pose_trig.pose; Eigen::Matrix AtPA; - point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( + point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_precomputed_trig( AtPA, pose_s.px, pose_s.py, pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, + pose_trig.sin_om, + pose_trig.cos_om, + pose_trig.sin_fi, + pose_trig.cos_fi, + pose_trig.sin_ka, + pose_trig.cos_ka, p_s.x(), p_s.y(), p_s.z(), @@ -1493,14 +1517,17 @@ static void compute_hessian_indoor( infm(2, 2)); Eigen::Matrix AtPB; - point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( + point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_precomputed_trig( AtPB, pose_s.px, pose_s.py, pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, + pose_trig.sin_om, + pose_trig.cos_om, + pose_trig.sin_fi, + pose_trig.cos_fi, + pose_trig.sin_ka, + pose_trig.cos_ka, p_s.x(), p_s.y(), p_s.z(), @@ -1557,7 +1584,7 @@ static void compute_hessian_indoor( static void compute_hessian_outdoor( const Point3Di& intermediate_points_i, const std::vector& intermediate_trajectory, - const std::vector& tait_bryan_poses, + const std::vector& tait_bryan_poses, const NDTBucketMapType& buckets_outdoor, const Eigen::Vector3d& b_outdoor, double max_distance, @@ -1599,17 +1626,21 @@ static void compute_hessian_outdoor( } const Eigen::Vector3d& p_s = intermediate_points_i.point; - const TaitBryanPose& pose_s = tait_bryan_poses[intermediate_points_i.index_pose]; + const TaitBryanPoseWithTrig& pose_trig = tait_bryan_poses[intermediate_points_i.index_pose]; + const TaitBryanPose& pose_s = pose_trig.pose; Eigen::Matrix AtPA; - point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified( + point_to_point_source_to_target_tait_bryan_wc_AtPA_simplified_precomputed_trig( AtPA, pose_s.px, pose_s.py, pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, + pose_trig.sin_om, + pose_trig.cos_om, + pose_trig.sin_fi, + pose_trig.cos_fi, + pose_trig.sin_ka, + pose_trig.cos_ka, p_s.x(), p_s.y(), p_s.z(), @@ -1624,14 +1655,17 @@ static void compute_hessian_outdoor( infm(2, 2)); Eigen::Matrix AtPB; - point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified( + point_to_point_source_to_target_tait_bryan_wc_AtPB_simplified_precomputed_trig( AtPB, pose_s.px, pose_s.py, pose_s.pz, - pose_s.om, - pose_s.fi, - pose_s.ka, + pose_trig.sin_om, + pose_trig.cos_om, + pose_trig.sin_fi, + pose_trig.cos_fi, + pose_trig.sin_ka, + pose_trig.cos_ka, p_s.x(), p_s.y(), p_s.z(), @@ -1733,8 +1767,8 @@ void optimize_lidar_odometry( AtPBndt.setZero(); Eigen::Vector3d b_indoor(rgd_params_indoor.resolution_X, rgd_params_indoor.resolution_Y, rgd_params_indoor.resolution_Z); - // Pre-compute TaitBryanPose conversions for all trajectory poses - std::vector tait_bryan_poses; + // Pre-compute TaitBryanPose conversions with sin/cos for all trajectory poses + std::vector tait_bryan_poses; tait_bryan_poses.reserve(intermediate_trajectory.size()); for (const auto& pose : intermediate_trajectory) tait_bryan_poses.emplace_back(pose_tait_bryan_from_affine_matrix(pose)); From 29e0fc9cee8ac650e3abecc3dcfb3a6bcb2cdc63 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 20 Jan 2026 18:35:35 +0400 Subject: [PATCH 5/6] TODO comment added about possible future optimization --- .../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 420db7c6..4ed5ced7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1550,6 +1550,8 @@ static void compute_hessian_indoor( if (ablation_study_use_planarity) { + // TODO: this can be precalculated during RGD update (and it's better to use closed-form solver) + // planarity Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); auto eigen_values = eigen_solver.eigenvalues(); @@ -1688,6 +1690,8 @@ static void compute_hessian_outdoor( if (ablation_study_use_planarity) { + // TODO: this can be precalculated during RGD update (and it's better to use closed-form solver) + // planarity Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); auto eigen_values = eigen_solver.eigenvalues(); From 0c81e86433dd3bb0c241e1b10638c063dd351040 Mon Sep 17 00:00:00 2001 From: Danil An Date: Tue, 20 Jan 2026 19:10:34 +0400 Subject: [PATCH 6/6] style: clang-format --- .../lidar_odometry_utils_optimizers.cpp | 54 +++++++++---------- 1 file changed, 27 insertions(+), 27 deletions(-) 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 4ed5ced7..22eee87e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1430,13 +1430,13 @@ struct TaitBryanPoseWithTrig double sin_ka, cos_ka; TaitBryanPoseWithTrig(const TaitBryanPose& p) - : pose(p), - sin_om(sin(p.om)), - cos_om(cos(p.om)), - sin_fi(sin(p.fi)), - cos_fi(cos(p.fi)), - sin_ka(sin(p.ka)), - cos_ka(cos(p.ka)) + : pose(p) + , sin_om(sin(p.om)) + , cos_om(cos(p.om)) + , sin_fi(sin(p.fi)) + , cos_fi(cos(p.fi)) + , sin_ka(sin(p.ka)) + , cos_ka(cos(p.ka)) { } }; @@ -1551,7 +1551,7 @@ static void compute_hessian_indoor( if (ablation_study_use_planarity) { // TODO: this can be precalculated during RGD update (and it's better to use closed-form solver) - + // planarity Eigen::SelfAdjointEigenSolver eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors); auto eigen_values = eigen_solver.eigenvalues(); @@ -1780,16 +1780,16 @@ void optimize_lidar_odometry( std::vector mutexes(intermediate_trajectory.size()); auto hessian_fun_indoor = [&intermediate_trajectory, - &tait_bryan_poses, - &buckets_indoor, - &b_indoor, - &mutexes, - &AtPAndt, - &AtPBndt, - max_distance, - ablation_study_use_view_point_and_normal_vectors, - ablation_study_use_planarity, - ablation_study_use_norm](const Point3Di& pt) + &tait_bryan_poses, + &buckets_indoor, + &b_indoor, + &mutexes, + &AtPAndt, + &AtPBndt, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity, + ablation_study_use_norm](const Point3Di& pt) { compute_hessian_indoor( pt, @@ -1816,15 +1816,15 @@ void optimize_lidar_odometry( Eigen::Vector3d b_outdoor(rgd_params_outdoor.resolution_X, rgd_params_outdoor.resolution_Y, rgd_params_outdoor.resolution_Z); auto hessian_fun_outdoor = [&intermediate_trajectory, - &tait_bryan_poses, - &buckets_outdoor, - &b_outdoor, - &mutexes, - &AtPAndt, - &AtPBndt, - max_distance, - ablation_study_use_view_point_and_normal_vectors, - ablation_study_use_planarity](const Point3Di& pt) + &tait_bryan_poses, + &buckets_outdoor, + &b_outdoor, + &mutexes, + &AtPAndt, + &AtPBndt, + max_distance, + ablation_study_use_view_point_and_normal_vectors, + ablation_study_use_planarity](const Point3Di& pt) { compute_hessian_outdoor( pt,