Add a 'cost_helpers' library. (#862)

* Add a 'cost_helpers' library.

* Change naming and add comments.

cartographer/mapping/pose_graph/cost_helpers.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_
+#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace mapping {
+namespace pose_graph {
+
+// Computes the error between the given relative pose and the difference of
+// poses 'start' and 'end' which are both in an arbitrary common frame.
+//
+// 'start' and 'end' poses have the format [x, y, rotation].
+template <typename T>
+static std::array<T, 3> ComputeUnscaledError2d(
+    const transform::Rigid2d& relative_pose, const T* const start,
+    const T* const end);
+
+// Computes the error between the given relative pose and the difference of
+// poses 'start' and 'end' which are both in an arbitrary common frame.
+//
+// 'start' and 'end' translation has the format [x, y, z].
+// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3].
+template <typename T>
+static std::array<T, 6> ComputeUnscaledError3d(
+    const transform::Rigid3d& relative_pose, const T* const start_rotation,
+    const T* const start_translation, const T* const end_rotation,
+    const T* const end_translation);
+
+}  // namespace pose_graph
+}  // namespace mapping
+}  // namespace cartographer
+
+#include "cartographer/mapping/pose_graph/cost_helpers_impl.h"
+
+#endif  // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_

cartographer/mapping/pose_graph/cost_helpers_impl.h

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_
+#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_
+
+namespace cartographer {
+namespace mapping {
+namespace pose_graph {
+
+template <typename T>
+static std::array<T, 3> ComputeUnscaledError2d(
+    const transform::Rigid2d& relative_pose, const T* const start,
+    const T* const end) {
+  const T cos_theta_i = cos(start[2]);
+  const T sin_theta_i = sin(start[2]);
+  const T delta_x = end[0] - start[0];
+  const T delta_y = end[1] - start[1];
+  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],
+           T(relative_pose.translation().y()) - h[1],
+           common::NormalizeAngleDifference(
+               T(relative_pose.rotation().angle()) - h[2])}};
+}
+
+template <typename T>
+static std::array<T, 6> ComputeUnscaledError3d(
+    const transform::Rigid3d& relative_pose, const T* const start_rotation,
+    const T* const start_translation, const T* const end_rotation,
+    const T* const end_translation) {
+  const Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1],
+                                         -start_rotation[2],
+                                         -start_rotation[3]);
+
+  const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
+                                     end_translation[1] - start_translation[1],
+                                     end_translation[2] - start_translation[2]);
+  const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
+
+  const Eigen::Quaternion<T> h_rotation_inverse =
+      Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2],
+                           -end_rotation[3]) *
+      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
+                           start_rotation[2], start_rotation[3]);
+
+  const Eigen::Matrix<T, 3, 1> angle_axis_difference =
+      transform::RotationQuaternionToAngleAxisVector(
+          h_rotation_inverse * relative_pose.rotation().cast<T>());
+
+  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]}};
+}
+
+}  // namespace pose_graph
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_

cartographer/mapping/pose_graph/landmark_cost_function.h

@@ -19,8 +19,8 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
+#include "cartographer/mapping/pose_graph/cost_helpers.h"
 #include "cartographer/mapping/pose_graph_interface.h"
