From 0084f14c1ce34c45274f154c9c24e73a1dff1ff5 Mon Sep 17 00:00:00 2001 From: gaschler Date: Fri, 24 Nov 2017 12:26:14 +0100 Subject: [PATCH] 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. --- .../internal/mapping_2d/local_trajectory_builder.cc | 7 ++++--- .../internal/mapping_3d/local_trajectory_builder.cc | 2 +- cartographer/mapping_2d/pose_graph/constraint_builder.cc | 2 +- .../mapping_2d/scan_matching/ceres_scan_matcher.cc | 4 ++-- cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h | 2 +- .../mapping_2d/scan_matching/ceres_scan_matcher_test.cc | 5 +++-- cartographer/mapping_3d/pose_graph/constraint_builder.cc | 2 +- .../mapping_3d/scan_matching/ceres_scan_matcher.cc | 4 ++-- cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h | 2 +- .../mapping_3d/scan_matching/ceres_scan_matcher_test.cc | 2 +- 10 files changed, 17 insertions(+), 15 deletions(-) diff --git a/cartographer/internal/mapping_2d/local_trajectory_builder.cc b/cartographer/internal/mapping_2d/local_trajectory_builder.cc index 03b43ad..5b6fb95 100644 --- a/cartographer/internal/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_2d/local_trajectory_builder.cc @@ -73,9 +73,10 @@ std::unique_ptr LocalTrajectoryBuilder::ScanMatch( auto pose_observation = common::make_unique(); ceres::Solver::Summary summary; - ceres_scan_matcher_.Match( - pose_prediction, initial_ceres_pose, filtered_gravity_aligned_point_cloud, - matching_submap->probability_grid(), pose_observation.get(), &summary); + ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose, + filtered_gravity_aligned_point_cloud, + matching_submap->probability_grid(), + pose_observation.get(), &summary); return pose_observation; } diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index 897ff7f..d0b7135 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/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()}, diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.cc b/cartographer/mapping_2d/pose_graph/constraint_builder.cc index 89d62af..1940be7 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.cc +++ b/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); diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index b209db7..10a05ef 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/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( new TranslationDeltaCostFunctor(options_.translation_weight(), - previous_pose.translation())), + target_translation)), nullptr, ceres_pose_estimate); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h index 955121f..1d89dab 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h +++ b/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, diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc index 03a5730..39e36d0 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc +++ b/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_, - probability_grid_, &pose, &summary); + ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, + 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() diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder.cc index 149e704..1f5024f 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/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}, diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index 5d1bb1f..d5fe0e0 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/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& point_clouds_and_hybrid_grids, @@ -106,7 +106,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationDeltaCostFunctor(options_.translation_weight(), - previous_pose.translation())), + target_translation)), nullptr, ceres_pose.translation()); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h index 2ee349f..b7ce2d5 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h +++ b/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& point_clouds_and_hybrid_grids, diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc index ed2310b..69dd2a3 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/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();