From b352c8edb91739badf7167d8e061c7784bcc14df Mon Sep 17 00:00:00 2001 From: Marc Alban Date: Thu, 28 Jul 2022 09:40:45 -0500 Subject: [PATCH] Build fixes. --- laser_scan_matcher/src/laser_scan_matcher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/laser_scan_matcher/src/laser_scan_matcher.cpp b/laser_scan_matcher/src/laser_scan_matcher.cpp index 7498057..56b0e21 100644 --- a/laser_scan_matcher/src/laser_scan_matcher.cpp +++ b/laser_scan_matcher/src/laser_scan_matcher.cpp @@ -522,7 +522,7 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) yaw_cov = gsl_matrix_get(output_.cov_x_m, 2, 2); // rotate xy covariance from the keyframe into odom frame - auto rotation = getLaserRotation(f2b_kf_); + auto rotation = getLaserRotation(keyframe_base_in_fixed_); xy_cov = rotation * xy_cov * rotation.transpose(); } else { @@ -859,7 +859,7 @@ void LaserScanMatcher::createTfFromXYTheta( } Eigen::Matrix2f LaserScanMatcher::getLaserRotation(const tf::Transform& odom_pose) const { - tf::Transform laser_in_fixed = odom_pose * laser_to_base_; + tf::Transform laser_in_fixed = odom_pose * laser_from_base_; tf::Matrix3x3 fixed_from_laser_rot(laser_in_fixed.getRotation()); double r,p,y; fixed_from_laser_rot.getRPY(r, p, y);