-#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -62,7 +62,7 @@ std::array<T, 4> SlerpQuaternions(const T* const prev_rotation,
 class LandmarkCostFunction {
  public:
   using LandmarkObservation =
-      mapping::PoseGraph::LandmarkNode::LandmarkObservation;
+      mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
   static ceres::CostFunction* CreateAutoDiffCostFunction(
       const LandmarkObservation& observation, common::Time prev_node_time,
@@ -99,13 +99,9 @@ class LandmarkCostFunction {
     std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
         prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
 
-    // TODO(pifon2a): Move functions common for all cost functions outside of
+    const std::array<T, 6> unscaled_error = ComputeUnscaledError3d(
-    // SpaCostFunction scope.
-    const std::array<T, 6> unscaled_error =
-        mapping_3d::pose_graph::SpaCostFunction::ComputeUnscaledError(
         landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
-            interpolated_pose_translation, landmark_rotation,
+        interpolated_pose_translation, landmark_rotation, landmark_translation);
-            landmark_translation);
 
     e[0] = T(translation_weight_) * unscaled_error[0];
     e[1] = T(translation_weight_) * unscaled_error[1];

cartographer/mapping/pose_graph/landmark_cost_function_test.cc

@@ -29,7 +29,7 @@ using ::testing::DoubleEq;
 using ::testing::ElementsAre;
 
 using LandmarkObservation =
-    mapping::PoseGraph::LandmarkNode::LandmarkObservation;
+    mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
 
 TEST(LandmarkCostFunctionTest, SmokeTest) {
   auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction(

cartographer/mapping_2d/pose_graph/spa_cost_function.h

@@ -23,6 +23,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping/pose_graph.h"
+#include "cartographer/mapping/pose_graph/cost_helpers.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -50,34 +51,14 @@ class SpaCostFunction {
     return true;
   }
 
-  // Computes the error between the node-to-submap alignment 'zbar_ij' and the
-  // difference of submap pose 'c_i' and node pose 'c_j' which are both in an
-  // arbitrary common frame.
-  template <typename T>
-  static std::array<T, 3> ComputeUnscaledError(
-      const transform::Rigid2d& zbar_ij, const T* const c_i,
-      const T* const c_j) {
-    const T cos_theta_i = cos(c_i[2]);
-    const T sin_theta_i = sin(c_i[2]);
-    const T delta_x = c_j[0] - c_i[0];
-    const T delta_y = c_j[1] - c_i[1];
-    const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
-                    -sin_theta_i * delta_x + cos_theta_i * delta_y,
-                    c_j[2] - c_i[2]};
-    return {{T(zbar_ij.translation().x()) - h[0],
-             T(zbar_ij.translation().y()) - h[1],
-             common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) -
-                                              h[2])}};
-  }
-
   // Computes the error scaled by 'translation_weight' and 'rotation_weight',
   // storing it in 'e'.
   template <typename T>
   static void ComputeScaledError(const Constraint::Pose& pose,
                                  const T* const c_i, const T* const c_j,
                                  T* const e) {
-    const std::array<T, 3> e_ij =
+    const std::array<T, 3> e_ij = mapping::pose_graph::ComputeUnscaledError2d(
-        ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
+        transform::Project2D(pose.zbar_ij), c_i, c_j);
     e[0] = e_ij[0] * T(pose.translation_weight);
     e[1] = e_ij[1] * T(pose.translation_weight);
     e[2] = e_ij[2] * T(pose.rotation_weight);

cartographer/mapping_3d/pose_graph/spa_cost_function.h

@@ -23,6 +23,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
 #include "cartographer/mapping/pose_graph.h"
+#include "cartographer/mapping/pose_graph/cost_helpers.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
@@ -53,39 +54,6 @@ class SpaCostFunction {
     return true;
   }
 
-  // Computes the error between the node-to-submap alignment 'zbar_ij' and the
-  // difference of submap pose 'c_i' and node pose 'c_j' which are both in an
-  // arbitrary common frame.
-  template <typename T>
-  static std::array<T, 6> ComputeUnscaledError(
-      const transform::Rigid3d& zbar_ij, const T* const c_i_rotation,
-      const T* const c_i_translation, const T* const c_j_rotation,
-      const T* const c_j_translation) {
-    const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1],
-                                           -c_i_rotation[2], -c_i_rotation[3]);
-
-    const Eigen::Matrix<T, 3, 1> delta(c_j_translation[0] - c_i_translation[0],
-                                       c_j_translation[1] - c_i_translation[1],
-                                       c_j_translation[2] - c_i_translation[2]);
-    const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
-
-    const Eigen::Quaternion<T> h_rotation_inverse =
-        Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
-                             -c_j_rotation[2], -c_j_rotation[3]) *
-        Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
-                             c_i_rotation[3]);
-
-    const Eigen::Matrix<T, 3, 1> angle_axis_difference =
-        transform::RotationQuaternionToAngleAxisVector(
-            h_rotation_inverse * zbar_ij.rotation().cast<T>());
-
-    return {{T(zbar_ij.translation().x()) - h_translation[0],
-             T(zbar_ij.translation().y()) - h_translation[1],
-             T(zbar_ij.translation().z()) - h_translation[2],
-             angle_axis_difference[0], angle_axis_difference[1],
-             angle_axis_difference[2]}};
-  }
-
   // Computes the error scaled by 'translation_weight' and 'rotation_weight',
   // storing it in 'e'.
   template <typename T>
@@ -94,9 +62,9 @@ class SpaCostFunction {
                                  const T* const c_i_translation,
                                  const T* const c_j_rotation,
                                  const T* const c_j_translation, T* const e) {
-    const std::array<T, 6> e_ij =
+    const std::array<T, 6> e_ij = mapping::pose_graph::ComputeUnscaledError3d(
-        ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation,
+        pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation,
-                             c_j_rotation, c_j_translation);
+        c_j_translation);
     for (int ij : {0, 1, 2}) {
       e[ij] = e_ij[ij] * T(pose.translation_weight);
     }