diff --git a/cartographer/common/math.h b/cartographer/common/math.h index ea96c0b..f0ae9e2 100644 --- a/cartographer/common/math.h +++ b/cartographer/common/math.h @@ -51,14 +51,6 @@ constexpr T Pow2(T a) { return Power(a, 2); } -// Calculates the real part of the square root of 'a'. This is helpful when -// rounding errors generate a small negative argument. Otherwise std::sqrt -// returns NaN if its argument is negative. -template -constexpr T RealSqrt(T a) { - return sqrt(std::max(T(0.), a)); -} - // Converts from degrees to radians. constexpr double DegToRad(double deg) { return M_PI * deg / 180.; } diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index 8a8b8a4..b8875c1 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -249,9 +249,9 @@ std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( // and rotation of the 'initial_pose', so that the rotation is around the // origin of the range data, and yaw is in map frame. const transform::Rigid3f pose( - Eigen::Translation3f(initial_pose.translation()) * + initial_pose.translation(), transform::AngleAxisVectorToRotationQuaternion(angle_axis) * - Eigen::Quaternionf(initial_pose.rotation())); + initial_pose.rotation()); result.push_back( DiscretizeScan(search_parameters, coarse_point_cloud, pose)); } diff --git a/cartographer/transform/rigid_transform.h b/cartographer/transform/rigid_transform.h index 5a45fb3..16779fb 100644 --- a/cartographer/transform/rigid_transform.h +++ b/cartographer/transform/rigid_transform.h @@ -32,7 +32,6 @@ namespace transform { template class Rigid2 { public: - using Affine = Eigen::Transform; using Vector = Eigen::Matrix; using Rotation2D = Eigen::Rotation2D; @@ -125,16 +124,12 @@ using Rigid2f = Rigid2; template class Rigid3 { public: - using Affine = Eigen::Transform; using Vector = Eigen::Matrix; using Quaternion = Eigen::Quaternion; using AngleAxis = Eigen::AngleAxis; Rigid3() : translation_(Vector::Identity()), rotation_(Quaternion::Identity()) {} - // TODO(damonkohler): Remove - explicit Rigid3(const Affine& affine) - : translation_(affine.translation()), rotation_(affine.rotation()) {} Rigid3(const Vector& translation, const Quaternion& rotation) : translation_(translation), rotation_(rotation) {} Rigid3(const Vector& translation, const AngleAxis& rotation)