Purge some additional jets (#1000)

cartographer/common/math.h

@@ -61,10 +61,10 @@ constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
 template <typename T>
 T NormalizeAngleDifference(T difference) {
   while (difference > M_PI) {
-    difference -= T(2. * M_PI);
+    difference -= 2. * M_PI;
   }
   while (difference < -M_PI) {
-    difference += T(2. * M_PI);
+    difference += 2. * M_PI;
   }
   return difference;
 }
@@ -74,6 +74,15 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
   return ceres::atan2(vector.y(), vector.x());
 }
 
+template <typename T>
+inline void QuaternionProduct(const double* const z, const T* const w,
+                              T* const zw) {
+  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
+  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
+  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
+  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
+}
+
 }  // namespace common
 }  // namespace cartographer
 

cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h

@@ -64,9 +64,9 @@ class OccupiedSpaceCostFunction2D {
       const Eigen::Matrix<T, 3, 1> world = transform * point;
       interpolator.Evaluate(
           (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
-              T(kPadding),
+              double(kPadding),
           (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
-              T(kPadding),
+              double(kPadding),
           &residual[i]);
       residual[i] = scaling_factor_ * (1. - residual[i]);
     }

cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h

@@ -20,6 +20,7 @@
 #include <cmath>
 
 #include "Eigen/Core"
+#include "cartographer/common/math.h"
 #include "ceres/ceres.h"
 #include "ceres/rotation.h"
 
@@ -41,12 +42,9 @@ class RotationDeltaCostFunctor3D {
 
   template <typename T>
   bool operator()(const T* const rotation_quaternion, T* residual) const {
-    T delta[4];
+    std::array<T, 4> delta;
-    T target_rotation_inverse[4] = {
+    common::QuaternionProduct(target_rotation_inverse_, rotation_quaternion,
-        T(target_rotation_inverse_[0]), T(target_rotation_inverse_[1]),
+                              delta.data());
-        T(target_rotation_inverse_[2]), T(target_rotation_inverse_[3])};
-    ceres::QuaternionProduct(target_rotation_inverse, rotation_quaternion,
-                             delta);
     // Will compute the squared norm of the imaginary component of the delta
     // quaternion which is sin(phi/2)^2.
     residual[0] = scaling_factor_ * delta[1];

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 {{T(relative_pose.translation().x()) - h[0],
+  return {{relative_pose.translation().x() - h[0],
-           T(relative_pose.translation().y()) - h[1],
+           relative_pose.translation().y() - h[1],
-           common::NormalizeAngleDifference(
+           common::NormalizeAngleDifference(relative_pose.rotation().angle() -
-               T(relative_pose.rotation().angle()) - h[2])}};
+                                            h[2])}};
 }
 
 template <typename T>
@@ -72,9 +72,9 @@ static std::array<T, 6> ComputeUnscaledError(
       transform::RotationQuaternionToAngleAxisVector(
           h_rotation_inverse * relative_pose.rotation().cast<T>());
 
-  return {{T(relative_pose.translation().x()) - h_translation[0],
+  return {{relative_pose.translation().x() - h_translation[0],
-           T(relative_pose.translation().y()) - h_translation[1],
+           relative_pose.translation().y() - h_translation[1],
-           T(relative_pose.translation().z()) - h_translation[2],
+           relative_pose.translation().z() - h_translation[2],
            angle_axis_difference[0], angle_axis_difference[1],
            angle_axis_difference[2]}};
 }
@@ -107,13 +107,15 @@ 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 < T(1. - 1e-5)) {
+  if (abs_cos_theta < 1. - 1e-5) {
     const T theta = acos(abs_cos_theta);
     const T sin_theta = sin(theta);
-    prev_scale = sin(prev_scale * theta) / sin_theta;
+    prev_scale = sin((1. - factor) * theta) / sin_theta;
-    next_scale = sin(next_scale * theta) / sin_theta;
+    next_scale = sin(factor * theta) / sin_theta;
+  }
+  if (cos_theta < 0.) {
+    next_scale = -next_scale;
   }
-  if (cos_theta < T(0.)) next_scale = -next_scale;
   return {{prev_scale * start[0] + next_scale * end[0],
            prev_scale * start[1] + next_scale * end[1],
            prev_scale * start[2] + next_scale * end[2],

cartographer/transform/transform.h

@@ -64,17 +64,17 @@ 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() *= T(-1.);
+    normalized_quaternion.w() *= -1.;
-    normalized_quaternion.x() *= T(-1.);
+    normalized_quaternion.x() *= -1.;
-    normalized_quaternion.y() *= T(-1.);
+    normalized_quaternion.y() *= -1.;
-    normalized_quaternion.z() *= T(-1.);
+    normalized_quaternion.z() *= -1.;
   }
   // We convert the normalized_quaternion into a vector along the rotation axis
   // with length of the rotation angle.
-  const T angle = T(2.) * atan2(normalized_quaternion.vec().norm(),
+  const T angle =
-                                normalized_quaternion.w());
+      2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
   constexpr double kCutoffAngle = 1e-7;  // We linearize below this angle.
-  const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.));
+  const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);
   return Eigen::Matrix<T, 3, 1>(scale * normalized_quaternion.x(),
                                 scale * normalized_quaternion.y(),
                                 scale * normalized_quaternion.z());