12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/internal/mapping_2d/local_trajectory_builder.cc
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;
} }

2
cartographer/internal/mapping_3d/local_trajectory_builder.cc
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()},

2
cartographer/mapping_2d/pose_graph/constraint_builder.cc
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);

4
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
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(

2
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h
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,

5
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
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()

2
cartographer/mapping_3d/pose_graph/constraint_builder.cc
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},

4
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc
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(

2
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h
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,

2
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc
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();