CeresScanMatcher target_translation (#702)

For its translation cost function, CeresScanMatcher now takes
a target translation as an argument instead of a pose, which 
was confusing.

cartographer/internal/mapping_2d/local_trajectory_builder.cc

@@ -73,9 +73,10 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
 
   auto pose_observation = common::make_unique<transform::Rigid2d>();
   ceres::Solver::Summary summary;
-  ceres_scan_matcher_.Match(
+  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
-      pose_prediction, initial_ceres_pose, filtered_gravity_aligned_point_cloud,
+                            filtered_gravity_aligned_point_cloud,
-      matching_submap->probability_grid(), pose_observation.get(), &summary);
+                            matching_submap->probability_grid(),
+                            pose_observation.get(), &summary);
   return pose_observation;
 }
 

cartographer/internal/mapping_3d/local_trajectory_builder.cc

@@ -158,7 +158,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
     return nullptr;
   }
   ceres_scan_matcher_->Match(
-      matching_submap->local_pose().inverse() * pose_prediction,
+      (matching_submap->local_pose().inverse() * pose_prediction).translation(),
       initial_ceres_pose,
       {{&high_resolution_point_cloud_in_tracking,
         &matching_submap->high_resolution_hybrid_grid()},

cartographer/mapping_2d/pose_graph/constraint_builder.cc

@@ -208,7 +208,7 @@ void ConstraintBuilder::ComputeConstraint(
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
-  ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
+  ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                             constant_data->filtered_gravity_aligned_point_cloud,
                             *submap_scan_matcher->probability_grid,
                             &pose_estimate, &unused_summary);

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc

@@ -59,7 +59,7 @@ CeresScanMatcher::CeresScanMatcher(
 
 CeresScanMatcher::~CeresScanMatcher() {}
 
-void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
+void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
                              const transform::Rigid2d& initial_pose_estimate,
                              const sensor::PointCloud& point_cloud,
                              const ProbabilityGrid& probability_grid,
@@ -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.translation())),
+                                          target_translation)),
       nullptr, ceres_pose_estimate);
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h

@@ -46,7 +46,7 @@ class CeresScanMatcher {
   // Aligns 'point_cloud' within the 'probability_grid' given an
   // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
   // 'summary'.
-  void Match(const transform::Rigid2d& previous_pose,
+  void Match(const Eigen::Vector2d& target_translation,
              const transform::Rigid2d& initial_pose_estimate,
              const sensor::PointCloud& point_cloud,
              const ProbabilityGrid& probability_grid,

cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc

@@ -63,8 +63,9 @@ class CeresScanMatcherTest : public ::testing::Test {
     const transform::Rigid2d expected_pose =
         transform::Rigid2d::Translation({-0.5, 0.5});
     ceres::Solver::Summary summary;
-    ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_,
+    ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
-                               probability_grid_, &pose, &summary);
+                               point_cloud_, probability_grid_, &pose,
+                               &summary);
     EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
     EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2))
         << "Actual: " << transform::ToProto(pose).DebugString()

cartographer/mapping_3d/pose_graph/constraint_builder.cc

@@ -221,7 +221,7 @@ void ConstraintBuilder::ComputeConstraint(
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
   transform::Rigid3d constraint_transform;
-  ceres_scan_matcher_.Match(match_result->pose_estimate,
+  ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
                             match_result->pose_estimate,
                             {{&constant_data->high_resolution_point_cloud,
                               submap_scan_matcher->high_resolution_hybrid_grid},

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc

@@ -68,7 +68,7 @@ CeresScanMatcher::CeresScanMatcher(
   ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
 }
 
-void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
+void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
                              const transform::Rigid3d& initial_pose_estimate,
                              const std::vector<PointCloudAndHybridGridPointers>&
                                  point_clouds_and_hybrid_grids,
@@ -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.translation())),
+                                          target_translation)),
       nullptr, ceres_pose.translation());
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h

@@ -48,7 +48,7 @@ class CeresScanMatcher {
   // Aligns 'point_clouds' within the 'hybrid_grids' given an
   // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
   // 'summary'.
-  void Match(const transform::Rigid3d& previous_pose,
+  void Match(const Eigen::Vector3d& target_translation,
              const transform::Rigid3d& initial_pose_estimate,
              const std::vector<PointCloudAndHybridGridPointers>&
                  point_clouds_and_hybrid_grids,

cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc

@@ -67,7 +67,7 @@ class CeresScanMatcherTest : public ::testing::Test {
     transform::Rigid3d pose;
 
     ceres::Solver::Summary summary;
-    ceres_scan_matcher_->Match(initial_pose, initial_pose,
+    ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
                                {{&point_cloud_, &hybrid_grid_}}, &pose,
                                &summary);
     EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();