Move interpolation of poses to 'cost_helpers'. (#992)

cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h

@@ -55,44 +55,16 @@ class LandmarkCostFunction2D {
   bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
                   const T* const landmark_rotation,
                   const T* const landmark_translation, T* const e) const {
-    const std::array<T, 3> interpolated_pose_translation{
+    const std::tuple<std::array<T, 4>, std::array<T, 3>>
-        {prev_node_pose[0] +
+        interpolated_rotation_and_translation = InterpolateNodes2D(
-             interpolation_parameter_ * (next_node_pose[0] - prev_node_pose[0]),
+            prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
-         prev_node_pose[1] +
+            next_node_gravity_alignment_, interpolation_parameter_);
-             interpolation_parameter_ * (next_node_pose[1] - prev_node_pose[1]),
-         T(0)}};
-
-    // The following is equivalent to (Embed3D(prev_pose) *
-    // Rigid3d::Rotation(prev_pose_gravity_alignment)).rotation().
-    const Eigen::Quaternion<T> prev_quaternion(
-        (Eigen::AngleAxis<T>(prev_node_pose[2],
-                             Eigen::Matrix<T, 3, 1>::UnitZ()) *
-         prev_node_gravity_alignment_.cast<T>())
-            .normalized());
-    const std::array<T, 4> prev_node_rotation = {
-        {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
-         prev_quaternion.z()}};
-
-    // The following is equivalent to (Embed3D(next_pose) *
-    // Rigid3d::Rotation(next_pose_gravity_alignment)).rotation().
-    const Eigen::Quaternion<T> next_quaternion(
-        (Eigen::AngleAxis<T>(next_node_pose[2],
-                             Eigen::Matrix<T, 3, 1>::UnitZ()) *
-         next_node_gravity_alignment_.cast<T>())
-            .normalized());
-    const std::array<T, 4> next_node_rotation = {
-        {next_quaternion.w(), next_quaternion.x(), next_quaternion.y(),
-         next_quaternion.z()}};
-
-    const std::array<T, 4> interpolated_pose_rotation =
-        SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),
-                         T(interpolation_parameter_));
-
     const std::array<T, 6> error = ScaleError(
-        ComputeUnscaledError(landmark_to_tracking_transform_,
+        ComputeUnscaledError(
-                             interpolated_pose_rotation.data(),
+            landmark_to_tracking_transform_,
-                             interpolated_pose_translation.data(),
+            std::get<0>(interpolated_rotation_and_translation).data(),
-                             landmark_rotation, landmark_translation),
+            std::get<1>(interpolated_rotation_and_translation).data(),
+            landmark_rotation, landmark_translation),
         T(translation_weight_), T(rotation_weight_));
     std::copy(std::begin(error), std::end(error), e);
     return true;

cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h

@@ -60,24 +60,16 @@ class LandmarkCostFunction3D {
                   const T* const next_node_translation,
                   const T* const landmark_rotation,
                   const T* const landmark_translation, T* const e) const {
-    const std::array<T, 3> interpolated_pose_translation{
+    const std::tuple<std::array<T, 4>, std::array<T, 3>>
-        {prev_node_translation[0] +
+        interpolated_rotation_and_translation = InterpolateNodes3D(
-             interpolation_parameter_ *
+            prev_node_rotation, prev_node_translation, next_node_rotation,
-                 (next_node_translation[0] - prev_node_translation[0]),
+            next_node_translation, interpolation_parameter_);
-         prev_node_translation[1] +
-             interpolation_parameter_ *
-                 (next_node_translation[1] - prev_node_translation[1]),
-         prev_node_translation[2] +
-             interpolation_parameter_ *
-                 (next_node_translation[2] - prev_node_translation[2])}};
-    const std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
-        prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
-
     const std::array<T, 6> error = ScaleError(
-        ComputeUnscaledError(landmark_to_tracking_transform_,
+        ComputeUnscaledError(
-                             interpolated_pose_rotation.data(),
+            landmark_to_tracking_transform_,
-                             interpolated_pose_translation.data(),
+            std::get<0>(interpolated_rotation_and_translation).data(),
-                             landmark_rotation, landmark_translation),
+            std::get<1>(interpolated_rotation_and_translation).data(),
+            landmark_rotation, landmark_translation),
         T(translation_weight_), T(rotation_weight_));
     std::copy(std::begin(error), std::end(error), e);
     return true;

cartographer/mapping/internal/pose_graph/cost_helpers.h

@@ -59,6 +59,27 @@ std::array<T, 6> ScaleError(std::array<T, 6> error, T translation_weight,
 template <typename T>
 std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
                                   T factor);
+
+// Interpolates 3D poses. Linear interpolation is performed for translation and
+// spherical-linear one for rotation.
+template <typename T>
+std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
+InterpolateNodes3D(const T* const prev_node_rotation,
+                   const T* const prev_node_translation,
+                   const T* const next_node_rotation,
+                   const T* const next_node_translation,
+                   const double interpolation_parameter);
+
+// Embeds 2D poses into 3D and interpolates them. Linear interpolation is
+// performed for translation and spherical-linear one for rotation.
+template <typename T>
+std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
+InterpolateNodes2D(const T* const prev_node_pose,
+                   const Eigen::Quaterniond& prev_node_gravity_alignment,
+                   const T* const next_node_pose,
+                   const Eigen::Quaterniond& next_node_gravity_alignment,
+                   const double interpolation_parameter);
+
 }  // namespace pose_graph
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/pose_graph/cost_helpers_impl.h

@@ -120,6 +120,66 @@ std::array<T, 4> SlerpQuaternions(const T* const start, const T* const end,
            prev_scale * start[3] + next_scale * end[3]}};
 }
 
+template <typename T>
+std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
+InterpolateNodes3D(const T* const prev_node_rotation,
+                   const T* const prev_node_translation,
+                   const T* const next_node_rotation,
+                   const T* const next_node_translation,
+                   const double interpolation_parameter) {
+  return std::make_tuple(
+      SlerpQuaternions(prev_node_rotation, next_node_rotation,
+                       T(interpolation_parameter)),
+      std::array<T, 3>{
+          {prev_node_translation[0] +
+               interpolation_parameter *
+                   (next_node_translation[0] - prev_node_translation[0]),
+           prev_node_translation[1] +
+               interpolation_parameter *
+                   (next_node_translation[1] - prev_node_translation[1]),
+           prev_node_translation[2] +
+               interpolation_parameter *
+                   (next_node_translation[2] - prev_node_translation[2])}});
+}
+
+template <typename T>
+std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
+InterpolateNodes2D(const T* const prev_node_pose,
+                   const Eigen::Quaterniond& prev_node_gravity_alignment,
+                   const T* const next_node_pose,
+                   const Eigen::Quaterniond& next_node_gravity_alignment,
+                   const double interpolation_parameter) {
+  // The following is equivalent to (Embed3D(prev_node_pose) *
+  // Rigid3d::Rotation(prev_node_gravity_alignment)).rotation().
+  const Eigen::Quaternion<T> prev_quaternion(
+      (Eigen::AngleAxis<T>(prev_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
+       prev_node_gravity_alignment.cast<T>())
+          .normalized());
+  const std::array<T, 4> prev_node_rotation = {
+      {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
+       prev_quaternion.z()}};
+
+  // The following is equivalent to (Embed3D(next_node_pose) *
+  // Rigid3d::Rotation(next_node_gravity_alignment)).rotation().
+  const Eigen::Quaternion<T> next_quaternion(
+      (Eigen::AngleAxis<T>(next_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
+       next_node_gravity_alignment.cast<T>())
+          .normalized());
+  const std::array<T, 4> next_node_rotation = {
+      {next_quaternion.w(), next_quaternion.x(), next_quaternion.y(),
+       next_quaternion.z()}};
+
+  return std::make_tuple(
+      SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),
+                       T(interpolation_parameter)),
+      std::array<T, 3>{
+          {prev_node_pose[0] + interpolation_parameter *
+                                   (next_node_pose[0] - prev_node_pose[0]),
+           prev_node_pose[1] + interpolation_parameter *
+                                   (next_node_pose[1] - prev_node_pose[1]),
+           T(0)}});
+}
+
 }  // namespace pose_graph
 }  // namespace mapping
 }  // namespace cartographer