Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Jan 12, 2024
1 parent a462006 commit 5f1ac7c
Showing 1 changed file with 8 additions and 6 deletions.
14 changes: 8 additions & 6 deletions localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -486,11 +486,13 @@ void NDTScanMatcher::callback_sensor_points(
}

// covariance estimation
Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond(result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y, result_pose_msg.orientation.z);
Eigen::Quaterniond map_to_base_link_quat = Eigen::Quaterniond(
result_pose_msg.orientation.w, result_pose_msg.orientation.x, result_pose_msg.orientation.y,
result_pose_msg.orientation.z);
Eigen::Matrix3d map_to_base_link_rotation = map_to_base_link_quat.normalized().toRotationMatrix();

std::array<double, 36> ndt_covariance = rotate_covariance(output_pose_covariance_, map_to_base_link_rotation);

std::array<double, 36> ndt_covariance =
rotate_covariance(output_pose_covariance_, map_to_base_link_rotation);

Check warning on line 496 in localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

NDTScanMatcher::callback_sensor_points already has high cyclomatic complexity, and now it increases in Lines of Code from 137 to 142. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
if (is_converged && use_cov_estimation_) {
const auto estimated_covariance =
Expand Down Expand Up @@ -777,9 +779,9 @@ std::array<double, 36> NDTScanMatcher::rotate_covariance(
std::array<double, 36> ret_covariance = src_covariance;

Eigen::Matrix3d src_cov;
src_cov << src_covariance[0], src_covariance[1], src_covariance[2],
src_covariance[6], src_covariance[7], src_covariance[8],
src_covariance[12], src_covariance[13], src_covariance[14];
src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6],
src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13],
src_covariance[14];

Eigen::Matrix3d ret_cov;
ret_cov = rotation * src_cov * rotation.transpose();
Expand Down

0 comments on commit 5f1ac7c

Please sign in to comment.