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.
master
gaschler 2017-11-24 12:26:14 +01:00 committed by Wally B. Feed
parent 0819e52a9c
commit 0084f14c1c
10 changed files with 17 additions and 15 deletions

View File

@ -73,9 +73,10 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder::ScanMatch(
auto pose_observation = common::make_unique<transform::Rigid2d>(); auto pose_observation = common::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary; 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; return pose_observation;
} }

View File

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

View File

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

View File

@ -59,7 +59,7 @@ CeresScanMatcher::CeresScanMatcher(
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 transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid, const ProbabilityGrid& probability_grid,
@ -83,7 +83,7 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>( new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor(options_.translation_weight(), new TranslationDeltaCostFunctor(options_.translation_weight(),
previous_pose.translation())), target_translation)),
nullptr, ceres_pose_estimate); nullptr, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.); CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(

View File

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

View File

@ -63,8 +63,9 @@ class CeresScanMatcherTest : public ::testing::Test {
const transform::Rigid2d expected_pose = const transform::Rigid2d expected_pose =
transform::Rigid2d::Translation({-0.5, 0.5}); transform::Rigid2d::Translation({-0.5, 0.5});
ceres::Solver::Summary summary; 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_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2))
<< "Actual: " << transform::ToProto(pose).DebugString() << "Actual: " << transform::ToProto(pose).DebugString()

View File

@ -221,7 +221,7 @@ void ConstraintBuilder::ComputeConstraint(
// CSM estimate. // CSM estimate.
ceres::Solver::Summary unused_summary; ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform; 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, match_result->pose_estimate,
{{&constant_data->high_resolution_point_cloud, {{&constant_data->high_resolution_point_cloud,
submap_scan_matcher->high_resolution_hybrid_grid}, submap_scan_matcher->high_resolution_hybrid_grid},

View File

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

View File

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

View File

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