From 962393074a4418963696c9aea771db35a6a991a5 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 27 Mar 2018 14:26:21 +0200 Subject: [PATCH] New fixes for Jets. (#1023) Issue #1015 --- cartographer/common/math.h | 9 +++------ .../internal/pose_graph/cost_helpers_impl.h | 18 +++++++++--------- cartographer/transform/transform.h | 8 ++++---- 3 files changed, 16 insertions(+), 19 deletions(-) diff --git a/cartographer/common/math.h b/cartographer/common/math.h index f35bbad..c4a77ef 100644 --- a/cartographer/common/math.h +++ b/cartographer/common/math.h @@ -60,12 +60,9 @@ constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; } // Bring the 'difference' between two angles into [-pi; pi]. template T NormalizeAngleDifference(T difference) { - while (difference > M_PI) { - difference -= 2. * M_PI; - } - while (difference < -M_PI) { - difference += 2. * M_PI; - } + const T kPi = T(M_PI); + while (difference > kPi) difference -= 2. * kPi; + while (difference < -kPi) difference += 2. * kPi; return difference; } diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h index 6066ab2..94484d1 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h @@ -32,10 +32,10 @@ static std::array ComputeUnscaledError( const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, -sin_theta_i * delta_x + cos_theta_i * delta_y, end[2] - start[2]}; - return {{relative_pose.translation().x() - h[0], - relative_pose.translation().y() - h[1], - common::NormalizeAngleDifference(relative_pose.rotation().angle() - - h[2])}}; + return {{T(relative_pose.translation().x()) - h[0], + T(relative_pose.translation().y()) - h[1], + common::NormalizeAngleDifference( + T(relative_pose.rotation().angle()) - h[2])}}; } template @@ -74,9 +74,9 @@ static std::array ComputeUnscaledError( transform::RotationQuaternionToAngleAxisVector( h_rotation_inverse * relative_pose.rotation().cast()); - return {{relative_pose.translation().x() - h_translation[0], - relative_pose.translation().y() - h_translation[1], - relative_pose.translation().z() - h_translation[2], + return {{T(relative_pose.translation().x()) - h_translation[0], + T(relative_pose.translation().y()) - h_translation[1], + T(relative_pose.translation().z()) - h_translation[2], angle_axis_difference[0], angle_axis_difference[1], angle_axis_difference[2]}}; } @@ -111,13 +111,13 @@ std::array SlerpQuaternions(const T* const start, const T* const end, // interval, then the quaternions are likely to be collinear. T prev_scale(1. - factor); T next_scale(factor); - if (abs_cos_theta < 1. - 1e-5) { + if (abs_cos_theta < T(1. - 1e-5)) { const T theta = acos(abs_cos_theta); const T sin_theta = sin(theta); prev_scale = sin((1. - factor) * theta) / sin_theta; next_scale = sin(factor * theta) / sin_theta; } - if (cos_theta < 0.) { + if (cos_theta < T(0.)) { next_scale = -next_scale; } return {{prev_scale * start[0] + next_scale * end[0], diff --git a/cartographer/transform/transform.h b/cartographer/transform/transform.h index 8c69cec..b84f8bc 100644 --- a/cartographer/transform/transform.h +++ b/cartographer/transform/transform.h @@ -64,10 +64,10 @@ Eigen::Matrix RotationQuaternionToAngleAxisVector( // angle that represents this orientation. if (normalized_quaternion.w() < 0.) { // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 - normalized_quaternion.w() = -1.* normalized_quaternion.w(); - normalized_quaternion.x() = -1.* normalized_quaternion.x(); - normalized_quaternion.y() = -1.* normalized_quaternion.y(); - normalized_quaternion.z() = -1.* normalized_quaternion.z(); + normalized_quaternion.w() = -1. * normalized_quaternion.w(); + normalized_quaternion.x() = -1. * normalized_quaternion.x(); + normalized_quaternion.y() = -1. * normalized_quaternion.y(); + normalized_quaternion.z() = -1. * normalized_quaternion.z(); } // We convert the normalized_quaternion into a vector along the rotation axis // with length of the rotation angle.