Rename arguments of CostFunctors to target_ (#700)

Rename constant targets of cost functors to target_*.
Pass only translation to Translation cost functors.

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc

@@ -83,7 +83,7 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
   problem.AddResidualBlock(
       new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
           new TranslationDeltaCostFunctor(options_.translation_weight(),
-                                          previous_pose)),
+                                          previous_pose.translation())),
       nullptr, ceres_pose_estimate);
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(

cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h

@@ -18,20 +18,19 @@
 #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
 
 #include "Eigen/Core"
-#include "cartographer/transform/transform.h"
 
 namespace cartographer {
 namespace mapping_2d {
 namespace scan_matching {
 
-// Computes the cost of rotating the initial pose estimate. Cost increases with
+// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
-// the solution's distance from the initial estimate.
+// the solution's distance from 'target_angle'.
 class RotationDeltaCostFunctor {
  public:
   // Constructs a new RotationDeltaCostFunctor for the given 'angle'.
   explicit RotationDeltaCostFunctor(const double scaling_factor,
-                                    const double angle)
+                                    const double target_angle)
-      : scaling_factor_(scaling_factor), angle_(angle) {}
+      : scaling_factor_(scaling_factor), angle_(target_angle) {}
 
   RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
   RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;

cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h

@@ -18,24 +18,22 @@
 #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
 
 #include "Eigen/Core"
-#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping_2d {
 namespace scan_matching {
 
-// Computes the cost of translating the initial pose estimate. Cost increases
+// Computes the cost of translating 'pose' to 'target_translation'.
-// with the solution's distance from the initial estimate.
+// Cost increases with the solution's distance from 'target_translation'.
 class TranslationDeltaCostFunctor {
  public:
   // Constructs a new TranslationDeltaCostFunctor from the given
-  // 'initial_pose_estimate' (x, y, theta).
+  // 'target_translation' (x, y).
   explicit TranslationDeltaCostFunctor(
-      const double scaling_factor,
+      const double scaling_factor, const Eigen::Vector2d& target_translation)
-      const transform::Rigid2d& initial_pose_estimate)
       : scaling_factor_(scaling_factor),
-        x_(initial_pose_estimate.translation().x()),
+        x_(target_translation.x()),
-        y_(initial_pose_estimate.translation().y()) {}
+        y_(target_translation.y()) {}
 
   TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
   TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc

@@ -106,7 +106,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
   problem.AddResidualBlock(
       new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>(
           new TranslationDeltaCostFunctor(options_.translation_weight(),
-                                          previous_pose)),
+                                          previous_pose.translation())),
       nullptr, ceres_pose.translation());
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(

cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h

@@ -20,27 +20,24 @@
 #include <cmath>
 
 #include "Eigen/Core"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/transform.h"
 #include "ceres/rotation.h"
 
 namespace cartographer {
 namespace mapping_3d {
 namespace scan_matching {
 
-// Computes the cost of rotating the pose estimate. Cost increases with the
+// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
-// solution's distance from the rotation estimate.
+// Cost increases with the solution's distance from 'target_rotation'.
 class RotationDeltaCostFunctor {
  public:
-  // Constructs a new RotationDeltaCostFunctor from the given
+  // Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
-  // 'rotation_estimate'.
   explicit RotationDeltaCostFunctor(const double scaling_factor,
-                                    const Eigen::Quaterniond& initial_rotation)
+                                    const Eigen::Quaterniond& target_rotation)
       : scaling_factor_(scaling_factor) {
-    initial_rotation_inverse_[0] = initial_rotation.w();
+    target_rotation_inverse_[0] = target_rotation.w();
-    initial_rotation_inverse_[1] = -initial_rotation.x();
+    target_rotation_inverse_[1] = -target_rotation.x();
-    initial_rotation_inverse_[2] = -initial_rotation.y();
+    target_rotation_inverse_[2] = -target_rotation.y();
-    initial_rotation_inverse_[3] = -initial_rotation.z();
+    target_rotation_inverse_[3] = -target_rotation.z();
   }
 
   RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
@@ -49,10 +46,10 @@ class RotationDeltaCostFunctor {
   template <typename T>
   bool operator()(const T* const rotation_quaternion, T* residual) const {
     T delta[4];
-    T initial_rotation_inverse[4] = {
+    T target_rotation_inverse[4] = {
-        T(initial_rotation_inverse_[0]), T(initial_rotation_inverse_[1]),
+        T(target_rotation_inverse_[0]), T(target_rotation_inverse_[1]),
-        T(initial_rotation_inverse_[2]), T(initial_rotation_inverse_[3])};
+        T(target_rotation_inverse_[2]), T(target_rotation_inverse_[3])};
-    ceres::QuaternionProduct(initial_rotation_inverse, rotation_quaternion,
+    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.
@@ -64,7 +61,7 @@ class RotationDeltaCostFunctor {
 
  private:
   const double scaling_factor_;
-  double initial_rotation_inverse_[4];
+  double target_rotation_inverse_[4];
 };
 
 }  // namespace scan_matching

cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h

@@ -18,25 +18,23 @@
 #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
 
 #include "Eigen/Core"
-#include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping_3d {
 namespace scan_matching {
 
-// Computes the cost of translating the initial pose estimate. Cost increases
+// Computes the cost of translating 'translation' to 'target_translation'.
-// with the solution's distance from the initial estimate.
+// Cost increases with the solution's distance from 'target_translation'.
 class TranslationDeltaCostFunctor {
  public:
   // Constructs a new TranslationDeltaCostFunctor from the given
-  // 'initial_pose_estimate'.
+  // 'target_translation'.
   explicit TranslationDeltaCostFunctor(
-      const double scaling_factor,
+      const double scaling_factor, const Eigen::Vector3d& target_translation)
-      const transform::Rigid3d& initial_pose_estimate)
       : scaling_factor_(scaling_factor),
-        x_(initial_pose_estimate.translation().x()),
+        x_(target_translation.x()),
-        y_(initial_pose_estimate.translation().y()),
+        y_(target_translation.y()),
-        z_(initial_pose_estimate.translation().z()) {}
+        z_(target_translation.z()) {}
 
   TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
   TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =