New fixes for Jets. (#1023)

Issue #1015

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 <typename T>
 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;
 }
 

cartographer/mapping/internal/pose_graph/cost_helpers_impl.h

@@ -32,10 +32,10 @@ static std::array<T, 3> 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 <typename T>
@@ -74,9 +74,9 @@ static std::array<T, 6> ComputeUnscaledError(
       transform::RotationQuaternionToAngleAxisVector(
           h_rotation_inverse * relative_pose.rotation().cast<T>());
 
-  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<T, 4> 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],

cartographer/transform/transform.h

@@ -64,10 +64,10 @@ Eigen::Matrix<T, 3, 1> 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.